$search
00001 /* 00002 Copyright (C) 2004 Samuel Bélanger 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: samuel.belanger@polymtl.ca or richard.gourdeau@polymtl.ca 00022 */ 00023 00024 #ifndef __cplusplus 00025 #error Must use C++ 00026 #endif 00027 #ifndef STEWART_H 00028 #define STEWART_H 00029 00035 00036 static const char header_stewart_rcsid[] = "$Id: stewart.h,v 1.2 2006/05/16 16:11:15 gourdeau Exp $"; 00037 00038 #include "utils.h" 00039 00040 #ifdef use_namespace 00041 namespace ROBOOP { 00042 using namespace NEWMAT; 00043 #endif 00044 00045 00053 class LinkStewart { 00054 friend class Stewart; 00055 private: 00056 ColumnVector ap, 00057 b; 00058 Real I1aa, 00059 I1nn, 00060 I2aa, 00061 I2nn, 00062 m1, 00063 m2, 00064 Lenght1, 00065 Lenght2; 00066 public: 00067 00068 ColumnVector UnitV, 00069 aPos, 00070 Vu, 00071 Vc, 00072 Vv, 00073 da, 00074 dda, 00075 LOmega, 00076 LAlpha, 00077 ACM1, 00078 M, 00079 N, 00080 gravity; 00081 Real L; 00082 00083 LinkStewart (const ColumnVector & InitLink, const Matrix wRp, const ColumnVector q); 00084 LinkStewart (const LinkStewart & x); 00085 LinkStewart (); 00086 00087 ~LinkStewart (); 00088 const LinkStewart & operator = (const LinkStewart & x); 00089 00090 void set_ap (const ColumnVector NewAp); 00091 void set_b (const ColumnVector Newb); 00092 void set_I1aa(const Real NewI1aa); 00093 void set_I1nn (const Real NewI1nn); 00094 void set_I2aa (const Real NewI2aa); 00095 void set_I2nn (const Real NewI2nn); 00096 void set_m1 (const Real Newm1); 00097 void set_m2 (const Real Newm2); 00098 void set_Lenght1 (const Real NewLenght1); 00099 void set_Lenght2 (const Real NewLenght2); 00100 00101 ReturnMatrix get_ap() const; 00102 ReturnMatrix get_b() const; 00103 Real get_I1aa () const; 00104 Real get_I1nn () const; 00105 Real get_I2aa () const; 00106 Real get_I2nn () const; 00107 Real get_m1 () const; 00108 Real get_m2 () const; 00109 Real get_Lenght1() const; 00110 Real get_Lenght2 () const; 00111 00112 void LTransform(const Matrix wRp, const ColumnVector q); 00113 void d_LTransform(const ColumnVector dq, const ColumnVector Omega, const Real dl, 00114 const Real ddl); 00115 void dd_LTransform(const ColumnVector ddq, const ColumnVector Omega, 00116 const ColumnVector Alpha, const Real dl, const Real ddl); 00117 void tau_LTransform(const Real dl, const Real ddl, const Real Gravity); 00118 ReturnMatrix Find_UnitV (); 00119 ReturnMatrix Find_a (const Matrix _wRp, const ColumnVector _q); 00120 ReturnMatrix Find_da (const ColumnVector dq,const ColumnVector Omega); 00121 ReturnMatrix Find_dda (const ColumnVector ddq, const ColumnVector Omega, 00122 const ColumnVector Alpha); 00123 Real Find_Lenght (); 00124 00125 ReturnMatrix Find_VctU (); 00126 ReturnMatrix Find_VctV (); 00127 ReturnMatrix Find_VctC (); 00128 ReturnMatrix Find_AngularKin (const Real dl, const Real ddl); 00129 ReturnMatrix NormalForce(); 00130 ReturnMatrix AxialForce (const Matrix J1, const ColumnVector C, const int Index); 00131 ReturnMatrix Find_N(const Real Gravity = GRAVITY); 00132 ReturnMatrix Moment(); 00133 Real ActuationForce (const Matrix J1, const ColumnVector C, const int Index, 00134 const Real Gravity = GRAVITY); 00135 ReturnMatrix Find_ACM1 (const Real dl, const Real ddl); 00136 }; 00137 00143 class Stewart { 00144 00145 private: 00146 bool UJointAtBase; 00147 ColumnVector q, 00148 dq, 00149 ddq, 00150 pR, 00151 gravity; 00152 Matrix pIp; 00153 Real mp, 00154 p, 00155 n, 00156 Js, 00157 Jm, 00158 bs, 00159 bm, 00160 Kb, 00161 L, 00162 R, 00163 Kt; 00164 LinkStewart Links[6]; 00165 00166 public: 00167 Matrix wRp, 00168 Jacobian, 00169 IJ1, 00170 IJ2; 00171 ColumnVector dl, 00172 ddl, 00173 Alpha, 00174 Omega; 00175 00176 Stewart (); 00177 Stewart (const Matrix InitPlat, bool Joint = true); 00178 Stewart (const Stewart & x); 00179 Stewart (const std::string & filename, const std::string & PlatformName); 00180 ~Stewart (); 00181 const Stewart & operator = (const Stewart& x); 00182 00183 void set_Joint(const bool _Joint); 00184 void set_q (const ColumnVector _q); 00185 void set_dq (const ColumnVector _dq); 00186 void set_ddq (const ColumnVector _ddq); 00187 void set_pR (const ColumnVector _pR); 00188 void set_pIp (const Matrix _pIp); 00189 void set_mp (const Real _mp); 00190 bool get_Joint () const; 00191 ReturnMatrix get_q() const; 00192 ReturnMatrix get_dq() const; 00193 ReturnMatrix get_ddq() const; 00194 ReturnMatrix get_pR() const; 00195 ReturnMatrix get_pIp() const; 00196 Real get_mp () const; 00197 00198 void Transform(); 00199 ReturnMatrix Find_wRp (); 00200 ReturnMatrix Find_Omega (); 00201 ReturnMatrix Find_Alpha (); 00202 ReturnMatrix jacobian (); 00203 ReturnMatrix Find_InvJacob1 (); 00204 ReturnMatrix Find_InvJacob2 (); 00205 ReturnMatrix jacobian_dot(); 00206 ReturnMatrix Find_dl (); 00207 ReturnMatrix Find_ddl (); 00208 ReturnMatrix Find_C(const Real Gravity = GRAVITY); 00209 ReturnMatrix Torque(const Real Gravity = GRAVITY); 00210 ReturnMatrix JointSpaceForceVct(const Real Gravity = GRAVITY); 00211 ReturnMatrix InvPosKine(); 00212 ReturnMatrix ForwardKine(const ColumnVector guess_q, const ColumnVector l_given, 00213 const Real tolerance = 0.001); 00214 ReturnMatrix Find_h(const Real Gravity = GRAVITY); 00215 ReturnMatrix Find_M(); 00216 ReturnMatrix ForwardDyn(const ColumnVector Torque, const Real Gravity=GRAVITY); 00217 void Find_Mc_Nc_Gc(Matrix & Mc, Matrix & Nc, Matrix & Gc); 00218 ReturnMatrix ForwardDyn_AD(const ColumnVector Command, const Real t); 00219 }; 00220 00221 #ifdef use_namespace 00222 } 00223 #endif 00224 00225 #endif //Class Stewart