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 #include "utils.h"
00120 
00121 #ifdef use_namespace
00122 namespace ROBOOP {  
00123   using namespace NEWMAT;
00124 #endif
00125   //! @brief RCS/CVS version.
00126   static const char header_rcsid[] __UNUSED__ = "$Id: robot.h,v 1.19 2005/07/26 03:22:08 ejt Exp $";
00127 
00128 
00129 /*!
00130   @class Link
00131   @brief Link definitions.
00132 
00133   A n degree of freedom (dof) serial manipulator is composed of n links. This class
00134   describe the property of a link. A n dof robot has n instance of the class Link.
00135 */
00136 class Link  
00137 {
00138     friend class Robot_basic;
00139     friend class Robot;
00140     friend class mRobot;
00141     friend class mRobot_min_para;
00142 
00143 public:
00144    Link(const int jt = 0, const Real it = 0.0, const Real id = 0.0,
00145         const Real ia = 0.0, const Real ial = 0.0, const Real theta_min = -M_PI/2,
00146         const Real theta_max = M_PI/2, const Real it_off = 0.0, const Real mass = 1.0,
00147         const Real cmx = 0.0, const Real cmy = 0.0, const Real cmz = 0.0,
00148         const Real ixx = 0.0, const Real ixy = 0.0, const Real ixz = 0.0,
00149         const Real iyy = 0.0, const Real iyz = 0.0, const Real izz = 0.0,
00150         const Real iIm = 0.0, const Real iGr = 0.0, const Real iB = 0.0,
00151         const Real iCf = 0.0, const bool dh = true, const bool min_inertial_para = false);
00152    Link(const Link & x);
00153    ~Link(){}                                            //!< Destructor.
00154    Link & operator=(const Link & x);
00155    void transform(const Real q);
00156    bool get_DH(void) const {return DH; }                //!< Return DH value.
00157    int get_joint_type(void) const { return joint_type; }//!< Return the joint type.
00158    Real get_theta(void) const { return theta; }         //!< Return theta.
00159    Real get_d(void) const { return d; }                 //!< Return d.
00160    Real get_a(void) const { return a; }                 //!< Return a.
00161    Real get_alpha(void) const { return alpha; }         //!< Return alpha.
00162    Real get_q(void) const;
00163    Real get_theta_min(void) const { return theta_min; } //!< Return theta_min.
00164    Real get_theta_max(void) const { return theta_max; } //!< Return theta_max.
00165    Real get_joint_offset(void) const { return joint_offset; } //!< Return joint_offset.
00166    ReturnMatrix get_mc(void) { return mc; }             //!< Return mc.
00167    ReturnMatrix get_r(void) { return r; }               //!< Return r.
00168    ReturnMatrix get_p(void) const { return p; }         //!< Return p.
00169    Real get_m(void) const { return m; }                 //!< Return m.
00170    Real get_Im(void) const { return Im; }               //!< Return Im.
00171    Real get_Gr(void) const { return Gr; }               //!< Return Gr.
00172    Real get_B(void) const { return B; }                 //!< Return B.
00173    Real get_Cf(void) const { return Cf; }               //!< Return Cf.
00174    ReturnMatrix get_I(void) const { return I; }         //!< Return I.
00175    bool get_immobile(void) const { return immobile; }   //!< Return immobile.
00176    void set_m(const Real m_) { m = m_; }                //!< Set m.
00177    void set_mc(const ColumnVector & mc_);               //!< Set mc.
00178    void set_r(const ColumnVector & r_);                 //!< Set r.
00179    void set_Im(const Real Im_) { Im = Im_; }            //!< Set Im.
00180    void set_B(const Real B_) { B = B_; }                //!< Set B.
00181    void set_Cf(const Real Cf_) { Cf = Cf_; }            //!< Set Cf.
00182    void set_I(const Matrix & I);                        //!< Set I.
00183    void set_immobile(bool im) { immobile=im; }          //!< Set immobile.
00184 
00185    Matrix R;          //!< Orientation matrix of actual link w.r.t to previous link.
00186    Real qp,           //!< Joint velocity.
00187         qpp;          //!< Joint acceleration.
00188 
00189 private:
00190    int joint_type;    //!< Joint type.
00191    Real theta,        //!< theta DH parameter.
00192         d,            //!< d DH parameter.
00193         a,            //!< a DH parameter.
00194         alpha,        //!< alpha DH parameter.
00195         theta_min,    //!< Min joint angle.
00196         theta_max,    //!< Max joint angle. 
00197         joint_offset; //!< Offset in joint angle (rotoide and prismatic).
00198    bool DH,           //!< DH notation(true) or DH modified notation.
00199         min_para;     //!< Minimum inertial parameter.
00200    ColumnVector r,    //!< Position of center of mass w.r.t. link coordinate system (min_para=F).
00201                 p;    //!< Position vector of actual link w.r.t to previous link.
00202    Real m,            //!< Mass of the link.
00203    Im,                //!< Motor Inertia.
00204    Gr,                //!< Gear Ratio.
00205    B,                 //!< Viscous coefficient.
00206    Cf;                //!< Coulomb fiction coefficient.
00207    ColumnVector mc;   //!< Mass \f$\times\f$ center of gravity (used if min_para = true).
00208    Matrix I;          //!< Inertia matrix w.r.t. center of mass and link coordinate system orientation.
00209    bool immobile;     //!< true if the joint is to be considered locked - ignored for inverse kinematics, but can still be reassigned through transform
00210 };
00211 
00212 /*!
00213   @class Robot_basic
00214   @brief Virtual base robot class.
00215 */
00216 class Robot_basic {
00217    friend class Robot;
00218    friend class mRobot;
00219    friend class mRobot_min_para;
00220    friend class Robotgl;
00221    friend class mRobotgl;
00222 public:
00223    explicit Robot_basic(const int ndof = 1, const bool dh_parameter = false,
00224                const bool min_inertial_para = false);
00225    explicit Robot_basic(const Matrix & initrobot_motor, const bool dh_parameter = false,
00226                const bool min_inertial_para = false);
00227    explicit Robot_basic(const Matrix & initrobot, const Matrix & initmotor,
00228                const bool dh_parameter = false, const bool min_inertial_para = false);
00229    explicit Robot_basic(const Config & robData, const string & robotName,
00230                const bool dh_parameter = false, const bool min_inertial_para = false);
00231    Robot_basic(const Robot_basic & x);
00232    virtual ~Robot_basic();
00233    Robot_basic & operator=(const Robot_basic & x);
00234 
00235    Real get_q(const int i) const {            //!< Return joint i position.
00236       if(i < 1 || i > dof) error("i must be 1 <= i <= dof");
00237       return links[i].get_q();
00238    }
00239    bool get_DH()const { return links[1].DH; } //!< Return true if in DH notation, false otherwise.
00240    int get_dof()const { return dof; }         //!< Return dof.
00241    int get_available_dof()const { return get_available_dof(dof); } //!<Counts number of currently non-immobile links
00242    int get_available_dof(const int endlink)const;
00243    int get_fix()const { return fix; }         //!< Return fix.
00244    ReturnMatrix get_q(void)const;
00245    ReturnMatrix get_qp(void)const;
00246    ReturnMatrix get_qpp(void)const;
00247    ReturnMatrix get_available_q(void)const { return get_available_q(dof); } //!< Return the joint position vector of available (non-immobile) joints.
00248    ReturnMatrix get_available_qp(void)const { return get_available_qp(dof); } //!< Return the joint velocity vector of available (non-immobile) joints.
00249    ReturnMatrix get_available_qpp(void)const { return get_available_qpp(dof); } //!< Return the joint acceleration vector of available (non-immobile) joints.
00250    ReturnMatrix get_available_q(const int endlink)const;
00251    ReturnMatrix get_available_qp(const int endlink)const;
00252    ReturnMatrix get_available_qpp(const int endlink)const;
00253    void set_q(const ColumnVector & q);
00254    void set_q(const Matrix & q);
00255    void set_q(const Real q, const int i) {     //!< Set joint i position.
00256       if(i < 1 || i > dof) error("i must be 1 <= i <= dof");
00257       links[i].transform(q);
00258    }
00259    void set_qp(const ColumnVector & qp);
00260    void set_qpp(const ColumnVector & qpp);
00261    void kine(Matrix & Rot, ColumnVector & pos)const;
00262    void kine(Matrix & Rot, ColumnVector & pos, const int j)const;
00263    ReturnMatrix kine(void)const;
00264    ReturnMatrix kine(const int j)const;
00265    ReturnMatrix kine_pd(const int ref=0)const;
00266    virtual void kine_pd(Matrix & Rot, ColumnVector & pos,
00267                         ColumnVector & pos_dot, const int ref)const=0; 
00268    virtual void robotType_inv_kin() = 0;
00269    virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj = 0);
00270    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)
00271    virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge);
00272    virtual ReturnMatrix inv_kin_pos(const ColumnVector & Pobj, const int mj, const int endlink, const ColumnVector& Plink, bool & converge);
00273    virtual ReturnMatrix inv_kin_orientation(const Matrix & Robj, const int mj, const int endlink, bool & converge);
00274    virtual ReturnMatrix inv_kin_rhino(const Matrix & Tobj, bool & converge) = 0;
00275    virtual ReturnMatrix inv_kin_puma(const Matrix & Tobj, bool & converge) = 0;
00276    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)
00277    virtual ReturnMatrix jacobian(const int endlink, const int ref)const = 0;
00278    virtual ReturnMatrix jacobian_dot(const int ref=0)const = 0;
00279    ReturnMatrix jacobian_DLS_inv(const double eps, const double lambda_max, const int ref=0)const;
00280    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)
00281    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)
00282    virtual void dTdqi(Matrix & dRot, ColumnVector & dpos, const int i, const int endlink) = 0;
00283    virtual ReturnMatrix dTdqi(const int i, const int endlink) = 0;
00284    ReturnMatrix acceleration(const ColumnVector & q, const ColumnVector & qp,
00285                              const ColumnVector & tau);
00286    ReturnMatrix acceleration(const ColumnVector & q, const ColumnVector & qp,
00287                              const ColumnVector & tau, const ColumnVector & Fext,
00288                              const ColumnVector & Next);
00289    ReturnMatrix inertia(const ColumnVector & q);
00290    virtual ReturnMatrix torque_novelocity(const ColumnVector & qpp) = 0;
00291    virtual ReturnMatrix torque(const ColumnVector & q, const ColumnVector & qp,
00292                                const ColumnVector & qpp) = 0;
00293    virtual ReturnMatrix torque(const ColumnVector & q, const ColumnVector & qp,
00294                                const ColumnVector & qpp,
00295                                const ColumnVector & Fext_,
00296                                const ColumnVector & Next_) = 0;
00297    virtual void delta_torque(const ColumnVector & q, const ColumnVector & qp,
00298                              const ColumnVector & qpp, const ColumnVector & dq,
00299                              const ColumnVector & dqp, const ColumnVector & dqpp,
00300                              ColumnVector & torque, ColumnVector & dtorque) =0;
00301    virtual void dq_torque(const ColumnVector & q, const ColumnVector & qp,
00302                           const ColumnVector & qpp, const ColumnVector & dq,
00303                           ColumnVector & torque, ColumnVector & dtorque) =0;
00304    virtual void dqp_torque(const ColumnVector & q, const ColumnVector & qp,
00305                            const ColumnVector & dqp, ColumnVector & torque,
00306                            ColumnVector & dtorque) =0;
00307    ReturnMatrix dtau_dq(const ColumnVector & q, const ColumnVector & qp,
00308                         const ColumnVector & qpp);
00309    ReturnMatrix dtau_dqp(const ColumnVector & q, const ColumnVector & qp);
00310    virtual ReturnMatrix G() = 0;
00311    virtual ReturnMatrix C(const ColumnVector & qp) = 0;
00312    void error(const string & msg1) const;
00313 
00314    //! useful for fixing x to be at least min or at most max
00315    static Real limit_range(Real x, Real min, Real max) { return x>max?max:(x<min?min:x); }
00316 
00317    //! 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)
00318    static Real limit_angle(Real x, Real min, Real max) {
00319       if(x<min || x>max) {
00320          Real mn_dist=fabs(normalize_angle(min-x));
00321          Real mx_dist=fabs(normalize_angle(max-x));
00322          if(mn_dist<mx_dist)
00323             return min;
00324          else
00325             return max;
00326       } else
00327          return x;
00328    }
00329 
00330    //! ensures that @a t is in the range (-pi,pi)  (upper boundary may not be inclusive...?)
00331    static Real normalize_angle(Real t) { return t-rint(t/(2*M_PI))*(2*M_PI); }
00332 
00333    //! converts a homogenous vector to a normal vector
00334    static Real homog_norm(const ColumnVector& v) {
00335       Real tot=0;
00336       for(int i=1; i<=v.nrows()-1; i++)
00337          tot+=v(i)*v(i);
00338       return sqrt(tot)/v(v.nrows());
00339    }
00340    
00341    void convertFrame(Matrix& R, ColumnVector& p, int cur, int dest) const;
00342    ReturnMatrix convertFrame(int cur, int dest) const;
00343    void convertLinkToFrame(Matrix& R, ColumnVector& p, int cur, int dest) const;
00344    ReturnMatrix convertLinkToFrame(int cur, int dest) const;   
00345    void convertFrameToLink(Matrix& R, ColumnVector& p, int cur, int dest) const;
00346    ReturnMatrix convertFrameToLink(int cur, int dest) const;   
00347    void convertLink(Matrix& R, ColumnVector& p, int cur, int dest) const;
00348    ReturnMatrix convertLink(int cur, int dest) const;   
00349    
00350    ColumnVector *w, *wp, *vp, *a, *f, *f_nv, *n, *n_nv, *F, *N, *p, *pp,
00351    *dw, *dwp, *dvp, *da, *df, *dn, *dF, *dN, *dp, 
00352        z0,      //!< Axis vector at each joint.
00353        gravity; //!< Gravity vector.
00354    Matrix *R;   //!< Temprary rotation matrix.
00355    Link *links; //!< Pointer on Link cclass.
00356 
00357 private:
00358    //! enum EnumRobotType
00359    enum EnumRobotType 
00360    { 
00361        DEFAULT = 0,   //!< Default robot familly.
00362        RHINO = 1,     //!< Rhino familly.
00363        PUMA = 2,      //!< Puma familly.
00364        ERS_LEG,       //!< AIBO leg
00365        ERS2XX_HEAD,   //!< AIBO 200 series camera chain (210, 220)
00366        ERS7_HEAD,     //!< AIBO 7 model camera chain
00367    };
00368    EnumRobotType robotType; //!< Robot type.
00369    int dof,                 //!< Degree of freedom.
00370        fix;                 //!< Virtual link, used with modified DH notation.
00371 
00372    static ReturnMatrix pack4x4(const Matrix& R, const ColumnVector& p) {
00373      Matrix T(4,4);
00374      T.SubMatrix(1,3,1,3)=R;
00375      T.SubMatrix(1,3,4,4)=p;
00376      T(4,1)=0; T(4,2)=0; T(4,3)=0; T(4,4)=1;
00377      T.Release(); return T;
00378    }     
00379 };
00380 
00381 /*!
00382   @class Robot
00383   @brief DH notation robot class.
00384 */
00385 class Robot : public Robot_basic
00386 {
00387 public:
00388    explicit Robot(const int ndof=1);
00389    explicit Robot(const Matrix & initrobot);
00390    explicit Robot(const Matrix & initrobot, const Matrix & initmotor);
00391    Robot(const Robot & x);
00392    explicit Robot(const string & filename, const string & robotName);
00393    explicit Robot(const Config & robData, const string & robotName);
00394    ~Robot(){}   //!< Destructor.
00395    Robot & operator=(const Robot & x);
00396    virtual void robotType_inv_kin();
00397    virtual void kine_pd(Matrix & Rot, ColumnVector & pos,
00398                         ColumnVector & pos_dot, const int ref)const;
00399    virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj = 0);
00400    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)
00401    virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge);
00402    virtual ReturnMatrix inv_kin_pos(const ColumnVector & Pobj, const int mj, const int endlink, const ColumnVector& Plink, bool & converge);
00403    virtual ReturnMatrix inv_kin_orientation(const Matrix & Robj, const int mj, const int endlink, bool & converge);
00404    virtual ReturnMatrix inv_kin_rhino(const Matrix & Tobj, bool & converge);
00405    virtual ReturnMatrix inv_kin_puma(const Matrix & Tobj, bool & converge);
00406 
00407    virtual ReturnMatrix inv_kin_ers_pos(const ColumnVector & Pobj, const int endlink, const ColumnVector& Plink, bool & converge);
00408 
00409    virtual ReturnMatrix jacobian(const int ref=0)const { return jacobian(dof,ref); } //!< Jacobian of mobile links expressed at frame ref.
00410    virtual ReturnMatrix jacobian(const int endlink, const int ref)const;
00411    virtual ReturnMatrix jacobian_dot(const int ref=0)const;
00412    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)
00413    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)
00414    virtual void dTdqi(Matrix & dRot, ColumnVector & dpos, const int i, const int endlink);
00415    virtual ReturnMatrix dTdqi(const int i, const int endlink);
00416    virtual ReturnMatrix torque(const ColumnVector & q, const ColumnVector & qp,
00417                                const ColumnVector & qpp);
00418    virtual ReturnMatrix torque(const ColumnVector & q, const ColumnVector & qp,
00419                                const ColumnVector & qpp,
00420                                const ColumnVector & Fext_,
00421                                const ColumnVector & Next_);
00422    virtual ReturnMatrix torque_novelocity(const ColumnVector & qpp);
00423    virtual void delta_torque(const ColumnVector & q, const ColumnVector & qp,
00424                              const ColumnVector & qpp, const ColumnVector & dq,
00425                              const ColumnVector & dqp, const ColumnVector & dqpp,
00426                              ColumnVector & ltorque, ColumnVector & dtorque);
00427    virtual void dq_torque(const ColumnVector & q, const ColumnVector & qp,
00428                           const ColumnVector & qpp, const ColumnVector & dq,
00429                           ColumnVector & torque, ColumnVector & dtorque);
00430    virtual void dqp_torque(const ColumnVector & q, const ColumnVector & qp,
00431                            const ColumnVector & dqp, ColumnVector & torque,
00432                            ColumnVector & dtorque);
00433    virtual ReturnMatrix G();
00434    virtual ReturnMatrix C(const ColumnVector & qp);
00435    
00436 protected:
00437    Real computeFirstERSLink(int curlink, const ColumnVector & Pobj, const int endlink, const ColumnVector& Plink, Real min, Real max, bool & converge);
00438    Real computeSecondERSLink(int curlink, const ColumnVector & Pobj, const int endlink, const ColumnVector& Plink, bool invert, Real min, Real max, bool & converge);
00439    Real computeThirdERSLink(int curlink, const ColumnVector & Pobj, const int endlink, const ColumnVector& Plink, bool invert, Real min, Real max, bool & converge);
00440 };
00441 
00442 // ---------- R O B O T   M O D I F I E D   DH   N O T A T I O N --------------
00443 
00444 /*!
00445   @class mRobot
00446   @brief Modified DH notation robot class.
00447 */
00448 class mRobot : public Robot_basic {
00449 public:
00450    explicit mRobot(const int ndof=1);
00451    explicit mRobot(const Matrix & initrobot_motor);
00452    explicit mRobot(const Matrix & initrobot, const Matrix & initmotor);
00453    mRobot(const mRobot & x);
00454    explicit mRobot(const string & filename, const string & robotName);
00455    explicit mRobot(const Config & robData, const string & robotName);
00456    ~mRobot(){}    //!< Destructor.
00457    mRobot & operator=(const mRobot & x);
00458    virtual void robotType_inv_kin();
00459    virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj = 0);
00460    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)
00461    virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge);
00462    virtual ReturnMatrix inv_kin_rhino(const Matrix & Tobj, bool & converge);
00463    virtual ReturnMatrix inv_kin_puma(const Matrix & Tobj, bool & converge);
00464    virtual void kine_pd(Matrix & Rot, ColumnVector & pos,
00465                         ColumnVector & pos_dot, const int ref)const;
00466    virtual ReturnMatrix jacobian(const int ref=0)const { return jacobian(dof,ref); } //!< Jacobian of mobile links expressed at frame ref.
00467    virtual ReturnMatrix jacobian(const int endlink, const int ref)const;
00468    virtual ReturnMatrix jacobian_dot(const int ref=0)const;
00469    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)
00470    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)
00471    virtual void dTdqi(Matrix & dRot, ColumnVector & dpos, const int i, const int endlink);
00472    virtual ReturnMatrix dTdqi(const int i, const int endlink);
00473    virtual ReturnMatrix torque(const ColumnVector & q, const ColumnVector & qp,
00474                                const ColumnVector & qpp);
00475    virtual ReturnMatrix torque(const ColumnVector & q, const ColumnVector & qp,
00476                                const ColumnVector & qpp,
00477                                const ColumnVector & Fext_,
00478                                const ColumnVector & Next_);
00479    virtual ReturnMatrix torque_novelocity(const ColumnVector & qpp);
00480    virtual void delta_torque(const ColumnVector & q, const ColumnVector & qp,
00481                              const ColumnVector & qpp, const ColumnVector & dq,
00482                              const ColumnVector & dqp, const ColumnVector & dqpp,
00483                              ColumnVector & torque, ColumnVector & dtorque);
00484    virtual void dq_torque(const ColumnVector & q, const ColumnVector & qp,
00485                           const ColumnVector & qpp, const ColumnVector & dq,
00486                           ColumnVector & torque, ColumnVector & dtorque);
00487    virtual void dqp_torque(const ColumnVector & q, const ColumnVector & qp,
00488                            const ColumnVector & dqp, ColumnVector & torque,
00489                            ColumnVector & dtorque);
00490    virtual ReturnMatrix G();
00491    virtual ReturnMatrix C(const ColumnVector & qp);
00492 };
00493 
00494 // --- 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 ---
00495 
00496 /*!
00497   @class mRobot_min_para
00498   @brief Modified DH notation and minimal inertial parameters robot class.
00499 */
00500 class mRobot_min_para : public Robot_basic 
00501 {
00502 public:
00503    explicit mRobot_min_para(const int ndof=1);
00504    explicit mRobot_min_para(const Matrix & dhinit);
00505    explicit mRobot_min_para(const Matrix & initrobot, const Matrix & initmotor);
00506    mRobot_min_para(const mRobot_min_para & x);
00507    explicit mRobot_min_para(const string & filename, const string & robotName);
00508    explicit mRobot_min_para(const Config & robData, const string & robotName);
00509    ~mRobot_min_para(){}    //!< Destructor.
00510    mRobot_min_para & operator=(const mRobot_min_para & x);
00511    virtual void robotType_inv_kin();
00512    virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj = 0);
00513    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)
00514    virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge);
00515    virtual ReturnMatrix inv_kin_rhino(const Matrix & Tobj, bool & converge);
00516    virtual ReturnMatrix inv_kin_puma(const Matrix & Tobj, bool & converge);
00517    virtual void kine_pd(Matrix & Rot, ColumnVector & pos,
00518                         ColumnVector & pos_dot, const int ref=0)const;
00519    virtual ReturnMatrix jacobian(const int ref=0)const { return jacobian(dof,ref); } //!< Jacobian of mobile links expressed at frame ref.
00520    virtual ReturnMatrix jacobian(const int endlink, const int ref)const;
00521    virtual ReturnMatrix jacobian_dot(const int ref=0)const;
00522    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)
00523    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)
00524    virtual void dTdqi(Matrix & dRot, ColumnVector & dpos, const int i, const int endlink);
00525    virtual ReturnMatrix dTdqi(const int i, const int endlink);
00526    virtual ReturnMatrix torque(const ColumnVector & q, const ColumnVector & qp,
00527                                const ColumnVector & qpp);
00528    virtual ReturnMatrix torque(const ColumnVector & q,
00529                                const ColumnVector & qp,
00530                                const ColumnVector & qpp,
00531                                const ColumnVector & Fext_,
00532                                const ColumnVector & Next_);
00533    virtual ReturnMatrix torque_novelocity(const ColumnVector & qpp);
00534    virtual void delta_torque(const ColumnVector & q, const ColumnVector & qp,
00535                              const ColumnVector & qpp, const ColumnVector & dq,
00536                              const ColumnVector & dqp, const ColumnVector & dqpp,
00537                              ColumnVector & torque, ColumnVector & dtorque);
00538    virtual void dq_torque(const ColumnVector & q, const ColumnVector & qp,
00539                           const ColumnVector & qpp, const ColumnVector & dq,
00540                           ColumnVector & torque, ColumnVector & dtorque);
00541    virtual void dqp_torque(const ColumnVector & q, const ColumnVector & qp,
00542                            const ColumnVector & dqp, ColumnVector & torque,
00543                            ColumnVector & dtorque);
00544    virtual ReturnMatrix G();
00545    virtual ReturnMatrix C(const ColumnVector & qp);
00546 };
00547 
00548 void perturb_robot(Robot_basic & robot, const double f = 0.1);
00549 
00550 bool Puma_DH(const Robot_basic *robot);
00551 bool Rhino_DH(const Robot_basic *robot);
00552 bool ERS_Leg_DH(const Robot_basic *robot);
00553 bool ERS2xx_Head_DH(const Robot_basic *robot);
00554 bool ERS7_Head_DH(const Robot_basic *robot);
00555 bool Puma_mDH(const Robot_basic *robot);
00556 bool Rhino_mDH(const Robot_basic *robot);
00557 
00558 #ifdef use_namespace
00559 }
00560 #endif
00561 
00562 #endif 
00563 

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