20 #ifndef _FLEXIBLE_BODY_FFR_H_
21 #define _FLEXIBLE_BODY_FFR_H_
23 #include "mbsimFlexibleBody/node_based_body.h"
24 #include "mbsim/functions/time_dependent_function.h"
25 #include "mbsim/functions/state_dependent_function.h"
26 #include "mbsim/utils/boost_parameters.h"
27 #include "mbsim/utils/index.h"
28 #include "mbsimFlexibleBody/utils/openmbv_utils.h"
32 BOOST_PARAMETER_NAME(indices)
35 namespace MBSimFlexibleBody {
37 class FixedNodalFrame;
55 void updateh(
int j=0);
56 void updateM() { (this->*
updateM_)(); }
57 void updateGeneralizedCoordinates();
58 void updatePositions();
59 void updateVelocities();
60 void updateAccelerations();
61 void updateJacobians();
62 void updateGyroscopicAccelerations();
66 void updateJacobians(
MBSim::Frame *frame,
int j=0) { (this->*updateJacobians_[j])(frame); }
72 void updateKJ(
int j=0) { (this->*updateKJ_[j])(); }
76 virtual void calcqSize();
77 virtual void calcuSize(
int j=0);
83 virtual void init(InitStage stage);
85 virtual void updateLLM() { (this->*
updateLLM_)(); }
86 virtual void setUpInverseKinetics();
90 virtual std::string getType()
const {
return "FlexibleBodyFFR"; }
103 fPrPK->setParent(
this);
104 fPrPK->setName(
"GeneralTranslation");
131 fAPK->setParent(
this);
132 fAPK->setName(
"GeneralRotation");
148 void setTranslationDependentRotation(
bool dep) { translationDependentRotation = dep; }
150 void setBodyFixedRepresentationOfAngularVelocity(
bool bf) { bodyFixedRepresentationOfAngularVelocity = bf; }
163 double getMass()
const {
return m; }
173 void setPositionIntegral(
const fmatvec::Vec3 &rdm_) { rdm = rdm_; }
174 void setPositionPositionIntegral(
const fmatvec::SymMat3& rrdm_) { rrdm = rrdm_; }
175 void setShapeFunctionIntegral(
const fmatvec::Mat3xV &Pdm_) { Pdm = Pdm_; }
176 void setPositionShapeFunctionIntegral(
const std::vector<fmatvec::Mat3xV> &rPdm_) { rPdm = rPdm_; }
177 void setShapeFunctionShapeFunctionIntegral(
const std::vector<std::vector<fmatvec::SqrMatV> > &PPdm_) { PPdm = PPdm_; }
178 void setStiffnessMatrix(
const fmatvec::SymMatV &Ke0_) { Ke0 = Ke0_; }
179 void setDampingMatrix(
const fmatvec::SymMatV &De0_) { De0 = De0_; }
180 void setProportionalDamping(
const fmatvec::Vec2 &beta_) { beta = beta_; }
184 void setNonlinearStiffnessMatrixOfFirstOrder(
const std::vector<fmatvec::SqrMatV> &Knl1_) { Knl1 = Knl1_; }
185 void setNonlinearStiffnessMatrixOfSecondOrder(
const std::vector<std::vector<fmatvec::SqrMatV> > &Knl2_) { Knl2 = Knl2_; }
189 void setInitialStressIntegral(
const fmatvec::VecV &ksigma0_) { ksigma0 = ksigma0_; }
190 void setNonlinearInitialStressIntegral(
const fmatvec::SqrMatV &ksigma1_) { ksigma1 = ksigma1_; }
194 void setGeometricStiffnessMatrixDueToAcceleration(
const std::vector<fmatvec::SqrMatV> &K0t_) { K0t = K0t_; }
195 void setGeometricStiffnessMatrixDueToAngularAcceleration(
const std::vector<fmatvec::SqrMatV> &K0r_) { K0r = K0r_; }
196 void setGeometricStiffnessMatrixDueToAngularVelocity(
const std::vector<fmatvec::SqrMatV> &K0om_) { K0om = K0om_; }
199 void setRelativeNodalPosition(
const fmatvec::VecV &r);
200 void setRelativeNodalOrientation(
const fmatvec::MatVx3 &A);
201 void setShapeMatrixOfTranslation(
const fmatvec::MatV &Phi_);
202 void setShapeMatrixOfRotation(
const fmatvec::MatV &Psi_);
203 void setStressMatrix(
const fmatvec::MatV &sigmahel_);
204 void setNonlinearStressMatrix(
const std::vector<fmatvec::MatV> &sigmahen_);
205 void setInitialStress(
const fmatvec::VecV &sigma0_);
206 void setGeometricStiffnessMatrixDueToForce(
const std::vector<fmatvec::SqrMatV> &K0F_);
207 void setGeometricStiffnessMatrixDueToMoment(
const std::vector<fmatvec::SqrMatV> &K0M_);
209 void addFrame(FixedNodalFrame *frame);
214 BOOST_PARAMETER_MEMBER_FUNCTION( (
void), enableOpenMBV, MBSim::tag, (optional (nodes,(
const std::vector<MBSim::Index>&),std::vector<MBSim::Index>())(indices,(
const std::vector<MBSim::Index>&),std::vector<MBSim::Index>())(minimalColorValue,(
double),0)(maximalColorValue,(
double),0))) {
215 OpenMBVDynamicIndexedFaceSet ombv(minimalColorValue,maximalColorValue);
216 openMBVBody=ombv.createOpenMBV();
218 ombvIndices = indices;
222 BOOST_PARAMETER_MEMBER_FUNCTION( (
void), enableOpenMBVWeight, MBSim::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 MBSim::OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
224 FWeight=ombv.createOpenMBV();
228 BOOST_PARAMETER_MEMBER_FUNCTION( (
void), enableOpenMBVJointForce, MBSim::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 MBSim::OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
230 FArrow=ombv.createOpenMBV();
234 BOOST_PARAMETER_MEMBER_FUNCTION( (
void), enableOpenMBVJointMoment, MBSim::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))) {
235 MBSim::OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
236 MArrow=ombv.createOpenMBV();
239 virtual void initializeUsingXML(xercesc::DOMElement *element);
241 fmatvec::Vec& getqRel(
bool check=
true) { assert((not check) or (not updGC));
return qRel; }
242 fmatvec::Vec& getuRel(
bool check=
true) { assert((not check) or (not updGC));
return uRel; }
243 fmatvec::Mat& getTRel(
bool check=
true) { assert((not check) or (not updT));
return TRel; }
247 int getqRelSize()
const {
return nq;}
248 int getuRelSize(
int i=0)
const {
return nu[i];}
250 bool transformCoordinates()
const {
return fTR!=NULL;}
252 void resetUpToDate();
253 const fmatvec::Vec& evalqRel() {
if(updGC) updateGeneralizedCoordinates();
return qRel; }
254 const fmatvec::Vec& evaluRel() {
if(updGC) updateGeneralizedCoordinates();
return uRel; }
255 const fmatvec::VecV& evalqTRel() {
if(updGC) updateGeneralizedCoordinates();
return qTRel; }
256 const fmatvec::VecV& evalqRRel() {
if(updGC) updateGeneralizedCoordinates();
return qRRel; }
257 const fmatvec::VecV& evaluTRel() {
if(updGC) updateGeneralizedCoordinates();
return uTRel; }
258 const fmatvec::VecV& evaluRRel() {
if(updGC) updateGeneralizedCoordinates();
return uRRel; }
259 const fmatvec::Mat& evalTRel() {
if(updT) updateT();
return TRel; }
260 const fmatvec::Vec3& evalGlobalRelativePosition() {
if(updPos) updatePositions();
return WrPK; }
261 const fmatvec::Vec3& evalGlobalRelativeVelocity() {
if(updVel) updateVelocities();
return WvPKrel; }
262 const fmatvec::Vec3& evalGlobalRelativeAngularVelocity() {
if(updVel) updateVelocities();
return WomPK; }
263 const fmatvec::Vec3& evalPjbT() {
if(updPjb) updateGyroscopicAccelerations();
return PjbT; }
264 const fmatvec::Vec3& evalPjbR() {
if(updPjb) updateGyroscopicAccelerations();
return PjbR; }
265 const fmatvec::Mat3xV& evalPJTT() {
if(updPJ) updateJacobians();
return PJTT; }
266 const fmatvec::Mat3xV& evalPJRR() {
if(updPJ) updateJacobians();
return PJRR; }
267 const fmatvec::SymMatV& evalMb() {
if(updMb) updateMb();
return M_; }
268 const fmatvec::VecV& evalhb() {
if(updMb) updateMb();
return h_; }
269 const fmatvec::MatV& evalKJ(
int j=0) {
if(updKJ[j]) updateKJ(j);
return KJ[j]; }
270 const fmatvec::VecV& evalKi() {
if(updKJ[0]) updateKJ(0);
return Ki; }
271 const fmatvec::Vec3& evalGlobalRelativePosition(
int i) {
if(updNodalPos[i]) updatePositions(i);
return WrRP[i]; }
272 const fmatvec::Vec3& evalGlobalRelativeAngularVelocity(
int i) {
if(updNodalVel[i]) updateVelocities(i);
return Womrel[i]; }
274 const fmatvec::Vec3& evalNodalDisp(
int i) {
if(updNodalPos[i]) updatePositions(i);
return disp[i]; }
275 const fmatvec::Vec3& evalNodalPosition(
int i) {
if(updNodalPos[i]) updatePositions(i);
return WrOP[i]; }
277 virtual void updateStresses(
int i);
278 virtual void updatePositions(
int i);
279 virtual void updateVelocities(
int i);
281 virtual void updatePositions(NodeFrame* frame);
282 virtual void updateVelocities(NodeFrame* frame);
283 virtual void updateAccelerations(NodeFrame* frame);
284 virtual void updateJacobians(NodeFrame* frame,
int j=0);
285 virtual void updateGyroscopicAccelerations(NodeFrame* frame);
290 fmatvec::SymMat3 rrdm, mmi0;
292 std::vector<std::vector<fmatvec::SqrMatV> > PPdm, Knl2, Ke2;
293 std::vector<fmatvec::Mat3xV> rPdm;
294 std::vector<std::vector<fmatvec::SqrMat3> > mmi2, Gr1;
295 std::vector<fmatvec::SqrMatV> Knl1, K0t, K0r, K0om, Ct1, Cr1, Ge, Oe1, Ke1, De1;
297 fmatvec::VecV ksigma0;
298 fmatvec::SqrMatV ksigma1;
299 std::vector<fmatvec::SymMat3> mmi1;
300 fmatvec::MatVx3 Ct0, Cr0;
301 fmatvec::SymMatV Me, Ke0, De0;
302 std::vector<fmatvec::SqrMat3> Gr0;
306 std::vector<fmatvec::Vec3> KrKP, WrOP, WrRP, disp, Wvrel, Womrel;
307 std::vector<fmatvec::SqrMat3> ARP, AWK;
308 std::vector<fmatvec::Mat3xV> Phi, Psi;
309 std::vector<std::vector<fmatvec::SqrMatV> > K0F, K0M;
310 std::vector<fmatvec::Vector<fmatvec::Fixed<6>,
double> > sigma0, sigma;
311 std::vector<fmatvec::Matrix<fmatvec::General, fmatvec::Fixed<6>,
fmatvec::Var,
double> > sigmahel;
312 std::vector<std::vector<fmatvec::Matrix<fmatvec::General, fmatvec::Fixed<6>,
fmatvec::Var,
double> > > sigmahen;
329 fmatvec::Vec3 PjhT, PjhR, PjbT, PjbR;
395 fmatvec::VecV qTRel, qRRel, uTRel, uRRel;
396 fmatvec::Mat3xV WJTrel, WJRrel, PJTT, PJRR;
404 bool translationDependentRotation, constJT, constJR, constjT, constjR;
406 bool updPjb, updGC, updT, updMb, updKJ[2];
407 std::vector<bool> updNodalPos, updNodalVel, updNodalStress;
415 void prefillMassMatrix();
417 bool bodyFixedRepresentationOfAngularVelocity;
424 std::shared_ptr<OpenMBV::Arrow> FWeight, FArrow, MArrow;
425 std::vector<MBSim::Index> ombvNodes, ombvIndices;
MBSim::Function< fmatvec::Vec3(fmatvec::VecV, double)> * fPrPK
translation from parent Frame to kinematic Frame in parent system
Definition: flexible_body_ffr.h:351
void setGeneralRotation(MBSim::Function< fmatvec::RotMat3(fmatvec::VecV, double)> *fAPK_)
set Kinematic for general rotational motion
Definition: flexible_body_ffr.h:129
MBSim::Function< fmatvec::RotMat3(fmatvec::VecV, double)> * getRotation()
get Kinematic for rotational motion
Definition: flexible_body_ffr.h:161
void updateLLMConst()
Cholesky decomposition of constant mass matrix.
Definition: flexible_body_ffr.h:381
bool coordinateTransformation
boolean to use body fixed Frame for rotation
Definition: flexible_body_ffr.h:327
void setStateDependentTranslation(MBSim::Function< fmatvec::Vec3(fmatvec::VecV)> *fPrPK_)
set Kinematic for only state dependent translational motion
Definition: flexible_body_ffr.h:117
void setStateDependentRotation(MBSim::Function< fmatvec::RotMat3(fmatvec::VecV)> *fAPK_)
set Kinematic for only state dependent rotational motion
Definition: flexible_body_ffr.h:143
MBSim::Function< fmatvec::RotMat3(fmatvec::VecV, double)> * fAPK
rotation from kinematic Frame to parent Frame
Definition: flexible_body_ffr.h:356
void updateMNotConst()
update time dependend mass matrix
Definition: flexible_body_ffr.cc:859
void setGeneralTranslation(MBSim::Function< fmatvec::Vec3(fmatvec::VecV, double)> *fPrPK_)
set Kinematic for genral translational motion
Definition: flexible_body_ffr.h:101
fmatvec::Vec3 PrPK
translation from parent to kinematic Frame in parent and world system
Definition: flexible_body_ffr.h:339
BOOST_PARAMETER_MEMBER_FUNCTION((void), enableOpenMBVJointForce, MBSim::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: flexible_body_ffr.h:228
void updateLLMNotConst()
Cholesky decomposition of time dependent mass matrix.
Definition: flexible_body_ffr.h:386
void setTimeDependentTranslation(MBSim::Function< fmatvec::Vec3(double)> *fPrPK_)
set Kinematic for only time dependent translational motion
Definition: flexible_body_ffr.h:110
fmatvec::SymMat Mbuf
TODO.
Definition: flexible_body_ffr.h:322
void setTimeDependentRotation(MBSim::Function< fmatvec::RotMat3(double)> *fAPK_)
set Kinematic for only time dependent rotational motion
Definition: flexible_body_ffr.h:138
fmatvec::Vec3 WvPKrel
translational and angular velocity from parent to kinematic Frame in world system ...
Definition: flexible_body_ffr.h:344
BOOST_PARAMETER_MEMBER_FUNCTION((void), enableOpenMBVWeight, MBSim::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: flexible_body_ffr.h:222
std::vector< Frame * > frame
MBSim::Frame * openMBVFrame
Frame of reference for drawing openMBVBody.
Definition: flexible_body_ffr.h:423
void(FlexibleBodyFFR::* updateM_)()
function pointer to update mass matrix
Definition: flexible_body_ffr.h:361
virtual void addContour(Contour *contour)
void setMass(double m_)
Set mass.
Definition: flexible_body_ffr.h:172
fmatvec::SqrMat3 APK
rotation matrix from kinematic Frame to parent Frame
Definition: flexible_body_ffr.h:334
BOOST_PARAMETER_MEMBER_FUNCTION((void), enableOpenMBVJointMoment, MBSim::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: flexible_body_ffr.h:234
Definition: node_based_body.h:30
virtual ~FlexibleBodyFFR()
destructor
Definition: flexible_body_ffr.cc:73
void updateMConst()
update constant mass matrix
Definition: flexible_body_ffr.cc:855
void addFrame(NodeFrame *frame)
Definition: node_based_body.cc:55
void(FlexibleBodyFFR::* updateLLM_)()
function pointer for Cholesky decomposition of mass matrix
Definition: flexible_body_ffr.h:376
MBSim::Function< fmatvec::Vec3(fmatvec::VecV, double)> * getTranslation()
get Kinematic for translational motion
Definition: flexible_body_ffr.h:156
Flexible body using a floating frame of reference formulation.
Definition: flexible_body_ffr.h:43