kni: kni: robot.h Source 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/10/02: Etienne Lachance
00097     -Added Schlling for analytic inverse kinematic.
00098 
00099 2005/11/06: Etienne Lachance
00100     - No need to provide a copy constructor and the assignment operator 
00101       (operator=) for Link class. Instead we use the one provide by the
00102       compiler.
00103     -No need to provide an assignment operator for Robot, mRobot and
00104      mRobot_min_para classes.
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 
00119 
00120 static const char header_rcsid[] = "$Id: robot.h,v 1.52 2006/05/16 16:11:15 gourdeau Exp $";
00121 
00122 #include "utils.h"
00123 
00124 #ifdef use_namespace
00125 namespace ROBOOP {
00126   using namespace NEWMAT;
00127 #endif
00128 
00129 
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         const bool immobile = false);
00154    ~Link(){}                                            
00155    void transform(const Real q);
00156    bool get_DH(void) const {return DH; }                
00157    int get_joint_type(void) const { return joint_type; }
00158    Real get_theta(void) const { return theta; }         
00159    Real get_d(void) const { return d; }                 
00160    Real get_a(void) const { return a; }                 
00161    Real get_alpha(void) const { return alpha; }         
00162    Real get_q(void) const;
00163    Real get_theta_min(void) const { return theta_min; } 
00164    Real get_theta_max(void) const { return theta_max; } 
00165    Real get_joint_offset(void) const { return joint_offset; } 
00166    ReturnMatrix get_mc(void) { return mc; }             
00167    ReturnMatrix get_r(void) { return r; }               
00168    ReturnMatrix get_p(void) const { return p; }         
00169    Real get_m(void) const { return m; }                 
00170    Real get_Im(void) const { return Im; }               
00171    Real get_Gr(void) const { return Gr; }               
00172    Real get_B(void) const { return B; }                 
00173    Real get_Cf(void) const { return Cf; }               
00174    ReturnMatrix get_I(void) const { return I; }         
00175    bool get_immobile(void) const { return immobile; }   
00176    void set_m(const Real m_) { m = m_; }                
00177    void set_mc(const ColumnVector & mc_);               
00178    void set_r(const ColumnVector & r_);                 
00179    void set_Im(const Real Im_) { Im = Im_; }            
00180    void set_B(const Real B_) { B = B_; }                
00181    void set_Cf(const Real Cf_) { Cf = Cf_; }            
00182    void set_I(const Matrix & I);                        
00183    void set_immobile(bool im) { immobile=im; }          
00184 
00185    Matrix R;          
00186    Real qp,           
00187         qpp;          
00188 
00189 private:
00190    int joint_type;    
00191    Real theta,        
00192         d,            
00193         a,            
00194         alpha,        
00195         theta_min,    
00196         theta_max,    
00197         joint_offset; 
00198    bool DH,           
00199         min_para;     
00200    ColumnVector r,    
00201                 p;    
00202    Real m,            
00203    Im,                
00204    Gr,                
00205    B,                 
00206    Cf;                
00207    ColumnVector mc;   
00208    Matrix I;          
00209    bool immobile;     
00210 };
00211 
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    Robot_basic(const int ndof = 1, const bool dh_parameter = false,
00224                const bool min_inertial_para = false);
00225    Robot_basic(const Matrix & initrobot_motor, const bool dh_parameter = false,
00226                const bool min_inertial_para = false);
00227    Robot_basic(const Matrix & initrobot, const Matrix & initmotor,
00228                const bool dh_parameter = false, const bool min_inertial_para = false);
00229    Robot_basic(const std::string & filename, 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 {            
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; } 
00240    int get_dof()const { return dof; }         
00241    int get_available_dof()const { return get_available_dof(dof); } 
00242    int get_available_dof(const int endlink)const;
00243    int get_fix()const { 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); } 
00248    ReturnMatrix get_available_qp(void)const { return get_available_qp(dof); } 
00249    ReturnMatrix get_available_qpp(void)const { return get_available_qpp(dof); } 
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) {     
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);
00271    ReturnMatrix inv_kin(const Matrix & Tobj, const int mj, 
00272                         bool & converge) {return inv_kin(Tobj,mj,dof,converge);} 
00273    virtual ReturnMatrix inv_kin(const Matrix & Tobj, 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 inv_kin_schilling(const Matrix & Tobj, bool & converge) = 0;
00277    virtual ReturnMatrix jacobian(const int ref=0)const { return jacobian(dof,ref); } 
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 & dp, const int i) = 0;
00282    virtual ReturnMatrix dTdqi(const int i) = 0;
00283    ReturnMatrix acceleration(const ColumnVector & q, const ColumnVector & qp,
00284                              const ColumnVector & tau);
00285    ReturnMatrix acceleration(const ColumnVector & q, const ColumnVector & qp,
00286                              const ColumnVector & tau, const ColumnVector & Fext,
00287                              const ColumnVector & Next);
00288    ReturnMatrix inertia(const ColumnVector & q);
00289    virtual ReturnMatrix torque_novelocity(const ColumnVector & qpp) = 0;
00290    virtual ReturnMatrix torque(const ColumnVector & q, const ColumnVector & qp,
00291                                const ColumnVector & qpp) = 0;
00292    virtual ReturnMatrix torque(const ColumnVector & q, const ColumnVector & qp,
00293                                const ColumnVector & qpp,
00294                                const ColumnVector & Fext_,
00295                                const ColumnVector & Next_) = 0;
00296    virtual void delta_torque(const ColumnVector & q, const ColumnVector & qp,
00297                              const ColumnVector & qpp, const ColumnVector & dq,
00298                              const ColumnVector & dqp, const ColumnVector & dqpp,
00299                              ColumnVector & torque, ColumnVector & dtorque) =0;
00300    virtual void dq_torque(const ColumnVector & q, const ColumnVector & qp,
00301                           const ColumnVector & qpp, const ColumnVector & dq,
00302                           ColumnVector & torque, ColumnVector & dtorque) =0;
00303    virtual void dqp_torque(const ColumnVector & q, const ColumnVector & qp,
00304                            const ColumnVector & dqp, ColumnVector & torque,
00305                            ColumnVector & dtorque) =0;
00306    ReturnMatrix dtau_dq(const ColumnVector & q, const ColumnVector & qp,
00307                         const ColumnVector & qpp);
00308    ReturnMatrix dtau_dqp(const ColumnVector & q, const ColumnVector & qp);
00309    virtual ReturnMatrix G() = 0;
00310    virtual ReturnMatrix C(const ColumnVector & qp) = 0;
00311    void error(const std::string & msg1) const;
00312 
00313    ColumnVector *w, *wp, *vp, *a, *f, *f_nv, *n, *n_nv, *F, *N, *p, *pp,
00314    *dw, *dwp, *dvp, *da, *df, *dn, *dF, *dN, *dp, 
00315        z0,      
00316        gravity; 
00317    Matrix *R;   
00318    Link *links; 
00319 
00320 private:
00321    void cleanUpPointers();
00322 
00324    enum EnumRobotType 
00325    { 
00326        DEFAULT = 0,   
00327        RHINO = 1,     
00328        PUMA = 2,      
00329        SCHILLING = 3  
00330    };
00331    EnumRobotType robotType; 
00332    int dof,                 
00333        fix;                 
00334 };
00335 
00340 class Robot : public Robot_basic
00341 {
00342 public:
00343    Robot(const int ndof=1);
00344    Robot(const Matrix & initrobot);
00345    Robot(const Matrix & initrobot, const Matrix & initmotor);
00346    Robot(const Robot & x);
00347    Robot(const std::string & filename, const std::string & robotName);
00348    virtual ~Robot(){}   
00349    virtual void robotType_inv_kin();
00350    virtual void kine_pd(Matrix & Rot, ColumnVector & pos,
00351                         ColumnVector & pos_dot, const int ref)const;
00352    ReturnMatrix inv_kin(const Matrix & Tobj, const int mj = 0);
00353    virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge);
00354    virtual ReturnMatrix inv_kin_rhino(const Matrix & Tobj, bool & converge);
00355    virtual ReturnMatrix inv_kin_puma(const Matrix & Tobj, bool & converge);
00356    virtual ReturnMatrix inv_kin_schilling(const Matrix & Tobj, bool & converge);
00357    virtual ReturnMatrix jacobian(const int ref=0)const { return jacobian(dof,ref); } 
00358    virtual ReturnMatrix jacobian(const int endlink, const int ref)const;
00359    virtual ReturnMatrix jacobian_dot(const int ref=0)const;
00360    virtual void dTdqi(Matrix & dRot, ColumnVector & dp, const int i);
00361    virtual ReturnMatrix dTdqi(const int i);
00362    virtual ReturnMatrix torque(const ColumnVector & q, const ColumnVector & qp,
00363                                const ColumnVector & qpp);
00364    virtual ReturnMatrix torque(const ColumnVector & q, const ColumnVector & qp,
00365                                const ColumnVector & qpp,
00366                                const ColumnVector & Fext_,
00367                                const ColumnVector & Next_);
00368    virtual ReturnMatrix torque_novelocity(const ColumnVector & qpp);
00369    virtual void delta_torque(const ColumnVector & q, const ColumnVector & qp,
00370                              const ColumnVector & qpp, const ColumnVector & dq,
00371                              const ColumnVector & dqp, const ColumnVector & dqpp,
00372                              ColumnVector & ltorque, ColumnVector & dtorque);
00373    virtual void dq_torque(const ColumnVector & q, const ColumnVector & qp,
00374                           const ColumnVector & qpp, const ColumnVector & dq,
00375                           ColumnVector & torque, ColumnVector & dtorque);
00376    virtual void dqp_torque(const ColumnVector & q, const ColumnVector & qp,
00377                            const ColumnVector & dqp, ColumnVector & torque,
00378                            ColumnVector & dtorque);
00379    virtual ReturnMatrix G();
00380    virtual ReturnMatrix C(const ColumnVector & qp);
00381 };
00382 
00383 // ---------- R O B O T   M O D I F I E D   DH   N O T A T I O N --------------
00384 
00389 class mRobot : public Robot_basic {
00390 public:
00391    mRobot(const int ndof=1);
00392    mRobot(const Matrix & initrobot_motor);
00393    mRobot(const Matrix & initrobot, const Matrix & initmotor);
00394    mRobot(const mRobot & x);
00395    mRobot(const std::string & filename, const std::string & robotName);
00396    virtual ~mRobot(){}    
00397    virtual void robotType_inv_kin();
00398    ReturnMatrix inv_kin(const Matrix & Tobj, const int mj = 0);
00399    virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge);
00400    virtual ReturnMatrix inv_kin_rhino(const Matrix & Tobj, bool & converge);
00401    virtual ReturnMatrix inv_kin_puma(const Matrix & Tobj, bool & converge);
00402    virtual ReturnMatrix inv_kin_schilling(const Matrix & Tobj, bool & converge);
00403    virtual void kine_pd(Matrix & Rot, ColumnVector & pos,
00404                         ColumnVector & pos_dot, const int ref)const;
00405    virtual ReturnMatrix jacobian(const int ref=0)const { return jacobian(dof,ref); } 
00406    virtual ReturnMatrix jacobian(const int endlink, const int ref)const;
00407    virtual ReturnMatrix jacobian_dot(const int ref=0)const;
00408    virtual void dTdqi(Matrix & dRot, ColumnVector & dp, const int i);
00409    virtual ReturnMatrix dTdqi(const int i);
00410    virtual ReturnMatrix torque(const ColumnVector & q, const ColumnVector & qp,
00411                                const ColumnVector & qpp);
00412    virtual ReturnMatrix torque(const ColumnVector & q, const ColumnVector & qp,
00413                                const ColumnVector & qpp,
00414                                const ColumnVector & Fext_,
00415                                const ColumnVector & Next_);
00416    virtual ReturnMatrix torque_novelocity(const ColumnVector & qpp);
00417    virtual void delta_torque(const ColumnVector & q, const ColumnVector & qp,
00418                              const ColumnVector & qpp, const ColumnVector & dq,
00419                              const ColumnVector & dqp, const ColumnVector & dqpp,
00420                              ColumnVector & torque, ColumnVector & dtorque);
00421    virtual void dq_torque(const ColumnVector & q, const ColumnVector & qp,
00422                           const ColumnVector & qpp, const ColumnVector & dq,
00423                           ColumnVector & torque, ColumnVector & dtorque);
00424    virtual void dqp_torque(const ColumnVector & q, const ColumnVector & qp,
00425                            const ColumnVector & dqp, ColumnVector & torque,
00426                            ColumnVector & dtorque);
00427    virtual ReturnMatrix G();
00428    virtual ReturnMatrix C(const ColumnVector & qp);
00429 };
00430 
00431 // --- 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 ---
00432 
00437 class mRobot_min_para : public Robot_basic 
00438 {
00439 public:
00440    mRobot_min_para(const int ndof=1);
00441    mRobot_min_para(const Matrix & dhinit);
00442    mRobot_min_para(const Matrix & initrobot, const Matrix & initmotor);
00443    mRobot_min_para(const mRobot_min_para & x);
00444    mRobot_min_para(const std::string & filename, const std::string & robotName);
00445    virtual ~mRobot_min_para(){}    
00446    virtual void robotType_inv_kin();
00447    ReturnMatrix inv_kin(const Matrix & Tobj, const int mj = 0);
00448    virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge);
00449    virtual ReturnMatrix inv_kin_rhino(const Matrix & Tobj, bool & converge);
00450    virtual ReturnMatrix inv_kin_puma(const Matrix & Tobj, bool & converge);
00451    virtual ReturnMatrix inv_kin_schilling(const Matrix & Tobj, bool & converge);
00452    virtual void kine_pd(Matrix & Rot, ColumnVector & pos,
00453                         ColumnVector & pos_dot, const int ref=0)const;
00454    virtual ReturnMatrix jacobian(const int ref=0)const { return jacobian(dof,ref); } 
00455    virtual ReturnMatrix jacobian(const int endlink, const int ref)const;
00456    virtual ReturnMatrix jacobian_dot(const int ref=0)const;
00457    virtual void dTdqi(Matrix & dRot, ColumnVector & dp, const int i);
00458    virtual ReturnMatrix dTdqi(const int i);
00459    virtual ReturnMatrix torque(const ColumnVector & q, const ColumnVector & qp,
00460                                const ColumnVector & qpp);
00461    virtual ReturnMatrix torque(const ColumnVector & q,
00462                                const ColumnVector & qp,
00463                                const ColumnVector & qpp,
00464                                const ColumnVector & Fext_,
00465                                const ColumnVector & Next_);
00466    virtual ReturnMatrix torque_novelocity(const ColumnVector & qpp);
00467    virtual void delta_torque(const ColumnVector & q, const ColumnVector & qp,
00468                              const ColumnVector & qpp, const ColumnVector & dq,
00469                              const ColumnVector & dqp, const ColumnVector & dqpp,
00470                              ColumnVector & torque, ColumnVector & dtorque);
00471    virtual void dq_torque(const ColumnVector & q, const ColumnVector & qp,
00472                           const ColumnVector & qpp, const ColumnVector & dq,
00473                           ColumnVector & torque, ColumnVector & dtorque);
00474    virtual void dqp_torque(const ColumnVector & q, const ColumnVector & qp,
00475                            const ColumnVector & dqp, ColumnVector & torque,
00476                            ColumnVector & dtorque);
00477    virtual ReturnMatrix G();
00478    virtual ReturnMatrix C(const ColumnVector & qp);
00479 };
00480 
00481 void perturb_robot(Robot_basic & robot, const double f = 0.1);
00482 
00483 bool Rhino_DH(const Robot_basic & robot);
00484 bool Puma_DH(const Robot_basic & robot);
00485 bool Schilling_DH(const Robot_basic & robot);
00486 
00487 bool Rhino_mDH(const Robot_basic  & robot);
00488 bool Puma_mDH(const Robot_basic  & robot);
00489 bool Schilling_mDH(const Robot_basic & robot);
00490 
00491 #ifdef use_namespace
00492 }
00493 #endif
00494 
00495 #endif 
00496