pinocchio  2.4.0-dirty
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
model.hpp
1 //
2 // Copyright (c) 2015-2020 CNRS INRIA
3 // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4 //
5 
6 #ifndef __pinocchio_multibody_model_hpp__
7 #define __pinocchio_multibody_model_hpp__
8 
9 #include "pinocchio/spatial/fwd.hpp"
10 #include "pinocchio/spatial/se3.hpp"
11 #include "pinocchio/spatial/force.hpp"
12 #include "pinocchio/spatial/motion.hpp"
13 #include "pinocchio/spatial/inertia.hpp"
14 
15 #include "pinocchio/multibody/fwd.hpp"
16 #include "pinocchio/multibody/frame.hpp"
17 #include "pinocchio/multibody/joint/joint-generic.hpp"
18 
19 #include "pinocchio/container/aligned-vector.hpp"
20 
21 #include "pinocchio/serialization/serializable.hpp"
22 
23 #include <iostream>
24 #include <map>
25 #include <iterator>
26 
27 namespace pinocchio
28 {
29 
30  template<typename _Scalar, int _Options, template<typename,int> class JointCollectionTpl>
31  struct ModelTpl
32  : serialization::Serializable< ModelTpl<_Scalar,_Options,JointCollectionTpl> >
33  {
34  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
35 
36  typedef _Scalar Scalar;
37  enum { Options = _Options };
38 
39  typedef JointCollectionTpl<Scalar,Options> JointCollection;
40  typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
41 
42  typedef SE3Tpl<Scalar,Options> SE3;
43  typedef MotionTpl<Scalar,Options> Motion;
44  typedef ForceTpl<Scalar,Options> Force;
45  typedef InertiaTpl<Scalar,Options> Inertia;
46  typedef FrameTpl<Scalar,Options> Frame;
47 
48  typedef pinocchio::Index Index;
49  typedef pinocchio::JointIndex JointIndex;
50  typedef pinocchio::GeomIndex GeomIndex;
51  typedef pinocchio::FrameIndex FrameIndex;
52  typedef std::vector<Index> IndexVector;
53 
54  typedef JointModelTpl<Scalar,Options,JointCollectionTpl> JointModel;
55  typedef JointDataTpl<Scalar,Options,JointCollectionTpl> JointData;
56 
57  typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModel) JointModelVector;
58  typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointData) JointDataVector;
59 
60  typedef PINOCCHIO_ALIGNED_STD_VECTOR(Frame) FrameVector;
61 
62  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> VectorXs;
63  typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
64 
66  typedef VectorXs ConfigVectorType;
67 
68  typedef std::map<std::string, ConfigVectorType> ConfigVectorMap;
69 
72  typedef VectorXs TangentVectorType;
73 
75  int nq;
76 
78  int nv;
79 
81  int njoints;
82 
84  int nbodies;
85 
87  int nframes;
88 
90  PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) inertias;
91 
93  PINOCCHIO_ALIGNED_STD_VECTOR(SE3) jointPlacements;
94 
96  JointModelVector joints;
97 
99  std::vector<int> idx_qs;
100 
102  std::vector<int> nqs;
103 
105  std::vector<int> idx_vs;
106 
108  std::vector<int> nvs;
109 
111  std::vector<JointIndex> parents;
112 
114  std::vector<std::string> names;
115 
117  ConfigVectorMap referenceConfigurations;
118 
120  VectorXs rotorInertia;
121 
123  VectorXs rotorGearRatio;
124 
126  TangentVectorType effortLimit;
127 
129  TangentVectorType velocityLimit;
130 
133 
136 
138  FrameVector frames;
139 
143  std::vector<IndexVector> supports;
144 
148  std::vector<IndexVector> subtrees;
149 
151  Motion gravity;
152 
154  static const Vector3 gravity981;
155 
157  std::string name;
158 
161  : nq(0)
162  , nv(0)
163  , njoints(1)
164  , nbodies(1)
165  , nframes(0)
166  , inertias(1, Inertia::Zero())
167  , jointPlacements(1, SE3::Identity())
168  , joints(1)
169  , idx_qs(1,0)
170  , nqs(1,0)
171  , idx_vs(1,0)
172  , nvs(1,0)
173  , parents(1, 0)
174  , names(1)
175  , supports(1,IndexVector(1,0))
176  , subtrees(1)
177  , gravity(gravity981,Vector3::Zero())
178  {
179  names[0] = "universe"; // Should be "universe joint (trivial)"
180  // FIXME Should the universe joint be a FIXED_JOINT even if it is
181  // in the list of joints ? See comment in definition of
182  // Model::addJointFrame and Model::addBodyFrame
183  addFrame(Frame("universe", 0, 0, SE3::Identity(), FIXED_JOINT));
184  }
185  ~ModelTpl() {} // std::cout << "Destroy model" << std::endl; }
186 
188  template<typename NewScalar>
190  {
192  ReturnType res;
193  res.nq = nq; res.nv = nv;
194  res.njoints = njoints;
195  res.nbodies = nbodies;
196  res.nframes = nframes;
197  res.parents = parents;
198  res.names = names;
199  res.subtrees = subtrees;
200  res.gravity = gravity.template cast<NewScalar>();
201  res.name = name;
202 
203  res.idx_qs = idx_qs;
204  res.nqs = nqs;
205  res.idx_vs = idx_vs;
206  res.nvs = nvs;
207 
208  // Eigen Vectors
209  res.rotorInertia = rotorInertia.template cast<NewScalar>();
210  res.rotorGearRatio = rotorGearRatio.template cast<NewScalar>();
211  res.effortLimit = effortLimit.template cast<NewScalar>();
212  res.velocityLimit = velocityLimit.template cast<NewScalar>();
213  res.lowerPositionLimit = lowerPositionLimit.template cast<NewScalar>();
214  res.upperPositionLimit = upperPositionLimit.template cast<NewScalar>();
215 
216  typename ReturnType::ConfigVectorMap::iterator rit = res.referenceConfigurations.begin();
217  typename ConfigVectorMap::const_iterator it;
218  for (it = referenceConfigurations.begin();
219  it != referenceConfigurations.end(); it++ )
220  {
221  rit->second = it->second.template cast<NewScalar>();
222  rit++;
223  }
224 
225  // reserve vectors
226  res.inertias.resize(inertias.size());
227  res.jointPlacements.resize(jointPlacements.size());
228  res.joints.resize(joints.size());
229  res.frames.resize(frames.size());
230 
232  for(size_t k = 0; k < joints.size(); ++k)
233  {
234  res.inertias[k] = inertias[k].template cast<NewScalar>();
235  res.jointPlacements[k] = jointPlacements[k].template cast<NewScalar>();
236  res.joints[k] = joints[k].template cast<NewScalar>();
237  }
238 
239  for(size_t k = 0; k < frames.size(); ++k)
240  {
241  res.frames[k] = frames[k].template cast<NewScalar>();
242  }
243 
244  return res;
245  }
246 
252  bool operator==(const ModelTpl & other) const
253  {
254  bool res =
255  other.nq == nq
256  && other.nv == nv
257  && other.njoints == njoints
258  && other.nbodies == nbodies
259  && other.nframes == nframes
260  && other.parents == parents
261  && other.names == names
262  && other.subtrees == subtrees
263  && other.gravity == gravity
264  && other.name == name;
265 
266  res &=
267  other.idx_qs == idx_qs
268  && other.nqs == nqs
269  && other.idx_vs == idx_vs
270  && other.nvs == nvs;
271 
272  if(other.referenceConfigurations.size() != referenceConfigurations.size())
273  return false;
274 
275  typename ConfigVectorMap::const_iterator it = referenceConfigurations.begin();
276  typename ConfigVectorMap::const_iterator it_other = other.referenceConfigurations.begin();
277  for(long k = 0; k < (long)referenceConfigurations.size(); ++k)
278  {
279  std::advance(it,k); std::advance(it_other,k);
280 
281  if(it->second.size() != it_other->second.size())
282  return false;
283  if(it->second != it_other->second)
284  return false;
285  }
286 
287  if(other.rotorInertia.size() != rotorInertia.size())
288  return false;
289  res &= other.rotorInertia == rotorInertia;
290  if(!res) return res;
291 
292  if(other.rotorGearRatio.size() != rotorGearRatio.size())
293  return false;
294  res &= other.rotorGearRatio == rotorGearRatio;
295  if(!res) return res;
296 
297  if(other.effortLimit.size() != effortLimit.size())
298  return false;
299  res &= other.effortLimit == effortLimit;
300  if(!res) return res;
301 
302  if(other.velocityLimit.size() != velocityLimit.size())
303  return false;
304  res &= other.velocityLimit == velocityLimit;
305  if(!res) return res;
306 
307  if(other.lowerPositionLimit.size() != lowerPositionLimit.size())
308  return false;
309  res &= other.lowerPositionLimit == lowerPositionLimit;
310  if(!res) return res;
311 
312  if(other.upperPositionLimit.size() != upperPositionLimit.size())
313  return false;
314  res &= other.upperPositionLimit == upperPositionLimit;
315  if(!res) return res;
316 
317  for(size_t k = 1; k < inertias.size(); ++k)
318  {
319  res &= other.inertias[k] == inertias[k];
320  if(!res) return res;
321  }
322 
323  for(size_t k = 1; k < other.jointPlacements.size(); ++k)
324  {
325  res &= other.jointPlacements[k] == jointPlacements[k];
326  if(!res) return res;
327  }
328 
329  res &=
330  other.joints == joints
331  && other.frames == frames;
332 
333  return res;
334  }
335 
339  bool operator!=(const ModelTpl & other) const
340  { return !(*this == other); }
341 
364  JointIndex addJoint(const JointIndex parent,
365  const JointModel & joint_model,
366  const SE3 & joint_placement,
367  const std::string & joint_name,
368  const VectorXs & max_effort,
369  const VectorXs & max_velocity,
370  const VectorXs & min_config,
371  const VectorXs & max_config
372  );
373 
391  JointIndex addJoint(const JointIndex parent,
392  const JointModel & joint_model,
393  const SE3 & joint_placement,
394  const std::string & joint_name);
395 
405  int addJointFrame(const JointIndex & joint_index,
406  int previous_frame_index = -1);
407 
417  void appendBodyToJoint(const JointIndex joint_index, const Inertia & Y,
418  const SE3 & body_placement = SE3::Identity());
419 
431  int addBodyFrame (const std::string & body_name,
432  const JointIndex & parentJoint,
433  const SE3 & body_placement = SE3::Identity(),
434  int previousFrame = -1);
435 
447  JointIndex getBodyId(const std::string & name) const;
448 
456  bool existBodyName(const std::string & name) const;
457 
458 
469  JointIndex getJointId(const std::string & name) const;
470 
478  bool existJointName(const std::string & name) const;
479 
493  FrameIndex getFrameId(const std::string & name,
494  const FrameType & type = (FrameType) (JOINT | FIXED_JOINT | BODY | OP_FRAME | SENSOR )) const;
495 
504  bool existFrame(const std::string & name,
505  const FrameType& type = (FrameType) (JOINT | FIXED_JOINT | BODY | OP_FRAME | SENSOR )) const;
506 
515  int addFrame(const Frame & frame);
516 
527  template<typename D>
528  inline bool check(const AlgorithmCheckerBase<D> & checker = AlgorithmCheckerBase<D>()) const
529  { return checker.checkModel(*this); }
530 
532  inline bool check() const;
533 
541  inline bool check(const Data & data) const;
542 
543  protected:
544 
550  void addJointIndexToParentSubtrees(const JointIndex joint_id);
551  };
552 
553 } // namespace pinocchio
554 
555 /* --- Details -------------------------------------------------------------- */
556 /* --- Details -------------------------------------------------------------- */
557 /* --- Details -------------------------------------------------------------- */
558 #include "pinocchio/multibody/model.hxx"
559 
560 #endif // ifndef __pinocchio_multibody_model_hpp__
bool existJointName(const std::string &name) const
Check if a joint given by its name exists.
std::vector< int > idx_vs
Starting index of the joint i in the tangent configuration space.
Definition: model.hpp:105
FrameIndex getFrameId(const std::string &name, const FrameType &type=(FrameType)(JOINT|FIXED_JOINT|BODY|OP_FRAME|SENSOR)) const
Returns the index of a frame given by its name.
JointModelVector joints
Model of joint i, encapsulated in a JointModelAccessor.
Definition: model.hpp:96
int addJointFrame(const JointIndex &joint_index, int previous_frame_index=-1)
Add a joint to the frame tree.
void appendBodyToJoint(const JointIndex joint_index, const Inertia &Y, const SE3 &body_placement=SE3::Identity())
Append a body to a given joint of the kinematic tree.
ModelTpl()
Default constructor. Builds an empty model with no joints.
Definition: model.hpp:160
JointIndex getJointId(const std::string &name) const
Return the index of a joint given by its name.
VectorXs ConfigVectorType
Dense vectorized version of a joint configuration vector.
Definition: model.hpp:66
int njoints
Number of joints.
Definition: model.hpp:81
ConfigVectorMap referenceConfigurations
Map of reference configurations, indexed by user given names.
Definition: model.hpp:117
int nv
Dimension of the velocity vector space.
Definition: model.hpp:78
int addBodyFrame(const std::string &body_name, const JointIndex &parentJoint, const SE3 &body_placement=SE3::Identity(), int previousFrame=-1)
Add a body to the frame tree.
bool check() const
Run check(fusion::list) with DEFAULT_CHECKERS as argument.
std::vector< IndexVector > supports
Vector of joint supports. supports[j] corresponds to the collection of all joints located on the path...
Definition: model.hpp:143
bool existBodyName(const std::string &name) const
Check if a body given by its name exists.
Motion gravity
Spatial gravity of the model.
Definition: model.hpp:151
std::vector< std::string > names
Name of joint i
Definition: model.hpp:114
void addJointIndexToParentSubtrees(const JointIndex joint_id)
Add the joint_id to its parent subtrees.
VectorXs upperPositionLimit
Upper joint configuration limit.
Definition: model.hpp:135
FrameVector frames
Vector of operational frames registered on the model.
Definition: model.hpp:138
VectorXs rotorInertia
Vector of rotor inertia parameters.
Definition: model.hpp:120
std::vector< IndexVector > subtrees
Vector of joint subtrees. subtree[j] corresponds to the subtree supported by the joint j...
Definition: model.hpp:148
TangentVectorType effortLimit
Vector of maximal joint torques.
Definition: model.hpp:126
std::vector< int > nqs
Dimension of the joint i configuration subspace.
Definition: model.hpp:102
VectorXs lowerPositionLimit
Lower joint configuration limit.
Definition: model.hpp:132
bool operator==(const ModelTpl &other) const
Equality comparison operator.
Definition: model.hpp:252
int nframes
Number of operational frames.
Definition: model.hpp:87
VectorXs rotorGearRatio
Vector of rotor gear ratio parameters.
Definition: model.hpp:123
ModelTpl< NewScalar, Options, JointCollectionTpl > cast() const
Definition: model.hpp:189
bool operator!=(const ModelTpl &other) const
Definition: model.hpp:339
FrameType
Enum on the possible types of frame.
Definition: frame.hpp:18
JointIndex getBodyId(const std::string &name) const
Return the index of a body given by its name.
CRTP class describing the API of the checkers.
Definition: check.hpp:22
Main pinocchio namespace.
Definition: treeview.dox:24
std::string name
Model name;.
Definition: model.hpp:157
int addFrame(const Frame &frame)
Adds a frame to the kinematic tree.
bool existFrame(const std::string &name, const FrameType &type=(FrameType)(JOINT|FIXED_JOINT|BODY|OP_FRAME|SENSOR)) const
Checks if a frame given by its name exists.
std::vector< JointIndex > parents
Joint parent of joint i, denoted li (li==parents[i]).
Definition: model.hpp:111
static const Vector3 gravity981
Default 3D gravity vector (=(0,0,-9.81)).
Definition: model.hpp:154
std::vector< int > nvs
Dimension of the joint i tangent subspace.
Definition: model.hpp:108
VectorXs TangentVectorType
Dense vectorized version of a joint tangent vector (e.g. velocity, acceleration, etc). It also handles the notion of co-tangent vector (e.g. torque, etc).
Definition: model.hpp:72
TangentVectorType velocityLimit
Vector of maximal joint velocities.
Definition: model.hpp:129
bool check(const AlgorithmCheckerBase< D > &checker=AlgorithmCheckerBase< D >()) const
Check the validity of the attributes of Model with respect to the specification of some algorithms...
Definition: model.hpp:528
int nbodies
Number of bodies.
Definition: model.hpp:84
int nq
Dimension of the configuration vector representation.
Definition: model.hpp:75
std::vector< int > idx_qs
Starting index of the joint i in the configuration space.
Definition: model.hpp:99
JointIndex addJoint(const JointIndex parent, const JointModel &joint_model, const SE3 &joint_placement, const std::string &joint_name, const VectorXs &max_effort, const VectorXs &max_velocity, const VectorXs &min_config, const VectorXs &max_config)
Add a joint to the kinematic tree with given bounds.