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 //! @brief RCS/CVS version.
00040 static const char header_controller_rcsid[] = "$Id: controller.h,v 1.3 2004/07/14 02:46:43 ejt Exp $";
00041 
00042 
00043 #include "robot.h"
00044 #include "quaternion.h"
00045 
00046 #ifdef use_namespace
00047 namespace ROBOOP {
00048   using namespace NEWMAT;
00049 #endif
00050 
00051 //! @brief Return value when input vectors or matrix don't have the right size.
00052 #define WRONG_SIZE -1
00053 
00054 /*!
00055   @class Impedance
00056   @brief Impedance controller class.
00057 
00058   The implemantation of the impedance controller is made of two section:
00059   the first one is the generation of a compliance trajectory and the second one 
00060   used a position controller to ensure the end effector follow the compliance
00061   trajectory (We recommended to used the resolve acceleration controller 
00062   scheme, implemented in the class Resolved_acc).
00063 
00064   This class generate a compliance path given by the translational
00065   and the rotational impedance.
00066   \f[
00067     M_p\ddot{\tilde{p}} + D_p\dot{\tilde{p}} + K_p\tilde{p} = f
00068   \f]
00069   \f[
00070     M_o\dot{\tilde{\omega}} + D_o\tilde{\omega} + K_o'\tilde{v} = n
00071   \f]
00072   where \f$\tilde{x} = x_c - x_d\f$ and \f$ v\f$ is the vector par of the 
00073   quaternion representing the orientation error between the compliant and desired
00074   frame. The orientation error can also be express by rotation matrix, 
00075   \f$ \tilde{R} = R_d^TR_c\f$. The quaternion mathematics are implemented in 
00076   the Quaternion class. The index \f$_c\f$ and \f$_d\f$ denote 
00077   the compliance and the desired respectively. 
00078 
00079   The impedance parameters \f$M_p\f$, \f$D_p\f$, \f$K_p\f$, \f$M_o\f$, \f$D_o\f$ 
00080   and \f$K_o\f$ are \f$3\times 3\f$ diagonal positive definite matrix
00081 */
00082 class Impedance{
00083 public:
00084    Impedance();
00085    Impedance(const Robot_basic & robot, const DiagonalMatrix & Mp_,
00086              const DiagonalMatrix & Dp_, const DiagonalMatrix & Kp_,
00087              const DiagonalMatrix & Mo_, const DiagonalMatrix & Do_, 
00088        const DiagonalMatrix & Ko_);
00089    Impedance(const Impedance & x);
00090    Impedance & operator=(const Impedance & x);
00091    short set_Mp(const DiagonalMatrix & Mp_);
00092    short set_Mp(const Real MP_i, const short i);
00093    short set_Dp(const DiagonalMatrix & Dp_);
00094    short set_Dp(const Real Dp_i, const short i);
00095    short set_Kp(const DiagonalMatrix & Kp_);
00096    short set_Kp(const Real Kp_i, const short i);
00097    short set_Mo(const DiagonalMatrix & Mo_);
00098    short set_Mo(const Real Mo_i, const short i);
00099    short set_Do(const DiagonalMatrix & Do_);
00100    short set_Do(const Real Do_i, const short i);
00101    short set_Ko(const DiagonalMatrix & Ko_);
00102    short set_Ko(const Real Ko_i, const short i);
00103    short control(const ColumnVector & pdpp, const ColumnVector & pdp,
00104                  const ColumnVector & pd, const ColumnVector & wdp,
00105                  const ColumnVector & wd, const Quaternion & qd,
00106                  const ColumnVector & f, const ColumnVector & n,
00107                  const Real dt);
00108 
00109    Quaternion qc,          //!< Compliant frame quaternion.
00110               qcp,         //!< Compliant frame quaternion derivative.
00111               qcp_prev,    //!< Previous value of qcp.
00112               qcd,         //!< Orientation error (betweem compliant and desired frame) quaternion.
00113              quat;         //!< Temporary quaternion.
00114    ColumnVector pc,        //!< Compliant position.
00115                 pcp,       //!< Compliant velocity.
00116                 pcpp,      //!< Compliant acceleration.
00117                 pcp_prev,  //!< Previous value of pcp.
00118                 pcpp_prev, //!< Previous value of pcpp.
00119                 pcd,       //!< Difference between pc and desired position.
00120                 pcdp,      //!< Difference between pcp and desired velocity.
00121                 wc,        //!< Compliant angular velocity.
00122                 wcp,       //!< Compliant angular acceleration.
00123                 wcp_prev,  //!< Previous value of wcp.
00124                 wcd;       //!< Difference between wc and desired angular velocity.
00125 private:
00126    DiagonalMatrix Mp,   //!< Translational impedance inertia matrix.
00127                   Dp,   //!< Translational impedance damping matrix.
00128                   Kp,   //!< Translational impedance stifness matrix.
00129                   Mo,   //!< Rotational impedance inertia matrix.
00130                   Do,   //!< Rotational impedance damping matrix.
00131                   Ko;   //!< Rotational impedance stifness matrix.
00132    Matrix Ko_prime;     //!< Modified rotational impedance stifness matrix.
00133 };
00134 
00135 /*!
00136   @class Resolved_acc
00137   @brief Resolved rate acceleration controller class.
00138 
00139   The dynamic model of a robot manipulator can be expressed in
00140   joint space as
00141   \f[
00142     B(q)\ddot{q} + C(q,\dot{q})\dot{q} + D\dot{q} + g(q) = \tau - J^T(q)f
00143   \f]
00144   According to the concept of inverse dynamics, the driving torques
00145   can be chosen as
00146   \f[
00147     \tau = B(q)J^{-1}(q)\big(a - \dot{J}(q,\dot{q})\dot{q}\big) +
00148     C(q,\dot{q})\dot{q} + D\dot{q} + g(q) - J^T(q)f
00149   \f]
00150   where \f$a\f$ is the a new control input defined by
00151   \f[
00152     a_p = \ddot{p}_d + k_{vp}\dot{\tilde{p}} + k_{pp}\tilde{p}
00153   \f]
00154   \f[
00155     a_o = \dot{\omega}_d + k_{vo}\dot{\tilde{\omega}} + k_{po}\tilde{v}
00156   \f]
00157   where \f$\tilde{x} = x_c - x_d\f$ and \f$ v\f$ is the vector par of the 
00158   quaternion representing the orientation error between the desired and end 
00159   effector frame. \f$k_{vp}\f$, \f$k_{pp}\f$, \f$k_{vo}\f$ and \f$k_{po}\f$ 
00160   are positive gains.
00161   
00162   Up to now this class has been tested only with a 6 dof robot.
00163 */
00164 class Resolved_acc {
00165 public:
00166    Resolved_acc(const short dof = 1);
00167    Resolved_acc(const Robot_basic & robot,
00168                 const Real Kvp, const Real Kpp,
00169                 const Real Kvo, const Real Kpo);
00170    Resolved_acc(const Resolved_acc & x);
00171    Resolved_acc & operator=(const Resolved_acc & x);
00172    void set_Kvp(const Real Kvp);
00173    void set_Kpp(const Real Kpp);
00174    void set_Kvo(const Real Kvo);
00175    void set_Kpo(const Real Kpo);
00176 
00177    ReturnMatrix torque_cmd(Robot_basic & robot, const ColumnVector & pdpp,
00178                            const ColumnVector & pdp, const ColumnVector & pd,
00179                            const ColumnVector & wdp, const ColumnVector & wd,
00180                            const Quaternion & qd, const short link_pc,
00181                            const Real dt);
00182 private:
00183    double Kvp,                //!< Controller gains.
00184           Kpp, 
00185           Kvo, 
00186           Kpo; 
00187    Matrix Rot;                //!< Temporay rotation matrix.
00188    ColumnVector zero3,        //!< \f$3\times 1\f$ zero vector.
00189                 qp,           //!< Robot joints velocity.
00190                 qpp,          //!< Robot joints acceleration.
00191                 a,            //!< Control input.
00192                 p,            //!< End effector position.
00193                 pp,           //!< End effector velocity.
00194                 quat_v_error; //!< Vector part of error quaternion.
00195    Quaternion quat;           //!< Temporary quaternion.
00196 };
00197 
00198 
00199 /*!
00200   @class Computed_torque_method
00201   @brief Computer torque method controller class.
00202 
00203   The dynamic model of a robot manipulator can be expressed in
00204   joint space as
00205   \f[
00206     B(q)\ddot{q} + C(q,\dot{q})\dot{q} + D\dot{q} + g(q) = \tau - J^T(q)f
00207   \f]
00208   The driving torques can be expressed as
00209   \f[
00210     \tau = B(q)\big(\ddot{q}_d + K_d(\dot{q}_d-\dot{q}) 
00211     + K_p(q_d-q)\big) + C(q,\dot{q})\dot{q} + D\dot{q} + g(q) + J^T(q)f
00212   \f]
00213   where \f$K_p\f$, \f$K_d\f$ are diagonal positive definie matrix.
00214 */
00215 class Computed_torque_method {
00216 public:
00217    Computed_torque_method(const short dof = 1);
00218    Computed_torque_method(const Robot_basic & robot,
00219                           const DiagonalMatrix & Kp, const DiagonalMatrix & Kd);
00220    Computed_torque_method(const Computed_torque_method & x);
00221    Computed_torque_method & operator=(const Computed_torque_method & x);
00222    ReturnMatrix torque_cmd(Robot_basic & robot, const ColumnVector & qd,
00223                            const ColumnVector & qpd);
00224    short set_Kd(const DiagonalMatrix & Kd);
00225    short set_Kp(const DiagonalMatrix & Kp);
00226 
00227 private:
00228    int dof;            //!< Degree of freedom.
00229    ColumnVector q,     //!< Robot joints positions.
00230                 qp,    //!< Robot joints velocity.
00231                 qpp,   //!< Robot joints acceleration.
00232                 zero3; //!< \f$3\times 1\f$ zero vector.
00233    DiagonalMatrix Kp,  //!< Position error gain.
00234                   Kd;  //!< Velocity error gain.
00235 };
00236 
00237 
00238 /*!
00239   @class Proportional_Derivative
00240   @brief Proportional derivative controller class
00241 
00242   The driving torques can be expressed as
00243   \f[
00244     \tau = K_p(q_d-q) + K_d(\dot{q}_d-q)
00245   \f]
00246   where \f$K_p\f$, \f$K_d\f$ are diagonal positive definie matrix.
00247 */
00248 class Proportional_Derivative {
00249 public:
00250    Proportional_Derivative(const short dof = 1);
00251    Proportional_Derivative(const Robot_basic & robot, const DiagonalMatrix & Kp, 
00252          const DiagonalMatrix & Kd);
00253    Proportional_Derivative(const Proportional_Derivative & x);
00254    Proportional_Derivative & operator=(const Proportional_Derivative & x);
00255    ReturnMatrix torque_cmd(Robot_basic & robot, const ColumnVector & qd,
00256                            const ColumnVector & qpd);
00257    short set_Kd(const DiagonalMatrix & Kd);
00258    short set_Kp(const DiagonalMatrix & Kp);
00259 
00260 private:
00261    int dof;            //!< Degree of freedom.
00262    ColumnVector q,     //!< Robot joints positions.
00263                 qp,    //!< Robot joints velocity.
00264                 qpp,   //!< Robot joints acceleration.
00265                 tau,   //!< Output torque.
00266                 zero3; //!< \f$3\times 1\f$ zero vector.
00267    DiagonalMatrix Kp,  //!< Position error gain.
00268                   Kd;  //!< Velocity error gain.
00269 };
00270 
00271 #ifdef use_namespace
00272 }
00273 #endif
00274 
00275 #endif
00276 
00277  
00278 
00279 
00280 
00281 
00282 
00283 
00284 
00285 
00286 

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