mbsim  4.0.0
MBSim Kernel
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 "mechanical_constraint.h"
23 #include "mbsim/functions/function.h"
24 #include "mbsim/frames/floating_relative_frame.h"
25 
26 namespace MBSim {
27 
28  class RigidBody;
29  class Frame;
30 
37  public:
38  enum FrameOfReference {
39  firstFrame=0,
40  secondFrame,
41  unknown
42  };
43 
44  JointConstraint(const std::string &name="");
45 
46  void calcisSize() override;
47 
48  void init(InitStage stage, const InitConfigSet &config) override;
49 
50  void resetUpToDate() override;
51 
52  void connect(Frame* frame0, Frame* frame1) { frame[0] = frame0; frame[1] = frame1; }
53  void addDependentRigidBodyOnFirstSide(RigidBody* bd) { bd1.push_back(bd); }
54  void addDependentRigidBodyOnSecondSide(RigidBody* bd) { bd2.push_back(bd); }
55  void setIndependentRigidBody(RigidBody* bi_) { bi.resize(1); bi[0] = bi_; }
56  void addIndependentRigidBody(RigidBody* bi_) { bi.push_back(bi_); }
57 
58  void setUpInverseKinetics() override;
59  void setForceDirection(const fmatvec::Mat3xV& fd) { forceDir <<= fd; }
60  void setMomentDirection(const fmatvec::Mat3xV& md) { momentDir <<= md; }
61 
65  void setFrameOfReference(FrameOfReference refFrame_) { refFrame = refFrame_; }
66 
67  void updatePositions(Frame *frame) override;
68  void updateGeneralizedCoordinates() override;
69  void updateGeneralizedJacobians(int jj=0) override;
70  void updateForceDirections();
71  void updateA();
72 
73  const fmatvec::Mat3xV& evalGlobalForceDirection() { if(updDF) updateForceDirections(); return DF; }
74  const fmatvec::Mat3xV& evalGlobalMomentDirection() { if(updDF) updateForceDirections(); return DM; }
75  const fmatvec::SqrMat& evalA() { if(updA) updateA(); return A; }
76 
77  fmatvec::Mat3xV& getGlobalForceDirection(bool check=true) { assert((not check) or (not updDF)); return DF; }
78  fmatvec::Mat3xV& getGlobalMomentDirection(bool check=true) { assert((not check) or (not updDF)); return DM; }
79  fmatvec::SqrMat& getA(bool check=true) { assert((not check) or (not updA)); return A; }
80 
81  void setInitialGuess(const fmatvec::VecV &q0_) { q0 <<= q0_; }
82 
83  void initializeUsingXML(xercesc::DOMElement *element) override;
84 
85  private:
86  class Residuum : public Function<fmatvec::Vec(fmatvec::Vec)> {
87  private:
88  std::vector<RigidBody*> body1, body2;
89  fmatvec::Mat3xV forceDir, momentDir;
90  std::vector<Frame*> frame;
91  FrameOfReference refFrame;
92  public:
93  Residuum(std::vector<RigidBody*> body1_, std::vector<RigidBody*> body2_, const fmatvec::Mat3xV &forceDir_, const fmatvec::Mat3xV &momentDir_, const std::vector<Frame*> frame_, FrameOfReference refFrame_);
94 
95  fmatvec::Vec operator()(const fmatvec::Vec &x) override;
96  };
97  std::vector<RigidBody*> bd1, bd2, bi;
98 
99  std::vector<Frame*> frame;
100 
104  FrameOfReference refFrame{firstFrame};
105 
107 
108  fmatvec::Mat3xV DF, DM, forceDir, momentDir;
109 
110  fmatvec::RangeV iF, iM;
111  std::vector<fmatvec::RangeV> Iq1, Iq2, Iu1, Iu2, Ih1, Ih2;
112  int nq{0};
113  int nu{0};
114  int nh{0};
115  fmatvec::Vec q0;
116  fmatvec::Mat JT, JR;
117  fmatvec::SqrMat A;
118 
119  bool updDF{true};
120  bool updA{true};
121 
122  std::string saved_ref1, saved_ref2;
123  std::vector<std::string> saved_RigidBodyFirstSide, saved_RigidBodySecondSide, saved_IndependentBody;
124  };
125 
126 }
127 
128 #endif
Class for mechanical constraints.
Definition: mechanical_constraint.h:32
cartesian frame on rigid bodies
Definition: floating_relative_frame.h:31
FrameOfReference refFrame
frame of reference the force is defined in
Definition: joint_constraint.h:104
void setFrameOfReference(FrameOfReference refFrame_)
The frame of reference for the force/moment direction vectors. If firstFrame (default) the first fram...
Definition: joint_constraint.h:65
Definition: joint_constraint.h:86
fmatvec::Vec x
order one parameters
Definition: constraint.h:78
Joint contraint.
Definition: joint_constraint.h:36
InitStage
The stages of the initialization.
Definition: element.h:64
Definition: contact_kinematics.h:32
std::string name
name of element
Definition: element.h:262
cartesian frame on bodies used for application of e.g. links and loads
Definition: frame.h:39
void init(InitStage stage, const InitConfigSet &config) override
plots time series header
Definition: joint_constraint.cc:84
rigid bodies with arbitrary kinematics
Definition: rigid_body.h:50
namespace MBSim
Definition: bilateral_constraint.cc:29