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