Homepage Demos Overview Downloads Tutorials Reference
Credits

robot.h

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 Revision_history:
00032 
00033 2003/02/03: Etienne Lachance
00034    -Removed class mlink. DH and modified DH parameters are now included in link.
00035    -Created virtual class Robot_basic which is now the base class of Robot and 
00036     mRobot.
00037    -Removed classes RobotMotor and mRobotMotor. Motors effect are now included
00038     in classes Robot and mRobot. Code using the old motor class whould need to 
00039     be change by changing RobotMotor by Robot and mRobotMotor by mRobot.
00040    -Added classes mRobot_min_para (modified DH parameters) and IO_matrix_file.
00041    -Created a new torque member function that allowed to have load on last link
00042     (Robot_basic, Robot, mRobot, mRobot_min_para).
00043    -Added the following member functions in class Robot_basic:
00044      void kine_pd(Matrix & Rot, ColumnVector & pos, ColumnVector & posdot)const;
00045      void kine_pd(Matrix & Rot, ColumnVector & pos, ColumnVector & posdot, int j)const;
00046      ReturnMatrix kine_pd(void)const;
00047      ReturnMatrix kine_pd(int j)const;
00048     These functions are like the kine(), but with speed calculation.
00049    -Added labels theta_min and theta_max in class Link.
00050 
00051 2003/04/29: Etienne Lachance
00052    -All gnugrah.cpp definitions are now in gnugraph.h.
00053    -The following ColumnVector are now part of class Robot_basic: *dw, *dwp, *dvp, *da, 
00054     *df, *dn, *dF, *dN, *dp.
00055    -Added functons Robot_basic::jacobian_DLS_inv.
00056    -Added z0 in Robot_basic.
00057    -Fix kine_pd function.
00058 
00059 2003/08/22: Etienne Lachance
00060    -Added parameter converge in member function inv_kin.
00061 
00062 2004/01/23: Etienne Lachance
00063    -Added member function G() (gravity torque), and H() (corriolis torque)
00064     on all robot class.
00065    -Commun definitions are now in include file utils.h.
00066    -Added const in non reference argument for all functions prototypes.
00067 
00068 2004/03/01: Etienne Lachance
00069    -Added non member function perturb_robot.
00070 
00071 2004/03/21: Etienne Lachance
00072    -Added the following member functions: get_theta_min, get_theta_max,
00073     get_mc, get_r, get_p, get_m, get_Im, get_Gr, get_B, get_Cf, get_I,
00074     set_m, set_mc, set_r, set_Im, set_B, set_Cf, set_I.
00075 
00076 2004/04/26: Etienne Lachance and Vincent Drolet
00077    -Added member functions inv_kin_rhino and inv_kin_puma.
00078 
00079 2004/05/21: Etienne Lachance
00080    -Added Doxygen comments.
00081 
00082 2004/07/01: Ethan Tira-Thompson
00083     -Added support for newmat's use_namespace #define, using ROBOOP namespace
00084 
00085 2004/07/02: Etienne Lachance
00086     -Added joint_offset variable in class Link and Link member functions 
00087      get_joint_offset. Idea proposed by Ethan Tira-Thompson.
00088 
00089 2004/07/16: Ethan Tira-Thompson
00090     -Added Link::immobile flag and accessor functions
00091     -Added get_available_q* functions
00092     -inv_kin functions are now all virtual
00093     -Added parameters to jacobian and inv_kin functions to work with frames
00094      other than the end effector
00095 
00096 2004/07/21: Ethan Tira-Thompson
00097     -Added inv_kin_pos() for times when you only care about position
00098     -Added inv_kin_orientation() for times when you only care about orientation
00099     -Added dTdqi(...,endlink) variants
00100     -Fixed some hidden functions
00101     
00102 2004/09/01: Ethan Tira-Thompson
00103     -Added constructor for instantiation from already-read config file.
00104      This saves time when having to reconstruct repeatedly with slow disks
00105 -------------------------------------------------------------------------------
00106 */
00107 
00108 #ifndef __cplusplus
00109 #error Must use C++ for the type Robot
00110 #endif
00111 #ifndef ROBOT_H
00112 #define ROBOT_H
00113 
00114 /*!
00115   @file robot.h
00116   @brief Robots class definitions.
00117 */
00118 
00119 //! @brief RCS/CVS version.
00120 static const char header_rcsid[] = "$Id: robot.h,v 1.18 2004/10/11 22:01:28 ejt Exp $";
00121 
00122 #include "utils.h"
00123 
00124 #ifdef use_namespace
00125 namespace ROBOOP {
00126   using namespace NEWMAT;
00127 #endif
00128 
00129 
00130 /*!
00131   @class Link
00132   @brief Link definitions.
00133 
00134   A n degree of freedom (dof) serial manipulator is composed of n links. This class
00135   describe the property of a link. A n dof robot has n instance of the class Link.
00136 */
00137 class Link  
00138 {
00139     friend class Robot_basic;
00140     friend class Robot;
00141     friend class mRobot;
00142     friend class mRobot_min_para;
00143 
00144 public:
00145    Link(const int jt = 0, const Real it = 0.0, const Real id = 0.0,
00146         const Real ia = 0.0, const Real ial = 0.0, const Real theta_min = -M_PI/2,
00147         const Real theta_max = M_PI/2, const Real it_off = 0.0, const Real mass = 1.0,
00148         const Real cmx = 0.0, const Real cmy = 0.0, const Real cmz = 0.0,
00149         const Real ixx = 0.0, const Real ixy = 0.0, const Real ixz = 0.0,
00150         const Real iyy = 0.0, const Real iyz = 0.0, const Real izz = 0.0,
00151         const Real iIm = 0.0, const Real iGr = 0.0, const Real iB = 0.0,
00152         const Real iCf = 0.0, const bool dh = true, const bool min_inertial_para = false);
00153    Link(const Link & x);
00154    ~Link(){}                                            //!< Destructor.
00155    Link & operator=(const Link & x);
00156    void transform(const Real q);
00157    bool get_DH(void) const {return DH; }                //!< Return DH value.
00158    int get_joint_type(void) const { return joint_type; }//!< Return the joint type.
00159    Real get_theta(void) const { return theta; }         //!< Return theta.
00160    Real get_d(void) const { return d; }                 //!< Return d.
00161    Real get_a(void) const { return a; }                 //!< Return a.
00162    Real get_alpha(void) const { return alpha; }         //!< Return alpha.
00163    Real get_q(void) const;
00164    Real get_theta_min(void) const { return theta_min; } //!< Return theta_min.
00165    Real get_theta_max(void) const { return theta_max; } //!< Return theta_max.
00166    Real get_joint_offset(void) const { return joint_offset; } //!< Return joint_offset.
00167    ReturnMatrix get_mc(void) { return mc; }             //!< Return mc.
00168    ReturnMatrix get_r(void) { return r; }               //!< Return r.
00169    ReturnMatrix get_p(void) const { return p; }         //!< Return p.
00170    Real get_m(void) const { return m; }                 //!< Return m.
00171    Real get_Im(void) const { return Im; }               //!< Return Im.
00172    Real get_Gr(void) const { return Gr; }               //!< Return Gr.
00173    Real get_B(void) const { return B; }                 //!< Return B.
00174    Real get_Cf(void) const { return Cf; }               //!< Return Cf.
00175    ReturnMatrix get_I(void) const { return I; }         //!< Return I.
00176    bool get_immobile(void) const { return immobile; }   //!< Return immobile.
00177    void set_m(const Real m_) { m = m_; }                //!< Set m.
00178    void set_mc(const ColumnVector & mc_);               //!< Set mc.
00179    void set_r(const ColumnVector & r_);                 //!< Set r.
00180    void set_Im(const Real Im_) { Im = Im_; }            //!< Set Im.
00181    void set_B(const Real B_) { B = B_; }                //!< Set B.
00182    void set_Cf(const Real Cf_) { Cf = Cf_; }            //!< Set Cf.
00183    void set_I(const Matrix & I);                        //!< Set I.
00184    void set_immobile(bool im) { immobile=im; }          //!< Set immobile.
00185 
00186    Matrix R;          //!< Orientation matrix of actual link w.r.t to previous link.
00187    Real qp,           //!< Joint velocity.
00188         qpp;          //!< Joint acceleration.
00189 
00190 private:
00191    int joint_type;    //!< Joint type.
00192    Real theta,        //!< theta DH parameter.
00193         d,            //!< d DH parameter.
00194         a,            //!< a DH parameter.
00195         alpha,        //!< alpha DH parameter.
00196         theta_min,    //!< Min joint angle.
00197         theta_max,    //!< Max joint angle. 
00198         joint_offset; //!< Offset in joint angle (rotoide and prismatic).
00199    bool DH,           //!< DH notation(true) or DH modified notation.
00200         min_para;     //!< Minimum inertial parameter.
00201    ColumnVector r,    //!< Position of center of mass w.r.t. link coordinate system (min_para=F).
00202                 p;    //!< Position vector of actual link w.r.t to previous link.
00203    Real m,            //!< Mass of the link.
00204    Im,                //!< Motor Inertia.
00205    Gr,                //!< Gear Ratio.
00206    B,                 //!< Viscous coefficient.
00207    Cf;                //!< Coulomb fiction coefficient.
00208    ColumnVector mc;   //!< Mass \f$\times\f$ center of gravity (used if min_para = true).
00209    Matrix I;          //!< Inertia matrix w.r.t. center of mass and link coordinate system orientation.
00210    bool immobile;     //!< true if the joint is to be considered locked - ignored for inverse kinematics, but can still be reassigned through transform
00211 };
00212 
00213 /*!
00214   @class Robot_basic
00215   @brief Virtual base robot class.
00216 */
00217 class Robot_basic {
00218    friend class Robot;
00219    friend class mRobot;
00220    friend class mRobot_min_para;
00221    friend class Robotgl;
00222    friend class mRobotgl;
00223 public:
00224    explicit Robot_basic(const int ndof = 1, const bool dh_parameter = false,
00225                const bool min_inertial_para = false);
00226    explicit Robot_basic(const Matrix & initrobot_motor, const bool dh_parameter = false,
00227                const bool min_inertial_para = false);
00228    explicit Robot_basic(const Matrix & initrobot, const Matrix & initmotor,
00229                const bool dh_parameter = false, const bool min_inertial_para = false);
00230    explicit Robot_basic(const Config & robData, const string & robotName,
00231                const bool dh_parameter = false, const bool min_inertial_para = false);
00232    Robot_basic(const Robot_basic & x);
00233    virtual ~Robot_basic();
00234    Robot_basic & operator=(const Robot_basic & x);
00235 
00236    Real get_q(const int i) const {            //!< Return joint i position.
00237       if(i < 1 || i > dof) error("i must be 1 <= i <= dof");
00238       return links[i].get_q();
00239    }
00240    bool get_DH()const { return links[1].DH; } //!< Return true if in DH notation, false otherwise.
00241    int get_dof()const { return dof; }         //!< Return dof.
00242    int get_available_dof()const { return get_available_dof(dof); } //!<Counts number of currently non-immobile links
00243    int get_available_dof(const int endlink)const;
00244    int get_fix()const { return fix; }         //!< Return fix.
00245    ReturnMatrix get_q(void)const;
00246    ReturnMatrix get_qp(void)const;
00247    ReturnMatrix get_qpp(void)const;
00248    ReturnMatrix get_available_q(void)const { return get_available_q(dof); } //!< Return the joint position vector of available (non-immobile) joints.
00249    ReturnMatrix get_available_qp(void)const { return get_available_qp(dof); } //!< Return the joint velocity vector of available (non-immobile) joints.
00250    ReturnMatrix get_available_qpp(void)const { return get_available_qpp(dof); } //!< Return the joint acceleration vector of available (non-immobile) joints.
00251    ReturnMatrix get_available_q(const int endlink)const;
00252    ReturnMatrix get_available_qp(const int endlink)const;
00253    ReturnMatrix get_available_qpp(const int endlink)const;
00254    void set_q(const ColumnVector & q);
00255    void set_q(const Matrix & q);
00256    void set_q(const Real q, const int i) {     //!< Set joint i position.
00257       if(i < 1 || i > dof) error("i must be 1 <= i <= dof");
00258       links[i].transform(q);
00259    }
00260    void set_qp(const ColumnVector & qp);
00261    void set_qpp(const ColumnVector & qpp);
00262    void kine(Matrix & Rot, ColumnVector & pos)const;
00263    void kine(Matrix & Rot, ColumnVector & pos, const int j)const;
00264    ReturnMatrix kine(void)const;
00265    ReturnMatrix kine(const int j)const;
00266    ReturnMatrix kine_pd(const int ref=0)const;
00267    virtual void kine_pd(Matrix & Rot, ColumnVector & pos,
00268                         ColumnVector & pos_dot, const int ref)const=0; 
00269    virtual void robotType_inv_kin() = 0;
00270    virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj = 0);
00271    virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj, bool & converge) {return inv_kin(Tobj,mj,dof,converge);} //!< Numerical inverse kinematics.  See inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge)
00272    virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge);
00273    virtual ReturnMatrix inv_kin_pos(const ColumnVector & Pobj, const int mj, const int endlink, const ColumnVector& Plink, bool & converge);
00274    virtual ReturnMatrix inv_kin_orientation(const Matrix & Robj, const int mj, const int endlink, bool & converge);
00275    virtual ReturnMatrix inv_kin_rhino(const Matrix & Tobj, bool & converge) = 0;
00276    virtual ReturnMatrix inv_kin_puma(const Matrix & Tobj, bool & converge) = 0;
00277    virtual ReturnMatrix jacobian(const int ref=0)const { return jacobian(dof,ref); } //!< Jacobian of mobile links expressed at frame ref. See jacobian(const int endlink, const int reg)
00278    virtual ReturnMatrix jacobian(const int endlink, const int ref)const = 0;
00279    virtual ReturnMatrix jacobian_dot(const int ref=0)const = 0;
00280    ReturnMatrix jacobian_DLS_inv(const double eps, const double lambda_max, const int ref=0)const;
00281    virtual void dTdqi(Matrix & dRot, ColumnVector & dpos, const int i) { dTdqi(dRot,dpos,i,dof); } //!< Partial derivative of the robot position (homogeneous transf.) See dTdqi(Matrix & dRot, ColumnVector & dpos, const int i, const int endlink)
00282    virtual ReturnMatrix dTdqi(const int i) { return dTdqi(i,dof); } //!< Partial derivative of the robot position (homogeneous transf.) See dTdqi(Matrix & dRot, ColumnVector & dpos, const int i, const int endlink)
00283    virtual void dTdqi(Matrix & dRot, ColumnVector & dpos, const int i, const int endlink) = 0;
00284    virtual ReturnMatrix dTdqi(const int i, const int endlink) = 0;
00285    ReturnMatrix acceleration(const ColumnVector & q, const ColumnVector & qp,
00286                              const ColumnVector & tau);
00287    ReturnMatrix acceleration(const ColumnVector & q, const ColumnVector & qp,
00288                              const ColumnVector & tau, const ColumnVector & Fext,
00289                              const ColumnVector & Next);
00290    ReturnMatrix inertia(const ColumnVector & q);
00291    virtual ReturnMatrix torque_novelocity(const ColumnVector & qpp) = 0;
00292    virtual ReturnMatrix torque(const ColumnVector & q, const ColumnVector & qp,
00293                                const ColumnVector & qpp) = 0;
00294    virtual ReturnMatrix torque(const ColumnVector & q, const ColumnVector & qp,
00295                                const ColumnVector & qpp,
00296                                const ColumnVector & Fext_,
00297                                const ColumnVector & Next_) = 0;
00298    virtual void delta_torque(const ColumnVector & q, const ColumnVector & qp,
00299                              const ColumnVector & qpp, const ColumnVector & dq,
00300                              const ColumnVector & dqp, const ColumnVector & dqpp,
00301                              ColumnVector & torque, ColumnVector & dtorque) =0;
00302    virtual void dq_torque(const ColumnVector & q, const ColumnVector & qp,
00303                           const ColumnVector & qpp, const ColumnVector & dq,
00304                           ColumnVector & torque, ColumnVector & dtorque) =0;
00305    virtual void dqp_torque(const ColumnVector & q, const ColumnVector & qp,
00306                            const ColumnVector & dqp, ColumnVector & torque,
00307                            ColumnVector & dtorque) =0;
00308    ReturnMatrix dtau_dq(const ColumnVector & q, const ColumnVector & qp,
00309                         const ColumnVector & qpp);
00310    ReturnMatrix dtau_dqp(const ColumnVector & q, const ColumnVector & qp);
00311    virtual ReturnMatrix G() = 0;
00312    virtual ReturnMatrix C(const ColumnVector & qp) = 0;
00313    void error(const string & msg1) const;
00314 
00315    //! useful for fixing x to be at least min or at most max
00316    static Real limit_range(Real x, Real min, Real max) { return x>max?max:(x<min?min:x); }
00317 
00318    //! useful for fixing x to be at least min or at most max; handles angles so closest boundary is picked, but assumes x is normalized (-pi,pi)
00319    static Real limit_angle(Real x, Real min, Real max) {
00320       if(x<min || x>max) {
00321          Real mn_dist=fabs(normalize_angle(min-x));
00322          Real mx_dist=fabs(normalize_angle(max-x));
00323          if(mn_dist<mx_dist)
00324             return min;
00325          else
00326             return max;
00327       } else
00328          return x;
00329    }
00330 
00331    //! ensures that @a t is in the range (-pi,pi)  (upper boundary may not be inclusive...?)
00332    static Real normalize_angle(Real t) { return t-rint(t/(2*M_PI))*(2*M_PI); }
00333 
00334    //! converts a homogenous vector to a normal vector
00335    static Real homog_norm(const ColumnVector& v) {
00336       Real tot=0;
00337       for(int i=1; i<=v.nrows()-1; i++)
00338          tot+=v(i)*v(i);
00339       return sqrt(tot)/v(v.nrows());
00340    }
00341    
00342    void convertFrame(Matrix& R, ColumnVector& p, int cur, int dest) const;
00343    ReturnMatrix convertFrame(int cur, int dest) const;
00344    void convertLinkToFrame(Matrix& R, ColumnVector& p, int cur, int dest) const;
00345    ReturnMatrix convertLinkToFrame(int cur, int dest) const;   
00346    void convertFrameToLink(Matrix& R, ColumnVector& p, int cur, int dest) const;
00347    ReturnMatrix convertFrameToLink(int cur, int dest) const;   
00348    void convertLink(Matrix& R, ColumnVector& p, int cur, int dest) const;
00349    ReturnMatrix convertLink(int cur, int dest) const;   
00350    
00351    ColumnVector *w, *wp, *vp, *a, *f, *f_nv, *n, *n_nv, *F, *N, *p, *pp,
00352    *dw, *dwp, *dvp, *da, *df, *dn, *dF, *dN, *dp, 
00353        z0,      //!< Axis vector at each joint.
00354        gravity; //!< Gravity vector.
00355    Matrix *R;   //!< Temprary rotation matrix.
00356    Link *links; //!< Pointer on Link cclass.
00357 
00358 private:
00359    //! enum EnumRobotType
00360    enum EnumRobotType 
00361    { 
00362        DEFAULT = 0,   //!< Default robot familly.
00363        RHINO = 1,     //!< Rhino familly.
00364        PUMA = 2,      //!< Puma familly.
00365        ERS_LEG,       //!< AIBO leg
00366        ERS2XX_HEAD,   //!< AIBO 200 series camera chain (210, 220)
00367        ERS7_HEAD,     //!< AIBO 7 model camera chain
00368    };
00369    EnumRobotType robotType; //!< Robot type.
00370    int dof,                 //!< Degree of freedom.
00371        fix;                 //!< Virtual link, used with modified DH notation.
00372 
00373    static ReturnMatrix pack4x4(const Matrix& R, const ColumnVector& p) {
00374      Matrix T(4,4);
00375      T.SubMatrix(1,3,1,3)=R;
00376      T.SubMatrix(1,3,4,4)=p;
00377      T(4,1)=0; T(4,2)=0; T(4,3)=0; T(4,4)=1;
00378      T.Release(); return T;
00379    }     
00380 };
00381 
00382 /*!
00383   @class Robot
00384   @brief DH notation robot class.
00385 */
00386 class Robot : public Robot_basic
00387 {
00388 public:
00389    explicit Robot(const int ndof=1);
00390    explicit Robot(const Matrix & initrobot);
00391    explicit Robot(const Matrix & initrobot, const Matrix & initmotor);
00392    Robot(const Robot & x);
00393    explicit Robot(const string & filename, const string & robotName);
00394    explicit Robot(const Config & robData, const string & robotName);
00395    ~Robot(){}   //!< Destructor.
00396    Robot & operator=(const Robot & x);
00397    virtual void robotType_inv_kin();
00398    virtual void kine_pd(Matrix & Rot, ColumnVector & pos,
00399                         ColumnVector & pos_dot, const int ref)const;
00400    virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj = 0);
00401    virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj, bool & converge) {return inv_kin(Tobj,mj,dof,converge);} //!< Numerical inverse kinematics.  See inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge)
00402    virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge);
00403    virtual ReturnMatrix inv_kin_pos(const ColumnVector & Pobj, const int mj, const int endlink, const ColumnVector& Plink, bool & converge);
00404    virtual ReturnMatrix inv_kin_orientation(const Matrix & Robj, const int mj, const int endlink, bool & converge);
00405    virtual ReturnMatrix inv_kin_rhino(const Matrix & Tobj, bool & converge);
00406    virtual ReturnMatrix inv_kin_puma(const Matrix & Tobj, bool & converge);
00407 
00408    virtual ReturnMatrix inv_kin_ers_pos(const ColumnVector & Pobj, const int endlink, const ColumnVector& Plink, bool & converge);
00409 
00410    virtual ReturnMatrix jacobian(const int ref=0)const { return jacobian(dof,ref); } //!< Jacobian of mobile links expressed at frame ref.
00411    virtual ReturnMatrix jacobian(const int endlink, const int ref)const;
00412    virtual ReturnMatrix jacobian_dot(const int ref=0)const;
00413    virtual void dTdqi(Matrix & dRot, ColumnVector & dpos, const int i) { dTdqi(dRot,dpos,i,dof); } //!< Partial derivative of the robot position (homogeneous transf.) See dTdqi(Matrix & dRot, ColumnVector & dpos, const int i, const int endlink)
00414    virtual ReturnMatrix dTdqi(const int i) { return dTdqi(i,dof); } //!< Partial derivative of the robot position (homogeneous transf.) See dTdqi(Matrix & dRot, ColumnVector & dpos, const int i, const int endlink)
00415    virtual void dTdqi(Matrix & dRot, ColumnVector & dpos, const int i, const int endlink);
00416    virtual ReturnMatrix dTdqi(const int i, const int endlink);
00417    virtual ReturnMatrix torque(const ColumnVector & q, const ColumnVector & qp,
00418                                const ColumnVector & qpp);
00419    virtual ReturnMatrix torque(const ColumnVector & q, const ColumnVector & qp,
00420                                const ColumnVector & qpp,
00421                                const ColumnVector & Fext_,
00422                                const ColumnVector & Next_);
00423    virtual ReturnMatrix torque_novelocity(const ColumnVector & qpp);
00424    virtual void delta_torque(const ColumnVector & q, const ColumnVector & qp,
00425                              const ColumnVector & qpp, const ColumnVector & dq,
00426                              const ColumnVector & dqp, const ColumnVector & dqpp,
00427                              ColumnVector & ltorque, ColumnVector & dtorque);
00428    virtual void dq_torque(const ColumnVector & q, const ColumnVector & qp,
00429                           const ColumnVector & qpp, const ColumnVector & dq,
00430                           ColumnVector & torque, ColumnVector & dtorque);
00431    virtual void dqp_torque(const ColumnVector & q, const ColumnVector & qp,
00432                            const ColumnVector & dqp, ColumnVector & torque,
00433                            ColumnVector & dtorque);
00434    virtual ReturnMatrix G();
00435    virtual ReturnMatrix C(const ColumnVector & qp);
00436    
00437 protected:
00438    Real computeFirstERSLink(int curlink, const ColumnVector & Pobj, const int endlink, const ColumnVector& Plink, Real min, Real max, bool & converge);
00439    Real computeSecondERSLink(int curlink, const ColumnVector & Pobj, const int endlink, const ColumnVector& Plink, bool invert, Real min, Real max, bool & converge);
00440    Real computeThirdERSLink(int curlink, const ColumnVector & Pobj, const int endlink, const ColumnVector& Plink, bool invert, Real min, Real max, bool & converge);
00441 };
00442 
00443 // ---------- R O B O T   M O D I F I E D   DH   N O T A T I O N --------------
00444 
00445 /*!
00446   @class mRobot
00447   @brief Modified DH notation robot class.
00448 */
00449 class mRobot : public Robot_basic {
00450 public:
00451    explicit mRobot(const int ndof=1);
00452    explicit mRobot(const Matrix & initrobot_motor);
00453    explicit mRobot(const Matrix & initrobot, const Matrix & initmotor);
00454    mRobot(const mRobot & x);
00455    explicit mRobot(const string & filename, const string & robotName);
00456    explicit mRobot(const Config & robData, const string & robotName);
00457    ~mRobot(){}    //!< Destructor.
00458    mRobot & operator=(const mRobot & x);
00459    virtual void robotType_inv_kin();
00460    virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj = 0);
00461    virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj, bool & converge) {return inv_kin(Tobj,mj,dof,converge);} //!< Numerical inverse kinematics.  See inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge)
00462    virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge);
00463    virtual ReturnMatrix inv_kin_rhino(const Matrix & Tobj, bool & converge);
00464    virtual ReturnMatrix inv_kin_puma(const Matrix & Tobj, bool & converge);
00465    virtual void kine_pd(Matrix & Rot, ColumnVector & pos,
00466                         ColumnVector & pos_dot, const int ref)const;
00467    virtual ReturnMatrix jacobian(const int ref=0)const { return jacobian(dof,ref); } //!< Jacobian of mobile links expressed at frame ref.
00468    virtual ReturnMatrix jacobian(const int endlink, const int ref)const;
00469    virtual ReturnMatrix jacobian_dot(const int ref=0)const;
00470    virtual void dTdqi(Matrix & dRot, ColumnVector & dpos, const int i) { dTdqi(dRot,dpos,i,dof); } //!< Partial derivative of the robot position (homogeneous transf.) See dTdqi(Matrix & dRot, ColumnVector & dpos, const int i, const int endlink)
00471    virtual ReturnMatrix dTdqi(const int i) { return dTdqi(i,dof); } //!< Partial derivative of the robot position (homogeneous transf.) See dTdqi(Matrix & dRot, ColumnVector & dpos, const int i, const int endlink)
00472    virtual void dTdqi(Matrix & dRot, ColumnVector & dpos, const int i, const int endlink);
00473    virtual ReturnMatrix dTdqi(const int i, const int endlink);
00474    virtual ReturnMatrix torque(const ColumnVector & q, const ColumnVector & qp,
00475                                const ColumnVector & qpp);
00476    virtual ReturnMatrix torque(const ColumnVector & q, const ColumnVector & qp,
00477                                const ColumnVector & qpp,
00478                                const ColumnVector & Fext_,
00479                                const ColumnVector & Next_);
00480    virtual ReturnMatrix torque_novelocity(const ColumnVector & qpp);
00481    virtual void delta_torque(const ColumnVector & q, const ColumnVector & qp,
00482                              const ColumnVector & qpp, const ColumnVector & dq,
00483                              const ColumnVector & dqp, const ColumnVector & dqpp,
00484                              ColumnVector & torque, ColumnVector & dtorque);
00485    virtual void dq_torque(const ColumnVector & q, const ColumnVector & qp,
00486                           const ColumnVector & qpp, const ColumnVector & dq,
00487                           ColumnVector & torque, ColumnVector & dtorque);
00488    virtual void dqp_torque(const ColumnVector & q, const ColumnVector & qp,
00489                            const ColumnVector & dqp, ColumnVector & torque,
00490                            ColumnVector & dtorque);
00491    virtual ReturnMatrix G();
00492    virtual ReturnMatrix C(const ColumnVector & qp);
00493 };
00494 
00495 // --- R O B O T  DH  M O D I F I E D,  M I N I M U M   P A R A M E T E R S ---
00496 
00497 /*!
00498   @class mRobot_min_para
00499   @brief Modified DH notation and minimal inertial parameters robot class.
00500 */
00501 class mRobot_min_para : public Robot_basic 
00502 {
00503 public:
00504    explicit mRobot_min_para(const int ndof=1);
00505    explicit mRobot_min_para(const Matrix & dhinit);
00506    explicit mRobot_min_para(const Matrix & initrobot, const Matrix & initmotor);
00507    mRobot_min_para(const mRobot_min_para & x);
00508    explicit mRobot_min_para(const string & filename, const string & robotName);
00509    explicit mRobot_min_para(const Config & robData, const string & robotName);
00510    ~mRobot_min_para(){}    //!< Destructor.
00511    mRobot_min_para & operator=(const mRobot_min_para & x);
00512    virtual void robotType_inv_kin();
00513    virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj = 0);
00514    virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj, bool & converge) {return inv_kin(Tobj,mj,dof,converge);} //!< Numerical inverse kinematics.  See inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge)
00515    virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge);
00516    virtual ReturnMatrix inv_kin_rhino(const Matrix & Tobj, bool & converge);
00517    virtual ReturnMatrix inv_kin_puma(const Matrix & Tobj, bool & converge);
00518    virtual void kine_pd(Matrix & Rot, ColumnVector & pos,
00519                         ColumnVector & pos_dot, const int ref=0)const;
00520    virtual ReturnMatrix jacobian(const int ref=0)const { return jacobian(dof,ref); } //!< Jacobian of mobile links expressed at frame ref.
00521    virtual ReturnMatrix jacobian(const int endlink, const int ref)const;
00522    virtual ReturnMatrix jacobian_dot(const int ref=0)const;
00523    virtual void dTdqi(Matrix & dRot, ColumnVector & dpos, const int i) { dTdqi(dRot,dpos,i,dof); } //!< Partial derivative of the robot position (homogeneous transf.) See dTdqi(Matrix & dRot, ColumnVector & dpos, const int i, const int endlink)
00524    virtual ReturnMatrix dTdqi(const int i) { return dTdqi(i,dof); } //!< Partial derivative of the robot position (homogeneous transf.) See dTdqi(Matrix & dRot, ColumnVector & dpos, const int i, const int endlink)
00525    virtual void dTdqi(Matrix & dRot, ColumnVector & dpos, const int i, const int endlink);
00526    virtual ReturnMatrix dTdqi(const int i, const int endlink);
00527    virtual ReturnMatrix torque(const ColumnVector & q, const ColumnVector & qp,
00528                                const ColumnVector & qpp);
00529    virtual ReturnMatrix torque(const ColumnVector & q,
00530                                const ColumnVector & qp,
00531                                const ColumnVector & qpp,
00532                                const ColumnVector & Fext_,
00533                                const ColumnVector & Next_);
00534    virtual ReturnMatrix torque_novelocity(const ColumnVector & qpp);
00535    virtual void delta_torque(const ColumnVector & q, const ColumnVector & qp,
00536                              const ColumnVector & qpp, const ColumnVector & dq,
00537                              const ColumnVector & dqp, const ColumnVector & dqpp,
00538                              ColumnVector & torque, ColumnVector & dtorque);
00539    virtual void dq_torque(const ColumnVector & q, const ColumnVector & qp,
00540                           const ColumnVector & qpp, const ColumnVector & dq,
00541                           ColumnVector & torque, ColumnVector & dtorque);
00542    virtual void dqp_torque(const ColumnVector & q, const ColumnVector & qp,
00543                            const ColumnVector & dqp, ColumnVector & torque,
00544                            ColumnVector & dtorque);
00545    virtual ReturnMatrix G();
00546    virtual ReturnMatrix C(const ColumnVector & qp);
00547 };
00548 
00549 void perturb_robot(Robot_basic & robot, const double f = 0.1);
00550 
00551 bool Puma_DH(const Robot_basic *robot);
00552 bool Rhino_DH(const Robot_basic *robot);
00553 bool ERS_Leg_DH(const Robot_basic *robot);
00554 bool ERS2xx_Head_DH(const Robot_basic *robot);
00555 bool ERS7_Head_DH(const Robot_basic *robot);
00556 bool Puma_mDH(const Robot_basic *robot);
00557 bool Rhino_mDH(const Robot_basic *robot);
00558 
00559 #ifdef use_namespace
00560 }
00561 #endif
00562 
00563 #endif 
00564 

ROBOOP v1.21a
Generated Tue Oct 19 14:18:25 2004 by Doxygen 1.3.9.1