23 #include "functions/auxiliary_functions.h"
25 #ifdef HAVE_OPENMBVCPPINTERFACE
26 #include "mbsim/utils/boost_parameters.h"
27 #include "mbsim/utils/openmbv_utils.h"
44 #ifdef HAVE_OPENMBVCPPINTERFACE
45 virtual boost::shared_ptr<OpenMBV::Group> getOpenMBVGrp() {
return boost::shared_ptr<OpenMBV::Group>();}
64 void setDependentBody(
RigidBody* body_) {bd=body_; }
66 void updateStateDependentVariables(
double t);
67 void updateJacobians(
double t,
int j=0);
70 void initializeUsingXML(xercesc::DOMElement * element);
72 virtual std::string
getType()
const {
return "GearConstraint"; }
74 #ifdef HAVE_OPENMBVCPPINTERFACE
76 BOOST_PARAMETER_MEMBER_FUNCTION( (
void), enableOpenMBVForce, 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))) {
77 OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
78 FArrow=ombv.createOpenMBV();
82 BOOST_PARAMETER_MEMBER_FUNCTION( (
void), enableOpenMBVMoment, 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))) {
83 OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toDoubleHead,referencePoint,scaleLength,scaleSize);
84 MArrow=ombv.createOpenMBV();
89 std::vector<RigidBody*> bi;
91 std::vector<double> ratio;
93 std::string saved_DependentBody;
94 std::vector<std::string> saved_IndependentBody;
96 #ifdef HAVE_OPENMBVCPPINTERFACE
97 boost::shared_ptr<OpenMBV::Arrow> FArrow, MArrow;
106 void setDependentBody(
RigidBody* body) {bd=body; }
111 void initializeUsingXML(xercesc::DOMElement * element);
113 #ifdef HAVE_OPENMBVCPPINTERFACE
115 BOOST_PARAMETER_MEMBER_FUNCTION( (
void), enableOpenMBVForce, 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))) {
116 OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
117 FArrow=ombv.createOpenMBV();
121 BOOST_PARAMETER_MEMBER_FUNCTION( (
void), enableOpenMBVMoment, 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))) {
122 OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toDoubleHead,referencePoint,scaleLength,scaleSize);
123 MArrow=ombv.createOpenMBV();
130 std::string saved_DependentBody;
132 #ifdef HAVE_OPENMBVCPPINTERFACE
133 boost::shared_ptr<OpenMBV::Arrow> FArrow, MArrow;
145 void setConstraintFunction(Function<fmatvec::VecV(
double)>* f_) {
148 f->setName(
"Constraint");
153 void updateStateDependentVariables(
double t);
154 void updateJacobians(
double t,
int j=0);
156 void initializeUsingXML(xercesc::DOMElement * element);
158 virtual std::string
getType()
const {
return "GeneralizedPositionConstraint"; }
161 Function<fmatvec::VecV(double)> *f;
175 void setGeneralConstraintFunction(Function<fmatvec::VecV(fmatvec::VecV,
double)>* f_) {
178 f->setName(
"Constraint");
180 void setTimeDependentConstraintFunction(Function<fmatvec::VecV(
double)>* f_) {
181 setGeneralConstraintFunction(
new TimeDependentFunction<fmatvec::VecV>(f_));
183 void setStateDependentConstraintFunction(Function<fmatvec::VecV(fmatvec::VecV)>* f_) {
184 setGeneralConstraintFunction(
new StateDependentFunction<fmatvec::VecV>(f_));
186 void setConstraintFunction(Function<fmatvec::VecV(fmatvec::VecV,
double)>* f_) { setGeneralConstraintFunction(f_); }
187 void setConstraintFunction(Function<fmatvec::VecV(
double)>* f_) { setTimeDependentConstraintFunction(f_); }
188 void setConstraintFunction(Function<fmatvec::VecV(fmatvec::VecV)>* f_) { setStateDependentConstraintFunction(f_); }
192 void updatexd(
double t);
193 void updateStateDependentVariables(
double t);
194 void updateJacobians(
double t,
int j=0);
196 void initializeUsingXML(xercesc::DOMElement * element);
198 virtual std::string
getType()
const {
return "GeneralizedVelocityConstraint"; }
201 Function<fmatvec::VecV(fmatvec::VecV,double)> *f;
215 void setGeneralConstraintFunction(Function<fmatvec::VecV(fmatvec::VecV,
double)>* f_) {
218 f->setName(
"Constraint");
220 void setTimeDependentConstraintFunction(Function<fmatvec::VecV(
double)>* f_) {
221 setGeneralConstraintFunction(
new TimeDependentFunction<fmatvec::VecV>(f_));
223 void setStateDependentConstraintFunction(Function<fmatvec::VecV(fmatvec::VecV)>* f_) {
224 setGeneralConstraintFunction(
new StateDependentFunction<fmatvec::VecV>(f_));
226 void setConstraintFunction(Function<fmatvec::VecV(fmatvec::VecV,
double)>* f_) { setGeneralConstraintFunction(f_); }
227 void setConstraintFunction(Function<fmatvec::VecV(
double)>* f_) { setTimeDependentConstraintFunction(f_); }
228 void setConstraintFunction(Function<fmatvec::VecV(fmatvec::VecV)>* f_) { setStateDependentConstraintFunction(f_); }
232 void updatexd(
double t);
233 void updateStateDependentVariables(
double t);
234 void updateJacobians(
double t,
int j=0);
236 void initializeUsingXML(xercesc::DOMElement * element);
238 virtual std::string
getType()
const {
return "GeneralizedAccelerationConstraint"; }
241 Function<fmatvec::VecV(fmatvec::VecV,double)> *f;
257 void setDependentBodiesFirstSide(std::vector<RigidBody*> bd);
258 void setDependentBodiesSecondSide(std::vector<RigidBody*> bd);
260 void setSecondIndependentBody(
RigidBody* bi2);
263 void setForceDirection(
const fmatvec::Mat3xV& d_);
264 void setMomentDirection(
const fmatvec::Mat3xV& d_);
272 void updateStateDependentVariables(
double t);
273 void updateJacobians(
double t,
int j=0);
274 virtual void initializeUsingXML(xercesc::DOMElement *element);
275 virtual xercesc::DOMElement* writeXMLFile(xercesc::DOMNode *element);
277 virtual std::string
getType()
const {
return "JointConstraint"; }
279 void setInitialGuess(
const fmatvec::VecV &q0_) { q0 = q0_; }
281 #ifdef HAVE_OPENMBVCPPINTERFACE
283 BOOST_PARAMETER_MEMBER_FUNCTION( (
void), enableOpenMBVForce, 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))) {
284 OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
285 FArrow=ombv.createOpenMBV();
289 BOOST_PARAMETER_MEMBER_FUNCTION( (
void), enableOpenMBVMoment, 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))) {
290 OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toDoubleHead,referencePoint,scaleLength,scaleSize);
291 MArrow=ombv.createOpenMBV();
296 class Residuum :
public Function<fmatvec::Vec(fmatvec::Vec)> {
298 std::vector<RigidBody*> body1, body2;
299 fmatvec::Mat3xV dT, dR;
300 Frame *frame1, *frame2;
302 std::vector<Frame*> i1,i2;
304 Residuum(std::vector<RigidBody*> body1_, std::vector<RigidBody*> body2_,
const fmatvec::Mat3xV &dT_,
const fmatvec::Mat3xV &dR_,
Frame *frame1_,
Frame *frame2_,
double t_,std::vector<Frame*> i1_, std::vector<Frame*> i2_);
307 std::vector<RigidBody*> bd1;
308 std::vector<RigidBody*> bd2;
310 std::vector<Frame*> if1;
311 std::vector<Frame*> if2;
313 Frame *frame1,*frame2;
321 fmatvec::Mat3xV dT, dR, forceDir, momentDir;
323 std::vector<fmatvec::Index> Iq1, Iq2, Iu1, Iu2, Ih1, Ih2;
331 std::string saved_ref1, saved_ref2;
332 std::vector<std::string> saved_RigidBodyFirstSide, saved_RigidBodySecondSide;
333 std::string saved_IndependentBody, saved_IndependentBody2;
334 #ifdef HAVE_OPENMBVCPPINTERFACE
335 boost::shared_ptr<OpenMBV::Arrow> FArrow, MArrow;
virtual void setUpInverseKinetics()
TODO.
Definition: constraint.cc:556
virtual std::string getType() const
Definition: constraint.h:277
void init(InitStage stage)
initialize object at start of simulation with respect to contours and frames
Definition: constraint.cc:86
Definition: constraint.h:204
Definition: constraint.h:137
virtual std::string getType() const
Definition: constraint.h:198
Definition: constraint.h:296
virtual void setUpInverseKinetics()
TODO.
Definition: constraint.cc:348
void init(InitStage stage)
initialize object at start of simulation with respect to contours and frames
Definition: constraint.cc:170
virtual std::string getType() const
Definition: constraint.h:158
void init(InitStage stage)
initialize object at start of simulation with respect to contours and frames
Definition: constraint.cc:385
Definition: constraint.h:55
virtual void setUpInverseKinetics()
TODO.
Definition: constraint.cc:290
Definition: constraint.h:49
Joint contraint.
Definition: constraint.h:249
virtual void setUpInverseKinetics()=0
TODO.
Frame * refFrame
frame of reference the force is defined in
Definition: constraint.h:318
void init(Element::InitStage stage)
initialize object at start of simulation with respect to contours and frames
Definition: constraint.cc:203
Definition: constraint.h:164
InitStage
The stages of the initialization.
Definition: element.h:97
void initz()
Definition: constraint.cc:435
void setUpInverseKinetics()
TODO.
Definition: constraint.cc:229
std::string name
name of element
Definition: element.h:290
Class for constraints between generalized coordinates of objects.
Definition: constraint.h:39
cartesian frame on bodies used for application of e.g. links and loads
Definition: frame.h:39
void init(InitStage stage)
initialize object at start of simulation with respect to contours and frames
Definition: constraint.cc:303
void setFrameOfReferenceID(int ID)
The frame of reference ID for the force/moment direction vectors. If ID=0 (default) the first frame...
Definition: constraint.h:269
void setUpInverseKinetics()
TODO.
Definition: constraint.cc:155
virtual std::string getType() const
Definition: constraint.h:72
Definition: constraint.h:101
virtual std::string getType() const
Definition: constraint.h:238
rigid bodies with arbitrary kinematics
Definition: rigid_body.h:53
class for all objects having own dynamics and mass
Definition: object.h:42
void init(InitStage stage)
initialize object at start of simulation with respect to contours and frames
Definition: constraint.cc:242