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

ROBOOP v1.21a
Generated Tue Aug 16 16:32:15 2005 by Doxygen 1.4.4