src/multibody/model.hpp
Go to the documentation of this file.
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;
41 
47 
52  typedef std::vector<Index> IndexVector;
53 
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 
69  typedef std::map<std::string, ConfigVectorType> ConfigVectorMap;
70 
73  typedef VectorXs TangentVectorType;
74 
76  int nq;
77 
79  int nv;
80 
82  int njoints;
83 
85  int nbodies;
86 
88  int nframes;
89 
91  PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) inertias;
92 
94  PINOCCHIO_ALIGNED_STD_VECTOR(SE3) jointPlacements;
95 
97  JointModelVector joints;
98 
100  std::vector<int> idx_qs;
101 
103  std::vector<int> nqs;
104 
106  std::vector<int> idx_vs;
107 
109  std::vector<int> nvs;
110 
112  std::vector<JointIndex> parents;
113 
115  std::vector<std::string> names;
116 
118  ConfigVectorMap referenceConfigurations;
119 
121  TangentVectorType rotorInertia;
122 
124  TangentVectorType rotorGearRatio;
125 
127  TangentVectorType friction;
128 
130  TangentVectorType damping;
131 
133  TangentVectorType effortLimit;
134 
136  TangentVectorType velocityLimit;
137 
139  ConfigVectorType lowerPositionLimit;
140 
142  ConfigVectorType upperPositionLimit;
143 
145  FrameVector frames;
146 
150  std::vector<IndexVector> supports;
151 
155  std::vector<IndexVector> subtrees;
156 
158  Motion gravity;
159 
161  static const Vector3 gravity981;
162 
164  std::string name;
165 
168  : nq(0)
169  , nv(0)
170  , njoints(1)
171  , nbodies(1)
172  , nframes(0)
173  , inertias(1, Inertia::Zero())
174  , jointPlacements(1, SE3::Identity())
175  , joints(1)
176  , idx_qs(1,0)
177  , nqs(1,0)
178  , idx_vs(1,0)
179  , nvs(1,0)
180  , parents(1, 0)
181  , names(1)
182  , supports(1,IndexVector(1,0))
183  , subtrees(1)
184  , gravity(gravity981,Vector3::Zero())
185  {
186  names[0] = "universe"; // Should be "universe joint (trivial)"
187  // FIXME Should the universe joint be a FIXED_JOINT even if it is
188  // in the list of joints ? See comment in definition of
189  // Model::addJointFrame and Model::addBodyFrame
190  addFrame(Frame("universe", 0, 0, SE3::Identity(), FIXED_JOINT));
191  }
192  ~ModelTpl() {} // std::cout << "Destroy model" << std::endl; }
193 
195  template<typename NewScalar>
197  {
199  ReturnType res;
200  res.nq = nq; res.nv = nv;
201  res.njoints = njoints;
202  res.nbodies = nbodies;
203  res.nframes = nframes;
204  res.parents = parents;
205  res.names = names;
206  res.supports = supports;
207  res.subtrees = subtrees;
208  res.gravity = gravity.template cast<NewScalar>();
209  res.name = name;
210 
211  res.idx_qs = idx_qs;
212  res.nqs = nqs;
213  res.idx_vs = idx_vs;
214  res.nvs = nvs;
215 
216  // Eigen Vectors
217  res.rotorInertia = rotorInertia.template cast<NewScalar>();
218  res.rotorGearRatio = rotorGearRatio.template cast<NewScalar>();
219  res.friction = friction.template cast<NewScalar>();
220  res.damping = damping.template cast<NewScalar>();
221  res.effortLimit = effortLimit.template cast<NewScalar>();
222  res.velocityLimit = velocityLimit.template cast<NewScalar>();
223  res.lowerPositionLimit = lowerPositionLimit.template cast<NewScalar>();
224  res.upperPositionLimit = upperPositionLimit.template cast<NewScalar>();
225 
226  typename ConfigVectorMap::const_iterator it;
227  for (it = referenceConfigurations.begin();
228  it != referenceConfigurations.end(); it++ )
229  {
230  res.referenceConfigurations.insert(std::make_pair(it->first, it->second.template cast<NewScalar>()));
231  }
232 
233  // reserve vectors
234  res.inertias.resize(inertias.size());
235  res.jointPlacements.resize(jointPlacements.size());
236  res.joints.resize(joints.size());
237  res.frames.resize(frames.size());
238 
240  for(size_t k = 0; k < joints.size(); ++k)
241  {
242  res.inertias[k] = inertias[k].template cast<NewScalar>();
243  res.jointPlacements[k] = jointPlacements[k].template cast<NewScalar>();
244  res.joints[k] = joints[k].template cast<NewScalar>();
245  }
246 
247  for(size_t k = 0; k < frames.size(); ++k)
248  {
249  res.frames[k] = frames[k].template cast<NewScalar>();
250  }
251 
252  return res;
253  }
254 
260  bool operator==(const ModelTpl & other) const
261  {
262  bool res =
263  other.nq == nq
264  && other.nv == nv
265  && other.njoints == njoints
266  && other.nbodies == nbodies
267  && other.nframes == nframes
268  && other.parents == parents
269  && other.names == names
270  && other.subtrees == subtrees
271  && other.gravity == gravity
272  && other.name == name;
273 
274  res &=
275  other.idx_qs == idx_qs
276  && other.nqs == nqs
277  && other.idx_vs == idx_vs
278  && other.nvs == nvs;
279 
280  if(other.referenceConfigurations.size() != referenceConfigurations.size())
281  return false;
282 
283  typename ConfigVectorMap::const_iterator it = referenceConfigurations.begin();
284  typename ConfigVectorMap::const_iterator it_other = other.referenceConfigurations.begin();
285  for(long k = 0; k < (long)referenceConfigurations.size(); ++k)
286  {
287  std::advance(it,k); std::advance(it_other,k);
288 
289  if(it->second.size() != it_other->second.size())
290  return false;
291  if(it->second != it_other->second)
292  return false;
293  }
294 
295  if(other.rotorInertia.size() != rotorInertia.size())
296  return false;
297  res &= other.rotorInertia == rotorInertia;
298  if(!res) return res;
299 
300  if(other.friction.size() != friction.size())
301  return false;
302  res &= other.friction == friction;
303  if(!res) return res;
304 
305  if(other.damping.size() != damping.size())
306  return false;
307  res &= other.damping == damping;
308  if(!res) return res;
309 
310  if(other.rotorGearRatio.size() != rotorGearRatio.size())
311  return false;
312  res &= other.rotorGearRatio == rotorGearRatio;
313  if(!res) return res;
314 
315  if(other.effortLimit.size() != effortLimit.size())
316  return false;
317  res &= other.effortLimit == effortLimit;
318  if(!res) return res;
319 
320  if(other.velocityLimit.size() != velocityLimit.size())
321  return false;
322  res &= other.velocityLimit == velocityLimit;
323  if(!res) return res;
324 
325  if(other.lowerPositionLimit.size() != lowerPositionLimit.size())
326  return false;
327  res &= other.lowerPositionLimit == lowerPositionLimit;
328  if(!res) return res;
329 
330  if(other.upperPositionLimit.size() != upperPositionLimit.size())
331  return false;
332  res &= other.upperPositionLimit == upperPositionLimit;
333  if(!res) return res;
334 
335  for(size_t k = 1; k < inertias.size(); ++k)
336  {
337  res &= other.inertias[k] == inertias[k];
338  if(!res) return res;
339  }
340 
341  for(size_t k = 1; k < other.jointPlacements.size(); ++k)
342  {
343  res &= other.jointPlacements[k] == jointPlacements[k];
344  if(!res) return res;
345  }
346 
347  res &=
348  other.joints == joints
349  && other.frames == frames;
350 
351  return res;
352  }
353 
357  bool operator!=(const ModelTpl & other) const
358  { return !(*this == other); }
359 
378  JointIndex addJoint(const JointIndex parent,
379  const JointModel & joint_model,
380  const SE3 & joint_placement,
381  const std::string & joint_name);
382 
391  JointIndex addJoint(const JointIndex parent,
392  const JointModel & joint_model,
393  const SE3 & joint_placement,
394  const std::string & joint_name,
395  const VectorXs & max_effort,
396  const VectorXs & max_velocity,
397  const VectorXs & min_config,
398  const VectorXs & max_config);
399 
406  JointIndex addJoint(const JointIndex parent,
407  const JointModel & joint_model,
408  const SE3 & joint_placement,
409  const std::string & joint_name,
410  const VectorXs & max_effort,
411  const VectorXs & max_velocity,
412  const VectorXs & min_config,
413  const VectorXs & max_config,
414  const VectorXs & friction,
415  const VectorXs & damping);
416 
426  FrameIndex addJointFrame(const JointIndex & joint_index,
427  int previous_frame_index = -1);
428 
438  void appendBodyToJoint(const JointIndex joint_index, const Inertia & Y,
439  const SE3 & body_placement = SE3::Identity());
440 
452  FrameIndex addBodyFrame(const std::string & body_name,
453  const JointIndex & parentJoint,
454  const SE3 & body_placement = SE3::Identity(),
455  int previousFrame = -1);
456 
468  FrameIndex getBodyId(const std::string & name) const;
469 
477  bool existBodyName(const std::string & name) const;
478 
479 
490  JointIndex getJointId(const std::string & name) const;
491 
499  bool existJointName(const std::string & name) const;
500 
514  FrameIndex getFrameId(const std::string & name,
515  const FrameType & type = (FrameType) (JOINT | FIXED_JOINT | BODY | OP_FRAME | SENSOR )) const;
516 
525  bool existFrame(const std::string & name,
526  const FrameType& type = (FrameType) (JOINT | FIXED_JOINT | BODY | OP_FRAME | SENSOR )) const;
527 
537  FrameIndex addFrame(const Frame & frame,
538  const bool append_inertia = true);
539 
550  template<typename D>
551  inline bool check(const AlgorithmCheckerBase<D> & checker = AlgorithmCheckerBase<D>()) const
552  { return checker.checkModel(*this); }
553 
559  std::vector<bool> hasConfigurationLimit();
560 
566  std::vector<bool> hasConfigurationLimitInTangent();
567 
569  inline bool check() const;
570 
578  inline bool check(const Data & data) const;
579 
580  protected:
581 
587  void addJointIndexToParentSubtrees(const JointIndex joint_id);
588  };
589 
590 } // namespace pinocchio
591 
592 /* --- Details -------------------------------------------------------------- */
593 /* --- Details -------------------------------------------------------------- */
594 /* --- Details -------------------------------------------------------------- */
595 #include "pinocchio/multibody/model.hxx"
596 
597 #endif // ifndef __pinocchio_multibody_model_hpp__
bool existBodyName(const std::string &name) const
Check if a body given by its name exists.
JointModelVector joints
Model of joint i, encapsulated in a JointModelAccessor.
parent
Definition: lambdas.py:16
FrameIndex addJointFrame(const JointIndex &joint_index, int previous_frame_index=-1)
Add a joint to the frame tree.
joint frame: attached to the child body of a joint (a.k.a. child frame)
DataTpl< Scalar, Options, JointCollectionTpl > Data
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.
ModelTpl< NewScalar, Options, JointCollectionTpl > cast() const
A Plucker coordinate frame attached to a parent joint inside a kinematic tree.
int njoints
Number of joints.
std::vector< std::string > names
Name of joint i
ConfigVectorType lowerPositionLimit
Lower joint configuration limit.
FrameVector frames
Vector of operational frames registered on the model.
std::vector< bool > hasConfigurationLimit()
Check if joints have configuration limits.
std::vector< int > nvs
Dimension of the joint i tangent subspace.
TangentVectorType rotorInertia
Vector of rotor inertia parameters.
bool existJointName(const std::string &name) const
Check if a joint given by its name exists.
bool operator==(const ModelTpl &other) const
Equality comparison operator.
typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModel) JointModelVector
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).
pinocchio::GeomIndex GeomIndex
fixed joint frame: joint frame but for a fixed joint
JointCollectionTpl< Scalar, Options > JointCollection
std::vector< IndexVector > subtrees
Vector of joint subtrees. subtree[j] corresponds to the subtree supported by the joint j...
ConfigVectorMap referenceConfigurations
Map of reference configurations, indexed by user given names.
operational frame: user-defined frames that are defined at runtime
int nframes
Number of operational frames.
FrameType
Enum on the possible types of frames.
void addJointIndexToParentSubtrees(const JointIndex joint_id)
Add the joint_id to its parent subtrees.
std::string name
Model name;.
TangentVectorType rotorGearRatio
Vector of rotor gear ratio parameters.
pinocchio::FrameIndex FrameIndex
body frame: attached to the collision, inertial or visual properties of a link
JointIndex addJoint(const JointIndex parent, const JointModel &joint_model, const SE3 &joint_placement, const std::string &joint_name)
Add a joint to the kinematic tree with infinite bounds.
std::vector< int > nqs
Dimension of the joint i configuration subspace.
TangentVectorType friction
Vector of joint friction parameters.
std::vector< int > idx_qs
Starting index of the joint i in the configuration space.
std::vector< Index > IndexVector
int nbodies
Number of bodies.
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...
pinocchio::JointIndex JointIndex
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.
Motion gravity
Spatial gravity of the model.
std::map< std::string, ConfigVectorType > ConfigVectorMap
Map between a string (key) and a configuration vector.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
JointModelTpl< Scalar, Options, JointCollectionTpl > JointModel
FrameIndex addFrame(const Frame &frame, const bool append_inertia=true)
Adds a frame to the kinematic tree. The inertia stored within the frame will be happened to the inert...
CRTP class describing the API of the checkers.
Definition: check.hpp:22
ForceTpl< Scalar, Options > Force
ModelTpl()
Default constructor. Builds an empty model with no joints.
Main pinocchio namespace.
Definition: timings.cpp:28
std::vector< int > idx_vs
Starting index of the joint i in the tangent configuration space.
std::vector< bool > hasConfigurationLimitInTangent()
Check if joints have configuration limits.
TangentVectorType effortLimit
Vector of maximal joint torques.
Eigen::Matrix< Scalar, Eigen::Dynamic, 1, Options > VectorXs
JointIndex getJointId(const std::string &name) const
Return the index of a joint given by its name.
TangentVectorType damping
Vector of joint damping parameters.
res
std::vector< JointIndex > parents
Joint parent of joint i, denoted li (li==parents[i]).
JointDataTpl< Scalar, Options, JointCollectionTpl > JointData
int nv
Dimension of the velocity vector space.
TangentVectorType velocityLimit
Vector of maximal joint velocities.
FrameTpl< Scalar, Options > Frame
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
std::size_t Index
FrameIndex getBodyId(const std::string &name) const
Return the index of a body given by its name.
VectorXs ConfigVectorType
Dense vectorized version of a joint configuration vector.
ConfigVectorType upperPositionLimit
Upper joint configuration limit.
sensor frame: defined in a sensor element
static const Vector3 gravity981
Default 3D gravity vector (=(0,0,-9.81)).
std::vector< IndexVector > supports
Vector of joint supports. supports[j] corresponds to the collection of all joints located on the path...
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.
int nq
Dimension of the configuration vector representation.
InertiaTpl< Scalar, Options > Inertia
bool check() const
Run check(fusion::list) with DEFAULT_CHECKERS as argument.
SE3Tpl< Scalar, Options > SE3
bool operator!=(const ModelTpl &other) const
MotionTpl< Scalar, Options > Motion
FrameIndex 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.


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:32