ROBOOP, A Robotics Object Oriented Package in C++
stewart.h
Go to the documentation of this file.
1 /*
2 Copyright (C) 2004 Samuel Bélanger
3 
4 This library is free software; you can redistribute it and/or modify
5 it under the terms of the GNU Lesser General Public License as
6 published by the Free Software Foundation; either version 2.1 of the
7 License, or (at your option) any later version.
8 
9 This library is distributed in the hope that it will be useful,
10 but WITHOUT ANY WARRANTY; without even the implied warranty of
11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 GNU Lesser General Public License for more details.
13 
14 You should have received a copy of the GNU Lesser General Public
15 License along with this library; if not, write to the Free Software
16 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
17 
18 
19 Report problems and direct all questions to:
20 
21 email: samuel.belanger@polymtl.ca or richard.gourdeau@polymtl.ca
22 */
23 
24 #ifndef __cplusplus
25 #error Must use C++
26 #endif
27 #ifndef STEWART_H
28 #define STEWART_H
29 
35 #include "utils.h"
36 
37 #ifdef use_namespace
38 namespace ROBOOP {
39  using namespace NEWMAT;
40 #endif
41 
42 
50 class LinkStewart {
51  friend class Stewart;
52 private:
53  ColumnVector ap,
54  b;
55  Real I1aa,
56  I1nn,
57  I2aa,
58  I2nn,
59  m1,
60  m2,
61  Lenght1,
62  Lenght2;
63 public:
64 
65  ColumnVector UnitV,
66  aPos,
67  Vu,
68  Vc,
69  Vv,
70  da,
71  dda,
72  LOmega,
73  LAlpha,
74  ACM1,
75  M,
76  N,
77  gravity;
78  Real L;
79 
80  LinkStewart (const ColumnVector & InitLink, const Matrix wRp, const ColumnVector q);
81  LinkStewart (const LinkStewart & x);
82  LinkStewart ();
83 
84  ~LinkStewart ();
85  const LinkStewart & operator = (const LinkStewart & x);
86 
87  void set_ap (const ColumnVector NewAp);
88  void set_b (const ColumnVector Newb);
89  void set_I1aa(const Real NewI1aa);
90  void set_I1nn (const Real NewI1nn);
91  void set_I2aa (const Real NewI2aa);
92  void set_I2nn (const Real NewI2nn);
93  void set_m1 (const Real Newm1);
94  void set_m2 (const Real Newm2);
95  void set_Lenght1 (const Real NewLenght1);
96  void set_Lenght2 (const Real NewLenght2);
97 
98  ReturnMatrix get_ap() const;
99  ReturnMatrix get_b() const;
100  Real get_I1aa () const;
101  Real get_I1nn () const;
102  Real get_I2aa () const;
103  Real get_I2nn () const;
104  Real get_m1 () const;
105  Real get_m2 () const;
106  Real get_Lenght1() const;
107  Real get_Lenght2 () const;
108 
109  void LTransform(const Matrix wRp, const ColumnVector q);
110  void d_LTransform(const ColumnVector dq, const ColumnVector Omega, const Real dl,
111  const Real ddl);
112  void dd_LTransform(const ColumnVector ddq, const ColumnVector Omega,
113  const ColumnVector Alpha, const Real dl, const Real ddl);
114  void tau_LTransform(const Real dl, const Real ddl, const Real Gravity);
115  ReturnMatrix Find_UnitV ();
116  ReturnMatrix Find_a (const Matrix _wRp, const ColumnVector _q);
117  ReturnMatrix Find_da (const ColumnVector dq,const ColumnVector Omega);
118  ReturnMatrix Find_dda (const ColumnVector ddq, const ColumnVector Omega,
119  const ColumnVector Alpha);
120  Real Find_Lenght ();
121 
122  ReturnMatrix Find_VctU ();
123  ReturnMatrix Find_VctV ();
124  ReturnMatrix Find_VctC ();
125  ReturnMatrix Find_AngularKin (const Real dl, const Real ddl);
126  ReturnMatrix NormalForce();
127  ReturnMatrix AxialForce (const Matrix J1, const ColumnVector C, const int Index);
128  ReturnMatrix Find_N(const Real Gravity = GRAVITY);
129  ReturnMatrix Moment();
130  Real ActuationForce (const Matrix J1, const ColumnVector C, const int Index,
131  const Real Gravity = GRAVITY);
132  ReturnMatrix Find_ACM1 (const Real dl, const Real ddl);
133 };
134 
140 class Stewart {
141 
142 private:
143  bool UJointAtBase;
144  ColumnVector q,
145  dq,
146  ddq,
147  pR,
148  gravity;
149  Matrix pIp;
150  Real mp,
151  p,
152  n,
153  Js,
154  Jm,
155  bs,
156  bm,
157  Kb,
158  L,
159  R,
160  Kt;
161  LinkStewart Links[6];
162 
163  public:
164  Matrix wRp,
165  Jacobian,
166  IJ1,
167  IJ2;
168  ColumnVector dl,
169  ddl,
170  Alpha,
171  Omega;
172 
173  Stewart ();
174  Stewart (const Matrix InitPlat, bool Joint = true);
175  Stewart (const Stewart & x);
176  Stewart (const std::string & filename, const std::string & PlatformName);
177  ~Stewart ();
178  const Stewart & operator = (const Stewart& x);
179 
180  void set_Joint(const bool _Joint);
181  void set_q (const ColumnVector _q);
182  void set_dq (const ColumnVector _dq);
183  void set_ddq (const ColumnVector _ddq);
184  void set_pR (const ColumnVector _pR);
185  void set_pIp (const Matrix _pIp);
186  void set_mp (const Real _mp);
187  bool get_Joint () const;
188  ReturnMatrix get_q() const;
189  ReturnMatrix get_dq() const;
190  ReturnMatrix get_ddq() const;
191  ReturnMatrix get_pR() const;
192  ReturnMatrix get_pIp() const;
193  Real get_mp () const;
194 
195  void Transform();
196  ReturnMatrix Find_wRp ();
197  ReturnMatrix Find_Omega ();
198  ReturnMatrix Find_Alpha ();
199  ReturnMatrix jacobian ();
200  ReturnMatrix Find_InvJacob1 ();
201  ReturnMatrix Find_InvJacob2 ();
202  ReturnMatrix jacobian_dot();
203  ReturnMatrix Find_dl ();
204  ReturnMatrix Find_ddl ();
205  ReturnMatrix Find_C(const Real Gravity = GRAVITY);
206  ReturnMatrix Torque(const Real Gravity = GRAVITY);
207  ReturnMatrix JointSpaceForceVct(const Real Gravity = GRAVITY);
208  ReturnMatrix InvPosKine();
209  ReturnMatrix ForwardKine(const ColumnVector guess_q, const ColumnVector l_given,
210  const Real tolerance = 0.001);
211  ReturnMatrix Find_h(const Real Gravity = GRAVITY);
212  ReturnMatrix Find_M();
213  ReturnMatrix ForwardDyn(const ColumnVector Torque, const Real Gravity=GRAVITY);
214  void Find_Mc_Nc_Gc(Matrix & Mc, Matrix & Nc, Matrix & Gc);
215  ReturnMatrix ForwardDyn_AD(const ColumnVector Command, const Real t);
216 };
217 
218 #ifdef use_namespace
219 }
220 #endif
221 
222 #endif //Class Stewart