demo.cpp
Go to the documentation of this file.
00001 /*
00002 ROBOOP -- A robotics object oriented package in C++
00003 Copyright (C) 1996-2004  Richard Gourdeau
00004 
00005 This library is free software; you can redistribute it and/or modify
00006 it under the terms of the GNU Lesser General Public License as
00007 published by the Free Software Foundation; either version 2.1 of the
00008 License, or (at your option) any later version.
00009 
00010 This library is distributed in the hope that it will be useful,
00011 but WITHOUT ANY WARRANTY; without even the implied warranty of
00012 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00013 GNU Lesser General Public License for more details.
00014 
00015 You should have received a copy of the GNU Lesser General Public
00016 License along with this library; if not, write to the Free Software
00017 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00018 
00019 
00020 Report problems and direct all questions to:
00021 
00022 Richard Gourdeau
00023 Professeur Agrege
00024 Departement de genie electrique
00025 Ecole Polytechnique de Montreal
00026 C.P. 6079, Succ. Centre-Ville
00027 Montreal, Quebec, H3C 3A7
00028 
00029 email: richard.gourdeau@polymtl.ca
00030 
00031 -------------------------------------------------------------------------------
00032 Revision_history:
00033 
00034 2003/02/03: Etienne Lachance
00035    -Added quaternions example in homogen_demo.
00036    -Using function set_plot2d for gnuplot graphs. 
00037    -Declare vector with "dof" instead of hardcoded value.
00038    -Changed RobotMotor to Robot.
00039 
00040 2003/29/04: Etienne Lachance
00041    -Using Robot("conf/puma560_dh.conf", "PUMA560_DH") in kinematics functions.
00042 
00043 2004/07/01: Ethan Tira-Thompson
00044     -Added support for newmat's use_namespace #define, using ROBOOP namespace
00045 
00046 2004/08/13: Etienne Lachance
00047     -Modified robot initialisation matrix.
00048 
00049 2005/11/15 : Richard Gourdeau
00050     - Fixed error on PUMA560 without motor dynamics
00051 -------------------------------------------------------------------------------
00052 
00053 */
00054 
00062 
00063 static const char rcsid[] = "$Id: demo.cpp,v 1.34 2006/05/16 16:27:43 gourdeau Exp $";
00064 
00065 #include "gnugraph.h"
00066 #include "quaternion.h"
00067 #include "robot.h"
00068 #include "utils.h"
00069 
00070 #ifdef use_namespace
00071 using namespace ROBOOP;
00072 #endif
00073 
00074 void homogen_demo(void);
00075 void kinematics_demo(void);
00076 void dynamics_demo(void);
00077 
00078 
00079 int main(void)
00080 {
00081    cout << "=====================================================\n";
00082    cout << " ROBOOP -- A robotics object oriented package in C++ \n";;
00083    cout << " DEMO program \n";
00084    cout << "=====================================================\n";
00085    cout << "\n";
00086 
00087    homogen_demo();
00088    kinematics_demo();
00089    dynamics_demo();
00090 
00091    return(0);
00092 }
00093 
00094 
00095 void homogen_demo(void)
00096 {
00097    ColumnVector p1(3), p2(3), p3(3);
00098    Real ott[] = {1.0,2.0,3.0};
00099    Real tto[] = {3.0,2.0,1.0};
00100 
00101    cout << "\n";
00102    cout << "=====================================================\n";
00103    cout << "Homogeneous transformations\n";
00104    cout << "=====================================================\n";
00105    cout << "\n";
00106    cout << "Translation of [1;2;3]\n";
00107    p1 << ott;
00108    cout << setw(7) << setprecision(3) << trans(p1);
00109    cout << "\n";
00110    cout << "\n";
00111    cout << "Rotation of pi/6 around axis X\n";
00112    cout << setw(7) << setprecision(3) << rotx(M_PI/6);
00113    cout << "\n";
00114 
00115    cout << "\n";
00116    cout << "Rotation of pi/8 around axis Y\n";
00117    cout << setw(7) << setprecision(3) << roty(M_PI/8);
00118    cout << "\n";
00119 
00120    cout << "\n";
00121    cout << "Rotation of pi/3 around axis Z\n";
00122    cout << setw(7) << setprecision(3) << rotz(M_PI/3);
00123    cout << "\n";
00124 
00125    cout << "\n";
00126    cout << "Rotation of pi/3 around axis [1;2;3]\n";
00127    cout << setw(7) << setprecision(3) << rotk(M_PI/3,p1);
00128    cout << "\n";
00129 
00130    cout << "\n";
00131    cout << "Rotation of pi/6 around line [1;2;3] -- [3;2;1]\n";
00132    p2 << tto;
00133    cout << setw(7) << setprecision(3) << rotd(M_PI/3,p1,p2);
00134    cout << "\n";
00135 
00136    cout << "\n";
00137    cout << "Roll Pitch Yaw Rotation [1;2;3]\n";
00138    cout << setw(7) << setprecision(3) << rpy(p1);
00139    cout << "\n";
00140 
00141    cout << "\n";
00142    cout << "Euler ZXZ Rotation [3;2;1]\n";
00143    cout << setw(7) << setprecision(3) << eulzxz(p2);
00144    cout << "\n";
00145 
00146    cout << "\n";
00147    cout << "Inverse of Rotation around axis\n";
00148    cout << setw(7) << setprecision(3) << irotk(rotk(M_PI/3,p1));
00149    cout << "\n";
00150 
00151    cout << "\n";
00152    cout << "Inverse of Roll Pitch Yaw Rotation\n";
00153    cout << setw(7) << setprecision(3) << irpy(rpy(p1));
00154    cout << "\n";
00155 
00156    cout << "\n";
00157    cout << "Inverse of Euler ZXZ Rotation\n";
00158    cout << setw(7) << setprecision(3) << ieulzxz(eulzxz(p2));
00159    cout << "\n";
00160 
00161    cout << "\n";
00162    cout << "Cross product between [1;2;3] and [3;2;1]\n";
00163    cout << setw(7) << setprecision(3) << CrossProduct(p1,p2);
00164    cout << "\n";
00165 
00166    cout << "\n";
00167    cout << "Rotation matrix from quaternion\n";
00168    ColumnVector axis(3); axis(1)=axis(2)=0; axis(3)=1.0;
00169    Quaternion q(M_PI/4, axis);
00170    cout << setw(7) << setprecision(3) << q.R();
00171    cout << "\n";
00172 
00173    cout << "\n";
00174    cout << "Transformation matrix from quaternion\n";
00175    cout << setw(7) << setprecision(3) << q.T();
00176 
00177    cout << "\n";
00178    cout << "Quaternion from rotation matrix\n";
00179    Quaternion qq(q.R());
00180    cout << setw(7) << setprecision(3) << qq.s() << endl;
00181    cout << setw(7) << setprecision(3) << qq.v() << endl;
00182    cout << "\n";
00183 
00184    cout << "\n";
00185    cout << "Quaternion from transformation matrix\n";
00186    qq = Quaternion(q.T());
00187    cout << setw(7) << setprecision(3) << qq.s() << endl;
00188    cout << setw(7) << setprecision(3) << qq.v() << endl;
00189 }
00190 
00191 const Real RR_data[] =
00192   {0, 0, 0, 1.0, 0, 0, 0, 0, 2.0,-0.5, 0, 0, 0, 0, 0, 0.1666666, 0, 0.1666666, 0, 0, 0, 0, 0,
00193    0, 0, 0, 1.0, 0, 0, 0, 0, 1.0,-0.5, 0, 0, 0, 0, 0, 0.0833333, 0, 0.0833333, 0, 0, 0, 0, 0};
00194 const Real RR_data_mdh[] =
00195   {0, 0, 0, 1.0, 0, 0, 0, 0, 2.0, 0.5, 0, 0, 0, 0, 0, 0.1666666, 0, 0.1666666, 0, 0, 0, 0, 0,
00196    0, 0, 0, 1.0, 0, 0, 0, 0, 1.0, 0.5, 0, 0, 0, 0, 0, 0.0833333, 0, 0.0833333, 0, 0, 0, 0, 0};
00197 const Real RR_data_mdh_min_para[] =
00198   {0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0.0, 0, 0,     0, 0, 0, 0, 0.0, 1.666666, 0, 0, 0, 0, 0,
00199    0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0.5, 0, 0, -0.25, 0, 0, 0, 0.0, 0.3333333, 0, 0, 0, 0, 0};
00200 
00201 const Real RP_data[] =
00202   {0, 0, 0, 0, -M_PI/2.0, 0, 0, 0, 2.0, 0, 0, 0.0, 1.0, 0, 0, 1.0, 0, 1.0, 0, 0, 0, 0, 0,
00203    1, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0,-1.0, 0.0833333, 0, 0, 0.0833333, 0, 0.0833333, 0, 0, 0, 0, 0};
00204 const Real PUMA560_data[] =
00205   {0, 0, 0, 0, M_PI/2.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.35, 0, 0, 0, 
00206    0, 0, 0, 0.4318, 0, 0, 0, 0, 17.4, -0.3638, 0.006, 0.2275, 0.13, 0, 0, 0.524, 0, 0.539, 0, 
00207    0, 0, 0.15005, 0.0203, -M_PI/2.0, 0, 0, 0, 4.8, -0.0203, -0.0141, 0.07, 0.066, 0, 0, 0.086, 0, 0.0125, 0,
00208    0, 0, 0.4318, 0.0, M_PI/2.0, 0, 0, 0, 0.82, 0, 0.019, 0, 0.0018, 0, 0, 0.0013, 0, 0.0018, 0, 
00209    0, 0, 0, 0.0, -M_PI/2.0, 0, 0, 0, 0.34, 0.0, 0.0, 0.0, 0.0003, 0.0, 0.0, 0.0004, 0.0, 0.0003, 0, 
00210    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, 0};
00211 const Real PUMA560_motor[] =
00212   {200e-6, -62.6111, 1.48e-3, (.395 +.435)/2, /* using + and - directions average */
00213    200e-6, 107.815, .817e-3, (.126 + .071)/2,
00214    200e-6, -53.7063, 1.38e-3, (.132     + .105)/2,
00215    33e-6,   76.0364, 71.2e-6, (11.2e-3 + 16.9e-3)/2,
00216    33e-6,   71.923, 82.6e-6, (9.26e-3 + 14.5e-3)/2,
00217    33e-6,   76.686, 36.7e-6, (3.96e-3 + 10.5e-3)/2};
00218 
00219 const Real STANFORD_data[] =
00220   {0.0, 0.0, 0.4120, 0.0, -M_PI/2, 0,0,0,9.29, 0.0, 0.0175, -0.1105, 0.276, 0.0, 0, 0.255, 0.0, 0.071,0,0,0,0,0,
00221    0.0, 0.0, 0.1540, 0.0, M_PI/2.0, 0,0,0,5.01, 0.0, -1.054, 0.0, 0.108, 0.0, 0.0, 0.018, 0.0, 0.1,0,0,0,0,0,
00222    1.0, -M_PI/2.0, 0.0, 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,0,0,0,0,0,
00223    0.0, 0.0, 0.0, 0.0, -M_PI/2.0, 0,0,0,1.08, 0.0, 0.092, -0.054, 0.002, 0.0, 0.0, 0.001, 0.0, 0.001,0,0,0,0,0,
00224    0.0, 0.0, 0.0, 0.0,  M_PI/2.0, 0,0,0,0.63, 0.0, 0.0, 0.566, 0.003, 0.0, 0.0, 0.003, 0.0, 0.0004,0,0,0,0,0,
00225    0.0, 0.0, 0.2630, 0.0, 0.0, 0,0,0,0.51, 0.0, 0.0, 1.5540, 0.013, 0.0, 0.0, 0.013, 0.0, 0.0003,0,0,0,0,0};
00226 
00227 Robot robot;
00228 Matrix K;
00229 ColumnVector q0;
00230 
00231 ReturnMatrix xdot(Real t, const Matrix & x)
00232 {
00233    int ndof;
00234    ColumnVector q, qp, qpp; /* position, velocities and accelerations */
00235    ColumnVector tau, dx;              /* torque and state space error */
00236    Matrix xd;
00237 
00238    ndof = robot.get_dof();                             /* get the # of dof  */
00239    q = x.SubMatrix(1,ndof,1,1);               /* position, velocities */
00240    qp = x.SubMatrix(ndof+1,2*ndof,1,1);          /* from state vector */
00241    qpp = ColumnVector(ndof);
00242    qpp = 0.0;                               /* set the vector to zero */
00243    //   tau = robot.torque(q0,qpp,qpp); /* compute the gravity torque */
00244    tau = ColumnVector(ndof);
00245    tau = 0.0;
00246 
00247    dx = (q-q0) & qp;       /* compute dx using vertical concatenation */
00248    tau = tau - K*dx;                           /* feedback correction */
00249    qpp = robot.acceleration(q, qp, tau);              /* acceleration */
00250    xd = qp & qpp;                          /* state vector derivative */
00251 
00252    xd.Release(); return xd;
00253 }
00254 
00255 void kinematics_demo(void)
00256 {
00257    Matrix initrob(2,23), Tobj;
00258    ColumnVector qs, qr;
00259    int dof = 0;
00260    int i;
00261 
00262    cout << "\n";
00263    cout << "=====================================================\n";
00264    cout << "Robot RR kinematics\n";
00265    cout << "=====================================================\n";
00266    initrob << RR_data;
00267    robot = Robot(initrob);
00268    dof = robot.get_dof();
00269 
00270    cout << "\n";
00271    cout << "Robot D-H parameters\n";
00272    cout << "   type     theta      d        a      alpha\n";
00273    cout << setw(7) << setprecision(3) << initrob.SubMatrix(1,dof,1,5);
00274    cout << "\n";
00275    cout << "Robot joints variables\n";
00276    cout << setw(7) << setprecision(3) << robot.get_q();
00277    cout << "\n";
00278    cout << "Robot position\n";
00279    cout << setw(7) << setprecision(3) << robot.kine();
00280    cout << "\n";
00281    qr = ColumnVector(dof);
00282    qr = M_PI/4.0;
00283    robot.set_q(qr);
00284    cout << "Robot joints variables\n";
00285    cout << setw(7) << setprecision(3) << robot.get_q();
00286    cout << "\n";
00287    cout << "Robot position\n";
00288    cout << setw(7) << setprecision(3) << robot.kine();
00289    cout << "\n";
00290    cout << "Robot Jacobian w/r to base frame\n";
00291    cout << setw(7) << setprecision(3) << robot.jacobian();
00292    cout << "\n";
00293    cout << "Robot Jacobian w/r to tool frame\n";
00294    cout << setw(7) << setprecision(3) << robot.jacobian(qr.Nrows());
00295    cout << "\n";
00296    for (i = 1; i <= qr.Nrows(); i++) {
00297       cout << "Robot position partial derivative with respect to joint " << i << " \n";
00298       cout << setw(7) << setprecision(3) << robot.dTdqi(i);
00299       cout << "\n";
00300    }
00301    Tobj = robot.kine();
00302    qs = ColumnVector(dof);
00303    qs = M_PI/16.0;
00304    robot.set_q(qs);
00305    cout << "Robot inverse kinematics\n";
00306    cout << "  q start  q final  q real\n";
00307    cout << setw(7) << setprecision(3) << (qs | robot.inv_kin(Tobj) | qr);
00308    cout << "\n";
00309    cout << "\n";
00310 
00311    cout << "=====================================================\n";
00312    cout << "Robot RP kinematics\n";
00313    cout << "=====================================================\n";
00314    initrob << RP_data;
00315    robot = Robot(initrob);
00316    dof = robot.get_dof();
00317    cout << "\n";
00318    cout << "Robot D-H parameters\n";
00319    cout << "  type    theta     d       a     alpha\n";
00320    cout << setw(7) << setprecision(3) << initrob.SubMatrix(1,dof,1,5);
00321    cout << "\n";
00322    cout << "Robot joints variables\n";
00323    cout << setw(7) << setprecision(3) << robot.get_q();
00324    cout << "\n";
00325    cout << "Robot position\n";
00326    cout << setw(7) << setprecision(3) << robot.kine();
00327    cout << "\n";
00328    robot.set_q(M_PI/4.0,1);
00329    robot.set_q(4.0,2);
00330    cout << "Robot joints variables\n";
00331    cout << setw(7) << setprecision(3) << robot.get_q();
00332    cout << "\n";
00333    cout << "Robot position\n";
00334    cout << setw(7) << setprecision(3) << robot.kine();
00335    cout << "\n";
00336    cout << "Robot Jacobian w/r to base frame\n";
00337    cout << setw(7) << setprecision(3) << robot.jacobian();
00338    cout << "\n";
00339    qr = robot.get_q();
00340    cout << "Robot Jacobian w/r to tool frame\n";
00341    cout << setw(7) << setprecision(3) << robot.jacobian(qr.Nrows());
00342    cout << "\n";
00343    for (i = 1; i <= qr.Nrows(); i++) {
00344       cout << "Robot position partial derivative with respect to joint " << i << " \n";
00345       cout << setw(7) << setprecision(3) << robot.dTdqi(i);
00346       cout << "\n";
00347    }
00348    Tobj = robot.kine();
00349    robot.set_q(M_PI/16.0,1);
00350    robot.set_q(1.0,2);
00351    qs = robot.get_q();
00352    cout << "Robot inverse kinematics\n";
00353    cout << " q start q final q real\n";
00354    cout << setw(7) << setprecision(3) << (qs | robot.inv_kin(Tobj) | qr);
00355    cout << "\n";
00356    cout << "\n";
00357 
00358    cout << "=====================================================\n";
00359    cout << "Robot PUMA560 kinematics\n";
00360    cout << "=====================================================\n";
00361    robot = Robot("conf/puma560_dh.conf", "PUMA560_DH");
00362    dof = robot.get_dof();
00363    cout << "\n";
00364    cout << "Robot joints variables\n";
00365    cout << setw(7) << setprecision(3) << robot.get_q();
00366    cout << "\n";
00367    cout << "Robot position\n";
00368    cout << setw(7) << setprecision(3) << robot.kine();
00369    cout << "\n";
00370    qr = ColumnVector(dof);
00371    qr = M_PI/4.0;
00372    robot.set_q(qr);
00373    cout << "Robot joints variables\n";
00374    cout << setw(7) << setprecision(3) << robot.get_q();
00375    cout << "\n";
00376    cout << "Robot position\n";
00377    cout << setw(7) << setprecision(3) << robot.kine();
00378    cout << "\n";
00379    cout << "Robot Jacobian w/r to base frame\n";
00380    cout << setw(7) << setprecision(3) << robot.jacobian();
00381    cout << "\n";
00382    cout << "Robot Jacobian w/r to tool frame\n";
00383    cout << setw(7) << setprecision(3) << robot.jacobian(qr.Nrows());
00384    cout << "\n";
00385    for (i = 1; i <= qr.Nrows(); i++) {
00386       cout << "Robot position partial derivative with respect to joint " << i << " \n";
00387       cout << setw(7) << setprecision(3) << robot.dTdqi(i);
00388       cout << "\n";
00389    }
00390    Tobj = robot.kine();
00391    qs = ColumnVector(dof);
00392    qs = 1.0;
00393    qs(1) = M_PI/16.0;
00394    robot.set_q(qs);
00395    cout << "Robot inverse kinematics\n";
00396    cout << " q start q final q real\n";
00397    cout << setw(7) << setprecision(3) << (qs | robot.inv_kin(Tobj) | qr);
00398    cout << "\n";
00399    cout << "\n";
00400    cout << "=====================================================\n";
00401    cout << "Robot STANFORD kinematics\n";
00402    cout << "=====================================================\n";
00403    initrob = Matrix(6,23);
00404    initrob << STANFORD_data;
00405    robot = Robot(initrob);
00406    dof = robot.get_dof();
00407    cout << "\n";
00408    cout << "Robot D-H parameters\n";
00409    cout << "  type    theta     d       a     alpha\n";
00410    cout << setw(7) << setprecision(3) << initrob.SubMatrix(1,dof,1,5);
00411    cout << "\n";
00412    cout << "Robot joints variables\n";
00413    cout << setw(7) << setprecision(3) << robot.get_q();
00414    cout << "\n";
00415    cout << "Robot position\n";
00416    cout << setw(7) << setprecision(3) << robot.kine();
00417    cout << "\n";
00418    qr = ColumnVector(dof);
00419    qr = M_PI/4.0;
00420    robot.set_q(qr);
00421    cout << "Robot joints variables\n";
00422    cout << setw(7) << setprecision(3) << robot.get_q();
00423    cout << "\n";
00424    cout << "Robot position\n";
00425    cout << setw(7) << setprecision(3) << robot.kine();
00426    cout << "\n";
00427    cout << "Robot Jacobian w/r to base frame\n";
00428    cout << setw(7) << setprecision(3) << robot.jacobian();
00429    cout << "\n";
00430    cout << "Robot Jacobian w/r to tool frame\n";
00431    cout << setw(7) << setprecision(3) << robot.jacobian(qr.Nrows());
00432    cout << "\n";
00433    for (i = 1; i <= qr.Nrows(); i++) {
00434       cout << "Robot position partial derivative with respect to joint " << i << " \n";
00435       cout << setw(7) << setprecision(3) << robot.dTdqi(i);
00436       cout << "\n";
00437    }
00438    Tobj = robot.kine();
00439    qs = ColumnVector(dof);
00440    qs = 1.0;
00441    qs(1) = M_PI/16.0;
00442    robot.set_q(qs);
00443    cout << "Robot inverse kinematics\n";
00444    cout << " q start q final q real\n";
00445    cout << setw(7) << setprecision(3) << (qs | robot.inv_kin(Tobj) | qr);
00446    cout << "\n";
00447    cout << "\n";
00448 }
00449 
00450 void dynamics_demo(void)
00451 {
00452    int nok, nbad, dof = 0;
00453    Matrix initrob(2,23), Tobj, xout;
00454    ColumnVector qs, qr;
00455    RowVector tout;
00456    int i;
00457 
00458    cout << "\n";
00459    cout << "=====================================================\n";
00460    cout << "Robot RR dynamics\n";
00461    cout << "=====================================================\n";
00462    initrob << RR_data;
00463    robot = Robot(initrob);
00464    dof = robot.get_dof();
00465    cout << "\n";
00466    cout << "Robot D-H parameters\n";
00467    cout << "  type    theta     d       a     alpha\n";
00468    cout << setw(7) << setprecision(3) << initrob.SubMatrix(1,dof,1,5);
00469    cout << "\n";
00470    cout << "Robot D-H inertial parameters\n";
00471    cout << "  mass     cx       cy      cz     Ixx     Ixy     Ixz     Iyy     Iyz     Izz\n";
00472    cout << setw(7) << setprecision(3) << initrob.SubMatrix(1,dof,9,18);
00473    cout << "\n";
00474    cout << "Robot joints variables\n";
00475    cout << setw(7) << setprecision(3) << robot.get_q();
00476    cout << "\n";
00477    cout << "Robot Inertia matrix\n";
00478    cout << setw(7) << setprecision(3) << robot.inertia(robot.get_q());
00479    cout << "\n";
00480 
00481    K = Matrix(dof,2*dof);
00482    K = 0.0;
00483    K(1,1) = K(2,2) = 25.0;      /* K = [25I 7.071I ] */
00484    K(1,3) = K(2,4) = 7.071;
00485    cout << "Feedback gain matrix K\n";
00486    cout << setw(7) << setprecision(3) << K;
00487    cout << "\n";
00488    q0 = ColumnVector(dof);
00489    q0 = M_PI/4.0;
00490    qs = ColumnVector(2*dof);
00491    qs = 0.0;
00492 
00493    cout << " time     ";
00494    for(i = 1; i <= dof; i++)
00495       cout <<"q" << i << "      ";
00496    for(i = 1; i <= dof; i++)
00497       cout <<"qp" << i << "     ";
00498    cout << endl;
00499 
00500    /*   Runge_Kutta4(xdot, qs, 0.0, 4.0, 160, tout, xout);
00501    replaced by adaptive step size */
00502    odeint(xdot, qs, 0.0, 4.0, 1e-4,0.1, 1e-12, nok, nbad, tout, xout, 0.05);
00503    cout << setw(7) << setprecision(3) << (tout & xout).t();
00504    cout << "\n";
00505    cout << "\n";
00506 
00507 
00508    set_plot2d("Robot joints position", "time (sec)", "q(i) (rad)", "q", DATAPOINTS,
00509               tout, xout, 1, dof);
00510 
00511    /* uncomment the following two lines to obtain a
00512       ps file of the graph */
00513    /*   plotposition.addcommand("set term post");
00514       plotposition.addcommand("set output \"demo.ps\"");*/
00515 
00516    set_plot2d("Robot joints velocity", "time (sec)", "qp(i) (rad/s)", "qp", DATAPOINTS,
00517               tout, xout, dof+1, 2*dof);
00518 
00519 
00520    cout << "=====================================================\n";
00521    cout << "Robot RP dynamics\n";
00522    cout << "=====================================================\n";
00523    initrob << RP_data;
00524    robot = Robot(initrob);
00525    dof = robot.get_dof();
00526    cout << "\n";
00527    cout << "Robot D-H parameters\n";
00528    cout << "  type    theta     d       a     alpha\n";
00529    cout << setw(7) << setprecision(3) << initrob.SubMatrix(1,dof,1,5);
00530    cout << "\n";
00531    cout << "Robot D-H inertial parameters\n";
00532    cout << "  mass     cx       cy      cz     Ixx     Ixy     Ixz     Iyy     Iyz     Izz\n";
00533    cout << setw(7) << setprecision(3) << initrob.SubMatrix(1,dof,9,18);
00534    cout << "\n";
00535    cout << "Robot joints variables\n";
00536    cout << setw(7) << setprecision(3) << robot.get_q();
00537    cout << "\n";
00538    cout << "Robot Inertia matrix\n";
00539    cout << setw(7) << setprecision(3) << robot.inertia(robot.get_q());
00540    cout << "\n";
00541    cout << "\n";
00542 
00543    cout << "=====================================================\n";
00544    cout << "Robot PUMA560 dynamics\n";
00545    cout << "=====================================================\n";
00546    initrob = Matrix(6,19);
00547    initrob << PUMA560_data;
00548    Matrix temp(6,4); 
00549    temp = 0; /* empty motor dynamics */
00550    robot = Robot((initrob | temp));
00551    dof = robot.get_dof();
00552    cout << "\n";
00553    cout << "Robot D-H parameters\n";
00554    cout << "  type    theta     d       a     alpha\n";
00555    cout << setw(7) << setprecision(3) << initrob.SubMatrix(1,dof,1,5);
00556    cout << "\n";
00557    cout << "Robot D-H inertial parameters\n";
00558    cout << "  mass     cx       cy      cz     Ixx     Ixy     Ixz     Iyy     Iyz     Izz\n";
00559    cout << setw(7) << setprecision(3) << initrob.SubMatrix(1,dof,9,18);
00560    cout << "\n";
00561    cout << "Robot joints variables\n";
00562    cout << setw(7) << setprecision(3) << robot.get_q();
00563    cout << "\n";
00564    cout << "Robot Inertia matrix\n";
00565    cout << setw(8) << setprecision(4) << robot.inertia(robot.get_q());
00566    cout << "\n";
00567    qs = ColumnVector(dof);
00568    qr = ColumnVector(dof);
00569    qs =0.0;
00570    qr =0.0;
00571    cout << "Robot Torque\n";
00572    cout << setw(8) << setprecision(4) << robot.torque(robot.get_q(),qs,qr);
00573    cout << "\n";
00574    cout << "Robot acceleration\n";
00575    cout << setw(8) << setprecision(4) << robot.acceleration(robot.get_q(),qs,qr);
00576    cout << "\n";
00577 
00578    cout << "\n";
00579    cout << "=====================================================\n";
00580    cout << "Robot STANFORD dynamics\n";
00581    cout << "=====================================================\n";
00582    initrob = Matrix(6,23);
00583    initrob << STANFORD_data;
00584    robot = Robot(initrob);
00585    dof = robot.get_dof();
00586    cout << "\n";
00587    cout << "Robot D-H parameters\n";
00588    cout << "  type    theta     d       a     alpha\n";
00589    cout << setw(7) << setprecision(3) << initrob.SubMatrix(1,dof,1,5);
00590    cout << "\n";
00591    cout << "Robot D-H inertial parameters\n";
00592    cout << "  mass     cx       cy      cz     Ixx     Ixy     Ixz     Iyy     Iyz     Izz\n";
00593    cout << setw(7) << setprecision(3) << initrob.SubMatrix(1,dof,9,18);
00594    cout << "\n";
00595    cout << "Robot joints variables\n";
00596    cout << setw(7) << setprecision(3) << robot.get_q();
00597    cout << "\n";
00598    cout << "Robot Inertia matrix\n";
00599    cout << setw(7) << setprecision(3) << robot.inertia(robot.get_q());
00600    cout << "\n";
00601    cout << "\n";
00602 
00603    cout << "=====================================================\n";
00604    cout << "Robot PUMA560 with motors dynamics\n";
00605    cout << "=====================================================\n";
00606    initrob = Matrix(6,19);
00607    initrob << PUMA560_data;
00608    Matrix initrobm = Matrix(6,4);
00609    initrobm << PUMA560_motor;
00610    robot = Robot(initrob,initrobm);
00611    dof =robot.get_dof();
00612    cout << "\n";
00613    cout << "Robot D-H parameters\n";
00614    cout << "  type    theta     d       a     alpha\n";
00615    cout << setw(7) << setprecision(3) << initrob.SubMatrix(1,dof,1,5);
00616    cout << "\n";
00617    cout << "Robot D-H inertial parameters\n";
00618    cout << "  mass     cx       cy      cz     Ixx     Ixy     Ixz     Iyy     Iyz     Izz\n";
00619    cout << setw(7) << setprecision(3) << initrob.SubMatrix(1,dof,9,18);
00620    cout << "\n";
00621    cout << "Robot motors inertia, gear ratio, viscous and Coulomb friction coefficients\n";
00622    cout << "  Im       Gr       B       Cf\n";
00623    cout << setw(7) << setprecision(3) << initrobm;
00624    cout << "\n";
00625    cout << "Robot joints variables\n";
00626    cout << setw(7) << setprecision(3) << robot.get_q();
00627    cout << "\n";
00628    cout << "Robot Inertia matrix\n";
00629    cout << setw(8) << setprecision(4) << robot.inertia(robot.get_q());
00630    cout << "\n";
00631    qs = ColumnVector(dof);
00632    qr = ColumnVector(dof);
00633    qs =0.0;
00634    qr =0.0;
00635    robot.set_q(qs);
00636    cout << "Robot Torque\n";
00637    cout << setw(8) << setprecision(4) << robot.torque(robot.get_q(),qs,qr);
00638    cout << "\n";
00639    cout << "Robot acceleration\n";
00640    cout << setw(8) << setprecision(4) << robot.acceleration(robot.get_q(),qs,qr);
00641    cout << "\n";
00642 }


kni
Author(s): Martin Günther
autogenerated on Mon Aug 14 2017 02:44:12