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