Homepage Demos Overview Downloads Tutorials Reference
Credits

controller.h

Go to the documentation of this file.
00001 /*
00002 Copyright (C) 2002-2004  Etienne Lachance
00003 
00004 This library is free software; you can redistribute it and/or modify
00005 it under the terms of the GNU Lesser General Public License as
00006 published by the Free Software Foundation; either version 2.1 of the
00007 License, or (at your option) any later version.
00008 
00009 This library is distributed in the hope that it will be useful,
00010 but WITHOUT ANY WARRANTY; without even the implied warranty of
00011 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012 GNU Lesser General Public License for more details.
00013 
00014 You should have received a copy of the GNU Lesser General Public
00015 License along with this library; if not, write to the Free Software
00016 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00017 
00018 
00019 Report problems and direct all questions to:
00020 
00021 email: etienne.lachance@polymtl.ca or richard.gourdeau@polymtl.ca
00022 
00023 -------------------------------------------------------------------------------
00024 Revision_history:
00025 
00026 2004/07/13: Ethan Tira-Thompson
00027     -Added support for newmat's use_namespace #define, using ROBOOP namespace
00028 -------------------------------------------------------------------------------
00029 */
00030 
00031 #ifndef CONTROLLER_H
00032 #define CONTROLLER_H
00033 
00034 /*!
00035   @file controller.h
00036   @brief Header file for controller class definitions.
00037 */
00038 
00039 #include "robot.h"
00040 #include "quaternion.h"
00041 
00042 #ifdef use_namespace
00043 namespace ROBOOP {
00044   using namespace NEWMAT;
00045 #endif
00046   //! @brief RCS/CVS version.
00047   static const char header_controller_rcsid[] __UNUSED__ = "$Id: controller.h,v 1.4 2005/07/26 03:22:09 ejt Exp $";
00048 
00049 //! @brief Return value when input vectors or matrix don't have the right size.
00050 #define WRONG_SIZE -1
00051 
00052 /*!
00053   @class Impedance
00054   @brief Impedance controller class.
00055 
00056   The implemantation of the impedance controller is made of two section:
00057   the first one is the generation of a compliance trajectory and the second one 
00058   used a position controller to ensure the end effector follow the compliance
00059   trajectory (We recommended to used the resolve acceleration controller 
00060   scheme, implemented in the class Resolved_acc).
00061 
00062   This class generate a compliance path given by the translational
00063   and the rotational impedance.
00064   \f[
00065     M_p\ddot{\tilde{p}} + D_p\dot{\tilde{p}} + K_p\tilde{p} = f
00066   \f]
00067   \f[
00068     M_o\dot{\tilde{\omega}} + D_o\tilde{\omega} + K_o'\tilde{v} = n
00069   \f]
00070   where \f$\tilde{x} = x_c - x_d\f$ and \f$ v\f$ is the vector par of the 
00071   quaternion representing the orientation error between the compliant and desired
00072   frame. The orientation error can also be express by rotation matrix, 
00073   \f$ \tilde{R} = R_d^TR_c\f$. The quaternion mathematics are implemented in 
00074   the Quaternion class. The index \f$_c\f$ and \f$_d\f$ denote 
00075   the compliance and the desired respectively. 
00076 
00077   The impedance parameters \f$M_p\f$, \f$D_p\f$, \f$K_p\f$, \f$M_o\f$, \f$D_o\f$ 
00078   and \f$K_o\f$ are \f$3\times 3\f$ diagonal positive definite matrix
00079 */
00080 class Impedance{
00081 public:
00082    Impedance();
00083    Impedance(const Robot_basic & robot, const DiagonalMatrix & Mp_,
00084              const DiagonalMatrix & Dp_, const DiagonalMatrix & Kp_,
00085              const DiagonalMatrix & Mo_, const DiagonalMatrix & Do_, 
00086        const DiagonalMatrix & Ko_);
00087    Impedance(const Impedance & x);
00088    Impedance & operator=(const Impedance & x);
00089    short set_Mp(const DiagonalMatrix & Mp_);
00090    short set_Mp(const Real MP_i, const short i);
00091    short set_Dp(const DiagonalMatrix & Dp_);
00092    short set_Dp(const Real Dp_i, const short i);
00093    short set_Kp(const DiagonalMatrix & Kp_);
00094    short set_Kp(const Real Kp_i, const short i);
00095    short set_Mo(const DiagonalMatrix & Mo_);
00096    short set_Mo(const Real Mo_i, const short i);
00097    short set_Do(const DiagonalMatrix & Do_);
00098    short set_Do(const Real Do_i, const short i);
00099    short set_Ko(const DiagonalMatrix & Ko_);
00100    short set_Ko(const Real Ko_i, const short i);
00101    short control(const ColumnVector & pdpp, const ColumnVector & pdp,
00102                  const ColumnVector & pd, const ColumnVector & wdp,
00103                  const ColumnVector & wd, const Quaternion & qd,
00104                  const ColumnVector & f, const ColumnVector & n,
00105                  const Real dt);
00106 
00107    Quaternion qc,          //!< Compliant frame quaternion.
00108               qcp,         //!< Compliant frame quaternion derivative.
00109               qcp_prev,    //!< Previous value of qcp.
00110               qcd,         //!< Orientation error (betweem compliant and desired frame) quaternion.
00111              quat;         //!< Temporary quaternion.
00112    ColumnVector pc,        //!< Compliant position.
00113                 pcp,       //!< Compliant velocity.
00114                 pcpp,      //!< Compliant acceleration.
00115                 pcp_prev,  //!< Previous value of pcp.
00116                 pcpp_prev, //!< Previous value of pcpp.
00117                 pcd,       //!< Difference between pc and desired position.
00118                 pcdp,      //!< Difference between pcp and desired velocity.
00119                 wc,        //!< Compliant angular velocity.
00120                 wcp,       //!< Compliant angular acceleration.
00121                 wcp_prev,  //!< Previous value of wcp.
00122                 wcd;       //!< Difference between wc and desired angular velocity.
00123 private:
00124    DiagonalMatrix Mp,   //!< Translational impedance inertia matrix.
00125                   Dp,   //!< Translational impedance damping matrix.
00126                   Kp,   //!< Translational impedance stifness matrix.
00127                   Mo,   //!< Rotational impedance inertia matrix.
00128                   Do,   //!< Rotational impedance damping matrix.
00129                   Ko;   //!< Rotational impedance stifness matrix.
00130    Matrix Ko_prime;     //!< Modified rotational impedance stifness matrix.
00131 };
00132 
00133 /*!
00134   @class Resolved_acc
00135   @brief Resolved rate acceleration controller class.
00136 
00137   The dynamic model of a robot manipulator can be expressed in
00138   joint space as
00139   \f[
00140     B(q)\ddot{q} + C(q,\dot{q})\dot{q} + D\dot{q} + g(q) = \tau - J^T(q)f
00141   \f]
00142   According to the concept of inverse dynamics, the driving torques
00143   can be chosen as
00144   \f[
00145     \tau = B(q)J^{-1}(q)\big(a - \dot{J}(q,\dot{q})\dot{q}\big) +
00146     C(q,\dot{q})\dot{q} + D\dot{q} + g(q) - J^T(q)f
00147   \f]
00148   where \f$a\f$ is the a new control input defined by
00149   \f[
00150     a_p = \ddot{p}_d + k_{vp}\dot{\tilde{p}} + k_{pp}\tilde{p}
00151   \f]
00152   \f[
00153     a_o = \dot{\omega}_d + k_{vo}\dot{\tilde{\omega}} + k_{po}\tilde{v}
00154   \f]
00155   where \f$\tilde{x} = x_c - x_d\f$ and \f$ v\f$ is the vector par of the 
00156   quaternion representing the orientation error between the desired and end 
00157   effector frame. \f$k_{vp}\f$, \f$k_{pp}\f$, \f$k_{vo}\f$ and \f$k_{po}\f$ 
00158   are positive gains.
00159   
00160   Up to now this class has been tested only with a 6 dof robot.
00161 */
00162 class Resolved_acc {
00163 public:
00164    Resolved_acc(const short dof = 1);
00165    Resolved_acc(const Robot_basic & robot,
00166                 const Real Kvp, const Real Kpp,
00167                 const Real Kvo, const Real Kpo);
00168    Resolved_acc(const Resolved_acc & x);
00169    Resolved_acc & operator=(const Resolved_acc & x);
00170    void set_Kvp(const Real Kvp);
00171    void set_Kpp(const Real Kpp);
00172    void set_Kvo(const Real Kvo);
00173    void set_Kpo(const Real Kpo);
00174 
00175    ReturnMatrix torque_cmd(Robot_basic & robot, const ColumnVector & pdpp,
00176                            const ColumnVector & pdp, const ColumnVector & pd,
00177                            const ColumnVector & wdp, const ColumnVector & wd,
00178                            const Quaternion & qd, const short link_pc,
00179                            const Real dt);
00180 private:
00181    double Kvp,                //!< Controller gains.
00182           Kpp, 
00183           Kvo, 
00184           Kpo; 
00185    Matrix Rot;                //!< Temporay rotation matrix.
00186    ColumnVector zero3,        //!< \f$3\times 1\f$ zero vector.
00187                 qp,           //!< Robot joints velocity.
00188                 qpp,          //!< Robot joints acceleration.
00189                 a,            //!< Control input.
00190                 p,            //!< End effector position.
00191                 pp,           //!< End effector velocity.
00192                 quat_v_error; //!< Vector part of error quaternion.
00193    Quaternion quat;           //!< Temporary quaternion.
00194 };
00195 
00196 
00197 /*!
00198   @class Computed_torque_method
00199   @brief Computer torque method controller class.
00200 
00201   The dynamic model of a robot manipulator can be expressed in
00202   joint space as
00203   \f[
00204     B(q)\ddot{q} + C(q,\dot{q})\dot{q} + D\dot{q} + g(q) = \tau - J^T(q)f
00205   \f]
00206   The driving torques can be expressed as
00207   \f[
00208     \tau = B(q)\big(\ddot{q}_d + K_d(\dot{q}_d-\dot{q}) 
00209     + K_p(q_d-q)\big) + C(q,\dot{q})\dot{q} + D\dot{q} + g(q) + J^T(q)f
00210   \f]
00211   where \f$K_p\f$, \f$K_d\f$ are diagonal positive definie matrix.
00212 */
00213 class Computed_torque_method {
00214 public:
00215    Computed_torque_method(const short dof = 1);
00216    Computed_torque_method(const Robot_basic & robot,
00217                           const DiagonalMatrix & Kp, const DiagonalMatrix & Kd);
00218    Computed_torque_method(const Computed_torque_method & x);
00219    Computed_torque_method & operator=(const Computed_torque_method & x);
00220    ReturnMatrix torque_cmd(Robot_basic & robot, const ColumnVector & qd,
00221                            const ColumnVector & qpd);
00222    short set_Kd(const DiagonalMatrix & Kd);
00223    short set_Kp(const DiagonalMatrix & Kp);
00224 
00225 private:
00226    int dof;            //!< Degree of freedom.
00227    ColumnVector q,     //!< Robot joints positions.
00228                 qp,    //!< Robot joints velocity.
00229                 qpp,   //!< Robot joints acceleration.
00230                 zero3; //!< \f$3\times 1\f$ zero vector.
00231    DiagonalMatrix Kp,  //!< Position error gain.
00232                   Kd;  //!< Velocity error gain.
00233 };
00234 
00235 
00236 /*!
00237   @class Proportional_Derivative
00238   @brief Proportional derivative controller class
00239 
00240   The driving torques can be expressed as
00241   \f[
00242     \tau = K_p(q_d-q) + K_d(\dot{q}_d-q)
00243   \f]
00244   where \f$K_p\f$, \f$K_d\f$ are diagonal positive definie matrix.
00245 */
00246 class Proportional_Derivative {
00247 public:
00248    Proportional_Derivative(const short dof = 1);
00249    Proportional_Derivative(const Robot_basic & robot, const DiagonalMatrix & Kp, 
00250          const DiagonalMatrix & Kd);
00251    Proportional_Derivative(const Proportional_Derivative & x);
00252    Proportional_Derivative & operator=(const Proportional_Derivative & x);
00253    ReturnMatrix torque_cmd(Robot_basic & robot, const ColumnVector & qd,
00254                            const ColumnVector & qpd);
00255    short set_Kd(const DiagonalMatrix & Kd);
00256    short set_Kp(const DiagonalMatrix & Kp);
00257 
00258 private:
00259    int dof;            //!< Degree of freedom.
00260    ColumnVector q,     //!< Robot joints positions.
00261                 qp,    //!< Robot joints velocity.
00262                 qpp,   //!< Robot joints acceleration.
00263                 tau,   //!< Output torque.
00264                 zero3; //!< \f$3\times 1\f$ zero vector.
00265    DiagonalMatrix Kp,  //!< Position error gain.
00266                   Kd;  //!< Velocity error gain.
00267 };
00268 
00269 #ifdef use_namespace
00270 }
00271 #endif
00272 
00273 #endif
00274 
00275  
00276 
00277 
00278 
00279 
00280 
00281 
00282 
00283 
00284 

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