20 #ifndef _FLEXIBLE_BODY_FFR_H_
21 #define _FLEXIBLE_BODY_FFR_H_
23 #include "mbsim/objects/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();
63 void updateNodalPositions();
64 void updateNodalStresses();
68 void updateJacobians(
MBSim::Frame *frame,
int j=0) { (this->*updateJacobians_[j])(frame); }
74 void updateKJ(
int j=0) { (this->*updateKJ_[j])(); }
78 virtual void calcqSize();
79 virtual void calcuSize(
int j=0);
85 virtual void init(InitStage stage);
87 virtual void updateLLM() { (this->*
updateLLM_)(); }
88 virtual void setUpInverseKinetics();
92 virtual std::string getType()
const {
return "FlexibleBodyFFR"; }
105 fPrPK->setParent(
this);
106 fPrPK->setName(
"GeneralTranslation");
133 fAPK->setParent(
this);
134 fAPK->setName(
"GeneralRotation");
150 void setTranslationDependentRotation(
bool dep) { translationDependentRotation = dep; }
152 void setBodyFixedRepresentationOfAngularVelocity(
bool bf) { bodyFixedRepresentationOfAngularVelocity = bf; }
165 double getMass()
const {
return m; }
175 void setPositionIntegral(
const fmatvec::Vec3 &rdm_) { rdm = rdm_; }
176 void setPositionPositionIntegral(
const fmatvec::SymMat3& rrdm_) { rrdm = rrdm_; }
177 void setShapeFunctionIntegral(
const fmatvec::Mat3xV &Pdm_) { Pdm = Pdm_; }
178 void setPositionShapeFunctionIntegral(
const std::vector<fmatvec::Mat3xV> &rPdm_) { rPdm = rPdm_; }
179 void setShapeFunctionShapeFunctionIntegral(
const std::vector<std::vector<fmatvec::SqrMatV> > &PPdm_) { PPdm = PPdm_; }
180 void setStiffnessMatrix(
const fmatvec::SymMatV &Ke0_) { Ke0 = Ke0_; }
181 void setDampingMatrix(
const fmatvec::SymMatV &De0_) { De0 = De0_; }
182 void setProportionalDamping(
const fmatvec::Vec2 &beta_) { beta = beta_; }
186 void setNonlinearStiffnessMatrixOfFirstOrder(
const std::vector<fmatvec::SqrMatV> &Knl1_) { Knl1 = Knl1_; }
187 void setNonlinearStiffnessMatrixOfSecondOrder(
const std::vector<std::vector<fmatvec::SqrMatV> > &Knl2_) { Knl2 = Knl2_; }
191 void setInitialStressIntegral(
const fmatvec::VecV &ksigma0_) { ksigma0 = ksigma0_; }
192 void setNonlinearInitialStressIntegral(
const fmatvec::SqrMatV &ksigma1_) { ksigma1 = ksigma1_; }
196 void setGeometricStiffnessMatrixDueToAcceleration(
const std::vector<fmatvec::SqrMatV> &K0t_) { K0t = K0t_; }
197 void setGeometricStiffnessMatrixDueToAngularAcceleration(
const std::vector<fmatvec::SqrMatV> &K0r_) { K0r = K0r_; }
198 void setGeometricStiffnessMatrixDueToAngularVelocity(
const std::vector<fmatvec::SqrMatV> &K0om_) { K0om = K0om_; }
201 void setRelativeNodalPosition(
const fmatvec::VecV &r);
202 void setRelativeNodalOrientation(
const fmatvec::MatVx3 &A);
203 void setShapeMatrixOfTranslation(
const fmatvec::MatV &Phi_);
204 void setShapeMatrixOfRotation(
const fmatvec::MatV &Psi_);
205 void setStressMatrix(
const fmatvec::MatV &sigmahel_);
206 void setNonlinearStressMatrix(
const std::vector<fmatvec::MatV> &sigmahen_);
207 void setInitialStress(
const fmatvec::VecV &sigma0_);
208 void setGeometricStiffnessMatrixDueToForce(
const std::vector<fmatvec::SqrMatV> &K0F_);
209 void setGeometricStiffnessMatrixDueToMoment(
const std::vector<fmatvec::SqrMatV> &K0M_);
211 void addFrame(FixedNodalFrame *frame);
213 using Body::addContour;
216 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))) {
217 OpenMBVDynamicIndexedFaceSet ombv(minimalColorValue,maximalColorValue);
218 openMBVBody=ombv.createOpenMBV();
220 ombvIndices = indices;
224 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))) {
225 MBSim::OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
226 FWeight=ombv.createOpenMBV();
230 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))) {
231 MBSim::OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
232 FArrow=ombv.createOpenMBV();
236 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))) {
237 MBSim::OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
238 MArrow=ombv.createOpenMBV();
241 virtual void initializeUsingXML(xercesc::DOMElement *element);
243 fmatvec::Vec& getqRel(
bool check=
true) { assert((not check) or (not updGC));
return qRel; }
244 fmatvec::Vec& getuRel(
bool check=
true) { assert((not check) or (not updGC));
return uRel; }
245 fmatvec::Mat& getTRel(
bool check=
true) { assert((not check) or (not updT));
return TRel; }
249 int getqRelSize()
const {
return nq;}
250 int getuRelSize(
int i=0)
const {
return nu[i];}
252 bool transformCoordinates()
const {
return fTR!=NULL;}
254 void resetUpToDate();
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& evalTRel() {
if(updT) updateT();
return TRel; }
262 const fmatvec::Vec3& evalGlobalRelativePosition() {
if(updPos) updatePositions();
return WrPK; }
263 const fmatvec::Vec3& evalGlobalRelativeVelocity() {
if(updVel) updateVelocities();
return WvPKrel; }
264 const fmatvec::Vec3& evalGlobalRelativeAngularVelocity() {
if(updVel) updateVelocities();
return WomPK; }
265 const fmatvec::Vec3& evalPjbT() {
if(updPjb) updateGyroscopicAccelerations();
return PjbT; }
266 const fmatvec::Vec3& evalPjbR() {
if(updPjb) updateGyroscopicAccelerations();
return PjbR; }
267 const fmatvec::Mat3xV& evalPJTT() {
if(updPJ) updateJacobians();
return PJTT; }
268 const fmatvec::Mat3xV& evalPJRR() {
if(updPJ) updateJacobians();
return PJRR; }
269 const fmatvec::SymMatV& evalMb() {
if(updMb) updateMb();
return M_; }
270 const fmatvec::VecV& evalhb() {
if(updMb) updateMb();
return h_; }
271 const fmatvec::MatV& evalKJ(
int j=0) {
if(updKJ[j]) updateKJ(j);
return KJ[j]; }
272 const fmatvec::VecV& evalKi() {
if(updKJ[0]) updateKJ(0);
return Ki; }
277 fmatvec::SymMat3 rrdm, mmi0;
279 std::vector<std::vector<fmatvec::SqrMatV> > PPdm, Knl2, Ke2;
280 std::vector<fmatvec::Mat3xV> rPdm;
281 std::vector<std::vector<fmatvec::SqrMat3> > mmi2, Gr1;
282 std::vector<fmatvec::SqrMatV> Knl1, K0t, K0r, K0om, Ct1, Cr1, Ge, Oe1, Ke1, De1;
284 fmatvec::VecV ksigma0;
285 fmatvec::SqrMatV ksigma1;
286 std::vector<fmatvec::SymMat3> mmi1;
287 fmatvec::MatVx3 Ct0, Cr0;
288 fmatvec::SymMatV Me, Ke0, De0;
289 std::vector<fmatvec::SqrMat3> Gr0;
293 std::vector<fmatvec::Vec3> KrKP, WrOP, disp;
294 std::vector<fmatvec::SqrMat3> ARP, AWK;
295 std::vector<fmatvec::Mat3xV> Phi, Psi;
296 std::vector<std::vector<fmatvec::SqrMatV> > K0F, K0M;
297 std::vector<fmatvec::Vector<fmatvec::Fixed<6>,
double> > sigma0, sigma;
298 std::vector<fmatvec::Matrix<fmatvec::General, fmatvec::Fixed<6>,
fmatvec::Var,
double> > sigmahel;
299 std::vector<std::vector<fmatvec::Matrix<fmatvec::General, fmatvec::Fixed<6>,
fmatvec::Var,
double> > > sigmahen;
316 fmatvec::Vec3 PjhT, PjhR, PjbT, PjbR;
382 fmatvec::VecV qTRel, qRRel, uTRel, uRRel;
383 fmatvec::Mat3xV WJTrel, WJRrel, PJTT, PJRR;
391 bool translationDependentRotation, constJT, constJR, constjT, constjR;
393 bool updPjb, updGC, updT, updMb, updKJ[2], updNodalPos, updNodalStress;
401 void prefillMassMatrix();
403 bool bodyFixedRepresentationOfAngularVelocity;
410 std::shared_ptr<OpenMBV::Arrow> FWeight, FArrow, MArrow;
411 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:338
void setGeneralRotation(MBSim::Function< fmatvec::RotMat3(fmatvec::VecV, double)> *fAPK_)
set Kinematic for general rotational motion
Definition: flexible_body_ffr.h:131
MBSim::Function< fmatvec::RotMat3(fmatvec::VecV, double)> * getRotation()
get Kinematic for rotational motion
Definition: flexible_body_ffr.h:163
void updateLLMConst()
Cholesky decomposition of constant mass matrix.
Definition: flexible_body_ffr.h:368
bool coordinateTransformation
boolean to use body fixed Frame for rotation
Definition: flexible_body_ffr.h:314
void setStateDependentTranslation(MBSim::Function< fmatvec::Vec3(fmatvec::VecV)> *fPrPK_)
set Kinematic for only state dependent translational motion
Definition: flexible_body_ffr.h:119
void setStateDependentRotation(MBSim::Function< fmatvec::RotMat3(fmatvec::VecV)> *fAPK_)
set Kinematic for only state dependent rotational motion
Definition: flexible_body_ffr.h:145
MBSim::Function< fmatvec::RotMat3(fmatvec::VecV, double)> * fAPK
rotation from kinematic Frame to parent Frame
Definition: flexible_body_ffr.h:343
void updateMNotConst()
update time dependend mass matrix
Definition: flexible_body_ffr.cc:863
void setGeneralTranslation(MBSim::Function< fmatvec::Vec3(fmatvec::VecV, double)> *fPrPK_)
set Kinematic for genral translational motion
Definition: flexible_body_ffr.h:103
fmatvec::Vec3 PrPK
translation from parent to kinematic Frame in parent and world system
Definition: flexible_body_ffr.h:326
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:230
void updateLLMNotConst()
Cholesky decomposition of time dependent mass matrix.
Definition: flexible_body_ffr.h:373
void setTimeDependentTranslation(MBSim::Function< fmatvec::Vec3(double)> *fPrPK_)
set Kinematic for only time dependent translational motion
Definition: flexible_body_ffr.h:112
fmatvec::SymMat Mbuf
TODO.
Definition: flexible_body_ffr.h:309
void setTimeDependentRotation(MBSim::Function< fmatvec::RotMat3(double)> *fAPK_)
set Kinematic for only time dependent rotational motion
Definition: flexible_body_ffr.h:140
fmatvec::Vec3 WvPKrel
translational and angular velocity from parent to kinematic Frame in world system ...
Definition: flexible_body_ffr.h:331
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:224
std::vector< Frame * > frame
MBSim::Frame * openMBVFrame
Frame of reference for drawing openMBVBody.
Definition: flexible_body_ffr.h:409
void(FlexibleBodyFFR::* updateM_)()
function pointer to update mass matrix
Definition: flexible_body_ffr.h:348
void setMass(double m_)
Set mass.
Definition: flexible_body_ffr.h:174
fmatvec::SqrMat3 APK
rotation matrix from kinematic Frame to parent Frame
Definition: flexible_body_ffr.h:321
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:236
virtual ~FlexibleBodyFFR()
destructor
Definition: flexible_body_ffr.cc:72
void updateMConst()
update constant mass matrix
Definition: flexible_body_ffr.cc:859
void(FlexibleBodyFFR::* updateLLM_)()
function pointer for Cholesky decomposition of mass matrix
Definition: flexible_body_ffr.h:363
MBSim::Function< fmatvec::Vec3(fmatvec::VecV, double)> * getTranslation()
get Kinematic for translational motion
Definition: flexible_body_ffr.h:158
Flexible body using a floating frame of reference formulation.
Definition: flexible_body_ffr.h:43