Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
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.21 2007/11/11 23:57:24 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 std::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 std::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        PANTILT,       //!< PanTilt Camera
00368        GOOSENECK,   //!<GooseNeck
00369        CRABARM,   //!CrabArm
00370    };
00371    EnumRobotType robotType; //!< Robot type.
00372    int dof,                 //!< Degree of freedom.
00373        fix;                 //!< Virtual link, used with modified DH notation.
00374 
00375    static ReturnMatrix pack4x4(const Matrix& R, const ColumnVector& p) {
00376      Matrix T(4,4);
00377      T.SubMatrix(1,3,1,3)=R;
00378      T.SubMatrix(1,3,4,4)=p;
00379      T(4,1)=0; T(4,2)=0; T(4,3)=0; T(4,4)=1;
00380      T.Release(); return T;
00381    }     
00382 };
00383 
00384 /*!
00385   @class Robot
00386   @brief DH notation robot class.
00387 */
00388 class Robot : public Robot_basic
00389 {
00390 public:
00391    explicit Robot(const int ndof=1);
00392    explicit Robot(const Matrix & initrobot);
00393    explicit Robot(const Matrix & initrobot, const Matrix & initmotor);
00394    Robot(const Robot & x);
00395    explicit Robot(const std::string & filename, const std::string & robotName);
00396    explicit Robot(const Config & robData, const std::string & robotName);
00397    ~Robot(){}   //!< Destructor.
00398    Robot & operator=(const Robot & x);
00399    virtual void robotType_inv_kin();
00400    virtual void kine_pd(Matrix & Rot, ColumnVector & pos,
00401                         ColumnVector & pos_dot, const int ref)const;
00402    virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj = 0);
00403    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)
00404    virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge);
00405    virtual ReturnMatrix inv_kin_pos(const ColumnVector & Pobj, const int mj, const int endlink, const ColumnVector& Plink, bool & converge);
00406    virtual ReturnMatrix inv_kin_orientation(const Matrix & Robj, const int mj, const int endlink, bool & converge);
00407    virtual ReturnMatrix inv_kin_rhino(const Matrix & Tobj, bool & converge);
00408    virtual ReturnMatrix inv_kin_puma(const Matrix & Tobj, bool & converge);
00409 
00410    virtual ReturnMatrix inv_kin_ers_pos(const ColumnVector & Pobj, const int endlink, const ColumnVector& Plink, bool & converge);
00411    virtual ReturnMatrix inv_kin_pan_tilt(const ColumnVector & Pobj, bool & converge);
00412    virtual ReturnMatrix inv_kin_goose_neck(const Matrix & Tobj);
00413    virtual ReturnMatrix inv_kin_goose_neck_pos(const ColumnVector & Pobj);
00414    virtual ReturnMatrix goose_neck_angles(const ColumnVector & Pobj, Real phi);
00415 
00416    virtual ReturnMatrix jacobian(const int ref=0)const { return jacobian(dof,ref); } //!< Jacobian of mobile links expressed at frame ref.
00417    virtual ReturnMatrix jacobian(const int endlink, const int ref)const;
00418    virtual ReturnMatrix jacobian_dot(const int ref=0)const;
00419    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)
00420    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)
00421    virtual void dTdqi(Matrix & dRot, ColumnVector & dpos, const int i, const int endlink);
00422    virtual ReturnMatrix dTdqi(const int i, const int endlink);
00423    virtual ReturnMatrix torque(const ColumnVector & q, const ColumnVector & qp,
00424                                const ColumnVector & qpp);
00425    virtual ReturnMatrix torque(const ColumnVector & q, const ColumnVector & qp,
00426                                const ColumnVector & qpp,
00427                                const ColumnVector & Fext_,
00428                                const ColumnVector & Next_);
00429    virtual ReturnMatrix torque_novelocity(const ColumnVector & qpp);
00430    virtual void delta_torque(const ColumnVector & q, const ColumnVector & qp,
00431                              const ColumnVector & qpp, const ColumnVector & dq,
00432                              const ColumnVector & dqp, const ColumnVector & dqpp,
00433                              ColumnVector & ltorque, ColumnVector & dtorque);
00434    virtual void dq_torque(const ColumnVector & q, const ColumnVector & qp,
00435                           const ColumnVector & qpp, const ColumnVector & dq,
00436                           ColumnVector & torque, ColumnVector & dtorque);
00437    virtual void dqp_torque(const ColumnVector & q, const ColumnVector & qp,
00438                            const ColumnVector & dqp, ColumnVector & torque,
00439                            ColumnVector & dtorque);
00440    virtual ReturnMatrix G();
00441    virtual ReturnMatrix C(const ColumnVector & qp);
00442    
00443 protected:
00444    Real computeFirstERSLink(int curlink, const ColumnVector & Pobj, const int endlink, const ColumnVector& Plink, Real min, Real max, bool & converge);
00445    Real computeSecondERSLink(int curlink, const ColumnVector & Pobj, const int endlink, const ColumnVector& Plink, bool invert, Real min, Real max, bool & converge);
00446    Real computeThirdERSLink(int curlink, const ColumnVector & Pobj, const int endlink, const ColumnVector& Plink, bool invert, Real min, Real max, bool & converge);
00447 };
00448 
00449 // ---------- R O B O T   M O D I F I E D   DH   N O T A T I O N --------------
00450 
00451 /*!
00452   @class mRobot
00453   @brief Modified DH notation robot class.
00454 */
00455 class mRobot : public Robot_basic {
00456 public:
00457    explicit mRobot(const int ndof=1);
00458    explicit mRobot(const Matrix & initrobot_motor);
00459    explicit mRobot(const Matrix & initrobot, const Matrix & initmotor);
00460    mRobot(const mRobot & x);
00461    explicit mRobot(const std::string & filename, const std::string & robotName);
00462    explicit mRobot(const Config & robData, const std::string & robotName);
00463    ~mRobot(){}    //!< Destructor.
00464    mRobot & operator=(const mRobot & x);
00465    virtual void robotType_inv_kin();
00466    virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj = 0);
00467    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)
00468    virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge);
00469    virtual ReturnMatrix inv_kin_rhino(const Matrix & Tobj, bool & converge);
00470    virtual ReturnMatrix inv_kin_puma(const Matrix & Tobj, bool & converge);
00471    virtual void kine_pd(Matrix & Rot, ColumnVector & pos,
00472                         ColumnVector & pos_dot, const int ref)const;
00473    virtual ReturnMatrix jacobian(const int ref=0)const { return jacobian(dof,ref); } //!< Jacobian of mobile links expressed at frame ref.
00474    virtual ReturnMatrix jacobian(const int endlink, const int ref)const;
00475    virtual ReturnMatrix jacobian_dot(const int ref=0)const;
00476    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)
00477    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)
00478    virtual void dTdqi(Matrix & dRot, ColumnVector & dpos, const int i, const int endlink);
00479    virtual ReturnMatrix dTdqi(const int i, const int endlink);
00480    virtual ReturnMatrix torque(const ColumnVector & q, const ColumnVector & qp,
00481                                const ColumnVector & qpp);
00482    virtual ReturnMatrix torque(const ColumnVector & q, const ColumnVector & qp,
00483                                const ColumnVector & qpp,
00484                                const ColumnVector & Fext_,
00485                                const ColumnVector & Next_);
00486    virtual ReturnMatrix torque_novelocity(const ColumnVector & qpp);
00487    virtual void delta_torque(const ColumnVector & q, const ColumnVector & qp,
00488                              const ColumnVector & qpp, const ColumnVector & dq,
00489                              const ColumnVector & dqp, const ColumnVector & dqpp,
00490                              ColumnVector & torque, ColumnVector & dtorque);
00491    virtual void dq_torque(const ColumnVector & q, const ColumnVector & qp,
00492                           const ColumnVector & qpp, const ColumnVector & dq,
00493                           ColumnVector & torque, ColumnVector & dtorque);
00494    virtual void dqp_torque(const ColumnVector & q, const ColumnVector & qp,
00495                            const ColumnVector & dqp, ColumnVector & torque,
00496                            ColumnVector & dtorque);
00497    virtual ReturnMatrix G();
00498    virtual ReturnMatrix C(const ColumnVector & qp);
00499 };
00500 
00501 // --- 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 ---
00502 
00503 /*!
00504   @class mRobot_min_para
00505   @brief Modified DH notation and minimal inertial parameters robot class.
00506 */
00507 class mRobot_min_para : public Robot_basic 
00508 {
00509 public:
00510    explicit mRobot_min_para(const int ndof=1);
00511    explicit mRobot_min_para(const Matrix & dhinit);
00512    explicit mRobot_min_para(const Matrix & initrobot, const Matrix & initmotor);
00513    mRobot_min_para(const mRobot_min_para & x);
00514    explicit mRobot_min_para(const std::string & filename, const std::string & robotName);
00515    explicit mRobot_min_para(const Config & robData, const std::string & robotName);
00516    ~mRobot_min_para(){}    //!< Destructor.
00517    mRobot_min_para & operator=(const mRobot_min_para & x);
00518    virtual void robotType_inv_kin();
00519    virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj = 0);
00520    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)
00521    virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge);
00522    virtual ReturnMatrix inv_kin_rhino(const Matrix & Tobj, bool & converge);
00523    virtual ReturnMatrix inv_kin_puma(const Matrix & Tobj, bool & converge);
00524    virtual void kine_pd(Matrix & Rot, ColumnVector & pos,
00525                         ColumnVector & pos_dot, const int ref=0)const;
00526    virtual ReturnMatrix jacobian(const int ref=0)const { return jacobian(dof,ref); } //!< Jacobian of mobile links expressed at frame ref.
00527    virtual ReturnMatrix jacobian(const int endlink, const int ref)const;
00528    virtual ReturnMatrix jacobian_dot(const int ref=0)const;
00529    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)
00530    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)
00531    virtual void dTdqi(Matrix & dRot, ColumnVector & dpos, const int i, const int endlink);
00532    virtual ReturnMatrix dTdqi(const int i, const int endlink);
00533    virtual ReturnMatrix torque(const ColumnVector & q, const ColumnVector & qp,
00534                                const ColumnVector & qpp);
00535    virtual ReturnMatrix torque(const ColumnVector & q,
00536                                const ColumnVector & qp,
00537                                const ColumnVector & qpp,
00538                                const ColumnVector & Fext_,
00539                                const ColumnVector & Next_);
00540    virtual ReturnMatrix torque_novelocity(const ColumnVector & qpp);
00541    virtual void delta_torque(const ColumnVector & q, const ColumnVector & qp,
00542                              const ColumnVector & qpp, const ColumnVector & dq,
00543                              const ColumnVector & dqp, const ColumnVector & dqpp,
00544                              ColumnVector & torque, ColumnVector & dtorque);
00545    virtual void dq_torque(const ColumnVector & q, const ColumnVector & qp,
00546                           const ColumnVector & qpp, const ColumnVector & dq,
00547                           ColumnVector & torque, ColumnVector & dtorque);
00548    virtual void dqp_torque(const ColumnVector & q, const ColumnVector & qp,
00549                            const ColumnVector & dqp, ColumnVector & torque,
00550                            ColumnVector & dtorque);
00551    virtual ReturnMatrix G();
00552    virtual ReturnMatrix C(const ColumnVector & qp);
00553 };
00554 
00555 void perturb_robot(Robot_basic & robot, const double f = 0.1);
00556 
00557 bool Puma_DH(const Robot_basic *robot);
00558 bool Rhino_DH(const Robot_basic *robot);
00559 bool ERS_Leg_DH(const Robot_basic *robot);
00560 bool ERS2xx_Head_DH(const Robot_basic *robot);
00561 bool ERS7_Head_DH(const Robot_basic *robot);
00562 bool PanTilt_DH(const Robot_basic *robot);
00563 bool Puma_mDH(const Robot_basic *robot);
00564 bool Rhino_mDH(const Robot_basic *robot);
00565 bool Goose_Neck_DH(const Robot_basic *robot);
00566 bool Crab_Arm_DH(const Robot_basic *robot);
00567 
00568 #ifdef use_namespace
00569 }
00570 #endif
00571 
00572 #endif 
00573 

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