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 static const char header_stewart_rcsid[] = "$Id: stewart.h,v 1.2 2006/05/16 16:11:15 gourdeau Exp $";
37 
38 #include "utils.h"
39 
40 #ifdef use_namespace
41 namespace ROBOOP {
42  using namespace NEWMAT;
43 #endif
44 
45 
53 class LinkStewart {
54  friend class Stewart;
55 private:
57  b;
58  Real I1aa,
59  I1nn,
60  I2aa,
61  I2nn,
62  m1,
63  m2,
64  Lenght1,
65  Lenght2;
66 public:
67 
68  ColumnVector UnitV,
69  aPos,
70  Vu,
71  Vc,
72  Vv,
73  da,
74  dda,
75  LOmega,
76  LAlpha,
77  ACM1,
78  M,
79  N,
80  gravity;
81  Real L;
82 
83  LinkStewart (const ColumnVector & InitLink, const Matrix wRp, const ColumnVector q);
84  LinkStewart (const LinkStewart & x);
85  LinkStewart ();
86 
87  ~LinkStewart ();
88  const LinkStewart & operator = (const LinkStewart & x);
89 
90  void set_ap (const ColumnVector NewAp);
91  void set_b (const ColumnVector Newb);
92  void set_I1aa(const Real NewI1aa);
93  void set_I1nn (const Real NewI1nn);
94  void set_I2aa (const Real NewI2aa);
95  void set_I2nn (const Real NewI2nn);
96  void set_m1 (const Real Newm1);
97  void set_m2 (const Real Newm2);
98  void set_Lenght1 (const Real NewLenght1);
99  void set_Lenght2 (const Real NewLenght2);
100 
101  ReturnMatrix get_ap() const;
102  ReturnMatrix get_b() const;
103  Real get_I1aa () const;
104  Real get_I1nn () const;
105  Real get_I2aa () const;
106  Real get_I2nn () const;
107  Real get_m1 () const;
108  Real get_m2 () const;
109  Real get_Lenght1() const;
110  Real get_Lenght2 () const;
111 
112  void LTransform(const Matrix wRp, const ColumnVector q);
113  void d_LTransform(const ColumnVector dq, const ColumnVector Omega, const Real dl,
114  const Real ddl);
115  void dd_LTransform(const ColumnVector ddq, const ColumnVector Omega,
116  const ColumnVector Alpha, const Real dl, const Real ddl);
117  void tau_LTransform(const Real dl, const Real ddl, const Real Gravity);
118  ReturnMatrix Find_UnitV ();
119  ReturnMatrix Find_a (const Matrix _wRp, const ColumnVector _q);
120  ReturnMatrix Find_da (const ColumnVector dq,const ColumnVector Omega);
121  ReturnMatrix Find_dda (const ColumnVector ddq, const ColumnVector Omega,
122  const ColumnVector Alpha);
123  Real Find_Lenght ();
124 
125  ReturnMatrix Find_VctU ();
126  ReturnMatrix Find_VctV ();
127  ReturnMatrix Find_VctC ();
128  ReturnMatrix Find_AngularKin (const Real dl, const Real ddl);
129  ReturnMatrix NormalForce();
130  ReturnMatrix AxialForce (const Matrix J1, const ColumnVector C, const int Index);
131  ReturnMatrix Find_N(const Real Gravity = GRAVITY);
132  ReturnMatrix Moment();
133  Real ActuationForce (const Matrix J1, const ColumnVector C, const int Index,
134  const Real Gravity = GRAVITY);
135  ReturnMatrix Find_ACM1 (const Real dl, const Real ddl);
136 };
137 
143 class Stewart {
144 
145 private:
148  dq,
149  ddq,
150  pR,
151  gravity;
153  Real mp,
154  p,
155  n,
156  Js,
157  Jm,
158  bs,
159  bm,
160  Kb,
161  L,
162  R,
163  Kt;
164  LinkStewart Links[6];
165 
166  public:
168  Jacobian,
169  IJ1,
170  IJ2;
172  ddl,
173  Alpha,
174  Omega;
175 
176  Stewart ();
177  Stewart (const Matrix InitPlat, bool Joint = true);
178  Stewart (const Stewart & x);
179  Stewart (const std::string & filename, const std::string & PlatformName);
180  ~Stewart ();
181  const Stewart & operator = (const Stewart& x);
182 
183  void set_Joint(const bool _Joint);
184  void set_q (const ColumnVector _q);
185  void set_dq (const ColumnVector _dq);
186  void set_ddq (const ColumnVector _ddq);
187  void set_pR (const ColumnVector _pR);
188  void set_pIp (const Matrix _pIp);
189  void set_mp (const Real _mp);
190  bool get_Joint () const;
191  ReturnMatrix get_q() const;
192  ReturnMatrix get_dq() const;
193  ReturnMatrix get_ddq() const;
194  ReturnMatrix get_pR() const;
195  ReturnMatrix get_pIp() const;
196  Real get_mp () const;
197 
198  void Transform();
199  ReturnMatrix Find_wRp ();
200  ReturnMatrix Find_Omega ();
201  ReturnMatrix Find_Alpha ();
202  ReturnMatrix jacobian ();
203  ReturnMatrix Find_InvJacob1 ();
204  ReturnMatrix Find_InvJacob2 ();
205  ReturnMatrix jacobian_dot();
206  ReturnMatrix Find_dl ();
207  ReturnMatrix Find_ddl ();
208  ReturnMatrix Find_C(const Real Gravity = GRAVITY);
209  ReturnMatrix Torque(const Real Gravity = GRAVITY);
210  ReturnMatrix JointSpaceForceVct(const Real Gravity = GRAVITY);
211  ReturnMatrix InvPosKine();
212  ReturnMatrix ForwardKine(const ColumnVector guess_q, const ColumnVector l_given,
213  const Real tolerance = 0.001);
214  ReturnMatrix Find_h(const Real Gravity = GRAVITY);
215  ReturnMatrix Find_M();
216  ReturnMatrix ForwardDyn(const ColumnVector Torque, const Real Gravity=GRAVITY);
217  void Find_Mc_Nc_Gc(Matrix & Mc, Matrix & Nc, Matrix & Gc);
218  ReturnMatrix ForwardDyn_AD(const ColumnVector Command, const Real t);
219 };
220 
221 #ifdef use_namespace
222 }
223 #endif
224 
225 #endif //Class Stewart
static const char header_stewart_rcsid[]
RCS/CVS version.
Definition: stewart.h:36
ColumnVector b
Base coordinates of the link int the global frame.
Definition: stewart.h:56
Real L
Lenght of the link.
Definition: stewart.h:81
#define GRAVITY
Definition: utils.h:86
LinkStewart definitions.
Definition: stewart.h:53
Real R
Motor armature resistance.
Definition: stewart.h:153
double Real
Definition: include.h:307
ColumnVector Alpha
Angular speed of the platform.
Definition: stewart.h:171
Utility header file.
ColumnVector Omega
Angular acceleration of the platform.
Definition: stewart.h:171
ColumnVector q
Platform position (xyz + euler angles)
Definition: stewart.h:147
The usual rectangular matrix.
Definition: newmat.h:625
bool UJointAtBase
Gives the position of the universal joint (true if at base, false if at platform) ...
Definition: stewart.h:146
Matrix wRp
Rotation matrix describing the orientation of the platform.
Definition: stewart.h:167
Matrix pIp
Platform Inertia (local ref.)
Definition: stewart.h:152
const Stewart & operator=(const Stewart &x)
Definition: stewart.cpp:1004
ColumnVector ddl
Acceleration of expension vector.
Definition: stewart.h:171
Stewart definitions.
Definition: stewart.h:143
ColumnVector gravity
Gravity vector.
Definition: stewart.h:147
ColumnVector dq
Platform speed.
Definition: stewart.h:147
Column vector.
Definition: newmat.h:1008
Real m2
Mass of part 2.
Definition: stewart.h:58
ColumnVector Vv
Unit Vector of the universal joint (Rotational).
Definition: stewart.h:68
ColumnVector ddq
Platform acceleration.
Definition: stewart.h:147
ColumnVector dl
Rate of expension vector.
Definition: stewart.h:171
ReturnMatrix Omega(const Quaternion &q, const Quaternion &q_dot)
Return angular velocity from a quaternion and it's time derivative.
Definition: quaternion.cpp:560


kni
Author(s): Martin Günther
autogenerated on Fri Jan 3 2020 04:01:17