mbsim 4.0.0 MBSim Kernel

rigid_body.h
1 /* Copyright (C) 2004-2014 MBSim Development Team
2 *
3 * This library is free software; you can redistribute it and/or
4 * modify it under the terms of the GNU Lesser General Public
5 * License as published by the Free Software Foundation; either
6 * version 2.1 of the License, or (at your option) any later version.
7 *
8 * This library is distributed in the hope that it will be useful,
9 * but WITHOUT ANY WARRANTY; without even the implied warranty of
10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
11 * Lesser General Public License for more details.
12 *
13 * You should have received a copy of the GNU Lesser General Public
14 * License along with this library; if not, write to the Free Software
15 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
16 *
17 * Contact: martin.o.foerg@googlemail.com
18 */
19
20 #ifndef _RIGID_BODY_H_
21 #define _RIGID_BODY_H_
22
23 #include "mbsim/objects/body.h"
24 #include "mbsim/frames/frame.h"
25 #include "mbsim/functions/time_dependent_function.h"
26 #include "mbsim/functions/state_dependent_function.h"
27
28 namespace MBSim {
29
30 class Frame;
31 class RigidContour;
32 class FixedRelativeFrame;
33 class Constraint;
34 class InverseKineticsJoint;
35
50 class RigidBody : public Body {
51 public:
52 enum GeneralizedVelocityOfRotation {
53 derivativeOfGeneralizedPositionOfRotation=0,
54 coordinatesOfAngularVelocityWrtFrameOfReference,
55 coordinatesOfAngularVelocityWrtFrameForKinematics,
56 unknown
57 };
58
63 RigidBody(const std::string &name="");
64
68 ~RigidBody() override;
69
70 void addDependency(Constraint* constraint_);
71
72 void updateqd() override;
73 void updateT() override;
74 void updateh(int j=0) override;
75 void updateM() override;
76 void updateInertiaTensor();
77 void updateGeneralizedPositions() override;
78 void updateGeneralizedVelocities() override;
79 void updateDerivativeOfGeneralizedPositions() override;
80 void updateGeneralizedAccelerations() override;
81 void updateGeneralizedJacobians(int j=0);
82 void updatePositions();
83 void updateVelocities();
84 void updateJacobians() override;
85 void updateGyroscopicAccelerations();
86 void updatePositions(Frame *frame) override;
87 void updateVelocities(Frame *frame) override;
88 void updateAccelerations(Frame *frame) override;
89 void updateJacobians(Frame *frame, int j=0) override { (this->*updateJacobians_[j])(frame); }
90 void updateGyroscopicAccelerations(Frame *frame) override;
91 void updateJacobians0(Frame *frame);
92 void updateJacobians1(Frame *frame) { }
93 void updateJacobians2(Frame *frame);
94
95 void calcSize() override;
96 void calcqSize() override;
97 void calcuSize(int j=0) override;
98 void sethSize(int hSize_, int i=0) override;
99 void sethInd(int hInd_, int i=0) override;
100
101 /* INHERITED INTERFACE OF OBJECT */
102 void init(InitStage stage, const InitConfigSet &config) override;
103 void setUpInverseKinetics() override;
104 /*****************************************************/
105
106 /* INHERITED INTERFACE OF ELEMENT */
107 void plot() override;
108 /*****************************************************/
109
110 /* GETTER / SETTER */
111
112 // NOTE: we can not use a overloaded setTranslation here due to restrictions in XML but define them for convinience in c++
117 void setGeneralTranslation(Function<fmatvec::Vec3(fmatvec::VecV, double)> *fPrPK_) {
118 fPrPK = fPrPK_;
119 fPrPK->setParent(this);
120 fPrPK->setName("GeneralTranslation");
121 }
126 void setTimeDependentTranslation(Function<fmatvec::Vec3(double)> *fPrPK_) {
127 setGeneralTranslation(new TimeDependentFunction<fmatvec::Vec3>(fPrPK_));
128 }
133 void setStateDependentTranslation(Function<fmatvec::Vec3(fmatvec::VecV)> *fPrPK_) {
134 setGeneralTranslation(new StateDependentFunction<fmatvec::Vec3>(fPrPK_));
135 }
136 void setTranslation(Function<fmatvec::Vec3(fmatvec::VecV, double)> *fPrPK_) { setGeneralTranslation(fPrPK_); }
137 void setTranslation(Function<fmatvec::Vec3(double)> *fPrPK_) { setTimeDependentTranslation(fPrPK_); }
138 void setTranslation(Function<fmatvec::Vec3(fmatvec::VecV)> *fPrPK_) { setStateDependentTranslation(fPrPK_); }
139
140 // NOTE: we can not use a overloaded setRotation here due to restrictions in XML but define them for convinience in c++
145 void setGeneralRotation(Function<fmatvec::RotMat3(fmatvec::VecV, double)>* fAPK_) {
146 fAPK = fAPK_;
147 fAPK->setParent(this);
148 fAPK->setName("GeneralRotation");
149 }
154 void setTimeDependentRotation(Function<fmatvec::RotMat3(double)>* fAPK_) { setGeneralRotation(new TimeDependentFunction<fmatvec::RotMat3>(fAPK_)); }
159 void setStateDependentRotation(Function<fmatvec::RotMat3(fmatvec::VecV)>* fAPK_) { setGeneralRotation(new StateDependentFunction<fmatvec::RotMat3>(fAPK_)); }
160 void setRotation(Function<fmatvec::RotMat3(fmatvec::VecV, double)>* fAPK_) { setGeneralRotation(fAPK_); }
161 void setRotation(Function<fmatvec::RotMat3(double)>* fAPK_) { setTimeDependentRotation(fAPK_); }
162 void setRotation(Function<fmatvec::RotMat3(fmatvec::VecV)>* fAPK_) { setStateDependentRotation(fAPK_); }
163
164 void setTranslationDependentRotation(bool dep) { translationDependentRotation = dep; }
165 void setGeneralizedVelocityOfRotation(GeneralizedVelocityOfRotation generalizedVelocityOfRotation_) { generalizedVelocityOfRotation = generalizedVelocityOfRotation_; }
166
171 Function<fmatvec::Vec3(fmatvec::VecV, double)>* getTranslation() { return fPrPK; }
176 Function<fmatvec::RotMat3(fmatvec::VecV, double)>* getRotation() { return fAPK; }
177
178 void setMass(double m_) { m = m_; }
179 double getMass() const { return m; }
180 Frame* getFrameForKinematics() { return &Z; };
181 FixedRelativeFrame* getFrameC() { return C; };
182
183 const fmatvec::Vec3& evalGlobalRelativePosition() { if(updPos) updatePositions(); return WrPK; }
184 const fmatvec::Vec3& evalGlobalRelativeVelocity() { if(updVel) updateVelocities(); return WvPKrel; }
185 const fmatvec::Vec3& evalGlobalRelativeAngularVelocity() { if(updVel) updateVelocities(); return WomPK; }
186 const fmatvec::SymMat3& evalGlobalInertiaTensor() { if(updWTS) updateInertiaTensor(); return WThetaS; }
187 const fmatvec::Mat3xV& evalPJTT() { if(updPJ) updateJacobians(); return PJTT; }
188 const fmatvec::Mat3xV& evalPJRR() { if(updPJ) updateJacobians(); return PJRR; }
189 const fmatvec::Vec3& evalPjhT() { if(updPJ) updateJacobians(); return PjhT; }
190 const fmatvec::Vec3& evalPjhR() { if(updPJ) updateJacobians(); return PjhR; }
191 const fmatvec::Vec3& evalPjbT() { if(updPjb) updateGyroscopicAccelerations(); return PjbT; }
192 const fmatvec::Vec3& evalPjbR() { if(updPjb) updateGyroscopicAccelerations(); return PjbR; }
193
194 fmatvec::SymMat3& getGlobalInertiaTensor(bool check=true) { assert((not check) or (not updWTS)); return WThetaS; }
195 fmatvec::Vec3& getGlobalRelativeVelocity(bool check=true) { assert((not check) or (not updVel)); return WvPKrel; }
196 fmatvec::SqrMat3& getRelativeOrientation(bool check=true) { assert((not check) or (not updPos)); return APK; }
197 fmatvec::Vec3& getPjbT(bool check=true) { assert((not check) or (not updPjb)); return PjbT; }
198 fmatvec::Vec3& getPjbR(bool check=true) { assert((not check) or (not updPjb)); return PjbR; }
199
204 void setInertiaTensor(const fmatvec::SymMat3& RThetaR) { SThetaS = RThetaR; }
205
206 const fmatvec::SymMat3& getInertiaTensor() const {return SThetaS;}
207 fmatvec::SymMat3& getInertiaTensor() {return SThetaS;}
208
212 void setFrameForKinematics(Frame *frame);
213
214 void setFrameForInertiaTensor(Frame *frame);
215
216 void setOpenMBVRigidBody(const std::shared_ptr<OpenMBV::RigidBody> &body);
217 void setOpenMBVFrameOfReference(Frame * frame) {openMBVFrame=frame; }
218 const Frame* getOpenMBVFrameOfReference() const {return openMBVFrame; }
219
220 void initializeUsingXML(xercesc::DOMElement *element) override;
221
222 fmatvec::MatV& getJRel(int i=0, bool check=true) { assert((not check) or (not updGJ)); return JRel[i]; }
223 fmatvec::VecV& getjRel(bool check=true) { assert((not check) or (not updGJ)); return jRel; }
224 void setqRel(const fmatvec::VecV &q);
225 void setuRel(const fmatvec::VecV &u);
226 void setJRel(const fmatvec::MatV &J);
227 void setjRel(const fmatvec::VecV &j);
228
229 bool transformCoordinates() const { return fTR!=nullptr; }
230
231 void resetUpToDate() override;
232 void resetPositionsUpToDate() override;
233 void resetVelocitiesUpToDate() override;
234 void resetJacobiansUpToDate() override;
235 void resetGyroscopicAccelerationsUpToDate() override;
236 const fmatvec::VecV& evalqTRel() { if(updq) updateGeneralizedPositions(); return qTRel; }
237 const fmatvec::VecV& evalqRRel() { if(updq) updateGeneralizedPositions(); return qRRel; }
238 const fmatvec::VecV& evaluTRel() { if(updu) updateGeneralizedVelocities(); return uTRel; }
239 const fmatvec::VecV& evaluRRel() { if(updu) updateGeneralizedVelocities(); return uRRel; }
240 const fmatvec::VecV& evalqdTRel() { if(updqd) updateDerivativeOfGeneralizedPositions(); return qdTRel; }
241 const fmatvec::VecV& evalqdRRel() { if(updqd) updateDerivativeOfGeneralizedPositions(); return qdRRel; }
242 const fmatvec::MatV& evalJRel(int j=0) { if(updGJ) updateGeneralizedJacobians(); return JRel[j]; }
243 const fmatvec::VecV& evaljRel() { if(updGJ) updateGeneralizedJacobians(); return jRel; }
244
245 fmatvec::VecV& getqTRel(bool check=true) { assert((not check) or (not updq)); return qTRel; }
246 fmatvec::VecV& getqRRel(bool check=true) { assert((not check) or (not updq)); return qRRel; }
247 fmatvec::VecV& getuTRel(bool check=true) { assert((not check) or (not updu)); return uTRel; }
248 fmatvec::VecV& getuRRel(bool check=true) { assert((not check) or (not updu)); return uRRel; }
249 fmatvec::VecV& getqdTRel(bool check=true) { assert((not check) or (not updqd)); return qdTRel; }
250 fmatvec::VecV& getqdRRel(bool check=true) { assert((not check) or (not updqd)); return qdRRel; }
251
252 void setUpdateByReference(bool updateByReference_) { updateByReference = updateByReference_; }
253
254 InverseKineticsJoint* getJoint() { return joint; }
255
256 void setDynamicSystemSolver(DynamicSystemSolver* sys) override;
257
258 protected:
262 double m{0};
263
267 fmatvec::SymMat3 SThetaS, WThetaS;
268
269 FixedRelativeFrame *K;
270
271 fmatvec::Vec3 PjhT, PjhR, PjbT, PjbR;
272
276 fmatvec::SqrMat3 APK;
277
281 fmatvec::Vec3 PrPK, WrPK;
282
286 fmatvec::Vec3 WvPKrel, WomPK;
287
288 Function<fmatvec::MatV(fmatvec::VecV)> *fTR{nullptr};
289
293 Function<fmatvec::Vec3(fmatvec::VecV, double)> *fPrPK{nullptr};
294
298 Function<fmatvec::RotMat3(fmatvec::VecV, double)> *fAPK{nullptr};
299
300 #ifndef SWIG
301 void (RigidBody::*updateJacobians_[3])(Frame *frame);
302 #endif
303
305 FixedRelativeFrame *C;
306
307 fmatvec::MatV JRel[2];
308 fmatvec::VecV jRel;
309
310 fmatvec::VecV qTRel, qRRel, uTRel, uRRel, qdTRel, qdRRel;
311 fmatvec::Mat3xV PJTT, PJRR;
312
313 Constraint *constraint{nullptr};
314
315 Frame *frameForJacobianOfRotation{nullptr};
316
317 Frame *frameForInertiaTensor{nullptr};
318
319 fmatvec::Range<fmatvec::Var,fmatvec::Var> iqT, iqR, iuT, iuR;
320
321 bool translationDependentRotation{false};
322 bool constJT{false};
323 bool constJR{false};
324 bool constjT{false};
325 bool constjR{false};
326
327 bool updPjb{true};
328 bool updGJ{true};
329 bool updWTS{true};
330 bool updateByReference{true};
331
332 Frame Z;
333
334 GeneralizedVelocityOfRotation generalizedVelocityOfRotation{derivativeOfGeneralizedPositionOfRotation};
335
336 InverseKineticsJoint *joint;
337
338 private:
342 Frame * openMBVFrame;
343 };
344
345 }
346
347 #endif
MBSim::RigidBody::setInertiaTensor
void setInertiaTensor(const fmatvec::SymMat3 &RThetaR)
Definition: rigid_body.h:204
MBSim::RigidBody::setGeneralTranslation
void setGeneralTranslation(Function< fmatvec::Vec3(fmatvec::VecV, double)> *fPrPK_)
set Kinematic for genral translational motion
Definition: rigid_body.h:117
MBSim::RigidBody::setFrameForKinematics
void setFrameForKinematics(Frame *frame)
Definition: rigid_body.cc:77
MBSim::RigidBody::init
void init(InitStage stage, const InitConfigSet &config) override
plots time series header
Definition: rigid_body.cc:149
MBSim::RigidBody::getRotation
Function< fmatvec::RotMat3(fmatvec::VecV, double)> * getRotation()
get Kinematic for rotational motion
Definition: rigid_body.h:176
MBSim::RigidBody::setStateDependentRotation
void setStateDependentRotation(Function< fmatvec::RotMat3(fmatvec::VecV)> *fAPK_)
set Kinematic for only state dependent rotational motion
Definition: rigid_body.h:159
MBSim::RigidBody::m
double m
mass
Definition: rigid_body.h:262
MBSim::FixedRelativeFrame
cartesian frame on rigid bodies
Definition: fixed_relative_frame.h:31
MBSim::Object::q
fmatvec::Vec q
positions, velocities
Definition: object.h:304
MBSim::RigidBody::getTranslation
Function< fmatvec::Vec3(fmatvec::VecV, double)> * getTranslation()
get Kinematic for translational motion
Definition: rigid_body.h:171
MBSim::RigidBody::setStateDependentTranslation
void setStateDependentTranslation(Function< fmatvec::Vec3(fmatvec::VecV)> *fPrPK_)
set Kinematic for only state dependent translational motion
Definition: rigid_body.h:133
MBSim::TimeDependentFunction
Definition: time_dependent_function.h:28
MBSim::RigidBody::openMBVFrame
Frame * openMBVFrame
Frame of reference for drawing openMBVBody.
Definition: rigid_body.h:342
MBSim::RigidBody::setUpInverseKinetics
void setUpInverseKinetics() override
TODO.
Definition: rigid_body.cc:287
MBSim::Element::InitStage
InitStage
The stages of the initialization.
Definition: element.h:62
MBSim::RigidBody::C
FixedRelativeFrame * C
Definition: rigid_body.h:305
MBSim::RigidBody::SThetaS
fmatvec::SymMat3 SThetaS
inertia tensor with respect to centre of gravity in centre of gravity and world Frame
Definition: rigid_body.h:267
MBSim::RigidBody::setGeneralRotation
void setGeneralRotation(Function< fmatvec::RotMat3(fmatvec::VecV, double)> *fAPK_)
set Kinematic for general rotational motion
Definition: rigid_body.h:145
MBSim::Function
Definition: contact_kinematics.h:32
MBSim::RigidBody::APK
fmatvec::SqrMat3 APK
rotation matrix from kinematic Frame to parent Frame
Definition: rigid_body.h:276
MBSim::Element::name
std::string name
name of element
Definition: element.h:260
MBSim::Body::frame
std::vector< Frame * > frame
vector of frames and contours
Definition: body.h:169
MBSim::Constraint
Class for constraints between generalized coordinates of objects.
Definition: constraint.h:30
MBSim::Frame
cartesian frame on bodies used for application of e.g. links and loads
Definition: frame.h:39
MBSim::RigidBody::fAPK
Function< fmatvec::RotMat3(fmatvec::VecV, double)> * fAPK
rotation from kinematic Frame to parent Frame
Definition: rigid_body.h:298
MBSim::RigidBody::plot
void plot() override
plots time dependent data
Definition: rigid_body.cc:300
MBSim::RigidBody::~RigidBody
~RigidBody() override
destructor
Definition: rigid_body.cc:71
MBSim::RigidBody::setDynamicSystemSolver
void setDynamicSystemSolver(DynamicSystemSolver *sys) override
sets the used dynamics system solver to the element
Definition: rigid_body.cc:645
MBSim::StateDependentFunction
Definition: state_dependent_function.h:28
MBSim::RigidBody::RigidBody
RigidBody(const std::string &name="")
constructor
Definition: rigid_body.cc:56
MBSim::Element::setName
void setName(const std::string &str)
Definition: element.h:115
MBSim::RigidBody::fPrPK
Function< fmatvec::Vec3(fmatvec::VecV, double)> * fPrPK
translation from parent Frame to kinematic Frame in parent system
Definition: rigid_body.h:293
MBSim::RigidBody::WvPKrel
fmatvec::Vec3 WvPKrel
translational and angular velocity from parent to kinematic Frame in world system
Definition: rigid_body.h:286
MBSim::RigidBody::setTimeDependentRotation
void setTimeDependentRotation(Function< fmatvec::RotMat3(double)> *fAPK_)
set Kinematic for only time dependent rotational motion
Definition: rigid_body.h:154
MBSim::RigidBody::PrPK
fmatvec::Vec3 PrPK
translation from parent to kinematic Frame in parent and world system
Definition: rigid_body.h:281
MBSim::RigidBody::setTimeDependentTranslation
void setTimeDependentTranslation(Function< fmatvec::Vec3(double)> *fPrPK_)
set Kinematic for only time dependent translational motion
Definition: rigid_body.h:126
MBSim::Body
base class for all mechanical bodies with mass and generalised coordinates
Definition: body.h:49
MBSim::RigidBody
rigid bodies with arbitrary kinematics
Definition: rigid_body.h:50
MBSim
namespace MBSim
Definition: bilateral_constraint.cc:30