All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Pages
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 _CONSTRAINT_H
20 #define _CONSTRAINT_H
21 
22 #include "object.h"
23 #include "functions/auxiliary_functions.h"
24 
25 #ifdef HAVE_OPENMBVCPPINTERFACE
26 #include "mbsim/utils/boost_parameters.h"
27 #include "mbsim/utils/openmbv_utils.h"
28 #endif
29 
30 namespace MBSim {
31 
32  class RigidBody;
33  class Frame;
34 
39  class Constraint : public Object {
40  private:
41 
42  public:
43  Constraint(const std::string &name);
44 #ifdef HAVE_OPENMBVCPPINTERFACE
45  virtual boost::shared_ptr<OpenMBV::Group> getOpenMBVGrp() {return boost::shared_ptr<OpenMBV::Group>();}
46 #endif
47  };
48 
49  struct Transmission {
50  Transmission(RigidBody *body_, double ratio_) : body(body_), ratio(ratio_) { }
51  RigidBody *body;
52  double ratio;
53  };
54 
55  class GearConstraint : public Constraint {
56 
57  public:
58  GearConstraint(const std::string &name="");
59 
60  void addTransmission(const Transmission &transmission);
61 
62  void init(InitStage stage);
63 
64  void setDependentBody(RigidBody* body_) {bd=body_; }
65 
66  void updateStateDependentVariables(double t);
67  void updateJacobians(double t, int j=0);
68  void setUpInverseKinetics();
69 
70  void initializeUsingXML(xercesc::DOMElement * element);
71 
72  virtual std::string getType() const { return "GearConstraint"; }
73 
74 #ifdef HAVE_OPENMBVCPPINTERFACE
75 
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();
79  }
80 
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();
85  }
86 #endif
87 
88  private:
89  std::vector<RigidBody*> bi;
90  RigidBody *bd;
91  std::vector<double> ratio;
92 
93  std::string saved_DependentBody;
94  std::vector<std::string> saved_IndependentBody;
95 
96 #ifdef HAVE_OPENMBVCPPINTERFACE
97  boost::shared_ptr<OpenMBV::Arrow> FArrow, MArrow;
98 #endif
99  };
100 
102 
103  public:
104  KinematicConstraint(const std::string &name="");
105 
106  void setDependentBody(RigidBody* body) {bd=body; }
107  virtual void setUpInverseKinetics() = 0;
108 
109  void init(InitStage stage);
110 
111  void initializeUsingXML(xercesc::DOMElement * element);
112 
113 #ifdef HAVE_OPENMBVCPPINTERFACE
114 
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();
118  }
119 
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();
124  }
125 #endif
126 
127  protected:
128  RigidBody *bd;
129 
130  std::string saved_DependentBody;
131 
132 #ifdef HAVE_OPENMBVCPPINTERFACE
133  boost::shared_ptr<OpenMBV::Arrow> FArrow, MArrow;
134 #endif
135  };
136 
138 
139  public:
140  GeneralizedPositionConstraint(const std::string &name="") : KinematicConstraint(name), f(NULL) {}
141  ~GeneralizedPositionConstraint() { delete f; }
142 
143  void init(Element::InitStage stage);
144 
145  void setConstraintFunction(Function<fmatvec::VecV(double)>* f_) {
146  f = f_;
147  f->setParent(this);
148  f->setName("Constraint");
149  }
150 
151  void setUpInverseKinetics();
152 
153  void updateStateDependentVariables(double t);
154  void updateJacobians(double t, int j=0);
155 
156  void initializeUsingXML(xercesc::DOMElement * element);
157 
158  virtual std::string getType() const { return "GeneralizedPositionConstraint"; }
159 
160  private:
161  Function<fmatvec::VecV(double)> *f;
162  };
163 
165 
166  public:
167  GeneralizedVelocityConstraint(const std::string &name="") : KinematicConstraint(name), f(NULL) {}
168  ~GeneralizedVelocityConstraint() { delete f; }
169 
170  void init(InitStage stage);
171 
172  void calcxSize();
173 
174  // NOTE: we can not use a overloaded setConstraintFunction here due to restrictions in XML but define them for convinience in c++
175  void setGeneralConstraintFunction(Function<fmatvec::VecV(fmatvec::VecV,double)>* f_) {
176  f = f_;
177  f->setParent(this);
178  f->setName("Constraint");
179  }
180  void setTimeDependentConstraintFunction(Function<fmatvec::VecV(double)>* f_) {
181  setGeneralConstraintFunction(new TimeDependentFunction<fmatvec::VecV>(f_));
182  }
183  void setStateDependentConstraintFunction(Function<fmatvec::VecV(fmatvec::VecV)>* f_) {
184  setGeneralConstraintFunction(new StateDependentFunction<fmatvec::VecV>(f_));
185  }
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_); }
189 
190  virtual void setUpInverseKinetics();
191 
192  void updatexd(double t);
193  void updateStateDependentVariables(double t);
194  void updateJacobians(double t, int j=0);
195 
196  void initializeUsingXML(xercesc::DOMElement * element);
197 
198  virtual std::string getType() const { return "GeneralizedVelocityConstraint"; }
199 
200  private:
201  Function<fmatvec::VecV(fmatvec::VecV,double)> *f;
202  };
203 
205 
206  public:
208  ~GeneralizedAccelerationConstraint() { delete f; }
209 
210  void init(InitStage stage);
211 
212  void calcxSize();
213 
214  // NOTE: we can not use a overloaded setConstraintFunction here due to restrictions in XML but define them for convinience in c++
215  void setGeneralConstraintFunction(Function<fmatvec::VecV(fmatvec::VecV,double)>* f_) {
216  f = f_;
217  f->setParent(this);
218  f->setName("Constraint");
219  }
220  void setTimeDependentConstraintFunction(Function<fmatvec::VecV(double)>* f_) {
221  setGeneralConstraintFunction(new TimeDependentFunction<fmatvec::VecV>(f_));
222  }
223  void setStateDependentConstraintFunction(Function<fmatvec::VecV(fmatvec::VecV)>* f_) {
224  setGeneralConstraintFunction( new StateDependentFunction<fmatvec::VecV>(f_));
225  }
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_); }
229 
230  virtual void setUpInverseKinetics();
231 
232  void updatexd(double t);
233  void updateStateDependentVariables(double t);
234  void updateJacobians(double t, int j=0);
235 
236  void initializeUsingXML(xercesc::DOMElement * element);
237 
238  virtual std::string getType() const { return "GeneralizedAccelerationConstraint"; }
239 
240  private:
241  Function<fmatvec::VecV(fmatvec::VecV,double)> *f;
242  };
243 
249  class JointConstraint : public Constraint {
250  public:
251  JointConstraint(const std::string &name="");
252 
253  void init(InitStage stage);
254  void initz();
255 
256  void connect(Frame* frame1, Frame* frame2);
257  void setDependentBodiesFirstSide(std::vector<RigidBody*> bd);
258  void setDependentBodiesSecondSide(std::vector<RigidBody*> bd);
259  void setIndependentBody(RigidBody* bi);
260  void setSecondIndependentBody(RigidBody* bi2);
261 
262  virtual void setUpInverseKinetics();
263  void setForceDirection(const fmatvec::Mat3xV& d_);
264  void setMomentDirection(const fmatvec::Mat3xV& d_);
265 
269  void setFrameOfReferenceID(int ID) { refFrameID=ID; }
270 
271  fmatvec::Vec res(const fmatvec::Vec& q, const double& t);
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);
276 
277  virtual std::string getType() const { return "JointConstraint"; }
278 
279  void setInitialGuess(const fmatvec::VecV &q0_) { q0 = q0_; }
280 
281 #ifdef HAVE_OPENMBVCPPINTERFACE
282 
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();
286  }
287 
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();
292  }
293 #endif
294 
295  private:
296  class Residuum : public Function<fmatvec::Vec(fmatvec::Vec)> {
297  private:
298  std::vector<RigidBody*> body1, body2;
299  fmatvec::Mat3xV dT, dR;
300  Frame *frame1, *frame2;
301  double t;
302  std::vector<Frame*> i1,i2;
303  public:
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_);
305  fmatvec::Vec operator()(const fmatvec::Vec &x);
306  };
307  std::vector<RigidBody*> bd1;
308  std::vector<RigidBody*> bd2;
309  RigidBody *bi, *bi2;
310  std::vector<Frame*> if1;
311  std::vector<Frame*> if2;
312 
313  Frame *frame1,*frame2;
314 
319  int refFrameID;
320 
321  fmatvec::Mat3xV dT, dR, forceDir, momentDir;
322 
323  std::vector<fmatvec::Index> Iq1, Iq2, Iu1, Iu2, Ih1, Ih2;
324  int nq, nu, nh;
325  fmatvec::Vec q, u;
326  fmatvec::Mat J;
327  fmatvec::Vec j;
328  fmatvec::Mat JT, JR;
329  fmatvec::Vec q0;
330 
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;
336 #endif
337  };
338 
339 }
340 
341 #endif
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

Impressum / Disclaimer / Datenschutz Generated by doxygen 1.8.5 Valid HTML