mbsim 4.0.0 MBSim Kernel

inverse_kinematics_constraint.h
1 /* Copyright (C) 2004-2019 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 _INVERSE_KINEMATICS_CONSTRAINT_H
20 #define _INVERSE_KINEMATICS_CONSTRAINT_H
21
22 #include "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 class MechanicalLink;
31
36 class InverseKinematicsConstraint : public Constraint {
37 public:
38 enum Kinematics {
39 planar=0,
40 spatial,
41 unknown
42 };
43
44 InverseKinematicsConstraint(const std::string &name="") : Constraint(name) { }
45 ~InverseKinematicsConstraint() override;
46
47 void init(InitStage stage, const InitConfigSet &config) override;
48
49 void resetUpToDate() override;
50
51 void setKinematics(Kinematics kinematics_) { kinematics = kinematics_; }
52 void setFrame(Frame* frame_) { frame = frame_; }
53
54 void setTranslation(Function<fmatvec::Vec3(double)> *fr_) {
55 fr = fr_;
56 fr->setParent(this);
57 fr->setName("Translation");
58 }
59 void setRotation(Function<fmatvec::RotMat3(double)>* fA_) {
60 fA = fA_;
61 fA->setParent(this);
62 fA->setName("Rotation");
63 }
64 void setUpInverseKinetics() override;
65
66 void updateGeneralizedCoordinates() override;
67 void updateGeneralizedJacobians(int jj=0) override;
68 void updateA();
69
70 const fmatvec::SqrMat& evalA() { if(updA) updateA(); return A; }
71
72 fmatvec::SqrMat& getA(bool check=true) { assert((not check) or (not updA)); return A; }
73
74 void setInitialGuess(const fmatvec::VecV &q0_) { q0 <<= q0_; }
75
76 void initializeUsingXML(xercesc::DOMElement *element) override;
77
78 MechanicalLink* getMechanicalLink(int i) { return link[i]; }
79 int getNumberOfMechanicalLinks() const { return link.size(); }
80
81 private:
82 class Residuum : public Function<fmatvec::Vec(fmatvec::Vec)> {
83 private:
84 fmatvec::Vec3 r;
85 fmatvec::SqrMat3 A;
86 std::vector<RigidBody*> body;
87 fmatvec::Mat3xV forceDir, momentDir;
88 Frame* frame;
89 public:
90 Residuum(const fmatvec::Vec3 &r_, const fmatvec::RotMat3 &A_, std::vector<RigidBody*> body_, const fmatvec::Mat3xV &forceDir_, const fmatvec::Mat3xV &momentDir_, Frame* frame_);
91
92 fmatvec::Vec operator()(const fmatvec::Vec &x) override;
93 };
94 std::vector<RigidBody*> bd;
95
96 Kinematics kinematics{spatial};
97 Frame* frame{nullptr};
98
99 Function<fmatvec::Vec3(double)> *fr{nullptr};
100 Function<fmatvec::RotMat3(double)>* fA{nullptr};
101
102 fmatvec::Mat3xV DF, DM, forceDir, momentDir;
103
104 fmatvec::RangeV iF, iM;
105 std::vector<fmatvec::RangeV> Iq, Iu, Ih;
106 int nq{0};
107 int nu{0};
108 int nh{0};
109 fmatvec::Vec q, q0;
110 fmatvec::Mat JT, JR;
111 fmatvec::SqrMat A;
112
113 std::vector<MechanicalLink*> link;
114
115 bool updA{true};
116
117 std::string saved_ref;
118 };
119
120 }
121
122 #endif
MBSim::InverseKinematicsConstraint::init
void init(InitStage stage, const InitConfigSet &config) override
plots time series header
Definition: inverse_kinematics_constraint.cc:68
MBSim::Constraint::x
fmatvec::Vec x
order one parameters
Definition: constraint.h:78
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::Constraint
Class for constraints between generalized coordinates of objects.
Definition: constraint.h:30
MBSim::Frame
cartesian frame on bodies used for application of e.g. links and loads
Definition: frame.h:39
MBSim::Element::setName
void setName(const std::string &str)
Definition: element.h:115
MBSim::MechanicalLink
mechanical link
Definition: mechanical_link.h:33
MBSim::InverseKinematicsConstraint
Inverse kinematics contraint.
Definition: inverse_kinematics_constraint.h:36
MBSim::InverseKinematicsConstraint::Residuum
Definition: inverse_kinematics_constraint.h:82
MBSim
namespace MBSim
Definition: bilateral_constraint.cc:30