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

ROBOOP v1.21a
Generated Thu Nov 22 00:51:28 2007 by Doxygen 1.5.4