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