20 #ifndef _RIGID_BODY_H_
21 #define _RIGID_BODY_H_
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"
28 #include "mbsim/utils/boost_parameters.h"
29 #include "mbsim/utils/openmbv_utils.h"
35 class FixedRelativeFrame;
70 void updateh(
int j=0);
71 void updateM() { (this->*
updateM_)(); }
72 void updateInertiaTensor();
73 void updateGeneralizedCoordinates();
74 void updateGeneralizedJacobians(
int j=0);
75 void updatePositions();
76 void updateVelocities();
77 void updateJacobians();
78 void updateGyroscopicAccelerations();
80 void updateVelocities(
Frame *frame);
81 void updateAccelerations(
Frame *frame);
82 void updateJacobians(
Frame *frame,
int j=0) { (this->*updateJacobians_[j])(frame); }
83 void updateGyroscopicAccelerations(
Frame *frame);
84 void updateJacobians0(
Frame *frame);
85 void updateJacobians1(
Frame *frame) { }
86 void updateJacobians2(
Frame *frame);
88 virtual void calcqSize();
89 virtual void calcuSize(
int j=0);
90 void sethSize(
int hSize_,
int i=0);
91 void sethInd(
int hInd_,
int i=0);
103 virtual std::string
getType()
const {
return "RigidBody"; }
116 fPrPK->setParent(
this);
144 fAPK->setParent(
this);
161 void setTranslationDependentRotation(
bool dep) { translationDependentRotation = dep; }
163 void setBodyFixedRepresentationOfAngularVelocity(
bool bf) { bodyFixedRepresentationOfAngularVelocity = bf; }
176 void setMass(
double m_) {
m = m_; }
177 double getMass()
const {
return m; }
178 Frame* getFrameForKinematics() {
return &Z; };
179 FixedRelativeFrame* getFrameC() {
return C; };
181 const fmatvec::Vec3& evalGlobalRelativePosition() {
if(updPos) updatePositions();
return WrPK; }
182 const fmatvec::Vec3& evalGlobalRelativeVelocity() {
if(updVel) updateVelocities();
return WvPKrel; }
183 const fmatvec::Vec3& evalGlobalRelativeAngularVelocity() {
if(updVel) updateVelocities();
return WomPK; }
184 const fmatvec::SymMat3& evalGlobalInertiaTensor() {
if(updWTS) updateInertiaTensor();
return WThetaS; }
185 const fmatvec::Mat3xV& evalPJTT() {
if(updPJ) updateJacobians();
return PJTT; }
186 const fmatvec::Mat3xV& evalPJRR() {
if(updPJ) updateJacobians();
return PJRR; }
187 const fmatvec::Vec3& evalPjhT() {
if(updPJ) updateJacobians();
return PjhT; }
188 const fmatvec::Vec3& evalPjhR() {
if(updPJ) updateJacobians();
return PjhR; }
189 const fmatvec::Vec3& evalPjbT() {
if(updPjb) updateGyroscopicAccelerations();
return PjbT; }
190 const fmatvec::Vec3& evalPjbR() {
if(updPjb) updateGyroscopicAccelerations();
return PjbR; }
198 const fmatvec::SymMat3& getInertiaTensor()
const {
return SThetaS;}
199 fmatvec::SymMat3& getInertiaTensor() {
return SThetaS;}
201 void addFrame(FixedRelativeFrame *frame);
202 void addContour(RigidContour *contour);
209 void setFrameForInertiaTensor(Frame *frame);
211 void setOpenMBVRigidBody(
const std::shared_ptr<OpenMBV::RigidBody> &body);
213 const Frame* getOpenMBVFrameOfReference()
const {
return openMBVFrame; }
216 BOOST_PARAMETER_MEMBER_FUNCTION( (
void), enableOpenMBVWeight, tag, (optional (scaleLength,(
double),1)(scaleSize,(
double),1)(referencePoint,(OpenMBV::Arrow::ReferencePoint),OpenMBV::Arrow::toPoint)(diffuseColor,(
const fmatvec::Vec3&),
"[-1;1;1]")(transparency,(
double),0))) {
217 OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
218 FWeight=ombv.createOpenMBV();
222 BOOST_PARAMETER_MEMBER_FUNCTION( (
void), enableOpenMBVJointForce, tag, (optional (scaleLength,(
double),1)(scaleSize,(
double),1)(referencePoint,(OpenMBV::Arrow::ReferencePoint),OpenMBV::Arrow::toPoint)(diffuseColor,(
const fmatvec::Vec3&),
"[-1;1;1]")(transparency,(
double),0))) {
223 OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
224 FArrow=ombv.createOpenMBV();
228 BOOST_PARAMETER_MEMBER_FUNCTION( (
void), enableOpenMBVJointMoment, tag, (optional (scaleLength,(
double),1)(scaleSize,(
double),1)(referencePoint,(OpenMBV::Arrow::ReferencePoint),OpenMBV::Arrow::toPoint)(diffuseColor,(
const fmatvec::Vec3&),
"[-1;1;1]")(transparency,(
double),0))) {
229 OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
230 MArrow=ombv.createOpenMBV();
233 virtual void initializeUsingXML(xercesc::DOMElement *element);
235 fmatvec::Mat& getJRel(
int i=0,
bool check=
true) { assert((not check) or (not updGJ));
return JRel[i]; }
236 fmatvec::Vec& getjRel(
bool check=
true) { assert((not check) or (not updGJ));
return jRel; }
237 fmatvec::Vec& getqRel(
bool check=
true) { assert((not check) or (not updGC));
return qRel; }
238 fmatvec::Vec& getuRel(
bool check=
true) { assert((not check) or (not updGC));
return uRel; }
239 fmatvec::Mat& getTRel(
bool check=
true) { assert((not check) or (not updT));
return TRel; }
245 int getqRelSize()
const {
return nq;}
246 int getuRelSize(
int i=0)
const {
return nu[i];}
248 bool transformCoordinates()
const {
return fTR!=NULL;}
250 void resetUpToDate();
251 void resetPositionsUpToDate();
252 void resetVelocitiesUpToDate();
253 void resetJacobiansUpToDate();
254 void resetGyroscopicAccelerationsUpToDate();
255 const fmatvec::Vec& evalqRel() {
if(updGC) updateGeneralizedCoordinates();
return qRel; }
256 const fmatvec::Vec& evaluRel() {
if(updGC) updateGeneralizedCoordinates();
return uRel; }
257 const fmatvec::VecV& evalqTRel() {
if(updGC) updateGeneralizedCoordinates();
return qTRel; }
258 const fmatvec::VecV& evalqRRel() {
if(updGC) updateGeneralizedCoordinates();
return qRRel; }
259 const fmatvec::VecV& evaluTRel() {
if(updGC) updateGeneralizedCoordinates();
return uTRel; }
260 const fmatvec::VecV& evaluRRel() {
if(updGC) updateGeneralizedCoordinates();
return uRRel; }
261 const fmatvec::Mat& evalJRel(
int j=0) {
if(updGJ) updateGeneralizedJacobians();
return JRel[j]; }
262 const fmatvec::Vec& evaljRel() {
if(updGJ) updateGeneralizedJacobians();
return jRel; }
263 const fmatvec::Mat& evalTRel() {
if(updT) updateT();
return TRel; }
265 void setUpdateByReference(
bool updateByReference_) { updateByReference = updateByReference_; }
290 fmatvec::Vec3 PjhT, PjhR, PjbT, PjbR;
361 fmatvec::VecV qTRel, qRRel, uTRel, uRRel;
362 fmatvec::Mat3xV PJTT, PJRR;
366 Frame *frameForJacobianOfRotation;
368 Frame *frameForInertiaTensor;
372 bool translationDependentRotation, constJT, constJR, constjT, constjR;
374 bool updPjb, updGC, updGJ, updWTS, updT, updateByReference;
378 bool bodyFixedRepresentationOfAngularVelocity;
385 std::shared_ptr<OpenMBV::Arrow> FWeight, FArrow, MArrow;
void setInertiaTensor(const fmatvec::SymMat3 &RThetaR)
Definition: rigid_body.h:196
virtual void plot()
plots time dependent data
Definition: rigid_body.cc:352
void setGeneralTranslation(Function< fmatvec::Vec3(fmatvec::VecV, double)> *fPrPK_)
set Kinematic for genral translational motion
Definition: rigid_body.h:114
void setFrameForKinematics(Frame *frame)
Definition: rigid_body.cc:76
Function< fmatvec::RotMat3(fmatvec::VecV, double)> * getRotation()
get Kinematic for rotational motion
Definition: rigid_body.h:174
void setStateDependentRotation(Function< fmatvec::RotMat3(fmatvec::VecV)> *fAPK_)
set Kinematic for only state dependent rotational motion
Definition: rigid_body.h:156
virtual void updateuRef(const fmatvec::Vec &ref)
references to velocities of dynamic system parent
Definition: rigid_body.cc:568
void updateLLMNotConst()
Cholesky decomposition of time dependent mass matrix.
Definition: rigid_body.h:347
double m
mass
Definition: rigid_body.h:271
Definition: openmbv_utils.h:54
virtual void updateLLM()
perform Cholesky decomposition of mass martix
Definition: object.h:220
cartesian frame on rigid bodies
Definition: fixed_relative_frame.h:31
fmatvec::Vec q
positions, velocities
Definition: object.h:298
virtual std::string getType() const
Definition: rigid_body.h:103
bool coordinateTransformation
boolean to use body fixed Frame for rotation
Definition: rigid_body.h:288
BOOST_PARAMETER_MEMBER_FUNCTION((void), enableOpenMBVWeight, tag,(optional(scaleLength,(double), 1)(scaleSize,(double), 1)(referencePoint,(OpenMBV::Arrow::ReferencePoint), OpenMBV::Arrow::toPoint)(diffuseColor,(const fmatvec::Vec3 &),"[-1;1;1]")(transparency,(double), 0)))
Visualize the weight.
Definition: rigid_body.h:216
virtual void setUpInverseKinetics()
TODO.
Definition: rigid_body.cc:337
Function< fmatvec::Vec3(fmatvec::VecV, double)> * getTranslation()
get Kinematic for translational motion
Definition: rigid_body.h:169
void setStateDependentTranslation(Function< fmatvec::Vec3(fmatvec::VecV)> *fPrPK_)
set Kinematic for only state dependent translational motion
Definition: rigid_body.h:130
Definition: time_dependent_function.h:28
BOOST_PARAMETER_MEMBER_FUNCTION((void), enableOpenMBVJointForce, tag,(optional(scaleLength,(double), 1)(scaleSize,(double), 1)(referencePoint,(OpenMBV::Arrow::ReferencePoint), OpenMBV::Arrow::toPoint)(diffuseColor,(const fmatvec::Vec3 &),"[-1;1;1]")(transparency,(double), 0)))
Visualize the joint force.
Definition: rigid_body.h:222
Frame * openMBVFrame
Frame of reference for drawing openMBVBody.
Definition: rigid_body.h:384
void(RigidBody::* updateM_)()
function pointer to update mass matrix
Definition: rigid_body.h:322
virtual void updateqRef(const fmatvec::Vec &ref)
references to positions of dynamic system parent
Definition: rigid_body.cc:563
void updateLLMConst()
Cholesky decomposition of constant mass matrix.
Definition: rigid_body.h:342
InitStage
The stages of the initialization.
Definition: element.h:97
virtual ~RigidBody()
destructor
Definition: rigid_body.cc:70
FixedRelativeFrame * C
Definition: rigid_body.h:354
fmatvec::SymMat3 SThetaS
inertia tensor with respect to centre of gravity in centre of gravity and world Frame ...
Definition: rigid_body.h:276
void setGeneralRotation(Function< fmatvec::RotMat3(fmatvec::VecV, double)> *fAPK_)
set Kinematic for general rotational motion
Definition: rigid_body.h:142
Definition: planar_contour.h:31
fmatvec::SqrMat3 APK
rotation matrix from kinematic Frame to parent Frame
Definition: rigid_body.h:295
std::string name
name of element
Definition: element.h:298
std::vector< Frame * > frame
vector of frames and contours
Definition: body.h:166
Class for constraints between generalized coordinates of objects.
Definition: constraint.h:30
void(RigidBody::* updateLLM_)()
function pointer for Cholesky decomposition of mass matrix
Definition: rigid_body.h:337
cartesian frame on bodies used for application of e.g. links and loads
Definition: frame.h:37
Function< fmatvec::RotMat3(fmatvec::VecV, double)> * fAPK
rotation from kinematic Frame to parent Frame
Definition: rigid_body.h:317
virtual void updateLLM()
perform Cholesky decomposition of mass martix
Definition: rigid_body.h:98
Definition: state_dependent_function.h:28
virtual void initz()
Definition: rigid_body.cc:328
RigidBody(const std::string &name="")
constructor
Definition: rigid_body.cc:55
void setName(const std::string &str)
Definition: element.h:163
Function< fmatvec::Vec3(fmatvec::VecV, double)> * fPrPK
translation from parent Frame to kinematic Frame in parent system
Definition: rigid_body.h:312
fmatvec::Vec3 WvPKrel
translational and angular velocity from parent to kinematic Frame in world system ...
Definition: rigid_body.h:305
void updateMConst()
update constant mass matrix
Definition: rigid_body.cc:585
fmatvec::SymMat Mbuf
TODO.
Definition: rigid_body.h:283
void updateMNotConst()
update time dependend mass matrix
Definition: rigid_body.cc:589
void setTimeDependentRotation(Function< fmatvec::RotMat3(double)> *fAPK_)
set Kinematic for only time dependent rotational motion
Definition: rigid_body.h:151
BOOST_PARAMETER_MEMBER_FUNCTION((void), enableOpenMBVJointMoment, tag,(optional(scaleLength,(double), 1)(scaleSize,(double), 1)(referencePoint,(OpenMBV::Arrow::ReferencePoint), OpenMBV::Arrow::toPoint)(diffuseColor,(const fmatvec::Vec3 &),"[-1;1;1]")(transparency,(double), 0)))
Visualize the joint moment.
Definition: rigid_body.h:228
fmatvec::Vec3 PrPK
translation from parent to kinematic Frame in parent and world system
Definition: rigid_body.h:300
void setTimeDependentTranslation(Function< fmatvec::Vec3(double)> *fPrPK_)
set Kinematic for only time dependent translational motion
Definition: rigid_body.h:123
virtual void init(InitStage stage)
initialize object at start of simulation with respect to contours and frames
Definition: rigid_body.cc:116
base class for all mechanical bodies with mass and generalised coordinates
Definition: body.h:50
rigid bodies with arbitrary kinematics
Definition: rigid_body.h:52