Homepage | Demos | Overview | Downloads | Tutorials | Reference | Credits |
controller.hGo 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 Jan 4 15:42:24 2005 by Doxygen 1.4.0 |