00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
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