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
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00120
00121 static const char rcsid[] = "$Id: robot.cpp,v 1.50 2006/05/16 19:24:26 gourdeau Exp $";
00122
00123 #include <time.h>
00124 #include "config.h"
00125 #include "robot.h"
00126
00127 #ifdef use_namespace
00128 namespace ROBOOP {
00129 using namespace NEWMAT;
00130 #endif
00131
00133 Real fourbyfourident[] = {1.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,1.0};
00134
00136 Real threebythreeident[] ={1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0};
00137
00150 Link::Link(const int jt, const Real it, const Real id, const Real ia, const Real ial,
00151 const Real it_min, const Real it_max, const Real it_off, const Real mass,
00152 const Real cmx, const Real cmy, const Real cmz, const Real ixx, const Real ixy,
00153 const Real ixz, const Real iyy, const Real iyz, const Real izz, const Real iIm,
00154 const Real iGr, const Real iB, const Real iCf, const bool dh,
00155 const bool min_inertial_parameters, const bool immobile_):
00156 R(3,3),
00157 joint_type(jt),
00158 theta(it),
00159 d(id),
00160 a(ia),
00161 alpha(ial),
00162 theta_min(it_min),
00163 theta_max(it_max),
00164 joint_offset(it_off),
00165 DH(dh),
00166 min_para(min_inertial_parameters),
00167 p(3),
00168 m(mass),
00169 Im(iIm),
00170 Gr(iGr),
00171 B(iB),
00172 Cf(iCf),
00173 I(3,3),
00174 immobile(immobile_)
00175 {
00176 if (joint_type == 0)
00177 theta += joint_offset;
00178 else
00179 d += joint_offset;
00180 Real ct, st, ca, sa;
00181 ct = cos(theta);
00182 st = sin(theta);
00183 ca = cos(alpha);
00184 sa = sin(alpha);
00185
00186 qp = qpp = 0.0;
00187
00188 if (DH)
00189 {
00190 R(1,1) = ct;
00191 R(2,1) = st;
00192 R(3,1) = 0.0;
00193 R(1,2) = -ca*st;
00194 R(2,2) = ca*ct;
00195 R(3,2) = sa;
00196 R(1,3) = sa*st;
00197 R(2,3) = -sa*ct;
00198 R(3,3) = ca;
00199
00200 p(1) = a*ct;
00201 p(2) = a*st;
00202 p(3) = d;
00203 }
00204 else
00205 {
00206 R(1,1) = ct;
00207 R(2,1) = st*ca;
00208 R(3,1) = st*sa;
00209 R(1,2) = -st;
00210 R(2,2) = ca*ct;
00211 R(3,2) = sa*ct;
00212 R(1,3) = 0;
00213 R(2,3) = -sa;
00214 R(3,3) = ca;
00215
00216 p(1) = a;
00217 p(2) = -sa*d;
00218 p(3) = ca*d;
00219 }
00220
00221 if (min_para)
00222 {
00223 mc = ColumnVector(3);
00224 mc(1) = cmx;
00225 mc(2) = cmy;
00226 mc(3) = cmz;
00227 }
00228 else
00229 {
00230 r = ColumnVector(3);
00231 r(1) = cmx;
00232 r(2) = cmy;
00233 r(3) = cmz;
00234 }
00235
00236 I(1,1) = ixx;
00237 I(1,2) = I(2,1) = ixy;
00238 I(1,3) = I(3,1) = ixz;
00239 I(2,2) = iyy;
00240 I(2,3) = I(3,2) = iyz;
00241 I(3,3) = izz;
00242 }
00243
00244 void Link::transform(const Real q)
00246 {
00247 if (DH)
00248 {
00249 if(joint_type == 0)
00250 {
00251 Real ct, st, ca, sa;
00252 theta = q + joint_offset;
00253 ct = cos(theta);
00254 st = sin(theta);
00255 ca = R(3,3);
00256 sa = R(3,2);
00257
00258 R(1,1) = ct;
00259 R(2,1) = st;
00260 R(1,2) = -ca*st;
00261 R(2,2) = ca*ct;
00262 R(1,3) = sa*st;
00263 R(2,3) = -sa*ct;
00264 p(1) = a*ct;
00265 p(2) = a*st;
00266 }
00267 else
00268 p(3) = d = q + joint_offset;
00269 }
00270 else
00271 {
00272 Real ca, sa;
00273 ca = R(3,3);
00274 sa = -R(2,3);
00275 if(joint_type == 0)
00276 {
00277 Real ct, st;
00278 theta = q + joint_offset;
00279 ct = cos(theta);
00280 st = sin(theta);
00281
00282 R(1,1) = ct;
00283 R(2,1) = st*ca;
00284 R(3,1) = st*sa;
00285 R(1,2) = -st;
00286 R(2,2) = ca*ct;
00287 R(3,2) = sa*ct;
00288 R(1,3) = 0;
00289 }
00290 else
00291 {
00292 d = q + joint_offset;
00293 p(2) = -sa*d;
00294 p(3) = ca*d;
00295 }
00296 }
00297 }
00298
00299 Real Link::get_q(void) const
00305 {
00306 if(joint_type == 0)
00307 return (theta - joint_offset);
00308 else
00309 return (d - joint_offset);
00310 }
00311
00312
00313 void Link::set_r(const ColumnVector & r_)
00315 {
00316 if(r_.Nrows() == 3)
00317 r = r_;
00318 else
00319 cerr << "Link::set_r: wrong size in input vector." << endl;
00320 }
00321
00322 void Link::set_mc(const ColumnVector & mc_)
00324 {
00325 if(mc_.Nrows() == 3)
00326 mc = mc_;
00327 else
00328 cerr << "Link::set_mc: wrong size in input vector." << endl;
00329 }
00330
00335 void Link::set_I(const Matrix & I_)
00336 {
00337 if( (I_.Nrows() == 3) && (I_.Ncols() == 3) )
00338 I = I_;
00339 else
00340 cerr << "Link::set_r: wrong size in input vector." << endl;
00341 }
00342
00343 Robot_basic::Robot_basic(const Matrix & dhinit, const bool dh_parameter,
00344 const bool min_inertial_para)
00354 {
00355 int ndof=0, i;
00356
00357 gravity = ColumnVector(3);
00358 gravity = 0.0;
00359 gravity(3) = GRAVITY;
00360 z0 = ColumnVector(3);
00361 z0(1) = z0(2) = 0.0; z0(3) = 1.0;
00362 fix = 0;
00363 for(i = 1; i <= dhinit.Nrows(); i++)
00364 if(dhinit(i,1) == 2)
00365 {
00366 if (i == dhinit.Nrows())
00367 fix = 1;
00368 else
00369 error("Fix link can only be on the last one");
00370 }
00371 else
00372 ndof++;
00373
00374 if(ndof < 1)
00375 error("Number of degree of freedom must be greater or equal to 1");
00376
00377 dof = ndof;
00378
00379 try
00380 {
00381 links = new Link[dof+fix];
00382 links = links-1;
00383 w = new ColumnVector[dof+1];
00384 wp = new ColumnVector[dof+1];
00385 vp = new ColumnVector[dof+fix+1];
00386 a = new ColumnVector[dof+1];
00387 f = new ColumnVector[dof+1];
00388 f_nv = new ColumnVector[dof+1];
00389 n = new ColumnVector[dof+1];
00390 n_nv = new ColumnVector[dof+1];
00391 F = new ColumnVector[dof+1];
00392 N = new ColumnVector[dof+1];
00393 p = new ColumnVector[dof+fix+1];
00394 pp = new ColumnVector[dof+fix+1];
00395 dw = new ColumnVector[dof+1];
00396 dwp = new ColumnVector[dof+1];
00397 dvp = new ColumnVector[dof+1];
00398 da = new ColumnVector[dof+1];
00399 df = new ColumnVector[dof+1];
00400 dn = new ColumnVector[dof+1];
00401 dF = new ColumnVector[dof+1];
00402 dN = new ColumnVector[dof+1];
00403 dp = new ColumnVector[dof+1];
00404 R = new Matrix[dof+fix+1];
00405 }
00406 catch(bad_alloc & e)
00407 {
00408 cerr << "Robot_basic constructor : new ran out of memory" << endl;
00409 cleanUpPointers();
00410 throw;
00411 }
00412
00413 for(i = 0; i <= dof; i++)
00414 {
00415 w[i] = ColumnVector(3);
00416 w[i] = 0.0;
00417 wp[i] = ColumnVector(3);
00418 wp[i] = 0.0;
00419 vp[i] = ColumnVector(3);
00420 dw[i] = ColumnVector(3);
00421 dw[i] = 0.0;
00422 dwp[i] = ColumnVector(3);
00423 dwp[i] = 0.0;
00424 dvp[i] = ColumnVector(3);
00425 dvp[i] = 0.0;
00426 }
00427 for(i = 0; i <= dof+fix; i++)
00428 {
00429 R[i] = Matrix(3,3);
00430 R[i] << threebythreeident;
00431 p[i] = ColumnVector(3);
00432 p[i] = 0.0;
00433 pp[i] = p[i];
00434 }
00435
00436 if(dhinit.Ncols() == 23)
00437 {
00438 for(i = 1; i <= dof+fix; i++)
00439 links[i] = Link((int) dhinit(i,1), dhinit(i,2), dhinit(i,3),
00440 dhinit(i,4), dhinit(i,5), dhinit(i,6), dhinit(i,7),
00441 dhinit(i,8), dhinit(i,9), dhinit(i,10), dhinit(i,11),
00442 dhinit(i,12), dhinit(i,13), dhinit(i,14), dhinit(i,15),
00443 dhinit(i,16), dhinit(i,17), dhinit(i,18), dhinit(i,19),
00444 dhinit(i,20), dhinit(i,21), dhinit(i,22),
00445 dh_parameter, min_inertial_para, dhinit(i,23));
00446 }
00447 else
00448 error("Initialisation Matrix does not have 23 columns.");
00449 }
00450
00451 Robot_basic::Robot_basic(const Matrix & initrobot, const Matrix & initmotor,
00452 const bool dh_parameter, const bool min_inertial_para)
00463 {
00464 int ndof=0, i;
00465
00466 gravity = ColumnVector(3);
00467 gravity = 0.0;
00468 gravity(3) = GRAVITY;
00469 z0 = ColumnVector(3);
00470 z0(1) = z0(2) = 0.0; z0(3) = 1.0;
00471 fix = 0;
00472
00473 for(i = 1; i <= initrobot.Nrows(); i++)
00474 if(initrobot(i,1) == 2)
00475 {
00476 if (i == initrobot.Nrows())
00477 fix = 1;
00478 else
00479 error("Fix link can only be on the last one");
00480 }
00481 else
00482 ndof++;
00483
00484 if(ndof < 1)
00485 error("Number of degree of freedom must be greater or equal to 1");
00486 dof = ndof;
00487
00488 try
00489 {
00490 links = new Link[dof+fix];
00491 links = links-1;
00492 w = new ColumnVector[dof+1];
00493 wp = new ColumnVector[dof+1];
00494 vp = new ColumnVector[dof+fix+1];
00495 a = new ColumnVector[dof+1];
00496 f = new ColumnVector[dof+1];
00497 f_nv = new ColumnVector[dof+1];
00498 n = new ColumnVector[dof+1];
00499 n_nv = new ColumnVector[dof+1];
00500 F = new ColumnVector[dof+1];
00501 N = new ColumnVector[dof+1];
00502 p = new ColumnVector[dof+fix+1];
00503 pp = new ColumnVector[dof+fix+1];
00504 dw = new ColumnVector[dof+1];
00505 dwp = new ColumnVector[dof+1];
00506 dvp = new ColumnVector[dof+1];
00507 da = new ColumnVector[dof+1];
00508 df = new ColumnVector[dof+1];
00509 dn = new ColumnVector[dof+1];
00510 dF = new ColumnVector[dof+1];
00511 dN = new ColumnVector[dof+1];
00512 dp = new ColumnVector[dof+1];
00513 R = new Matrix[dof+fix+1];
00514 }
00515 catch(bad_alloc & e)
00516 {
00517 cerr << "Robot_basic constructor : new ran out of memory" << endl;
00518 cleanUpPointers();
00519 throw;
00520 }
00521
00522 for(i = 0; i <= dof; i++)
00523 {
00524 w[i] = ColumnVector(3);
00525 w[i] = 0.0;
00526 wp[i] = ColumnVector(3);
00527 wp[i] = 0.0;
00528 vp[i] = ColumnVector(3);
00529 dw[i] = ColumnVector(3);
00530 dw[i] = 0.0;
00531 dwp[i] = ColumnVector(3);
00532 dwp[i] = 0.0;
00533 dvp[i] = ColumnVector(3);
00534 dvp[i] = 0.0;
00535 }
00536 for(i = 0; i <= dof+fix; i++)
00537 {
00538 R[i] = Matrix(3,3);
00539 R[i] << threebythreeident;
00540 p[i] = ColumnVector(3);
00541 p[i] = 0.0;
00542 pp[i] = p[i];
00543 }
00544
00545 if ( initrobot.Nrows() == initmotor.Nrows() )
00546 {
00547 if(initmotor.Ncols() == 4)
00548 {
00549 if(initrobot.Ncols() == 19)
00550 {
00551 for(i = 1; i <= dof+fix; i++)
00552 links[i] = Link((int) initrobot(i,1), initrobot(i,2), initrobot(i,3),
00553 initrobot(i,4), initrobot(i,5), initrobot(i,6),
00554 initrobot(i,7), initrobot(i,8), initrobot(i,9),
00555 initrobot(i,10),initrobot(i,11), initrobot(i,12),
00556 initrobot(i,13),initrobot(i,14), initrobot(i,15),
00557 initrobot(i,16),initrobot(i,17), initrobot(i,18),
00558 initmotor(i,1), initmotor(i,2), initmotor(i,3),
00559 initmotor(i,4), dh_parameter, min_inertial_para,
00560 initrobot(i,19));
00561 }
00562 else
00563 error("Initialisation robot Matrix does not have 19 columns.");
00564 }
00565 else
00566 error("Initialisation robot motor Matrix does not have 4 columns.");
00567
00568 }
00569 else
00570 error("Initialisation robot and motor matrix does not have same numbers of Rows.");
00571 }
00572
00573 Robot_basic::Robot_basic(const int ndof, const bool dh_parameter,
00574 const bool min_inertial_para)
00584 {
00585 int i = 0;
00586 gravity = ColumnVector(3);
00587 gravity = 0.0;
00588 gravity(3) = GRAVITY;
00589 z0 = ColumnVector(3);
00590 z0(1) = z0(2) = 0.0; z0(3) = 1.0;
00591 dof = ndof;
00592 fix = 0;
00593
00594 try
00595 {
00596 links = new Link[dof];
00597 links = links-1;
00598 w = new ColumnVector[dof+1];
00599 wp = new ColumnVector[dof+1];
00600 vp = new ColumnVector[dof+1];
00601 a = new ColumnVector[dof+1];
00602 f = new ColumnVector[dof+1];
00603 f_nv = new ColumnVector[dof+1];
00604 n = new ColumnVector[dof+1];
00605 n_nv = new ColumnVector[dof+1];
00606 F = new ColumnVector[dof+1];
00607 N = new ColumnVector[dof+1];
00608 p = new ColumnVector[dof+1];
00609 pp = new ColumnVector[dof+fix+1];
00610 dw = new ColumnVector[dof+1];
00611 dwp = new ColumnVector[dof+1];
00612 dvp = new ColumnVector[dof+1];
00613 da = new ColumnVector[dof+1];
00614 df = new ColumnVector[dof+1];
00615 dn = new ColumnVector[dof+1];
00616 dF = new ColumnVector[dof+1];
00617 dN = new ColumnVector[dof+1];
00618 dp = new ColumnVector[dof+1];
00619 R = new Matrix[dof+1];
00620 }
00621 catch(bad_alloc & e)
00622 {
00623 cerr << "Robot_basic constructor : new ran out of memory" << endl;
00624 cleanUpPointers();
00625 }
00626
00627 for(i = 0; i <= dof; i++)
00628 {
00629 w[i] = ColumnVector(3);
00630 w[i] = 0.0;
00631 wp[i] = ColumnVector(3);
00632 wp[i] = 0.0;
00633 vp[i] = ColumnVector(3);
00634 dw[i] = ColumnVector(3);
00635 dw[i] = 0.0;
00636 dwp[i] = ColumnVector(3);
00637 dwp[i] = 0.0;
00638 dvp[i] = ColumnVector(3);
00639 dvp[i] = 0.0;
00640 }
00641 for(i = 0; i <= dof+fix; i++)
00642 {
00643 R[i] = Matrix(3,3);
00644 R[i] << threebythreeident;
00645 p[i] = ColumnVector(3);
00646 p[i] = 0.0;
00647 pp[i] = p[i];
00648 }
00649 }
00650
00651 Robot_basic::Robot_basic(const Robot_basic & x)
00653 {
00654 int i = 0;
00655 dof = x.dof;
00656 fix = x.fix;
00657 gravity = x.gravity;
00658 z0 = x.z0;
00659
00660 try
00661 {
00662 links = new Link[dof+fix];
00663 links = links-1;
00664 w = new ColumnVector[dof+1];
00665 wp = new ColumnVector[dof+1];
00666 vp = new ColumnVector[dof+1];
00667 a = new ColumnVector[dof+1];
00668 f = new ColumnVector[dof+1];
00669 f_nv = new ColumnVector[dof+1];
00670 n = new ColumnVector[dof+1];
00671 n_nv = new ColumnVector[dof+1];
00672 F = new ColumnVector[dof+1];
00673 N = new ColumnVector[dof+1];
00674 p = new ColumnVector[dof+fix+1];
00675 pp = new ColumnVector[dof+fix+1];
00676 dw = new ColumnVector[dof+1];
00677 dwp = new ColumnVector[dof+1];
00678 dvp = new ColumnVector[dof+1];
00679 da = new ColumnVector[dof+1];
00680 df = new ColumnVector[dof+1];
00681 dn = new ColumnVector[dof+1];
00682 dF = new ColumnVector[dof+1];
00683 dN = new ColumnVector[dof+1];
00684 dp = new ColumnVector[dof+1];
00685 R = new Matrix[dof+fix+1];
00686 }
00687 catch(bad_alloc & e)
00688 {
00689 cerr << "Robot_basic constructor : new ran out of memory" << endl;
00690 cleanUpPointers();
00691 }
00692
00693 for(i = 0; i <= dof; i++)
00694 {
00695 w[i] = ColumnVector(3);
00696 w[i] = 0.0;
00697 wp[i] = ColumnVector(3);
00698 wp[i] = 0.0;
00699 vp[i] = ColumnVector(3);
00700 dw[i] = ColumnVector(3);
00701 dw[i] = 0.0;
00702 dwp[i] = ColumnVector(3);
00703 dwp[i] = 0.0;
00704 dvp[i] = ColumnVector(3);
00705 dvp[i] = 0.0;
00706 }
00707 for(i = 0; i <= dof+fix; i++)
00708 {
00709 R[i] = Matrix(3,3);
00710 R[i] << threebythreeident;
00711 p[i] = ColumnVector(3);
00712 p[i] = 0.0;
00713 pp[i] = p[i];
00714 }
00715
00716 for(i = 1; i <= dof+fix; i++)
00717 links[i] = x.links[i];
00718 }
00719
00720 Robot_basic::Robot_basic(const string & filename, const string & robotName,
00721 const bool dh_parameter, const bool min_inertial_para)
00731 {
00732 int i = 0;
00733 gravity = ColumnVector(3);
00734 gravity = 0.0;
00735 gravity(3) = GRAVITY;
00736 z0 = ColumnVector(3);
00737 z0(1) = z0(2) = 0.0; z0(3) = 1.0;
00738
00739 Config robData(true);
00740 ifstream inconffile(filename.c_str(), std::ios::in);
00741 if (robData.read_conf(inconffile))
00742 {
00743 error("Robot_basic::Robot_basic: unable to read input config file.");
00744 }
00745
00746 bool motor = false, angle_in_degree = false;
00747
00748 if(robData.select(robotName, "dof", dof))
00749 {
00750 if(dof < 1)
00751 error("Robot_basic::Robot_basic: dof is less then one.");
00752 }
00753 else
00754 error("Robot_basic::Robot_basic: error in extracting dof from conf file.");
00755
00756
00757 robData.select(robotName, "Fix", fix);
00758 robData.select(robotName, "Motor", motor);
00759 robData.select(robotName, "angle_in_degree", angle_in_degree);
00760
00761 try
00762 {
00763 links = new Link[dof+fix];
00764 links = links-1;
00765 w = new ColumnVector[dof+1];
00766 wp = new ColumnVector[dof+1];
00767 vp = new ColumnVector[dof+fix+1];
00768 a = new ColumnVector[dof+1];
00769 f = new ColumnVector[dof+1];
00770 f_nv = new ColumnVector[dof+1];
00771 n = new ColumnVector[dof+1];
00772 n_nv = new ColumnVector[dof+1];
00773 F = new ColumnVector[dof+1];
00774 N = new ColumnVector[dof+1];
00775 p = new ColumnVector[dof+fix+1];
00776 pp = new ColumnVector[dof+fix+1];
00777 dw = new ColumnVector[dof+1];
00778 dwp = new ColumnVector[dof+1];
00779 dvp = new ColumnVector[dof+1];
00780 da = new ColumnVector[dof+1];
00781 df = new ColumnVector[dof+1];
00782 dn = new ColumnVector[dof+1];
00783 dF = new ColumnVector[dof+1];
00784 dN = new ColumnVector[dof+1];
00785 dp = new ColumnVector[dof+1];
00786 R = new Matrix[dof+fix+1];
00787 }
00788 catch(bad_alloc & e)
00789 {
00790 cerr << "Robot_basic constructor : new ran out of memory" << endl;
00791 cleanUpPointers();
00792 }
00793
00794 for(i = 0; i <= dof; i++)
00795 {
00796 w[i] = ColumnVector(3);
00797 w[i] = 0.0;
00798 wp[i] = ColumnVector(3);
00799 wp[i] = 0.0;
00800 vp[i] = ColumnVector(3);
00801 dw[i] = ColumnVector(3);
00802 dw[i] = 0.0;
00803 dwp[i] = ColumnVector(3);
00804 dwp[i] = 0.0;
00805 dvp[i] = ColumnVector(3);
00806 dvp[i] = 0.0;
00807 }
00808 for(i = 0; i <= dof+fix; i++)
00809 {
00810 R[i] = Matrix(3,3);
00811 R[i] << threebythreeident;
00812 p[i] = ColumnVector(3);
00813 p[i] = 0.0;
00814 pp[i] = p[i];
00815 }
00816
00817 for(int j = 1; j <= dof+fix; j++)
00818 {
00819 int joint_type =0;
00820 double theta=0, d=0, a=0, alpha=0, theta_min=0, theta_max=0, joint_offset=0,
00821 m=0, cx=0, cy=0, cz=0, Ixx=0, Ixy=0, Ixz=0, Iyy=0, Iyz=0, Izz=0,
00822 Im=0, Gr=0, B=0, Cf=0;
00823 bool immobile=false;
00824
00825 string robotName_link;
00826 ostringstream ostr;
00827 ostr << robotName << "_LINK" << j;
00828 robotName_link = ostr.str();
00829
00830 robData.select(robotName_link, "joint_type", joint_type);
00831 robData.select(robotName_link, "theta", theta);
00832 robData.select(robotName_link, "d", d);
00833 robData.select(robotName_link, "a", a);
00834 robData.select(robotName_link, "alpha", alpha);
00835 robData.select(robotName_link, "theta_max", theta_max);
00836 robData.select(robotName_link, "theta_min", theta_min);
00837 robData.select(robotName_link, "joint_offset", joint_offset);
00838 robData.select(robotName_link, "m", m);
00839 robData.select(robotName_link, "cx", cx);
00840 robData.select(robotName_link, "cy", cy);
00841 robData.select(robotName_link, "cz", cz);
00842 robData.select(robotName_link, "Ixx", Ixx);
00843 robData.select(robotName_link, "Ixy", Ixy);
00844 robData.select(robotName_link, "Ixz", Ixz);
00845 robData.select(robotName_link, "Iyy", Iyy);
00846 robData.select(robotName_link, "Iyz", Iyz);
00847 robData.select(robotName_link, "Izz", Izz);
00848 robData.select(robotName_link,"immobile", immobile);
00849
00850 if(angle_in_degree)
00851 {
00852 theta = deg2rad(theta);
00853 theta_max = deg2rad(theta_max);
00854 theta_min = deg2rad(theta_min);
00855 alpha = deg2rad(alpha);
00856 joint_offset = deg2rad(joint_offset);
00857 }
00858
00859 if(motor)
00860 {
00861 robData.select(robotName_link, "Im", Im);
00862 robData.select(robotName_link, "Gr", Gr);
00863 robData.select(robotName_link, "B", B);
00864 robData.select(robotName_link, "Cf", Cf);
00865 }
00866
00867 links[j] = Link(joint_type, theta, d, a, alpha, theta_min, theta_max,
00868 joint_offset, m, cx, cy, cz, Ixx, Ixy, Ixz, Iyy, Iyz,
00869 Izz, Im, Gr, B, Cf, dh_parameter, min_inertial_para);
00870 links[j].set_immobile(immobile);
00871 }
00872
00873 if(fix)
00874 links[dof+fix] = Link(2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
00875 0, 0, 0, 0, 0, 0, 0, 0, 0, dh_parameter, min_inertial_para);
00876
00877 }
00878
00879 Robot_basic::~Robot_basic()
00880
00885 {
00886 cleanUpPointers();
00887 }
00888
00889 Robot_basic & Robot_basic::operator=(const Robot_basic & x)
00891 {
00892 if (this != &x)
00893 {
00894 int i = 0;
00895 if ( (dof != x.dof) || (fix != x.fix) )
00896 {
00897 links = links+1;
00898 delete []links;
00899 delete []R;
00900 delete []dp;
00901 delete []dN;
00902 delete []dF;
00903 delete []dn;
00904 delete []df;
00905 delete []da;
00906 delete []dvp;
00907 delete []dwp;
00908 delete []dw;
00909 delete []pp;
00910 delete []p;
00911 delete []N;
00912 delete []F;
00913 delete []n_nv;
00914 delete []n;
00915 delete []f_nv;
00916 delete []f;
00917 delete []a;
00918 delete []vp;
00919 delete []wp;
00920 delete []w;
00921 dof = x.dof;
00922 fix = x.fix;
00923 gravity = x.gravity;
00924 z0 = x.z0;
00925
00926 try
00927 {
00928 links = new Link[dof+fix];
00929 links = links-1;
00930 w = new ColumnVector[dof+1];
00931 wp = new ColumnVector[dof+1];
00932 vp = new ColumnVector[dof+fix+1];
00933 a = new ColumnVector[dof+1];
00934 f = new ColumnVector[dof+1];
00935 f_nv = new ColumnVector[dof+1];
00936 n = new ColumnVector[dof+1];
00937 n_nv = new ColumnVector[dof+1];
00938 F = new ColumnVector[dof+1];
00939 N = new ColumnVector[dof+1];
00940 p = new ColumnVector[dof+fix+1];
00941 pp = new ColumnVector[dof+fix+1];
00942 dw = new ColumnVector[dof+1];
00943 dwp = new ColumnVector[dof+1];
00944 dvp = new ColumnVector[dof+1];
00945 da = new ColumnVector[dof+1];
00946 df = new ColumnVector[dof+1];
00947 dn = new ColumnVector[dof+1];
00948 dF = new ColumnVector[dof+1];
00949 dN = new ColumnVector[dof+1];
00950 dp = new ColumnVector[dof+1];
00951 R = new Matrix[dof+fix+1];
00952 }
00953 catch(bad_alloc & e)
00954 {
00955 cerr << "Robot_basic::operator= : new ran out of memory" << endl;
00956 exit(1);
00957 }
00958 }
00959 for(i = 0; i <= dof; i++)
00960 {
00961 w[i] = ColumnVector(3);
00962 w[i] = 0.0;
00963 wp[i] = ColumnVector(3);
00964 wp[i] = 0.0;
00965 vp[i] = ColumnVector(3);
00966 dw[i] = ColumnVector(3);
00967 dw[i] = 0.0;
00968 dwp[i] = ColumnVector(3);
00969 dwp[i] = 0.0;
00970 dvp[i] = ColumnVector(3);
00971 dvp[i] = 0.0;
00972 }
00973 for(i = 0; i <= dof+fix; i++)
00974 {
00975 R[i] = Matrix(3,3);
00976 R[i] << threebythreeident;
00977 p[i] = ColumnVector(3);
00978 p[i] = 0.0;
00979 pp[i] = p[i];
00980 }
00981 for(i = 1; i <= dof+fix; i++)
00982 links[i] = x.links[i];
00983 }
00984 return *this;
00985 }
00986
00987 void Robot_basic::error(const string & msg1) const
00989 {
00990 cerr << endl << "Robot error: " << msg1.c_str() << endl;
00991
00992 }
00993
00994 int Robot_basic::get_available_dof(const int endlink)const
00996 {
00997 int ans=0;
00998 for(int i=1; i<=endlink; i++)
00999 if(!links[i].immobile)
01000 ans++;
01001 return ans;
01002 }
01003
01004 ReturnMatrix Robot_basic::get_q(void)const
01006 {
01007 ColumnVector q(dof);
01008
01009 for(int i = 1; i <= dof; i++)
01010 q(i) = links[i].get_q();
01011 q.Release(); return q;
01012 }
01013
01014 ReturnMatrix Robot_basic::get_qp(void)const
01016 {
01017 ColumnVector qp(dof);
01018
01019 for(int i = 1; i <= dof; i++)
01020 qp(i) = links[i].qp;
01021 qp.Release(); return qp;
01022 }
01023
01024 ReturnMatrix Robot_basic::get_qpp(void)const
01026 {
01027 ColumnVector qpp(dof);
01028
01029 for(int i = 1; i <= dof; i++)
01030 qpp(i) = links[i].qpp;
01031 qpp.Release(); return qpp;
01032 }
01033
01034 ReturnMatrix Robot_basic::get_available_q(const int endlink)const
01036 {
01037 ColumnVector q(get_available_dof(endlink));
01038
01039 int j=1;
01040 for(int i = 1; i <= endlink; i++)
01041 if(!links[i].immobile)
01042 q(j++) = links[i].get_q();
01043 q.Release(); return q;
01044 }
01045
01046 ReturnMatrix Robot_basic::get_available_qp(const int endlink)const
01048 {
01049 ColumnVector qp(get_available_dof(endlink));
01050
01051 int j=1;
01052 for(int i = 1; i <= endlink; i++)
01053 if(!links[i].immobile)
01054 qp(j++) = links[i].qp;
01055 qp.Release(); return qp;
01056 }
01057
01058 ReturnMatrix Robot_basic::get_available_qpp(const int endlink)const
01060 {
01061 ColumnVector qpp(get_available_dof(endlink));
01062
01063 int j=1;
01064 for(int i = 1; i <= endlink; i++)
01065 if(!links[i].immobile)
01066 qpp(j) = links[i].qpp;
01067 qpp.Release(); return qpp;
01068 }
01069
01070 void Robot_basic::set_q(const Matrix & q)
01078 {
01079 int adof=get_available_dof();
01080 if(q.Nrows() == dof && q.Ncols() == 1) {
01081 for(int i = 1; i <= dof; i++)
01082 {
01083 links[i].transform(q(i,1));
01084 if(links[1].DH)
01085 {
01086 p[i](1) = links[i].get_a();
01087 p[i](2) = links[i].get_d() * links[i].R(3,2);
01088 p[i](3) = links[i].get_d() * links[i].R(3,3);
01089 }
01090 else
01091 p[i] = links[i].p;
01092 }
01093 } else if(q.Nrows() == 1 && q.Ncols() == dof) {
01094 for(int i = 1; i <= dof; i++)
01095 {
01096 links[i].transform(q(1,i));
01097 if(links[1].DH)
01098 {
01099 p[i](1) = links[i].get_a();
01100 p[i](2) = links[i].get_d() * links[i].R(3,2);
01101 p[i](3) = links[i].get_d() * links[i].R(3,3);
01102 }
01103 else
01104 p[i] = links[i].p;
01105 }
01106 } else if(q.Nrows() == adof && q.Ncols() == 1) {
01107 int j=1;
01108 for(int i = 1; i <= dof; i++)
01109 if(!links[i].immobile) {
01110 links[i].transform(q(j++,1));
01111 if(links[1].DH)
01112 {
01113 p[i](1) = links[i].get_a();
01114 p[i](2) = links[i].get_d() * links[i].R(3,2);
01115 p[i](3) = links[i].get_d() * links[i].R(3,3);
01116 }
01117 else
01118 p[i] = links[i].p;
01119 }
01120 } else if(q.Nrows() == 1 && q.Ncols() == adof) {
01121 int j=1;
01122 for(int i = 1; i <= dof; i++)
01123 if(!links[i].immobile) {
01124 links[i].transform(q(1,j++));
01125 if(links[1].DH)
01126 {
01127 p[i](1) = links[i].get_a();
01128 p[i](2) = links[i].get_d() * links[i].R(3,2);
01129 p[i](3) = links[i].get_d() * links[i].R(3,3);
01130 }
01131 else
01132 p[i] = links[i].p;
01133 }
01134 } else error("q has the wrong dimension in set_q()");
01135 }
01136
01137 void Robot_basic::set_q(const ColumnVector & q)
01145 {
01146 if(q.Nrows() == dof) {
01147 for(int i = 1; i <= dof; i++)
01148 {
01149 links[i].transform(q(i));
01150 if(links[1].DH)
01151 {
01152 p[i](1) = links[i].get_a();
01153 p[i](2) = links[i].get_d() * links[i].R(3,2);
01154 p[i](3) = links[i].get_d() * links[i].R(3,3);
01155 }
01156 else
01157 p[i] = links[i].p;
01158 }
01159 } else if(q.Nrows() == get_available_dof()) {
01160 int j=1;
01161 for(int i = 1; i <= dof; i++)
01162 if(!links[i].immobile) {
01163 links[i].transform(q(j++));
01164 if(links[1].DH)
01165 {
01166 p[i](1) = links[i].get_a();
01167 p[i](2) = links[i].get_d() * links[i].R(3,2);
01168 p[i](3) = links[i].get_d() * links[i].R(3,3);
01169 }
01170 else
01171 p[i] = links[i].p;
01172 }
01173 } else error("q has the wrong dimension in set_q()");
01174 }
01175
01176 void Robot_basic::set_qp(const ColumnVector & qp)
01178 {
01179 if(qp.Nrows() == dof)
01180 for(int i = 1; i <= dof; i++)
01181 links[i].qp = qp(i);
01182 else if(qp.Nrows() == get_available_dof()) {
01183 int j=1;
01184 for(int i = 1; i <= dof; i++)
01185 if(!links[i].immobile)
01186 links[i].qp = qp(j++);
01187 } else
01188 error("qp has the wrong dimension in set_qp()");
01189 }
01190
01191 void Robot_basic::set_qpp(const ColumnVector & qpp)
01193 {
01194 if(qpp.Nrows() == dof)
01195 for(int i = 1; i <= dof; i++)
01196 links[i].qpp = qpp(i);
01197 else
01198 error("qpp has the wrong dimension in set_qpp()");
01199 }
01200
01201
01202 void Robot_basic::cleanUpPointers()
01203
01206 {
01207 try
01208 {
01209 links = links+1;
01210 delete []links;
01211 delete []R;
01212 delete []dp;
01213 delete []dN;
01214 delete []dF;
01215 delete []dn;
01216 delete []df;
01217 delete []da;
01218 delete []dvp;
01219 delete []dwp;
01220 delete []dw;
01221 delete []pp;
01222 delete []p;
01223 delete []N;
01224 delete []F;
01225 delete []n_nv;
01226 delete []n;
01227 delete []f_nv;
01228 delete []f;
01229 delete []a;
01230 delete []vp;
01231 delete []wp;
01232 delete []w;
01233 }
01234 catch(...) {}
01235 }
01236
01237
01242 Robot::Robot(const int ndof): Robot_basic(ndof, true, false)
01243 {
01244 robotType_inv_kin();
01245 }
01246
01251 Robot::Robot(const Matrix & dhinit): Robot_basic(dhinit, true, false)
01252 {
01253 robotType_inv_kin();
01254 }
01255
01260 Robot::Robot(const Matrix & initrobot, const Matrix & initmotor):
01261 Robot_basic(initrobot, initmotor, true, false)
01262 {
01263 robotType_inv_kin();
01264 }
01265
01270 Robot::Robot(const string & filename, const string & robotName):
01271 Robot_basic(filename, robotName, true, false)
01272 {
01273 robotType_inv_kin();
01274 }
01275
01280 Robot::Robot(const Robot & x) : Robot_basic(x)
01281 {
01282 }
01283
01284 void Robot::robotType_inv_kin()
01294 {
01295 if ( Puma_DH(*this) == true)
01296 robotType = PUMA;
01297 else if ( Rhino_DH(*this) == true)
01298 robotType = RHINO;
01299 else if (Schilling_DH(*this) == true )
01300 robotType = SCHILLING;
01301 else
01302 robotType = DEFAULT;
01303 }
01304
01305
01306
01311 mRobot::mRobot(const int ndof) : Robot_basic(ndof, false, false)
01312 {
01313 robotType_inv_kin();
01314 }
01315
01320 mRobot::mRobot(const Matrix & dhinit) : Robot_basic(dhinit, false, false)
01321 {
01322 robotType_inv_kin();
01323 }
01324
01329 mRobot::mRobot(const Matrix & initrobot, const Matrix & initmotor) :
01330 Robot_basic(initrobot, initmotor, false, false)
01331 {
01332 robotType_inv_kin();
01333 }
01334
01339 mRobot::mRobot(const string & filename, const string & robotName):
01340 Robot_basic(filename, robotName, false, false)
01341 {
01342 robotType_inv_kin();
01343 }
01344
01349 mRobot::mRobot(const mRobot & x) : Robot_basic(x)
01350 {
01351 robotType_inv_kin();
01352 }
01353
01354 void mRobot::robotType_inv_kin()
01364 {
01365 if ( Puma_mDH(*this) == true )
01366 robotType = PUMA;
01367 else if ( Rhino_mDH(*this) == true)
01368 robotType = RHINO;
01369 else if (Schilling_mDH(*this) == true)
01370 robotType = SCHILLING;
01371 else
01372 robotType = DEFAULT;
01373 }
01374
01379 mRobot_min_para::mRobot_min_para(const int ndof) : Robot_basic(ndof, false, true)
01380 {
01381 robotType_inv_kin();
01382 }
01383
01388 mRobot_min_para::mRobot_min_para(const Matrix & dhinit):
01389 Robot_basic(dhinit, false, true)
01390 {
01391 robotType_inv_kin();
01392 }
01393
01398 mRobot_min_para::mRobot_min_para(const Matrix & initrobot, const Matrix & initmotor) :
01399 Robot_basic(initrobot, initmotor, false, true)
01400 {
01401 robotType_inv_kin();
01402 }
01403
01408 mRobot_min_para::mRobot_min_para(const mRobot_min_para & x) : Robot_basic(x)
01409 {
01410 robotType_inv_kin();
01411 }
01412
01417 mRobot_min_para::mRobot_min_para(const string & filename, const string & robotName):
01418 Robot_basic(filename, robotName, false, true)
01419 {
01420 robotType_inv_kin();
01421 }
01422
01423 void mRobot_min_para::robotType_inv_kin()
01433 {
01434 if ( Puma_mDH(*this) == true)
01435 robotType = PUMA;
01436 else if ( Rhino_mDH(*this) == true)
01437 robotType = RHINO;
01438 else if (Schilling_mDH(*this) == true)
01439 robotType = SCHILLING;
01440 else
01441 robotType = DEFAULT;
01442 }
01443
01444
01445
01446 void perturb_robot(Robot_basic & robot, const double f)
01455 {
01456 if( (f < 0) || (f > 1) )
01457 {
01458 cerr << "perturb_robot: f is not between 0 and 1" << endl;
01459 return;
01460 }
01461
01462 double fact;
01463 srand(clock());
01464 for(int i = 1; i <= robot.get_dof()+robot.get_fix(); i++)
01465 {
01466 fact = (2.0*rand()/RAND_MAX - 1)*f + 1;
01467 robot.links[i].set_Im(robot.links[i].get_Im()*fact);
01468 fact = (2.0*rand()/RAND_MAX - 1)*f + 1;
01469 robot.links[i].set_B(robot.links[i].get_B()*fact);
01470 fact = (2.0*rand()/RAND_MAX - 1)*f + 1;
01471 robot.links[i].set_Cf(robot.links[i].get_Cf()*fact);
01472 fact = (2.0*rand()/RAND_MAX - 1)*f + 1;
01473 robot.links[i].set_m(robot.links[i].get_m()*fact);
01474
01475
01476 fact = (2.0*rand()/RAND_MAX - 1)*f + 1;
01477 Matrix I = robot.links[i].get_I()*fact;
01478 robot.links[i].set_I(I);
01479 }
01480 }
01481
01482
01483 bool Rhino_DH(const Robot_basic & robot)
01491 {
01492 if (robot.get_dof() == 5)
01493 {
01494 double a[6], d[6], alpha[6];
01495 for (int j = 1; j <= 5; j++)
01496 {
01497 if (robot.links[j].get_joint_type())
01498 {
01499 return false;
01500 }
01501 a[j] = robot.links[j].get_a();
01502 d[j] = robot.links[j].get_d();
01503 alpha[j] = robot.links[j].get_alpha();
01504 }
01505
01506 if ( isZero(a[1]) && isZero(a[5]) && isZero(d[2]) && isZero(d[3]) &&
01507 isZero(d[4]) && isZero(alpha[2]) && isZero(alpha[3]) && isZero(alpha[5]))
01508 {
01509 return true;
01510 }
01511 }
01512 return false;
01513 }
01514
01515
01516 bool Puma_DH(const Robot_basic & robot)
01524 {
01525 if (robot.get_dof() == 6)
01526 {
01527 double a[7], d[7], alpha[7];
01528 for (int j = 1; j <= 6; j++)
01529 {
01530 if (robot.links[j].get_joint_type())
01531 {
01532 return false;
01533 }
01534 a[j] = robot.links[j].get_a();
01535 d[j] = robot.links[j].get_d();
01536 alpha[j] = robot.links[j].get_alpha();
01537 }
01538
01539
01540 if( isZero(a[1]) && a[2] && a[3] && isZero(a[4]) && isZero(a[5]) && isZero(a[6]) &&
01541 isZero(d[5]) && isZero(alpha[2]) && isZero(alpha[6]))
01542 {
01543 return true;
01544 }
01545 }
01546 return false;
01547 }
01548
01549 bool Schilling_DH(const Robot_basic & robot)
01557 {
01558 if (robot.get_dof() == 6)
01559 {
01560 double a[7], d[7], alpha[7];
01561 for (int j = 1; j <= 6; j++)
01562 {
01563 if (robot.links[j].get_joint_type())
01564 return false;
01565 a[j] = robot.links[j].get_a();
01566 d[j] = robot.links[j].get_d();
01567 alpha[j] = robot.links[j].get_alpha();
01568 }
01569
01570
01571 if( isZero(a[5]) && isZero(a[6]) && isZero(d[2]) && isZero(d[3]) &&
01572 isZero(d[4]) && isZero(d[5]) && isZero(alpha[2]) && isZero(alpha[3]) &&
01573 isZero(alpha[6]))
01574 {
01575 return true;
01576 }
01577 }
01578
01579 return false;
01580 }
01581
01582
01583 bool Rhino_mDH(const Robot_basic & robot)
01591 {
01592 if (robot.get_dof() == 5)
01593 {
01594 double a[6], d[6], alpha[6];
01595 for (int j = 1; j <= 5; j++)
01596 {
01597 if (robot.links[j].get_joint_type())
01598 {
01599 return false;
01600 }
01601 a[j] = robot.links[j].get_a();
01602 d[j] = robot.links[j].get_d();
01603 alpha[j] = robot.links[j].get_alpha();
01604 }
01605
01606 if ( isZero(a[1]) && isZero(a[2]) && isZero(d[2]) && isZero(d[3]) &&
01607 isZero(d[4]) && isZero(alpha[1]) && isZero(alpha[3]) && isZero(alpha[4]))
01608 {
01609 return true;
01610 }
01611 }
01612 return false;
01613 }
01614
01615 bool Puma_mDH(const Robot_basic & robot)
01623 {
01624 if (robot.get_dof() == 6)
01625 {
01626 double a[7], d[7], alpha[7];
01627 for (int j = 1; j <= 6; j++)
01628 {
01629 if (robot.links[j].get_joint_type())
01630 {
01631 return false;
01632 }
01633 a[j] = robot.links[j].get_a();
01634 d[j] = robot.links[j].get_d();
01635 alpha[j] = robot.links[j].get_alpha();
01636 }
01637
01638
01639
01640 if ( isZero(a[1]) && isZero(a[2]) && isZero(a[5]) && isZero(a[6]) &&
01641 isZero(d[5]) && isZero(alpha[1]) && isZero(alpha[3]))
01642 {
01643 return true;
01644 }
01645 }
01646 return false;
01647 }
01648
01649 bool Schilling_mDH(const Robot_basic & robot)
01657 {
01658 if (robot.get_dof() == 6)
01659 {
01660 double a[7], d[7], alpha[7];
01661 for (int j = 1; j <= 6; j++)
01662 {
01663 if (robot.links[j].get_joint_type())
01664 {
01665 return false;
01666 }
01667 a[j] = robot.links[j].get_a();
01668 d[j] = robot.links[j].get_d();
01669 alpha[j] = robot.links[j].get_alpha();
01670 }
01671
01672
01673 if ( isZero(a[1]) && isZero(a[6]) && isZero(d[2]) && isZero(d[3]) && isZero(d[4]) &&
01674 isZero(d[5]) && isZero(alpha[1]) && isZero(alpha[3]) && isZero(alpha[4]))
01675 {
01676 return true;
01677 }
01678 }
01679 return false;
01680 }
01681
01682 #ifdef use_namespace
01683 }
01684 #endif
01685
01686