pinocchio  2.3.1-dirty
multibody/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.
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.
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.
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.
int njoints
Number of joints.
ConfigVectorMap referenceConfigurations
Map of reference configurations, indexed by user given names.
int nv
Dimension of the velocity vector space.
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...
bool existBodyName(const std::string &name) const
Check if a body given by its name exists.
Motion gravity
Spatial gravity of the model.
std::vector< std::string > names
Name of joint i
void addJointIndexToParentSubtrees(const JointIndex joint_id)
Add the joint_id to its parent subtrees.
VectorXs upperPositionLimit
Upper joint configuration limit.
FrameVector frames
Vector of operational frames registered on the model.
VectorXs rotorInertia
Vector of rotor inertia parameters.
std::vector< IndexVector > subtrees
Vector of joint subtrees. subtree[j] corresponds to the subtree supported by the joint j...
TangentVectorType effortLimit
Vector of maximal joint torques.
std::vector< int > nqs
Dimension of the joint i configuration subspace.
VectorXs lowerPositionLimit
Lower joint configuration limit.
bool operator==(const ModelTpl &other) const
Equality comparison operator.
int nframes
Number of operational frames.
VectorXs rotorGearRatio
Vector of rotor gear ratio parameters.
ModelTpl< NewScalar, Options, JointCollectionTpl > cast() const
bool operator!=(const ModelTpl &other) const
FrameType
Enum on the possible types of frame.
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;.
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]).
static const Vector3 gravity981
Default 3D gravity vector (=(0,0,-9.81)).
std::vector< int > nvs
Dimension of the joint i tangent subspace.
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).
TangentVectorType velocityLimit
Vector of maximal joint velocities.
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...
int nbodies
Number of bodies.
int nq
Dimension of the configuration vector representation.
std::vector< int > idx_qs
Starting index of the joint i in the configuration space.
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.