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 #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 |