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