robot.cpp
Go to the documentation of this file.
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 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


kni
Author(s): Neuronics AG (see AUTHORS.txt); ROS wrapper by Martin Günther
autogenerated on Tue May 28 2013 14:52:54