stewart.h
Go to the documentation of this file.
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


kni
Author(s): Martin Günther
autogenerated on Thu Aug 27 2015 13:40:07