"Fossies" - the Fresh Open Source Software Archive

Member "heaplayers-351/benchmarks/roboop/source/demo.cpp" (17 Oct 2003, 18863 Bytes) of package /linux/misc/old/heaplayers_3_5_1.tar.gz:


As a special service "Fossies" has tried to format the requested source page into HTML format using (guessed) C and C++ source code syntax highlighting (style: standard) with prefixed line numbers and code folding option. Alternatively you can here view or download the uninterpreted source code file. For more information about "demo.cpp" see the Fossies "Dox" file reference documentation.

    1 /*
    2 ROBOOP -- A robotics object oriented package in C++
    3 Copyright (C) 1996, 1997  Richard Gourdeau
    4 
    5 This library is free software; you can redistribute it and/or
    6 modify it under the terms of the GNU Library General Public
    7 License as published by the Free Software Foundation; either
    8 version 2 of the License, or (at your option) any later version.
    9 
   10 This library is distributed in the hope that it will be useful,
   11 but WITHOUT ANY WARRANTY; without even the implied warranty of
   12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
   13 Library General Public License for more details.
   14 
   15 You should have received a copy of the GNU Library General Public
   16 License along with this library (GNUlgpl.txt); if not, write to the
   17 Free Software Foundation, Inc., 675 Mass Ave, Cambridge,
   18 MA 02139, USA.
   19 
   20 Report problems and direct all questions to:
   21 
   22 Richard Gourdeau
   23 Professeur Agrégé
   24 Departement de mathematiques et de genie industriel
   25 Ecole Polytechnique de Montreal
   26 C.P. 6079, Succ. Centre-Ville
   27 Montreal, Quebec, H3C 3A7
   28 
   29 email: richard.gourdeau@polymtl.ca
   30 */
   31 
   32 static char rcsid[] = "$Id: demo.cpp,v 1.1 2003/10/17 02:39:03 emery Exp $";
   33 
   34 #include "robot.h"
   35 
   36 void homogen_demo(void);
   37 void kinematics_demo(void);
   38 void dynamics_demo(void);
   39 
   40 
   41 int main(void)
   42 {
   43    cout << "=====================================================\n";
   44    cout << " ROBOOP -- A robotics object oriented package in C++ \n";;
   45    cout << " DEMO program \n";
   46    cout << "=====================================================\n";
   47    cout << "\n";
   48 
   49    homogen_demo();
   50    kinematics_demo();
   51    dynamics_demo();
   52 
   53    return(0);
   54 }
   55 
   56 
   57 void homogen_demo(void)
   58 {
   59    ColumnVector p1(3), p2(3), p3(3);
   60    Real ott[] = {1.0,2.0,3.0};
   61    Real tto[] = {3.0,2.0,1.0};
   62 
   63    cout << "\n";
   64    cout << "=====================================================\n";
   65    cout << "Homogeneous transformations\n";
   66    cout << "=====================================================\n";
   67    cout << "\n";
   68    cout << "Translation of [1;2;3]\n";
   69    p1 << ott;
   70    cout << setw(7) << setprecision(3) << trans(p1);
   71    cout << "\n";
   72    cout << "\n";
   73    cout << "Rotation of pi/6 around axis X\n";
   74    cout << setw(7) << setprecision(3) << rotx(M_PI/6);
   75    cout << "\n";
   76 
   77    cout << "\n";
   78    cout << "Rotation of pi/8 around axis Y\n";
   79    cout << setw(7) << setprecision(3) << roty(M_PI/8);
   80    cout << "\n";
   81 
   82    cout << "\n";
   83    cout << "Rotation of pi/3 around axis Z\n";
   84    cout << setw(7) << setprecision(3) << rotz(M_PI/3);
   85    cout << "\n";
   86 
   87    cout << "\n";
   88    cout << "Rotation of pi/3 around axis [1;2;3]\n";
   89    cout << setw(7) << setprecision(3) << rotk(M_PI/3,p1);
   90    cout << "\n";
   91 
   92    cout << "\n";
   93    cout << "Rotation of pi/6 around line [1;2;3] -- [3;2;1]\n";
   94    p2 << tto;
   95    cout << setw(7) << setprecision(3) << rotd(M_PI/3,p1,p2);
   96    cout << "\n";
   97 
   98    cout << "\n";
   99    cout << "Roll Pitch Yaw Rotation [1;2;3]\n";
  100    cout << setw(7) << setprecision(3) << rpy(p1);
  101    cout << "\n";
  102 
  103    cout << "\n";
  104    cout << "Euler ZXZ Rotation [3;2;1]\n";
  105    cout << setw(7) << setprecision(3) << eulzxz(p2);
  106    cout << "\n";
  107 
  108    cout << "\n";
  109    cout << "Inverse of Rotation around axis\n";
  110    cout << setw(7) << setprecision(3) << irotk(rotk(M_PI/3,p1));
  111    cout << "\n";
  112 
  113    cout << "\n";
  114    cout << "Inverse of Roll Pitch Yaw Rotation\n";
  115    cout << setw(7) << setprecision(3) << irpy(rpy(p1));
  116    cout << "\n";
  117 
  118    cout << "\n";
  119    cout << "Inverse of Euler ZXZ Rotation\n";
  120    cout << setw(7) << setprecision(3) << ieulzxz(eulzxz(p2));
  121    cout << "\n";
  122 
  123    cout << "\n";
  124    cout << "Cross product between [1;2;3] and [3;2;1]\n";
  125    cout << setw(7) << setprecision(3) << vec_x_prod(p1,p2);
  126    cout << "\n";
  127 
  128 }
  129 
  130 Real RR_data[] =
  131   {0.0, 0.0, 0.0, 1.0, 0.0, 2.0,-0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1666666, 0.0, 0.1666666,
  132    0.0, 0.0, 0.0, 1.0, 0.0, 1.0,-0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0833333, 0.0, 0.0833333};
  133 Real RP_data[] =
  134   {0.0, 0.0, 0.0, 0.0, -M_PI/2.0, 2.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 1.0,
  135    1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0,-1.0, 0.0833333, 0.0, 0.0, 0.0833333, 0.0, 0.0833333};
  136 Real PUMA560_data[] =
  137   {0.0, 0.0, 0.0, 0.0, M_PI/2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.35, 0.0, 0.0,
  138    0.0, 0.0, 0.0, 0.4318, 0.0, 17.4, -0.3638, 0.006, 0.2275, 0.13, 0.0, 0.0, 0.524, 0.0, 0.539,
  139    0.0, 0.0, 0.15005, 0.0203, -M_PI/2.0, 4.8, -0.0203, -0.0141, 0.07, 0.066, 0.0, 0.0, 0.086, 0.0, 0.0125,
  140    0.0, 0.0, 0.4318, 0.0, M_PI/2.0, 0.82, 0.0, 0.019, 0.0, 0.0018, 0.0, 0.0, 0.0013, 0.0, 0.0018,
  141    0.0, 0.0, 0.0, 0.0, -M_PI/2.0, 0.34, 0.0, 0.0, 0.0, 0.0003, 0.0, 0.0, 0.0004, 0.0, 0.0003,
  142    0.0, 0.0, 0.0, 0.0, 0.0, 0.09, 0.0, 0.0, 0.032, 0.00015, 0.0, 0.0, 0.00015, 0.0, 0.00004};
  143 
  144 Real STANFORD_data[] =
  145   {0.0, 0.0, 0.4120, 0.0, -M_PI/2, 9.29, 0.0, 0.0175, -0.1105, 0.276, 0.0, 0, 0.255, 0.0, 0.071,
  146    0.0, 0.0, 0.1540, 0.0, M_PI/2.0, 5.01, 0.0, -1.054, 0.0, 0.108, 0.0, 0.0, 0.018, 0.0, 0.1,
  147    1.0, -M_PI/2.0, 0.0, 0.0, 0.0, 4.25, 0.0, 0.0, -6.447, 2.51, 0.0, 0.0, 2.51, 0.0, 0.006,
  148    0.0, 0.0, 0.0, 0.0, -M_PI/2.0, 1.08, 0.0, 0.092, -0.054, 0.002, 0.0, 0.0, 0.001, 0.0, 0.001,
  149    0.0, 0.0, 0.0, 0.0,  M_PI/2.0, 0.63, 0.0, 0.0, 0.566, 0.003, 0.0, 0.0, 0.003, 0.0, 0.0004,
  150    0.0, 0.0, 0.2630, 0.0, 0.0, 0.51, 0.0, 0.0, 1.5540, 0.013, 0.0, 0.0, 0.013, 0.0, 0.0003};
  151 
  152 Robot robot;
  153 Matrix K;
  154 ColumnVector q0;
  155 
  156 ReturnMatrix xdot(Real t, const Matrix & x)
  157 {
  158    int ndof;
  159    ColumnVector q, qp, qpp; /* position, velocities and accelerations */
  160    ColumnVector tau, dx;              /* torque and state space error */
  161    Matrix xd;
  162 
  163    ndof = robot.dof;                             /* get the # of dof  */
  164    q = x.SubMatrix(1,ndof,1,1);               /* position, velocities */
  165    qp = x.SubMatrix(ndof+1,2*ndof,1,1);          /* from state vector */
  166    qpp = ColumnVector(ndof);
  167    qpp = 0.0;                               /* set the vector to zero */
  168    tau = robot.torque(q0,qpp,qpp);      /* compute the gravity torque */
  169    dx = (q-q0) & qp;       /* compute dx using vertical concatenation */
  170    tau = tau - K*dx;                           /* feedback correction */
  171    qpp = robot.acceleration(q, qp, tau);              /* acceleration */
  172    xd = qp & qpp;                          /* state vector derivative */
  173 
  174    xd.Release(); return xd;
  175 }
  176 
  177 void kinematics_demo(void)
  178 {
  179    Matrix initrob(2,15), Tobj;
  180    ColumnVector qs, qr;
  181    int i;
  182 
  183    cout << "\n";
  184    cout << "=====================================================\n";
  185    cout << "Robot RR kinematics\n";
  186    cout << "=====================================================\n";
  187    initrob << RR_data;
  188    robot = Robot(initrob);
  189    cout << "\n";
  190    cout << "Robot D-H parameters\n";
  191    cout << "   type     theta      d        a      alpha\n";
  192    cout << setw(7) << setprecision(3) << initrob.SubMatrix(1,2,1,5);
  193    cout << "\n";
  194    cout << "Robot joints variables\n";
  195    cout << setw(7) << setprecision(3) << robot.get_q();
  196    cout << "\n";
  197    cout << "Robot position\n";
  198    cout << setw(7) << setprecision(3) << robot.kine();
  199    cout << "\n";
  200    qr = ColumnVector(2);
  201    qr = M_PI/4.0;
  202    robot.set_q(qr);
  203    cout << "Robot joints variables\n";
  204    cout << setw(7) << setprecision(3) << robot.get_q();
  205    cout << "\n";
  206    cout << "Robot position\n";
  207    cout << setw(7) << setprecision(3) << robot.kine();
  208    cout << "\n";
  209    cout << "Robot Jacobian w/r to base frame\n";
  210    cout << setw(7) << setprecision(3) << robot.jacobian();
  211    cout << "\n";
  212    cout << "Robot Jacobian w/r to tool frame\n";
  213    cout << setw(7) << setprecision(3) << robot.jacobian(qr.Nrows());
  214    cout << "\n";
  215    for (i = 1; i <= qr.Nrows(); i++) {
  216       cout << "Robot position partial derivative with respect to joint " << i << " \n";
  217       cout << setw(7) << setprecision(3) << robot.dTdqi(i);
  218       cout << "\n";
  219    }
  220    Tobj = robot.kine();
  221    qs = ColumnVector(2);
  222    qs = M_PI/16.0;
  223    cout << "Robot inverse kinematics\n";
  224    cout << "  q start  q final  q real\n";
  225    cout << setw(7) << setprecision(3) << (qs | robot.inv_kin(Tobj) | qr);
  226    cout << "\n";
  227    cout << "\n";
  228 
  229    cout << "=====================================================\n";
  230    cout << "Robot RP kinematics\n";
  231    cout << "=====================================================\n";
  232    initrob << RP_data;
  233    robot = Robot(initrob);
  234    cout << "\n";
  235    cout << "Robot D-H parameters\n";
  236    cout << "  type    theta     d       a     alpha\n";
  237    cout << setw(7) << setprecision(3) << initrob.SubMatrix(1,2,1,5);
  238    cout << "\n";
  239    cout << "Robot joints variables\n";
  240    cout << setw(7) << setprecision(3) << robot.get_q();
  241    cout << "\n";
  242    cout << "Robot position\n";
  243    cout << setw(7) << setprecision(3) << robot.kine();
  244    cout << "\n";
  245    robot.set_q(M_PI/4.0,1);
  246    robot.set_q(4.0,2);
  247    cout << "Robot joints variables\n";
  248    cout << setw(7) << setprecision(3) << robot.get_q();
  249    cout << "\n";
  250    cout << "Robot position\n";
  251    cout << setw(7) << setprecision(3) << robot.kine();
  252    cout << "\n";
  253    cout << "Robot Jacobian w/r to base frame\n";
  254    cout << setw(7) << setprecision(3) << robot.jacobian();
  255    cout << "\n";
  256    qr = robot.get_q();
  257    cout << "Robot Jacobian w/r to tool frame\n";
  258    cout << setw(7) << setprecision(3) << robot.jacobian(qr.Nrows());
  259    cout << "\n";
  260    for (i = 1; i <= qr.Nrows(); i++) {
  261       cout << "Robot position partial derivative with respect to joint " << i << " \n";
  262       cout << setw(7) << setprecision(3) << robot.dTdqi(i);
  263       cout << "\n";
  264    }
  265    Tobj = robot.kine();
  266    robot.set_q(M_PI/16.0,1);
  267    robot.set_q(1.0,2);
  268    qs = robot.get_q();
  269    cout << "Robot inverse kinematics\n";
  270    cout << " q start q final q real\n";
  271    cout << setw(7) << setprecision(3) << (qs | robot.inv_kin(Tobj) | qr);
  272    cout << "\n";
  273    cout << "\n";
  274 
  275    cout << "=====================================================\n";
  276    cout << "Robot PUMA560 kinematics\n";
  277    cout << "=====================================================\n";
  278    initrob = Matrix(6,15);
  279    initrob << PUMA560_data;
  280    robot = Robot(initrob);
  281    cout << "\n";
  282    cout << "Robot D-H parameters\n";
  283    cout << "  type    theta     d       a     alpha\n";
  284    cout << setw(7) << setprecision(3) << initrob.SubMatrix(1,6,1,5);
  285    cout << "\n";
  286    cout << "Robot joints variables\n";
  287    cout << setw(7) << setprecision(3) << robot.get_q();
  288    cout << "\n";
  289    cout << "Robot position\n";
  290    cout << setw(7) << setprecision(3) << robot.kine();
  291    cout << "\n";
  292    qr = ColumnVector(6);
  293    qr = M_PI/4.0;
  294    robot.set_q(qr);
  295    cout << "Robot joints variables\n";
  296    cout << setw(7) << setprecision(3) << robot.get_q();
  297    cout << "\n";
  298    cout << "Robot position\n";
  299    cout << setw(7) << setprecision(3) << robot.kine();
  300    cout << "\n";
  301    cout << "Robot Jacobian w/r to base frame\n";
  302    cout << setw(7) << setprecision(3) << robot.jacobian();
  303    cout << "\n";
  304    cout << "Robot Jacobian w/r to tool frame\n";
  305    cout << setw(7) << setprecision(3) << robot.jacobian(qr.Nrows());
  306    cout << "\n";
  307    for (i = 1; i <= qr.Nrows(); i++) {
  308       cout << "Robot position partial derivative with respect to joint " << i << " \n";
  309       cout << setw(7) << setprecision(3) << robot.dTdqi(i);
  310       cout << "\n";
  311    }
  312    Tobj = robot.kine();
  313    qs = ColumnVector(6);
  314    qs = 1.0;
  315    qs(1) = M_PI/16.0;
  316    robot.set_q(qs);
  317    cout << "Robot inverse kinematics\n";
  318    cout << " q start q final q real\n";
  319    cout << setw(7) << setprecision(3) << (qs | robot.inv_kin(Tobj) | qr);
  320    cout << "\n";
  321    cout << "\n";
  322 
  323    cout << "=====================================================\n";
  324    cout << "Robot STANFORD kinematics\n";
  325    cout << "=====================================================\n";
  326    initrob = Matrix(6,15);
  327    initrob << STANFORD_data;
  328    robot = Robot(initrob);
  329    cout << "\n";
  330    cout << "Robot D-H parameters\n";
  331    cout << "  type    theta     d       a     alpha\n";
  332    cout << setw(7) << setprecision(3) << initrob.SubMatrix(1,6,1,5);
  333    cout << "\n";
  334    cout << "Robot joints variables\n";
  335    cout << setw(7) << setprecision(3) << robot.get_q();
  336    cout << "\n";
  337    cout << "Robot position\n";
  338    cout << setw(7) << setprecision(3) << robot.kine();
  339    cout << "\n";
  340    qr = ColumnVector(6);
  341    qr = M_PI/4.0;
  342    robot.set_q(qr);
  343    cout << "Robot joints variables\n";
  344    cout << setw(7) << setprecision(3) << robot.get_q();
  345    cout << "\n";
  346    cout << "Robot position\n";
  347    cout << setw(7) << setprecision(3) << robot.kine();
  348    cout << "\n";
  349    cout << "Robot Jacobian w/r to base frame\n";
  350    cout << setw(7) << setprecision(3) << robot.jacobian();
  351    cout << "\n";
  352    cout << "Robot Jacobian w/r to tool frame\n";
  353    cout << setw(7) << setprecision(3) << robot.jacobian(qr.Nrows());
  354    cout << "\n";
  355    for (i = 1; i <= qr.Nrows(); i++) {
  356       cout << "Robot position partial derivative with respect to joint " << i << " \n";
  357       cout << setw(7) << setprecision(3) << robot.dTdqi(i);
  358       cout << "\n";
  359    }
  360    Tobj = robot.kine();
  361    qs = ColumnVector(6);
  362    qs = 1.0;
  363    qs(1) = M_PI/16.0;
  364    robot.set_q(qs);
  365    cout << "Robot inverse kinematics\n";
  366    cout << " q start q final q real\n";
  367    cout << setw(7) << setprecision(3) << (qs | robot.inv_kin(Tobj) | qr);
  368    cout << "\n";
  369    cout << "\n";
  370 }
  371 
  372 void dynamics_demo(void)
  373 {
  374    int nok, nbad;
  375    Matrix initrob(2,15), Tobj, xout;
  376    ColumnVector qs, qr;
  377    RowVector tout;
  378 
  379    cout << "\n";
  380    cout << "=====================================================\n";
  381    cout << "Robot RR dynamics\n";
  382    cout << "=====================================================\n";
  383    initrob << RR_data;
  384    robot = Robot(initrob);
  385    cout << "\n";
  386    cout << "Robot D-H parameters\n";
  387    cout << "  type    theta     d       a     alpha\n";
  388    cout << setw(7) << setprecision(3) << initrob.SubMatrix(1,2,1,5);
  389    cout << "\n";
  390    cout << "Robot D-H inertial parameters\n";
  391    cout << "  mass     cx       cy      cz     Ixx     Ixy     Ixz     Iyy     Iyz     Izz\n";
  392    cout << setw(7) << setprecision(3) << initrob.SubMatrix(1,2,6,15);
  393    cout << "\n";
  394    cout << "Robot joints variables\n";
  395    cout << setw(7) << setprecision(3) << robot.get_q();
  396    cout << "\n";
  397    cout << "Robot Inertia matrix\n";
  398    cout << setw(7) << setprecision(3) << robot.inertia(robot.get_q());
  399    cout << "\n";
  400    K = Matrix(2,4);
  401    K = 0.0;
  402    K(1,1) = K(2,2) = 25.0;      /* K = [25I 7.071I ] */
  403    K(1,3) = K(2,4) = 7.071;
  404    cout << "Feedback gain matrix K\n";
  405    cout << setw(7) << setprecision(3) << K;
  406    cout << "\n";
  407    q0 = ColumnVector(2);
  408    q0 = M_PI/4.0;
  409    qs = ColumnVector(4);
  410    qs = 0.0;
  411    cout << "  time     q1       q2      q1p    q2p\n";
  412 /*   Runge_Kutta4(xdot, qs, 0.0, 4.0, 160, tout, xout);
  413 replaced by adaptive step size */
  414    odeint(xdot, qs, 0.0, 4.0, 1e-4,0.1, 1e-12, nok, nbad, tout, xout, 0.05);
  415    cout << setw(7) << setprecision(3) << (tout & xout).t();
  416    cout << "\n";
  417    cout << "\n";
  418 #if 0
  419    Plot2d plotposition, plotvelocity;
  420    plotposition.settitle("Robot joints position");
  421    plotposition.setxlabel("time (sec)");
  422    plotposition.setylabel("q1 and q2 (rad)");
  423    plotposition.addcurve((tout & xout.SubMatrix(1,1,1,xout.Ncols())).t(),
  424                         "q1",POINTS);
  425    plotposition.addcurve((tout & xout.SubMatrix(2,2,1,xout.Ncols())).t(),
  426                         "q2",POINTS);
  427    /* uncomment the following two lines to obtain a
  428       ps file of the graph */
  429 /*   plotposition.addcommand("set term post");
  430    plotposition.addcommand("set output \"demo.ps\"");*/
  431    plotposition.gnuplot();
  432 
  433    plotvelocity.settitle("Robot joints velocity");
  434    plotvelocity.setxlabel("time (sec)");
  435    plotvelocity.setylabel("qp1 and qp2 (rad/s)");
  436    plotvelocity.addcurve((tout & xout.SubMatrix(3,3,1,xout.Ncols())).t(),
  437                         "qp1",POINTS);
  438    plotvelocity.addcurve((tout & xout.SubMatrix(4,4,1,xout.Ncols())).t(),
  439                         "qp2",POINTS);
  440    plotvelocity.gnuplot();
  441 #endif
  442 
  443    cout << "=====================================================\n";
  444    cout << "Robot RP dynamics\n";
  445    cout << "=====================================================\n";
  446    initrob << RP_data;
  447    robot = Robot(initrob);
  448    cout << "\n";
  449    cout << "Robot D-H parameters\n";
  450    cout << "  type    theta     d       a     alpha\n";
  451    cout << setw(7) << setprecision(3) << initrob.SubMatrix(1,2,1,5);
  452    cout << "\n";
  453    cout << "Robot D-H inertial parameters\n";
  454    cout << "  mass     cx       cy      cz     Ixx     Ixy     Ixz     Iyy     Iyz     Izz\n";
  455    cout << setw(7) << setprecision(3) << initrob.SubMatrix(1,2,6,15);
  456    cout << "\n";
  457    cout << "Robot joints variables\n";
  458    cout << setw(7) << setprecision(3) << robot.get_q();
  459    cout << "\n";
  460    cout << "Robot Inertia matrix\n";
  461    cout << setw(7) << setprecision(3) << robot.inertia(robot.get_q());
  462    cout << "\n";
  463    cout << "\n";
  464 
  465    cout << "=====================================================\n";
  466    cout << "Robot PUMA560 dynamics\n";
  467    cout << "=====================================================\n";
  468    initrob = Matrix(6,15);
  469    initrob << PUMA560_data;
  470    robot = Robot(initrob);
  471    cout << "\n";
  472    cout << "Robot D-H parameters\n";
  473    cout << "  type    theta     d       a     alpha\n";
  474    cout << setw(7) << setprecision(3) << initrob.SubMatrix(1,6,1,5);
  475    cout << "\n";
  476    cout << "Robot D-H inertial parameters\n";
  477    cout << "  mass     cx       cy      cz     Ixx     Ixy     Ixz     Iyy     Iyz     Izz\n";
  478    cout << setw(7) << setprecision(3) << initrob.SubMatrix(1,6,6,15);
  479    cout << "\n";
  480    cout << "Robot joints variables\n";
  481    cout << setw(7) << setprecision(3) << robot.get_q();
  482    cout << "\n";
  483    cout << "Robot Inertia matrix\n";
  484    cout << setw(8) << setprecision(4) << robot.inertia(robot.get_q());
  485    cout << "\n";
  486    qs = ColumnVector(6);
  487    qr = ColumnVector(6);
  488    qs =0.0;
  489    qr =0.0;
  490    cout << "Robot Torque\n";
  491    cout << setw(8) << setprecision(4) << robot.torque(robot.get_q(),qs,qr);
  492    cout << "\n";
  493    cout << "Robot acceleration\n";
  494    cout << setw(8) << setprecision(4) << robot.acceleration(robot.get_q(),qs,qr);
  495    cout << "\n";
  496 
  497    cout << "\n";
  498    cout << "=====================================================\n";
  499    cout << "Robot STANFORD dynamics\n";
  500    cout << "=====================================================\n";
  501    initrob = Matrix(6,15);
  502    initrob << STANFORD_data;
  503    robot = Robot(initrob);
  504    cout << "\n";
  505    cout << "Robot D-H parameters\n";
  506    cout << "  type    theta     d       a     alpha\n";
  507    cout << setw(7) << setprecision(3) << initrob.SubMatrix(1,6,1,5);
  508    cout << "\n";
  509    cout << "Robot D-H inertial parameters\n";
  510    cout << "  mass     cx       cy      cz     Ixx     Ixy     Ixz     Iyy     Iyz     Izz\n";
  511    cout << setw(7) << setprecision(3) << initrob.SubMatrix(1,6,6,15);
  512    cout << "\n";
  513    cout << "Robot joints variables\n";
  514    cout << setw(7) << setprecision(3) << robot.get_q();
  515    cout << "\n";
  516    cout << "Robot Inertia matrix\n";
  517    cout << setw(7) << setprecision(3) << robot.inertia(robot.get_q());
  518    cout << "\n";
  519 }