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 #include "mbsim/utils/nonlinear_algebra.h"
26
27 namespace MBSim {
28
29 class RigidBody;
30 class Frame;
31
37 class JointConstraint : public MechanicalConstraint {
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
110 FloatingRelativeFrame C;
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
MBSim::MechanicalConstraint
Class for mechanical constraints.
Definition: mechanical_constraint.h:32
MBSim::FloatingRelativeFrame
cartesian frame on rigid bodies
Definition: floating_relative_frame.h:31
MBSim::JointConstraint::refFrame
FrameOfReference refFrame
frame of reference the force is defined in
Definition: joint_constraint.h:108
MBSim::JointConstraint::setFrameOfReference
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
MBSim::JointConstraint::Residuum
Definition: joint_constraint.h:87
MBSim::Constraint::x
fmatvec::Vec x
order one parameters
Definition: constraint.h:78
MBSim::JointConstraint
Joint contraint.
Definition: joint_constraint.h:37
MBSim::Element::InitStage
InitStage
The stages of the initialization.
Definition: element.h:62
MBSim::Function
Definition: contact_kinematics.h:32
MBSim::Element::name
std::string name
name of element
Definition: element.h:260
MBSim::Frame
cartesian frame on bodies used for application of e.g. links and loads
Definition: frame.h:39
MBSim::JointConstraint::init
void init(InitStage stage, const InitConfigSet &config) override
plots time series header
Definition: joint_constraint.cc:83
MBSim::RigidBody
rigid bodies with arbitrary kinematics
Definition: rigid_body.h:50
MBSim
namespace MBSim
Definition: bilateral_constraint.cc:30