All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Pages
joint_constraint.h
1 /* Copyright (C) 2004-2009 MBSim Development Team
2  * This library is free software; you can redistribute it and/or
3  * modify it under the terms of the GNU Lesser General Public
4  * License as published by the Free Software Foundation; either
5  * version 2.1 of the License, or (at your option) any later version.
6  *
7  * This library is distributed in the hope that it will be useful,
8  * but WITHOUT ANY WARRANTY; without even the implied warranty of
9  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
10  * Lesser General Public License for more details.
11  *
12  * You should have received a copy of the GNU Lesser General Public
13  * License along with this library; if not, write to the Free Software
14  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
15  *
16  * Contact: martin.o.foerg@googlemail.com
17  */
18 
19 #ifndef _JOINT_CONSTRAINT_H
20 #define _JOINT_CONSTRAINT_H
21 
22 #include "mbsim/constraints/constraint.h"
23 #include "mbsim/functions/function.h"
24 #include "mbsim/frames/floating_relative_frame.h"
25 
26 #include "mbsim/utils/boost_parameters.h"
27 #include "mbsim/utils/openmbv_utils.h"
28 
29 namespace MBSim {
30 
31  class RigidBody;
32  class Frame;
33 
39  class JointConstraint : public Constraint {
40  public:
41  JointConstraint(const std::string &name="");
42 
43  void init(InitStage stage);
44  void initz();
45 
46  void resetUpToDate();
47 
48  void connect(Frame* frame1_, Frame* frame2_) { frame1 = frame1_; frame2 = frame2_; }
49  void addDependentRigidBodyOnFirstSide(RigidBody* bd) { bd1.push_back(bd); }
50  void addDependentRigidBodyOnSecondSide(RigidBody* bd) { bd2.push_back(bd); }
51  void setIndependentRigidBody(RigidBody* bi_) { bi.resize(1); bi[0] = bi_; }
52  void addIndependentRigidBody(RigidBody* bi_) { bi.push_back(bi_); }
53 
54  virtual void setUpInverseKinetics();
55  void setForceDirection(const fmatvec::Mat3xV& d_);
56  void setMomentDirection(const fmatvec::Mat3xV& d_);
57 
61  void setFrameOfReferenceID(int ID) { refFrameID=ID; }
62 
63  void updatePositions(Frame *frame_);
64  void updateGeneralizedCoordinates();
65  void updateGeneralizedJacobians(int j=0);
66  virtual void initializeUsingXML(xercesc::DOMElement *element);
67 
68  virtual std::string getType() const { return "JointConstraint"; }
69 
70  void setInitialGuess(const fmatvec::VecV &q0_) { q0 = q0_; }
71 
73  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))) {
74  OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
75  FArrow=ombv.createOpenMBV();
76  }
77 
79  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))) {
80  OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toDoubleHead,referencePoint,scaleLength,scaleSize);
81  MArrow=ombv.createOpenMBV();
82  }
83 
84  private:
85  class Residuum : public Function<fmatvec::Vec(fmatvec::Vec)> {
86  private:
87  std::vector<RigidBody*> body1, body2;
88  fmatvec::Mat3xV forceDir, momentDir;
89  Frame *frame1, *frame2, *refFrame;
90  std::vector<Frame*> i1,i2;
91  public:
92  Residuum(std::vector<RigidBody*> body1_, std::vector<RigidBody*> body2_, const fmatvec::Mat3xV &forceDir_, const fmatvec::Mat3xV &momentDir_, Frame *frame1_, Frame *frame2_, Frame *refFrame, std::vector<Frame*> i1_, std::vector<Frame*> i2_);
93  fmatvec::Vec operator()(const fmatvec::Vec &x);
94  };
95  std::vector<RigidBody*> bd1, bd2, bi;
96  std::vector<Frame*> if1, if2;
97 
98  Frame *frame1, *frame2;
99 
104  int refFrameID;
105 
107 
108  fmatvec::Mat3xV dT, dR, forceDir, momentDir;
109 
110  std::vector<fmatvec::RangeV> Iq1, Iq2, Iu1, Iu2, Ih1, Ih2;
111  int nq, nu, nh;
112  fmatvec::Vec q, q0;
113  fmatvec::Mat JT, JR;
114 
115  std::string saved_ref1, saved_ref2;
116  std::vector<std::string> saved_RigidBodyFirstSide, saved_RigidBodySecondSide, saved_IndependentBody;
117  std::shared_ptr<OpenMBV::Arrow> FArrow, MArrow;
118  };
119 
120 }
121 
122 #endif
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)))
Visualize a force arrow acting on frame2.
Definition: joint_constraint.h:73
virtual std::string getType() const
Definition: joint_constraint.h:68
Definition: openmbv_utils.h:54
cartesian frame on rigid bodies
Definition: floating_relative_frame.h:31
Definition: joint_constraint.h:85
void init(InitStage stage)
plots time series header
Definition: joint_constraint.cc:74
fmatvec::Vec x
order one parameters
Definition: constraint.h:66
Joint contraint.
Definition: joint_constraint.h:39
Frame * refFrame
frame of reference the force is defined in
Definition: joint_constraint.h:103
InitStage
The stages of the initialization.
Definition: element.h:97
Definition: planar_contour.h:31
std::string name
name of element
Definition: element.h:298
Class for constraints between generalized coordinates of objects.
Definition: constraint.h:30
cartesian frame on bodies used for application of e.g. links and loads
Definition: frame.h:37
void setFrameOfReferenceID(int ID)
The frame of reference ID for the force/moment direction vectors. If ID=0 (default) the first frame...
Definition: joint_constraint.h:61
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)))
Visualize a moment arrow.
Definition: joint_constraint.h:79
rigid bodies with arbitrary kinematics
Definition: rigid_body.h:52

Impressum / Disclaimer / Datenschutz Generated by doxygen 1.8.5 Valid HTML