All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Pages
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 
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
void init(InitStage stage, const InitConfigSet &config) override
plots time series header
Definition: inverse_kinematics_constraint.cc:67
fmatvec::Vec x
order one parameters
Definition: constraint.h:60
InitStage
The stages of the initialization.
Definition: element.h:62
Definition: planar_contour.h:30
std::string name
name of element
Definition: element.h:235
Class for constraints between generalized coordinates of objects.
Definition: constraint.h:30
cartesian frame on bodies used for application of e.g. links and loads
Definition: frame.h:39
void setName(const std::string &str)
Definition: element.h:115
Inverse kinematics contraint.
Definition: inverse_kinematics_constraint.h:36
Definition: inverse_kinematics_constraint.h:82

Impressum / Disclaimer / Datenschutz Generated by doxygen 1.8.5 Home