Homepage Demos Overview Downloads Tutorials Reference
Credits

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

ROBOOP v1.21a
Generated Tue Jan 4 15:42:25 2005 by Doxygen 1.4.0