00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
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,
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;
00235 ColumnVector tau, dx;
00236 Matrix xd;
00237
00238 ndof = robot.get_dof();
00239 q = x.SubMatrix(1,ndof,1,1);
00240 qp = x.SubMatrix(ndof+1,2*ndof,1,1);
00241 qpp = ColumnVector(ndof);
00242 qpp = 0.0;
00243
00244 tau = ColumnVector(ndof);
00245 tau = 0.0;
00246
00247 dx = (q-q0) & qp;
00248 tau = tau - K*dx;
00249 qpp = robot.acceleration(q, qp, tau);
00250 xd = qp & qpp;
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;
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
00501
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
00512
00513
00514
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;
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 }