MBSim

Project Page
joint.h
1 /* Copyright (C) 2004-2009 MBSim Development Team
2  *
3  * This library is free software; you can redistribute it and/or
4  * modify it under the terms of the GNU Lesser General Public
5  * License as published by the Free Software Foundation; either
6  * version 2.1 of the License, or (at your option) any later version.
7  *
8  * This library is distributed in the hope that it will be useful,
9  * but WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
11  * Lesser General Public License for more details.
12  *
13  * You should have received a copy of the GNU Lesser General Public
14  * License along with this library; if not, write to the Free Software
15  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
16  *
17  * Contact: martin.o.foerg@googlemail.com
18  */
19 
20 #ifndef _JOINT_H_
21 #define _JOINT_H_
22 
23 #include "mbsim/link_mechanics.h"
24 #include "mbsim/frame.h"
25 #include "mbsim/kinematics.h"
26 
27 namespace MBSim {
28 
29  class GeneralizedForceLaw;
30  class GeneralizedImpactLaw;
31  class FrictionForceLaw;
32  class FrictionImpactLaw;
33  class RigidBody;
34 
43  class Joint: public LinkMechanics {
44  public:
49  Joint(const std::string &name);
50 
54  virtual ~Joint();
55 
56  /* INHERITED INTERFACE OF LINKINTERFACE */
57  virtual void updatewb(double t, int i=0);
58  virtual void updateW(double t, int i=0);
59  virtual void updateh(double t, int i=0);
60  virtual void updateg(double t);
61  virtual void updategd(double t);
62  virtual void updateJacobians(double t, int j=0);
63  /***************************************************/
64 
65  /* INHERITED INTERFACE OF EXTRADYNAMICINTERFACE */
66  virtual void updatexd(double t);
67  virtual void updatedx(double t, double dt);
68  virtual void calcxSize();
69  virtual void init(InitStage stage);
70  /***************************************************/
71 
72  /* INHERITED INTERFACE OF LINK */
73  virtual void calclaSize(int j);
74  virtual void calcgSize(int j);
75  virtual void calcgdSize(int j);
76  virtual void calcrFactorSize(int j);
77  virtual bool isSetValued() const;
78  virtual bool isActive() const { return true; }
79  virtual bool gActiveChanged() { return false; }
80  virtual void solveImpactsFixpointSingle(double dt);
81  virtual void solveConstraintsFixpointSingle();
82  virtual void solveImpactsGaussSeidel(double dt);
83  virtual void solveConstraintsGaussSeidel();
84  virtual void solveImpactsRootFinding(double dt);
85  virtual void solveConstraintsRootFinding();
86  virtual void jacobianConstraints();
87  virtual void jacobianImpacts();
88  virtual void updaterFactors();
89  virtual void checkImpactsForTermination(double dt);
90  virtual void checkConstraintsForTermination();
91  /***************************************************/
92 
93  /* INHERITED INTERFACE OF ELEMENT */
94  virtual void plot(double t, double dt = 1);
95  /***************************************************/
96 
97  /* INTERFACE FOR DERIVED CLASSES */
102  virtual void connect(Frame *frame1, Frame* frame2);
103  /***************************************************/
104 
105  /* GETTER / SETTER */
106  void setForceLaw(GeneralizedForceLaw * rc) { ffl = rc; }
107  void setMomentLaw(GeneralizedForceLaw * rc) { fml = rc; }
108  void setImpactForceLaw(GeneralizedImpactLaw * rc) { fifl = rc; }
109  void setImpactMomentLaw(GeneralizedImpactLaw * rc) { fiml = rc; }
110  /***************************************************/
111 
115  void setForceDirection(const fmatvec::Mat& fd);
116 
120  void setMomentDirection(const fmatvec::Mat& md);
121 
122  virtual void initializeUsingXML(TiXmlElement *element);
123 
124 #ifdef HAVE_OPENMBVCPPINTERFACE
125 
126  void setOpenMBVForceArrow(OpenMBV::Arrow *arrow) {
127  std::vector<bool> which; which.resize(2, false);
128  which[1]=true;
129  LinkMechanics::setOpenMBVForceArrow(arrow, which);
130  }
131 
133  void setOpenMBVMomentArrow(OpenMBV::Arrow *arrow) {
134  std::vector<bool> which; which.resize(2, false);
135  which[1]=true;
136  LinkMechanics::setOpenMBVMomentArrow(arrow, which);
137  }
138 #endif
139 
140  protected:
144  fmatvec::Index IT, IR;
145 
149  fmatvec::Mat forceDir, momentDir;
150 
154  fmatvec::Mat Wf, Wm;
155 
159  fmatvec::Mat JT;
160 
164  fmatvec::Vec WrP0P1, WvP0P1, WomP0P1;
165 
170  GeneralizedImpactLaw *fifl, *fiml;
171 
175  fmatvec::Vec gdn, gdd;
176 
181 
182  private:
183  std::string saved_ref1, saved_ref2;
184  };
185 
186  class InverseKineticsJoint: public Joint {
187  public:
188  InverseKineticsJoint(const std::string &name);
189  virtual void updateb(double t);
190  void calcbSize();
191  void setBody(RigidBody* body_) { body = body_; }
192 
193  protected:
194  RigidBody* body;
195  };
196 
197 }
198 
199 #endif
200