MBSim

Project Page
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 "utils/function.h"
24 
25 namespace MBSim {
26 
27  class RigidBody;
28  class Frame;
29 
34  class Constraint : public Object {
35  private:
36 
37  public:
38  Constraint(const std::string &name);
39 #ifdef HAVE_OPENMBVCPPINTERFACE
40  virtual OpenMBV::Group* getOpenMBVGrp() {return 0;}
41 #endif
42  };
43 
49  class GearConstraint : public Constraint {
50 
51  public:
52  GearConstraint(const std::string &name, RigidBody* body);
53  GearConstraint(const std::string &name);
54 
55  void addDependency(RigidBody* body_, double ratio1, double ratio2=0);
56  void setFrame(Frame* frame_) {frame = frame_;}
57 
58  void init(InitStage stage);
59 
60  void setReferenceBody(RigidBody* body_) {bd=body_; }
61 
62  void updateStateDependentVariables(double t);
63  void updateJacobians(double t, int j=0);
64  void setUpInverseKinetics();
65 
66  void initializeUsingXML(TiXmlElement * element);
67 
68  private:
69  std::vector<RigidBody*> bi;
70  RigidBody *bd;
71  std::vector<double> ratio[2];
72  Frame* frame;
73 
74  std::string saved_ReferenceBody;
75  std::vector<std::string> saved_DependencyBodies;
76  std::vector<double> saved_ratio;
77  };
78 
84  class Constraint3 : public Constraint {
85  private:
86  RigidBody *bd;
87 
88  public:
89  Constraint3(const std::string &name, RigidBody* body);
90 
91  void updateStateDependentVariables(double t);
92  void updateJacobians(double t, int j=0);
93  };
94 
101  class JointConstraint : public Constraint {
102  public:
103  JointConstraint(const std::string &name);
104 
105  void init(InitStage stage);
106  void initz();
107 
108  void connect(Frame* frame1, Frame* frame2);
109  void setDependentBodiesFirstSide(std::vector<RigidBody*> bd);
110  void setDependentBodiesSecondSide(std::vector<RigidBody*> bd);
111  void setIndependentBody(RigidBody* bi);
112 
113  virtual void setUpInverseKinetics();
114  void setForceDirection(const fmatvec::Mat& d_) {dT = d_;}
115  void setMomentDirection(const fmatvec::Mat& d_) {dR = d_;}
116  void setq0(const fmatvec::Vec& q0_) {q0 = q0_;}
117 
118  fmatvec::Vec res(const fmatvec::Vec& q, const double& t);
119  void updateStateDependentVariables(double t);
120  void updateJacobians(double t, int j=0);
121  virtual void initializeUsingXML(TiXmlElement *element);
122 
123  private:
124  class Residuum : public Function1<fmatvec::Vec,fmatvec::Vec> {
125  private:
126  std::vector<RigidBody*> body1, body2;
127  fmatvec::Mat dT, dR;
128  Frame *frame1, *frame2;
129  double t;
130  std::vector<int> i1,i2;
131  public:
132  Residuum(std::vector<RigidBody*> body1_, std::vector<RigidBody*> body2_, const fmatvec::Mat &dT_, const fmatvec::Mat &dR_,Frame *frame1_, Frame *frame2_,double t_,std::vector<int> i1_, std::vector<int> i2_);
133  fmatvec::Vec operator()(const fmatvec::Vec &x, const void * =NULL);
134  };
135  std::vector<RigidBody*> bd1;
136  std::vector<RigidBody*> bd2;
137  RigidBody *bi;
138  std::vector<int> if1;
139  std::vector<int> if2;
140 
141  Frame *frame1,*frame2;
142 
143  fmatvec::Mat dT;
144  fmatvec::Mat dR;
145 
146  std::vector<fmatvec::Index> Iq1, Iq2, Iu1, Iu2, Ih1, Ih2;
147  int nq, nu, nh;
148  fmatvec::Vec q, u;
149  fmatvec::Mat J;
150  fmatvec::Vec j;
151  fmatvec::Mat JT, JR;
152  fmatvec::Vec q0;
153 
154  std::string saved_ref1, saved_ref2;
155  std::vector<std::string> saved_RigidBodyFirstSide, saved_RigidBodySecondSide;
156  std::string saved_IndependentBody;
157  };
158 
159 }
160 
161 #endif