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 static const char rcsid[] = "$Id: robot.cpp,v 1.19 2004/12/10 23:49:35 ejt Exp $";
00096
00097 #include "robot.h"
00098 #include <time.h>
00099
00100 #ifdef use_namespace
00101 namespace ROBOOP {
00102 using namespace NEWMAT;
00103 #endif
00104
00105
00106 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};
00107
00108
00109 Real threebythreeident[] ={1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0};
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122 Link::Link(const int jt, const Real it, const Real id, const Real ia, const Real ial,
00123 const Real it_min, const Real it_max, const Real it_off, const Real mass,
00124 const Real cmx, const Real cmy, const Real cmz, const Real ixx, const Real ixy,
00125 const Real ixz, const Real iyy, const Real iyz, const Real izz, const Real iIm,
00126 const Real iGr, const Real iB, const Real iCf, const bool dh,
00127 const bool min_inertial_parameters):
00128 R(3,3),
00129 qp(0),
00130 qpp(0),
00131 joint_type(jt),
00132 theta(it),
00133 d(id),
00134 a(ia),
00135 alpha(ial),
00136 theta_min(it_min),
00137 theta_max(it_max),
00138 joint_offset(it_off),
00139 DH(dh),
00140 min_para(min_inertial_parameters),
00141 r(),
00142 p(3),
00143 m(mass),
00144 Im(iIm),
00145 Gr(iGr),
00146 B(iB),
00147 Cf(iCf),
00148 mc(),
00149 I(3,3),
00150 immobile(false)
00151 {
00152 if (joint_type == 0)
00153 theta += joint_offset;
00154 else
00155 d += joint_offset;
00156 Real ct, st, ca, sa;
00157 ct = cos(theta);
00158 st = sin(theta);
00159 ca = cos(alpha);
00160 sa = sin(alpha);
00161
00162 qp = qpp = 0.0;
00163
00164 if (DH)
00165 {
00166 R(1,1) = ct;
00167 R(2,1) = st;
00168 R(3,1) = 0.0;
00169 R(1,2) = -ca*st;
00170 R(2,2) = ca*ct;
00171 R(3,2) = sa;
00172 R(1,3) = sa*st;
00173 R(2,3) = -sa*ct;
00174 R(3,3) = ca;
00175
00176 p(1) = a*ct;
00177 p(2) = a*st;
00178 p(3) = d;
00179 }
00180 else
00181 {
00182 R(1,1) = ct;
00183 R(2,1) = st*ca;
00184 R(3,1) = st*sa;
00185 R(1,2) = -st;
00186 R(2,2) = ca*ct;
00187 R(3,2) = sa*ct;
00188 R(1,3) = 0;
00189 R(2,3) = -sa;
00190 R(3,3) = ca;
00191
00192 p(1) = a;
00193 p(2) = -sa*d;
00194 p(3) = ca*d;
00195 }
00196
00197 if (min_para)
00198 {
00199 mc = ColumnVector(3);
00200 mc(1) = cmx;
00201 mc(2) = cmy;
00202 mc(3) = cmz;
00203 }
00204 else
00205 {
00206 r = ColumnVector(3);
00207 r(1) = cmx;
00208 r(2) = cmy;
00209 r(3) = cmz;
00210 }
00211
00212 I(1,1) = ixx;
00213 I(1,2) = I(2,1) = ixy;
00214 I(1,3) = I(3,1) = ixz;
00215 I(2,2) = iyy;
00216 I(2,3) = I(3,2) = iyz;
00217 I(3,3) = izz;
00218 }
00219
00220 Link::Link(const Link & x)
00221
00222 :
00223 R(x.R),
00224 qp(x.qp),
00225 qpp(x.qpp),
00226 joint_type(x.joint_type),
00227 theta(x.theta),
00228 d(x.d),
00229 a(x.a),
00230 alpha(x.alpha),
00231 theta_min(x.theta_min),
00232 theta_max(x.theta_max),
00233 joint_offset(x.joint_offset),
00234 DH(x.DH),
00235 min_para(x.min_para),
00236 r(x.r),
00237 p(x.p),
00238 m(x.m),
00239 Im(x.Im),
00240 Gr(x.Gr),
00241 B(x.B),
00242 Cf(x.Cf),
00243 mc(x.mc),
00244 I(x.I),
00245 immobile(x.immobile)
00246 {
00247 }
00248
00249 Link & Link::operator=(const Link & x)
00250
00251 {
00252 joint_type = x.joint_type;
00253 theta = x.theta;
00254 qp = x.qp;
00255 qpp = x.qpp;
00256 d = x.d;
00257 a = x.a;
00258 alpha = x.alpha;
00259 theta_min = x.theta_min;
00260 theta_max = x.theta_max;
00261 joint_offset = x.joint_offset;
00262 R = x.R;
00263 p = x.p;
00264 m = x.m;
00265 r = x.r;
00266 mc = x.mc;
00267 I = x.I;
00268 Im = x.Im;
00269 Gr = x.Gr;
00270 B = x.B;
00271 Cf = x.Cf;
00272 DH = x.DH;
00273 min_para = x.min_para;
00274 immobile = x.immobile;
00275 return *this;
00276 }
00277
00278 void Link::transform(const Real q)
00279
00280 {
00281 if (DH)
00282 {
00283 if(joint_type == 0)
00284 {
00285 Real ct, st, ca, sa;
00286 Real nt=q + joint_offset;
00287 if(theta==nt)
00288 return;
00289 theta=nt;
00290 ct = cos(theta);
00291 st = sin(theta);
00292 ca = R(3,3);
00293 sa = R(3,2);
00294
00295 R(1,1) = ct;
00296 R(2,1) = st;
00297 R(1,2) = -ca*st;
00298 R(2,2) = ca*ct;
00299 R(1,3) = sa*st;
00300 R(2,3) = -sa*ct;
00301 p(1) = a*ct;
00302 p(2) = a*st;
00303 }
00304 else
00305 p(3) = d = q + joint_offset;
00306 }
00307 else
00308 {
00309 Real ca, sa;
00310 ca = R(3,3);
00311 sa = -R(2,3);
00312 if(joint_type == 0)
00313 {
00314 Real ct, st;
00315 Real nt=q + joint_offset;
00316 if(theta==nt)
00317 return;
00318 theta=nt;
00319 ct = cos(theta);
00320 st = sin(theta);
00321
00322 R(1,1) = ct;
00323 R(2,1) = st*ca;
00324 R(3,1) = st*sa;
00325 R(1,2) = -st;
00326 R(2,2) = ca*ct;
00327 R(3,2) = sa*ct;
00328 R(1,3) = 0;
00329 }
00330 else
00331 {
00332 d = q + joint_offset;
00333 p(2) = -sa*d;
00334 p(3) = ca*d;
00335 }
00336 }
00337 }
00338
00339 Real Link::get_q(void) const
00340
00341
00342
00343
00344
00345 {
00346 if(joint_type == 0)
00347 return theta - joint_offset;
00348 else
00349 return d - joint_offset;
00350 }
00351
00352
00353 void Link::set_r(const ColumnVector & r_)
00354
00355 {
00356 if(r_.Nrows() == 3)
00357 r = r_;
00358 else
00359 cerr << "Link::set_r: wrong size in input vector." << endl;
00360 }
00361
00362 void Link::set_mc(const ColumnVector & mc_)
00363
00364 {
00365 if(mc_.Nrows() == 3)
00366 mc = mc_;
00367 else
00368 cerr << "Link::set_mc: wrong size in input vector." << endl;
00369 }
00370
00371
00372
00373
00374
00375 void Link::set_I(const Matrix & I_)
00376 {
00377 if( (I_.Nrows() == 3) && (I_.Ncols() == 3) )
00378 I = I_;
00379 else
00380 cerr << "Link::set_r: wrong size in input vector." << endl;
00381 }
00382
00383 Robot_basic::Robot_basic(const Matrix & dhinit, const bool dh_parameter,
00384 const bool min_inertial_para)
00385
00386
00387
00388
00389
00390
00391
00392
00393
00394 : w(NULL), wp(NULL), vp(NULL), a(NULL), f(NULL), f_nv(NULL), n(NULL), n_nv(NULL),
00395 F(NULL), N(NULL), p(NULL), pp(NULL), dw(NULL), dwp(NULL), dvp(NULL), da(NULL),
00396 df(NULL), dn(NULL), dF(NULL), dN(NULL), dp(NULL), z0(3), gravity(3), R(NULL),
00397 links(NULL), robotType(DEFAULT), dof(0), fix(0)
00398 {
00399 int ndof=0, i;
00400
00401 gravity = 0.0;
00402 gravity(3) = 9.81;
00403 z0(1) = z0(2) = 0.0; z0(3) = 1.0;
00404 fix = 0;
00405 for(i = 1; i <= dhinit.Nrows(); i++)
00406 if(dhinit(i,1) == 2)
00407 {
00408 if (i == dhinit.Nrows())
00409 fix = 1;
00410 else
00411 error("Fix link can only be on the last one");
00412 }
00413 else
00414 ndof++;
00415
00416 if(ndof < 1)
00417 error("Number of degree of freedom must be greater or equal to 1");
00418
00419 dof = ndof;
00420
00421 try
00422 {
00423 links = new Link[dof+fix];
00424 links = links-1;
00425 w = new ColumnVector[dof+1];
00426 wp = new ColumnVector[dof+1];
00427 vp = new ColumnVector[dof+fix+1];
00428 a = new ColumnVector[dof+1];
00429 f = new ColumnVector[dof+1];
00430 f_nv = new ColumnVector[dof+1];
00431 n = new ColumnVector[dof+1];
00432 n_nv = new ColumnVector[dof+1];
00433 F = new ColumnVector[dof+1];
00434 N = new ColumnVector[dof+1];
00435 p = new ColumnVector[dof+fix+1];
00436 pp = new ColumnVector[dof+fix+1];
00437 dw = new ColumnVector[dof+1];
00438 dwp = new ColumnVector[dof+1];
00439 dvp = new ColumnVector[dof+1];
00440 da = new ColumnVector[dof+1];
00441 df = new ColumnVector[dof+1];
00442 dn = new ColumnVector[dof+1];
00443 dF = new ColumnVector[dof+1];
00444 dN = new ColumnVector[dof+1];
00445 dp = new ColumnVector[dof+1];
00446 R = new Matrix[dof+fix+1];
00447 }
00448 catch(bad_alloc exception)
00449 {
00450 cerr << "Robot_basic constructor : new ran out of memory" << endl;
00451 exit(1);
00452 }
00453
00454 for(i = 0; i <= dof; i++)
00455 {
00456 w[i] = ColumnVector(3);
00457 w[i] = 0.0;
00458 wp[i] = ColumnVector(3);
00459 wp[i] = 0.0;
00460 vp[i] = ColumnVector(3);
00461 dw[i] = ColumnVector(3);
00462 dw[i] = 0.0;
00463 dwp[i] = ColumnVector(3);
00464 dwp[i] = 0.0;
00465 dvp[i] = ColumnVector(3);
00466 dvp[i] = 0.0;
00467 }
00468 for(i = 0; i <= dof+fix; i++)
00469 {
00470 R[i] = Matrix(3,3);
00471 R[i] << threebythreeident;
00472 p[i] = ColumnVector(3);
00473 p[i] = 0.0;
00474 pp[i] = p[i];
00475 }
00476
00477 switch (dhinit.Ncols()){
00478 case 5:
00479 for(i = 1; i <= dof+fix; i++)
00480 {
00481 links[i] = Link((int) dhinit(i,1), dhinit(i,2), dhinit(i,3),
00482 dhinit(i,4), dhinit(i,5), 0, 0, 0, 0, 0, 0, 0,
00483 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, dh_parameter, min_inertial_para);
00484 }
00485 break;
00486 case 7:
00487 for(i = 1; i <= dof+fix; i++)
00488 {
00489 links[i] = Link((int) dhinit(i,1), dhinit(i,2), dhinit(i,3),
00490 dhinit(i,4), dhinit(i,5), dhinit(i,6), dhinit(i,7),
00491 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
00492 dh_parameter, min_inertial_para);
00493 }
00494 break;
00495 case 15:
00496 for(i = 1; i <= dof+fix; i++)
00497 {
00498 links[i] = Link((int) dhinit(i,1), dhinit(i,2), dhinit(i,3),
00499 dhinit(i,4), dhinit(i,5), 0, 0, 0, dhinit(i,6), dhinit(i,7),
00500 dhinit(i,8), dhinit(i,9), dhinit(i,10), dhinit(i,11),
00501 dhinit(i,12), dhinit(i,13), dhinit(i,14), dhinit(i,15),
00502 0, 0, 0, 0, dh_parameter, min_inertial_para);
00503 }
00504 break;
00505 case 18:
00506 for(i = 1; i <= dof+fix; i++)
00507 {
00508 links[i] = Link((int) dhinit(i,1), dhinit(i,2), dhinit(i,3),
00509 dhinit(i,4), dhinit(i,5), dhinit(i,6), dhinit(i,7),
00510 dhinit(i,8), dhinit(i,9), dhinit(i,10), dhinit(i,11),
00511 dhinit(i,12), dhinit(i,13), dhinit(i,14), dhinit(i,15),
00512 dhinit(i,16), dhinit(i,17), dhinit(i,18), 0, 0, 0, 0,
00513 dh_parameter, min_inertial_para);
00514 }
00515 break;
00516 case 20:
00517 for(i = 1; i <= dof+fix; i++)
00518 {
00519 links[i] = Link((int) dhinit(i,1), dhinit(i,2), dhinit(i,3),
00520 dhinit(i,4), dhinit(i,5), 0, 0, dhinit(i,6), dhinit(i,7),
00521 dhinit(i,8), dhinit(i,9), dhinit(i,10), dhinit(i,11),
00522 dhinit(i,12), dhinit(i,13), dhinit(i,14), dhinit(i,15),
00523 dhinit(i,16), dhinit(i,17), dhinit(i,18), dhinit(i,19),
00524 dhinit(i,20), dh_parameter, min_inertial_para);
00525 }
00526 break;
00527 case 22:
00528 for(i = 1; i <= dof+fix; i++)
00529 {
00530 links[i] = Link((int) dhinit(i,1), dhinit(i,2), dhinit(i,3),
00531 dhinit(i,4), dhinit(i,5), dhinit(i,6), dhinit(i,7),
00532 dhinit(i,8), dhinit(i,9), dhinit(i,10), dhinit(i,11),
00533 dhinit(i,12), dhinit(i,13), dhinit(i,14), dhinit(i,15),
00534 dhinit(i,16), dhinit(i,17), dhinit(i,18), dhinit(i,19),
00535 dhinit(i,20), dhinit(i,21), dhinit(i,22),
00536 dh_parameter, min_inertial_para);
00537 }
00538 break;
00539 default:
00540 error("Initialisation Matrix does not have 5, ,7, 16 18, 20 or 22 columns.");
00541 }
00542
00543 }
00544
00545 Robot_basic::Robot_basic(const Matrix & initrobot, const Matrix & initmotor,
00546 const bool dh_parameter, const bool min_inertial_para)
00547
00548
00549
00550
00551
00552
00553
00554
00555
00556
00557 : w(NULL), wp(NULL), vp(NULL), a(NULL), f(NULL), f_nv(NULL), n(NULL), n_nv(NULL),
00558 F(), N(NULL), p(NULL), pp(NULL), dw(NULL), dwp(NULL), dvp(NULL), da(NULL),
00559 df(NULL), dn(NULL), dF(NULL), dN(NULL), dp(NULL), z0(3), gravity(3), R(NULL),
00560 links(NULL), robotType(DEFAULT), dof(0), fix(0)
00561 {
00562 int ndof=0, i;
00563
00564 gravity = 0.0;
00565 gravity(3) = 9.81;
00566 z0(1) = z0(2) = 0.0; z0(3) = 1.0;
00567
00568 for(i = 1; i <= initrobot.Nrows(); i++)
00569 if(initrobot(i,1) == 2)
00570 {
00571 if (i == initrobot.Nrows())
00572 fix = 1;
00573 else
00574 error("Fix link can only be on the last one");
00575 }
00576 else
00577 ndof++;
00578
00579 if(ndof < 1)
00580 error("Number of degree of freedom must be greater or equal to 1");
00581 dof = ndof;
00582
00583 try
00584 {
00585 links = new Link[dof+fix];
00586 links = links-1;
00587 w = new ColumnVector[dof+1];
00588 wp = new ColumnVector[dof+1];
00589 vp = new ColumnVector[dof+fix+1];
00590 a = new ColumnVector[dof+1];
00591 f = new ColumnVector[dof+1];
00592 f_nv = new ColumnVector[dof+1];
00593 n = new ColumnVector[dof+1];
00594 n_nv = new ColumnVector[dof+1];
00595 F = new ColumnVector[dof+1];
00596 N = new ColumnVector[dof+1];
00597 p = new ColumnVector[dof+fix+1];
00598 pp = new ColumnVector[dof+fix+1];
00599 dw = new ColumnVector[dof+1];
00600 dwp = new ColumnVector[dof+1];
00601 dvp = new ColumnVector[dof+1];
00602 da = new ColumnVector[dof+1];
00603 df = new ColumnVector[dof+1];
00604 dn = new ColumnVector[dof+1];
00605 dF = new ColumnVector[dof+1];
00606 dN = new ColumnVector[dof+1];
00607 dp = new ColumnVector[dof+1];
00608 R = new Matrix[dof+fix+1];
00609 }
00610 catch(bad_alloc exception)
00611 {
00612 cerr << "Robot_basic constructor : new ran out of memory" << endl;
00613 exit(1);
00614 }
00615
00616 for(i = 0; i <= dof; i++)
00617 {
00618 w[i] = ColumnVector(3);
00619 w[i] = 0.0;
00620 wp[i] = ColumnVector(3);
00621 wp[i] = 0.0;
00622 vp[i] = ColumnVector(3);
00623 dw[i] = ColumnVector(3);
00624 dw[i] = 0.0;
00625 dwp[i] = ColumnVector(3);
00626 dwp[i] = 0.0;
00627 dvp[i] = ColumnVector(3);
00628 dvp[i] = 0.0;
00629 }
00630 for(i = 0; i <= dof+fix; i++)
00631 {
00632 R[i] = Matrix(3,3);
00633 R[i] << threebythreeident;
00634 p[i] = ColumnVector(3);
00635 p[i] = 0.0;
00636 pp[i] = p[i];
00637 }
00638
00639 if ( initrobot.Nrows() == initmotor.Nrows() )
00640 {
00641 if(initmotor.Ncols() == 4)
00642 {
00643 switch(initrobot.Ncols()){
00644 case 15:
00645 for(i = 1; i <= dof+fix; i++)
00646 {
00647 links[i] = Link((int) initrobot(i,1), initrobot(i,2), initrobot(i,3),
00648 initrobot(i,4), initrobot(i,5), 0, 0, 0, initrobot(i,6),
00649 initrobot(i,7), initrobot(i,8), initrobot(i,9),
00650 initrobot(i,10),initrobot(i,11), initrobot(i,12),
00651 initrobot(i,13),initrobot(i,14), initrobot(i,15),
00652 initmotor(i,1), initmotor(i,2), initmotor(i,3),
00653 initmotor(i,4), dh_parameter, min_inertial_para);
00654 }
00655 break;
00656 case 18:
00657 for(i = 1; i <= dof+fix; i++)
00658 {
00659 links[i] = Link((int) initrobot(i,1), initrobot(i,2), initrobot(i,3),
00660 initrobot(i,4), initrobot(i,5), initrobot(i,6),
00661 initrobot(i,7), initrobot(i,8), initrobot(i,9),
00662 initrobot(i,10),initrobot(i,11), initrobot(i,12),
00663 initrobot(i,13),initrobot(i,14), initrobot(i,15),
00664 initrobot(i,16),initrobot(i,17), initrobot(i,18),
00665 initmotor(i,1), initmotor(i,2), initmotor(i,3),
00666 initmotor(i,4), dh_parameter, min_inertial_para);
00667 }
00668 break;
00669 default:
00670 error("Initialisation robot Matrix does not have 16 or 18 columns.");
00671 }
00672 }
00673 else
00674 error("Initialisation robot motor Matrix does not have 4 columns.");
00675
00676 }
00677 else
00678 error("Initialisation robot and motor matrix does not have same numbers of Rows.");
00679 }
00680
00681 Robot_basic::Robot_basic(const int ndof, const bool dh_parameter,
00682 const bool min_inertial_para)
00683
00684
00685
00686
00687
00688
00689
00690
00691
00692 : w(NULL), wp(NULL), vp(NULL), a(NULL), f(NULL), f_nv(NULL), n(NULL), n_nv(NULL),
00693 F(NULL), N(NULL), p(NULL), pp(NULL), dw(NULL), dwp(NULL), dvp(NULL), da(NULL),
00694 df(NULL), dn(NULL), dF(NULL), dN(NULL), dp(NULL), z0(3), gravity(3), R(NULL),
00695 links(NULL), robotType(DEFAULT), dof(ndof), fix(0)
00696 {
00697 int i = 0;
00698 gravity = 0.0;
00699 gravity(3) = 9.81;
00700 z0(1) = z0(2) = 0.0; z0(3) = 1.0;
00701
00702 try
00703 {
00704 links = new Link[dof];
00705 links = links-1;
00706 w = new ColumnVector[dof+1];
00707 wp = new ColumnVector[dof+1];
00708 vp = new ColumnVector[dof+1];
00709 a = new ColumnVector[dof+1];
00710 f = new ColumnVector[dof+1];
00711 f_nv = new ColumnVector[dof+1];
00712 n = new ColumnVector[dof+1];
00713 n_nv = new ColumnVector[dof+1];
00714 F = new ColumnVector[dof+1];
00715 N = new ColumnVector[dof+1];
00716 p = new ColumnVector[dof+1];
00717 pp = new ColumnVector[dof+fix+1];
00718 dw = new ColumnVector[dof+1];
00719 dwp = new ColumnVector[dof+1];
00720 dvp = new ColumnVector[dof+1];
00721 da = new ColumnVector[dof+1];
00722 df = new ColumnVector[dof+1];
00723 dn = new ColumnVector[dof+1];
00724 dF = new ColumnVector[dof+1];
00725 dN = new ColumnVector[dof+1];
00726 dp = new ColumnVector[dof+1];
00727 R = new Matrix[dof+1];
00728 }
00729 catch(bad_alloc exception)
00730 {
00731 cerr << "Robot_basic constructor : new ran out of memory" << endl;
00732 exit(1);
00733 }
00734
00735 for(i = 0; i <= dof; i++)
00736 {
00737 w[i] = ColumnVector(3);
00738 w[i] = 0.0;
00739 wp[i] = ColumnVector(3);
00740 wp[i] = 0.0;
00741 vp[i] = ColumnVector(3);
00742 dw[i] = ColumnVector(3);
00743 dw[i] = 0.0;
00744 dwp[i] = ColumnVector(3);
00745 dwp[i] = 0.0;
00746 dvp[i] = ColumnVector(3);
00747 dvp[i] = 0.0;
00748 }
00749 for(i = 0; i <= dof+fix; i++)
00750 {
00751 R[i] = Matrix(3,3);
00752 R[i] << threebythreeident;
00753 p[i] = ColumnVector(3);
00754 p[i] = 0.0;
00755 pp[i] = p[i];
00756 }
00757 }
00758
00759 Robot_basic::Robot_basic(const Robot_basic & x)
00760
00761 : w(NULL), wp(NULL), vp(NULL), a(NULL), f(NULL), f_nv(NULL), n(NULL), n_nv(NULL),
00762 F(NULL), N(NULL), p(NULL), pp(NULL), dw(NULL), dwp(NULL), dvp(NULL), da(NULL),
00763 df(NULL), dn(NULL), dF(NULL), dN(NULL), dp(NULL), z0(x.z0), gravity(x.gravity), R(NULL),
00764 links(NULL), robotType(x.robotType), dof(x.dof), fix(x.fix)
00765 {
00766 int i = 0;
00767
00768 try
00769 {
00770 links = new Link[dof+fix];
00771 links = links-1;
00772 w = new ColumnVector[dof+1];
00773 wp = new ColumnVector[dof+1];
00774 vp = new ColumnVector[dof+1];
00775 a = new ColumnVector[dof+1];
00776 f = new ColumnVector[dof+1];
00777 f_nv = new ColumnVector[dof+1];
00778 n = new ColumnVector[dof+1];
00779 n_nv = new ColumnVector[dof+1];
00780 F = new ColumnVector[dof+1];
00781 N = new ColumnVector[dof+1];
00782 p = new ColumnVector[dof+fix+1];
00783 pp = new ColumnVector[dof+fix+1];
00784 dw = new ColumnVector[dof+1];
00785 dwp = new ColumnVector[dof+1];
00786 dvp = new ColumnVector[dof+1];
00787 da = new ColumnVector[dof+1];
00788 df = new ColumnVector[dof+1];
00789 dn = new ColumnVector[dof+1];
00790 dF = new ColumnVector[dof+1];
00791 dN = new ColumnVector[dof+1];
00792 dp = new ColumnVector[dof+1];
00793 R = new ColumnVector[dof+fix+1];
00794 }
00795 catch(bad_alloc exception)
00796 {
00797 cerr << "Robot_basic constructor : new ran out of memory" << endl;
00798 exit(1);
00799 }
00800
00801 for(i = 0; i <= dof; i++)
00802 {
00803 w[i] = ColumnVector(3);
00804 w[i] = 0.0;
00805 wp[i] = ColumnVector(3);
00806 wp[i] = 0.0;
00807 vp[i] = ColumnVector(3);
00808 dw[i] = ColumnVector(3);
00809 dw[i] = 0.0;
00810 dwp[i] = ColumnVector(3);
00811 dwp[i] = 0.0;
00812 dvp[i] = ColumnVector(3);
00813 dvp[i] = 0.0;
00814 }
00815 for(i = 0; i <= dof+fix; i++)
00816 {
00817 R[i] = Matrix(3,3);
00818 R[i] << threebythreeident;
00819 p[i] = ColumnVector(3);
00820 p[i] = 0.0;
00821 pp[i] = p[i];
00822 }
00823
00824 for(i = 1; i <= dof+fix; i++)
00825 links[i] = x.links[i];
00826 }
00827
00828 Robot_basic::Robot_basic(const Config& robData, const string & robotName,
00829 const bool dh_parameter, const bool min_inertial_para)
00830
00831
00832
00833
00834
00835
00836
00837
00838
00839 : w(NULL), wp(NULL), vp(NULL), a(NULL), f(NULL), f_nv(NULL), n(NULL), n_nv(NULL),
00840 F(NULL), N(NULL), p(NULL), pp(NULL), dw(NULL), dwp(NULL), dvp(NULL), da(NULL),
00841 df(NULL), dn(NULL), dF(NULL), dN(NULL), dp(NULL), z0(3), gravity(3), R(NULL),
00842 links(NULL), robotType(DEFAULT), dof(0), fix(0)
00843 {
00844 int i = 0;
00845 gravity = 0.0;
00846 gravity(3) = 9.81;
00847 z0(1) = z0(2) = 0.0; z0(3) = 1.0;
00848
00849 bool motor;
00850
00851 if(!robData.select_int(robotName, "dof", dof))
00852 {
00853 if(dof < 0)
00854 error("Robot_basic::Robot_basic: dof is less then zero.");
00855 }
00856 else
00857 error("Robot_basic::Robot_basic: error in extracting dof from conf file.");
00858
00859 if(robData.select_int(robotName, "Fix", fix))
00860 fix = 0;
00861 if(robData.select_bool(robotName, "Motor", motor))
00862 motor = false;
00863
00864 try
00865 {
00866 links = new Link[dof+fix];
00867 links = links-1;
00868 w = new ColumnVector[dof+1];
00869 wp = new ColumnVector[dof+1];
00870 vp = new ColumnVector[dof+fix+1];
00871 a = new ColumnVector[dof+1];
00872 f = new ColumnVector[dof+1];
00873 f_nv = new ColumnVector[dof+1];
00874 n = new ColumnVector[dof+1];
00875 n_nv = new ColumnVector[dof+1];
00876 F = new ColumnVector[dof+1];
00877 N = new ColumnVector[dof+1];
00878 p = new ColumnVector[dof+fix+1];
00879 pp = new ColumnVector[dof+fix+1];
00880 dw = new ColumnVector[dof+1];
00881 dwp = new ColumnVector[dof+1];
00882 dvp = new ColumnVector[dof+1];
00883 da = new ColumnVector[dof+1];
00884 df = new ColumnVector[dof+1];
00885 dn = new ColumnVector[dof+1];
00886 dF = new ColumnVector[dof+1];
00887 dN = new ColumnVector[dof+1];
00888 dp = new ColumnVector[dof+1];
00889 R = new Matrix[dof+fix+1];
00890 }
00891 catch(bad_alloc exception)
00892 {
00893 cerr << "Robot_basic constructor : new ran out of memory" << endl;
00894 exit(1);
00895 }
00896
00897 for(i = 0; i <= dof; i++)
00898 {
00899 w[i] = ColumnVector(3);
00900 w[i] = 0.0;
00901 wp[i] = ColumnVector(3);
00902 wp[i] = 0.0;
00903 vp[i] = ColumnVector(3);
00904 dw[i] = ColumnVector(3);
00905 dw[i] = 0.0;
00906 dwp[i] = ColumnVector(3);
00907 dwp[i] = 0.0;
00908 dvp[i] = ColumnVector(3);
00909 dvp[i] = 0.0;
00910 }
00911 for(i = 0; i <= dof+fix; i++)
00912 {
00913 R[i] = Matrix(3,3);
00914 R[i] << threebythreeident;
00915 p[i] = ColumnVector(3);
00916 p[i] = 0.0;
00917 pp[i] = p[i];
00918 }
00919
00920 for(int j = 1; j <= dof+fix; j++)
00921 {
00922 int joint_type =0;
00923 double theta=0, d=0, a=0, alpha=0, theta_min=0, theta_max=0, joint_offset=0,
00924 m=0, cx=0, cy=0, cz=0, Ixx=0, Ixy=0, Ixz=0, Iyy=0, Iyz=0, Izz=0,
00925 Im=0, Gr=0, B=0, Cf=0;
00926 bool immobile=false;
00927
00928 string robotName_link;
00929 #ifdef __WATCOMC__
00930 ostrstream ostr;
00931 {
00932 char temp[256];
00933 sprintf(temp,"_LINK%d",j);
00934 robotName_link = robotName;
00935 robotName_link.append(temp);
00936 }
00937 #else
00938 ostringstream ostr;
00939 ostr << robotName << "_LINK" << j;
00940 robotName_link = ostr.str();
00941 #endif
00942
00943 robData.select_int(robotName_link, "joint_type", joint_type);
00944 robData.select_double(robotName_link, "theta", theta);
00945 robData.select_double(robotName_link, "d", d);
00946 robData.select_double(robotName_link, "a", a);
00947 robData.select_double(robotName_link, "alpha", alpha);
00948 robData.select_double(robotName_link, "theta_max", theta_max);
00949 robData.select_double(robotName_link, "theta_min", theta_min);
00950 if(robData.parameter_exists(robotName_link, "joint_offset"))
00951 robData.select_double(robotName_link, "joint_offset", joint_offset);
00952 else if(joint_type==0) {
00953 joint_offset=theta;
00954 theta=0;
00955 } else {
00956 joint_offset=d;
00957 d=0;
00958 }
00959 robData.select_double(robotName_link, "m", m);
00960 robData.select_double(robotName_link, "cx", cx);
00961 robData.select_double(robotName_link, "cy", cy);
00962 robData.select_double(robotName_link, "cz", cz);
00963 robData.select_double(robotName_link, "Ixx", Ixx);
00964 robData.select_double(robotName_link, "Ixy", Ixy);
00965 robData.select_double(robotName_link, "Ixz", Ixz);
00966 robData.select_double(robotName_link, "Iyy", Iyy);
00967 robData.select_double(robotName_link, "Iyz", Iyz);
00968 robData.select_double(robotName_link, "Izz", Izz);
00969 if(robData.parameter_exists(robotName_link,"immobile"))
00970 robData.select_bool(robotName_link,"immobile", immobile);
00971
00972 if(motor)
00973 {
00974 robData.select_double(robotName_link, "Im", Im);
00975 robData.select_double(robotName_link, "Gr", Gr);
00976 robData.select_double(robotName_link, "B", B);
00977 robData.select_double(robotName_link, "Cf", Cf);
00978 }
00979
00980 links[j] = Link(joint_type, theta, d, a, alpha, theta_min, theta_max,
00981 joint_offset, m, cx, cy, cz, Ixx, Ixy, Ixz, Iyy, Iyz,
00982 Izz, Im, Gr, B, Cf, dh_parameter, min_inertial_para);
00983 links[j].set_immobile(immobile);
00984 }
00985
00986 if(fix)
00987 links[dof+fix] = Link(2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
00988 0, 0, 0, 0, 0, 0, 0, 0, 0, dh_parameter, min_inertial_para);
00989 }
00990
00991 Robot_basic::~Robot_basic()
00992
00993
00994
00995
00996
00997 {
00998 links = links+1;
00999 delete []links;
01000 delete []R;
01001 delete []dp;
01002 delete []dN;
01003 delete []dF;
01004 delete []dn;
01005 delete []df;
01006 delete []da;
01007 delete []dvp;
01008 delete []dwp;
01009 delete []dw;
01010 delete []pp;
01011 delete []p;
01012 delete []N;
01013 delete []F;
01014 delete []n_nv;
01015 delete []n;
01016 delete []f_nv;
01017 delete []f;
01018 delete []a;
01019 delete []vp;
01020 delete []wp;
01021 delete []w;
01022 }
01023
01024 Robot_basic & Robot_basic::operator=(const Robot_basic & x)
01025
01026 {
01027 int i = 0;
01028 if ( (dof != x.dof) || (fix != x.fix) )
01029 {
01030 links = links+1;
01031 delete []links;
01032 delete []R;
01033 delete []dp;
01034 delete []dN;
01035 delete []dF;
01036 delete []dn;
01037 delete []df;
01038 delete []da;
01039 delete []dvp;
01040 delete []dwp;
01041 delete []dw;
01042 delete []pp;
01043 delete []p;
01044 delete []N;
01045 delete []F;
01046 delete []n_nv;
01047 delete []n;
01048 delete []f_nv;
01049 delete []f;
01050 delete []a;
01051 delete []vp;
01052 delete []wp;
01053 delete []w;
01054 dof = x.dof;
01055 fix = x.fix;
01056 gravity = x.gravity;
01057 z0 = x.z0;
01058
01059 try
01060 {
01061 links = new Link[dof+fix];
01062 links = links-1;
01063 w = new ColumnVector[dof+1];
01064 wp = new ColumnVector[dof+1];
01065 vp = new ColumnVector[dof+fix+1];
01066 a = new ColumnVector[dof+1];
01067 f = new ColumnVector[dof+1];
01068 f_nv = new ColumnVector[dof+1];
01069 n = new ColumnVector[dof+1];
01070 n_nv = new ColumnVector[dof+1];
01071 F = new ColumnVector[dof+1];
01072 N = new ColumnVector[dof+1];
01073 p = new ColumnVector[dof+fix+1];
01074 pp = new ColumnVector[dof+fix+1];
01075 dw = new ColumnVector[dof+1];
01076 dwp = new ColumnVector[dof+1];
01077 dvp = new ColumnVector[dof+1];
01078 da = new ColumnVector[dof+1];
01079 df = new ColumnVector[dof+1];
01080 dn = new ColumnVector[dof+1];
01081 dF = new ColumnVector[dof+1];
01082 dN = new ColumnVector[dof+1];
01083 dp = new ColumnVector[dof+1];
01084 R = new Matrix[dof+fix+1];
01085 }
01086 catch(bad_alloc exception)
01087 {
01088 cerr << "Robot_basic::operator= : new ran out of memory" << endl;
01089 exit(1);
01090 }
01091 }
01092 for(i = 0; i <= dof; i++)
01093 {
01094 w[i] = ColumnVector(3);
01095 w[i] = 0.0;
01096 wp[i] = ColumnVector(3);
01097 wp[i] = 0.0;
01098 vp[i] = ColumnVector(3);
01099 dw[i] = ColumnVector(3);
01100 dw[i] = 0.0;
01101 dwp[i] = ColumnVector(3);
01102 dwp[i] = 0.0;
01103 dvp[i] = ColumnVector(3);
01104 dvp[i] = 0.0;
01105 }
01106 for(i = 0; i <= dof+fix; i++)
01107 {
01108 R[i] = Matrix(3,3);
01109 R[i] << threebythreeident;
01110 p[i] = ColumnVector(3);
01111 p[i] = 0.0;
01112 pp[i] = p[i];
01113 }
01114 for(i = 1; i <= dof+fix; i++)
01115 links[i] = x.links[i];
01116
01117 robotType = x.robotType;
01118
01119 return *this;
01120 }
01121
01122 void Robot_basic::error(const string & msg1) const
01123
01124 {
01125 cerr << endl << "Robot error: " << msg1.c_str() << endl;
01126
01127 }
01128
01129 int Robot_basic::get_available_dof(const int endlink)const
01130
01131 {
01132 int ans=0;
01133 for(int i=1; i<=endlink; i++)
01134 if(!links[i].immobile)
01135 ans++;
01136 return ans;
01137 }
01138
01139 ReturnMatrix Robot_basic::get_q(void)const
01140
01141 {
01142 ColumnVector q(dof);
01143
01144 for(int i = 1; i <= dof; i++)
01145 q(i) = links[i].get_q();
01146 q.Release(); return q;
01147 }
01148
01149 ReturnMatrix Robot_basic::get_qp(void)const
01150
01151 {
01152 ColumnVector qp(dof);
01153
01154 for(int i = 1; i <= dof; i++)
01155 qp(i) = links[i].qp;
01156 qp.Release(); return qp;
01157 }
01158
01159 ReturnMatrix Robot_basic::get_qpp(void)const
01160
01161 {
01162 ColumnVector qpp(dof);
01163
01164 for(int i = 1; i <= dof; i++)
01165 qpp(i) = links[i].qpp;
01166 qpp.Release(); return qpp;
01167 }
01168
01169 ReturnMatrix Robot_basic::get_available_q(const int endlink)const
01170
01171 {
01172 ColumnVector q(get_available_dof(endlink));
01173
01174 int j=1;
01175 for(int i = 1; i <= endlink; i++)
01176 if(!links[i].immobile)
01177 q(j++) = links[i].get_q();
01178 q.Release(); return q;
01179 }
01180
01181 ReturnMatrix Robot_basic::get_available_qp(const int endlink)const
01182
01183 {
01184 ColumnVector qp(get_available_dof(endlink));
01185
01186 int j=1;
01187 for(int i = 1; i <= endlink; i++)
01188 if(!links[i].immobile)
01189 qp(j++) = links[i].qp;
01190 qp.Release(); return qp;
01191 }
01192
01193 ReturnMatrix Robot_basic::get_available_qpp(const int endlink)const
01194
01195 {
01196 ColumnVector qpp(get_available_dof(endlink));
01197
01198 int j=1;
01199 for(int i = 1; i <= endlink; i++)
01200 if(!links[i].immobile)
01201 qpp(j) = links[i].qpp;
01202 qpp.Release(); return qpp;
01203 }
01204
01205 void Robot_basic::set_q(const Matrix & q)
01206
01207
01208
01209
01210
01211
01212
01213 {
01214 int adof=get_available_dof();
01215 if(q.Nrows() == dof && q.Ncols() == 1) {
01216 for(int i = 1; i <= dof; i++)
01217 {
01218 links[i].transform(q(i,1));
01219 if(links[1].DH)
01220 {
01221 p[i](1) = links[i].get_a();
01222 p[i](2) = links[i].get_d() * links[i].R(3,2);
01223 p[i](3) = links[i].get_d() * links[i].R(3,3);
01224 }
01225 else
01226 p[i] = links[i].p;
01227 }
01228 } else if(q.Nrows() == 1 && q.Ncols() == dof) {
01229 for(int i = 1; i <= dof; i++)
01230 {
01231 links[i].transform(q(1,i));
01232 if(links[1].DH)
01233 {
01234 p[i](1) = links[i].get_a();
01235 p[i](2) = links[i].get_d() * links[i].R(3,2);
01236 p[i](3) = links[i].get_d() * links[i].R(3,3);
01237 }
01238 else
01239 p[i] = links[i].p;
01240 }
01241 } else if(q.Nrows() == adof && q.Ncols() == 1) {
01242 int j=1;
01243 for(int i = 1; i <= dof; i++)
01244 if(!links[i].immobile) {
01245 links[i].transform(q(j++,1));
01246 if(links[1].DH)
01247 {
01248 p[i](1) = links[i].get_a();
01249 p[i](2) = links[i].get_d() * links[i].R(3,2);
01250 p[i](3) = links[i].get_d() * links[i].R(3,3);
01251 }
01252 else
01253 p[i] = links[i].p;
01254 }
01255 } else if(q.Nrows() == 1 && q.Ncols() == adof) {
01256 int j=1;
01257 for(int i = 1; i <= dof; i++)
01258 if(!links[i].immobile) {
01259 links[i].transform(q(1,j++));
01260 if(links[1].DH)
01261 {
01262 p[i](1) = links[i].get_a();
01263 p[i](2) = links[i].get_d() * links[i].R(3,2);
01264 p[i](3) = links[i].get_d() * links[i].R(3,3);
01265 }
01266 else
01267 p[i] = links[i].p;
01268 }
01269 } else error("q has the wrong dimension in set_q()");
01270 }
01271
01272 void Robot_basic::set_q(const ColumnVector & q)
01273
01274
01275
01276
01277
01278
01279
01280 {
01281 if(q.Nrows() == dof) {
01282 for(int i = 1; i <= dof; i++)
01283 {
01284 links[i].transform(q(i));
01285 if(links[1].DH)
01286 {
01287 p[i](1) = links[i].get_a();
01288 p[i](2) = links[i].get_d() * links[i].R(3,2);
01289 p[i](3) = links[i].get_d() * links[i].R(3,3);
01290 }
01291 else
01292 p[i] = links[i].p;
01293 }
01294 } else if(q.Nrows() == get_available_dof()) {
01295 int j=1;
01296 for(int i = 1; i <= dof; i++)
01297 if(!links[i].immobile) {
01298 links[i].transform(q(j++));
01299 if(links[1].DH)
01300 {
01301 p[i](1) = links[i].get_a();
01302 p[i](2) = links[i].get_d() * links[i].R(3,2);
01303 p[i](3) = links[i].get_d() * links[i].R(3,3);
01304 }
01305 else
01306 p[i] = links[i].p;
01307 }
01308 } else error("q has the wrong dimension in set_q()");
01309 }
01310
01311 void Robot_basic::set_qp(const ColumnVector & qp)
01312
01313 {
01314 if(qp.Nrows() == dof)
01315 for(int i = 1; i <= dof; i++)
01316 links[i].qp = qp(i);
01317 else if(qp.Nrows() == get_available_dof()) {
01318 int j=1;
01319 for(int i = 1; i <= dof; i++)
01320 if(!links[i].immobile)
01321 links[i].qp = qp(j++);
01322 } else
01323 error("qp has the wrong dimension in set_qp()");
01324 }
01325
01326 void Robot_basic::set_qpp(const ColumnVector & qpp)
01327
01328 {
01329 if(qpp.Nrows() == dof)
01330 for(int i = 1; i <= dof; i++)
01331 links[i].qpp = qpp(i);
01332 else
01333 error("qpp has the wrong dimension in set_qpp()");
01334 }
01335
01336
01337
01338
01339
01340 Robot::Robot(const int ndof): Robot_basic(ndof, true, false)
01341 {
01342 robotType_inv_kin();
01343 }
01344
01345
01346
01347
01348
01349 Robot::Robot(const Matrix & dhinit): Robot_basic(dhinit, true, false)
01350 {
01351 robotType_inv_kin();
01352 }
01353
01354
01355
01356
01357
01358 Robot::Robot(const Matrix & initrobot, const Matrix & initmotor):
01359 Robot_basic(initrobot, initmotor, true, false)
01360 {
01361 robotType_inv_kin();
01362 }
01363
01364
01365
01366
01367
01368 Robot::Robot(const string & filename, const string & robotName):
01369 Robot_basic(Config(filename,true), robotName, true, false)
01370 {
01371 robotType_inv_kin();
01372 }
01373
01374
01375
01376
01377
01378 Robot::Robot(const Config & robData, const string & robotName):
01379 Robot_basic(robData, robotName, true, false)
01380 {
01381 robotType_inv_kin();
01382 }
01383
01384
01385
01386
01387
01388 Robot::Robot(const Robot & x) : Robot_basic(x)
01389 {
01390 }
01391
01392 Robot & Robot::operator=(const Robot & x)
01393
01394 {
01395 Robot_basic::operator=(x);
01396 return *this;
01397 }
01398
01399 void Robot::robotType_inv_kin()
01400
01401
01402
01403
01404
01405
01406
01407
01408
01409 {
01410 if ( Puma_DH(this))
01411 robotType = PUMA;
01412 else if ( Rhino_DH(this))
01413 robotType = RHINO;
01414 else if ( ERS_Leg_DH(this))
01415 robotType = ERS_LEG;
01416 else if ( ERS2xx_Head_DH(this))
01417 robotType = ERS2XX_HEAD;
01418 else if ( ERS7_Head_DH(this))
01419 robotType = ERS7_HEAD;
01420 else
01421 robotType = DEFAULT;
01422 }
01423
01424
01425
01426
01427
01428
01429
01430 mRobot::mRobot(const int ndof) : Robot_basic(ndof, false, false)
01431 {
01432 robotType_inv_kin();
01433 }
01434
01435
01436
01437
01438
01439 mRobot::mRobot(const Matrix & dhinit) : Robot_basic(dhinit, false, false)
01440 {
01441 robotType_inv_kin();
01442 }
01443
01444
01445
01446
01447
01448 mRobot::mRobot(const Matrix & initrobot, const Matrix & initmotor) :
01449 Robot_basic(initrobot, initmotor, false, false)
01450 {
01451 robotType_inv_kin();
01452 }
01453
01454
01455
01456
01457
01458 mRobot::mRobot(const string & filename, const string & robotName):
01459 Robot_basic(Config(filename,true), robotName, false, false)
01460 {
01461 robotType_inv_kin();
01462 }
01463
01464
01465
01466
01467
01468 mRobot::mRobot(const Config & robData, const string & robotName):
01469 Robot_basic(robData, robotName, false, false)
01470 {
01471 robotType_inv_kin();
01472 }
01473
01474
01475
01476
01477
01478 mRobot::mRobot(const mRobot & x) : Robot_basic(x)
01479 {
01480 robotType_inv_kin();
01481 }
01482
01483 mRobot & mRobot::operator=(const mRobot & x)
01484
01485 {
01486 Robot_basic::operator=(x);
01487 return *this;
01488 }
01489
01490 void mRobot::robotType_inv_kin()
01491
01492
01493
01494
01495
01496
01497
01498
01499
01500 {
01501 if ( Puma_mDH(this))
01502 robotType = PUMA;
01503 else if ( Rhino_mDH(this))
01504 robotType = RHINO;
01505 else
01506 robotType = DEFAULT;
01507 }
01508
01509
01510
01511
01512
01513 mRobot_min_para::mRobot_min_para(const int ndof) : Robot_basic(ndof, false, true)
01514 {
01515 robotType_inv_kin();
01516 }
01517
01518
01519
01520
01521
01522 mRobot_min_para::mRobot_min_para(const Matrix & dhinit):
01523 Robot_basic(dhinit, false, true)
01524 {
01525 robotType_inv_kin();
01526 }
01527
01528
01529
01530
01531
01532 mRobot_min_para::mRobot_min_para(const Matrix & initrobot, const Matrix & initmotor) :
01533 Robot_basic(initrobot, initmotor, false, true)
01534 {
01535 robotType_inv_kin();
01536 }
01537
01538
01539
01540
01541
01542 mRobot_min_para::mRobot_min_para(const mRobot_min_para & x) : Robot_basic(x)
01543 {
01544 robotType_inv_kin();
01545 }
01546
01547
01548
01549
01550
01551 mRobot_min_para::mRobot_min_para(const string & filename, const string & robotName):
01552 Robot_basic(Config(filename,true), robotName, false, true)
01553 {
01554 robotType_inv_kin();
01555 }
01556
01557
01558
01559
01560
01561 mRobot_min_para::mRobot_min_para(const Config & robData, const string & robotName):
01562 Robot_basic(robData, robotName, false, true)
01563 {
01564 robotType_inv_kin();
01565 }
01566
01567 mRobot_min_para & mRobot_min_para::operator=(const mRobot_min_para & x)
01568
01569 {
01570 Robot_basic::operator=(x);
01571 return *this;
01572 }
01573
01574 void mRobot_min_para::robotType_inv_kin()
01575
01576
01577
01578
01579
01580
01581
01582
01583
01584 {
01585 if ( Puma_mDH(this))
01586 robotType = PUMA;
01587 else if ( Rhino_mDH(this))
01588 robotType = RHINO;
01589 else
01590 robotType = DEFAULT;
01591 }
01592
01593
01594
01595 void perturb_robot(Robot_basic & robot, const double f)
01596
01597
01598
01599
01600
01601
01602
01603
01604 {
01605 if( (f < 0) || (f > 1) )
01606 cerr << "perturb_robot: f is not between 0 and 1" << endl;
01607
01608 double fact;
01609 srand(clock());
01610 for(int i = 1; i <= robot.get_dof()+robot.get_fix(); i++)
01611 {
01612 fact = (2.0*rand()/RAND_MAX - 1)*f + 1;
01613 robot.links[i].set_Im(robot.links[i].get_Im()*fact);
01614 fact = (2.0*rand()/RAND_MAX - 1)*f + 1;
01615 robot.links[i].set_B(robot.links[i].get_B()*fact);
01616 fact = (2.0*rand()/RAND_MAX - 1)*f + 1;
01617 robot.links[i].set_Cf(robot.links[i].get_Cf()*fact);
01618 fact = (2.0*rand()/RAND_MAX - 1)*f + 1;
01619 robot.links[i].set_m(robot.links[i].get_m()*fact);
01620
01621
01622 fact = (2.0*rand()/RAND_MAX - 1)*f + 1;
01623 Matrix I = robot.links[i].get_I()*fact;
01624 robot.links[i].set_I(I);
01625 }
01626 }
01627
01628 bool Puma_DH(const Robot_basic *robot)
01629
01630
01631
01632
01633
01634
01635
01636 {
01637 if (robot->get_dof() == 6)
01638 {
01639 double a[7], d[7], alpha[7];
01640 for (int j = 1; j <= 6; j++)
01641 {
01642 if (robot->links[j].get_joint_type())
01643 return false;
01644 a[j] = robot->links[j].get_a();
01645 d[j] = robot->links[j].get_d();
01646 alpha[j] = robot->links[j].get_alpha();
01647 }
01648
01649
01650 if( !a[1] && !a[4] && !a[5] && !a[6] && !d[1] && !d[5] &&
01651 !alpha[2] && !alpha[6] && (a[2]>=0) && (a[3]>=0) && (d[2]+d[3]>=0)
01652 && (d[4]>=0) && (d[6]>=0) )
01653 {
01654 return true;
01655 }
01656
01657 }
01658
01659 return false;
01660 }
01661
01662 bool Rhino_DH(const Robot_basic *robot)
01663
01664
01665
01666
01667
01668
01669
01670 {
01671 if (robot->get_dof() == 5)
01672 {
01673 double a[6], d[6], alpha[6];
01674 for (int j = 1; j <= 5; j++)
01675 {
01676 if (robot->links[j].get_joint_type())
01677 return false;
01678 a[j] = robot->links[j].get_a();
01679 d[j] = robot->links[j].get_d();
01680 alpha[j] = robot->links[j].get_alpha();
01681 }
01682
01683 if ( !a[1] && !a[5] && !d[2] && !d[3] && !d[4] &&
01684 !alpha[2] && !alpha[3] && !alpha[5] &&
01685 (a[2]>=0) && (a[3]>=0) && (a[4]>=0) && (d[1]>=0) && (d[5]>=0) )
01686 {
01687 return true;
01688 }
01689
01690 }
01691
01692 return false;
01693 }
01694
01695 bool ERS_Leg_DH(const Robot_basic *robot)
01696
01697 {
01698 if(robot->get_dof()==5) {
01699 Real alpha[6], theta[6];
01700 for(unsigned int i=1; i<=5; i++) {
01701 if(robot->links[i].get_joint_type()!=0)
01702 return false;
01703 alpha[i]=robot->links[i].get_alpha();
01704 theta[i]=robot->links[i].get_theta();
01705 }
01706 return (theta[1]==0 && alpha[1]!=0 && theta[2]!=0 && alpha[2]!=0 && theta[3]==0 && alpha[3]!=0);
01707 }
01708 return false;
01709 }
01710
01711 bool ERS2xx_Head_DH(const Robot_basic *robot)
01712
01713 {
01714 if(robot->get_dof()==5) {
01715 Real alpha[6], theta[6];
01716 bool revolute[6];
01717 for(unsigned int i=1; i<=5; i++) {
01718 alpha[i]=robot->links[i].get_alpha();
01719 theta[i]=robot->links[i].get_theta();
01720 revolute[i]=robot->links[i].get_joint_type()==0;
01721 }
01722 return (theta[1]==0 && alpha[1]!=0 &&
01723 theta[2]==0 && alpha[2]!=0 && revolute[2] &&
01724 theta[3]!=0 && alpha[3]!=0 && revolute[3] &&
01725 revolute[4]);
01726 }
01727 return false;
01728 }
01729
01730 bool ERS7_Head_DH(const Robot_basic *robot)
01731
01732 {
01733 if(robot->get_dof()==5 || robot->get_dof()==6) {
01734 Real alpha[6], theta[6];
01735 bool revolute[6];
01736 for(unsigned int i=1; i<=5; i++) {
01737 alpha[i]=robot->links[i].get_alpha();
01738 theta[i]=robot->links[i].get_theta();
01739 revolute[i]=robot->links[i].get_joint_type()==0;
01740 }
01741 return (theta[1]==0 && alpha[1]!=0 &&
01742 theta[2]==0 && alpha[2]!=0 && revolute[2] &&
01743 theta[3]==0 && alpha[3]!=0 && revolute[3] &&
01744 revolute[4]);
01745 }
01746 return false;
01747 }
01748
01749
01750 bool Puma_mDH(const Robot_basic *robot)
01751
01752
01753
01754
01755
01756
01757
01758 {
01759 if (robot->get_dof() == 6)
01760 {
01761 double a[7], d[7], alpha[7];
01762 for (int j = 1; j <= 6; j++)
01763 {
01764 if (robot->links[j].get_joint_type())
01765 return false;
01766 a[j] = robot->links[j].get_a();
01767 d[j] = robot->links[j].get_d();
01768 alpha[j] = robot->links[j].get_alpha();
01769 }
01770
01771
01772
01773 if ( !a[1] && !a[2] && !a[5] && !a[6] && !d[1] && !d[5] &&
01774 !alpha[1] && !alpha[3] && (a[3] >= 0) && (a[4] >= 0) &&
01775 (d[2] + d[3] >=0) && (d[4]>=0) && (d[6]>=0) )
01776 {
01777 return true;
01778 }
01779
01780 }
01781 return false;
01782 }
01783
01784 bool Rhino_mDH(const Robot_basic *robot)
01785
01786
01787
01788
01789
01790
01791
01792 {
01793 if (robot->get_dof() == 5)
01794 {
01795 double a[6], d[6], alpha[6];
01796 for (int j = 1; j <= 5; j++)
01797 {
01798 if (robot->links[j].get_joint_type())
01799 return false;
01800 a[j] = robot->links[j].get_a();
01801 d[j] = robot->links[j].get_d();
01802 alpha[j] = robot->links[j].get_alpha();
01803 }
01804
01805 if ( !a[1] && !a[2] && !d[2] && !d[3] && !d[4] && !alpha[1] &&
01806 !alpha[3] && !alpha[4] && (d[1]>=0) && (a[3]>=0) &&
01807 (a[4]>=0) && (a[5]>=0) && (d[5]>=0) )
01808 {
01809 return true;
01810 }
01811
01812 }
01813 return false;
01814 }
01815
01816 #ifdef use_namespace
01817 }
01818 #endif
01819
01820
01821
01822
01823
01824
01825
01826
01827