pinocchio  2.2.1-dirty
multibody/model.hpp
1 //
2 // Copyright (c) 2015-2019 CNRS INRIA
3 // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4 //
5 
6 #ifndef __pinocchio_model_hpp__
7 #define __pinocchio_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 
26 namespace pinocchio
27 {
28 
29  template<typename _Scalar, int _Options, template<typename,int> class JointCollectionTpl>
30  struct ModelTpl
31  : serialization::Serializable< ModelTpl<_Scalar,_Options,JointCollectionTpl> >
32  {
33  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
34 
35  typedef _Scalar Scalar;
36  enum { Options = _Options };
37 
38  typedef JointCollectionTpl<Scalar,Options> JointCollection;
39  typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
40 
41  typedef SE3Tpl<Scalar,Options> SE3;
42  typedef MotionTpl<Scalar,Options> Motion;
43  typedef ForceTpl<Scalar,Options> Force;
44  typedef InertiaTpl<Scalar,Options> Inertia;
45  typedef FrameTpl<Scalar,Options> Frame;
46 
47  typedef pinocchio::Index Index;
48  typedef pinocchio::JointIndex JointIndex;
49  typedef pinocchio::GeomIndex GeomIndex;
50  typedef pinocchio::FrameIndex FrameIndex;
51  typedef std::vector<Index> IndexVector;
52 
53  typedef JointModelTpl<Scalar,Options,JointCollectionTpl> JointModel;
54  typedef JointDataTpl<Scalar,Options,JointCollectionTpl> JointData;
55 
56  typedef container::aligned_vector<JointModel> JointModelVector;
57  typedef container::aligned_vector<JointData> JointDataVector;
58 
59  typedef container::aligned_vector<Frame> FrameVector;
60 
61  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> VectorXs;
62  typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
63 
65  typedef VectorXs ConfigVectorType;
66 
67  typedef std::map<std::string, ConfigVectorType> ConfigVectorMap;
68 
71  typedef VectorXs TangentVectorType;
72 
74  int nq;
75 
77  int nv;
78 
80  int njoints;
81 
83  int nbodies;
84 
86  int nframes;
87 
90 
93 
95  JointModelVector joints;
96 
98  std::vector<int> idx_qs;
99 
101  std::vector<int> nqs;
102 
104  std::vector<int> idx_vs;
105 
107  std::vector<int> nvs;
108 
110  std::vector<JointIndex> parents;
111 
113  std::vector<std::string> names;
114 
116  ConfigVectorMap referenceConfigurations;
117 
119  VectorXs rotorInertia;
120 
122  VectorXs rotorGearRatio;
123 
125  TangentVectorType effortLimit;
126 
128  TangentVectorType velocityLimit;
129 
132 
135 
137  FrameVector frames;
138 
142  std::vector<IndexVector> supports;
143 
147  std::vector<IndexVector> subtrees;
148 
150  Motion gravity;
151 
153  static const Vector3 gravity981;
154 
156  std::string name;
157 
160  : nq(0)
161  , nv(0)
162  , njoints(1)
163  , nbodies(1)
164  , nframes(0)
165  , inertias(1, Inertia::Zero())
166  , jointPlacements(1, SE3::Identity())
167  , joints(1)
168  , idx_qs(1,0)
169  , nqs(1,0)
170  , idx_vs(1,0)
171  , nvs(1,0)
172  , parents(1, 0)
173  , names(1)
174  , supports(1,IndexVector(1,0))
175  , subtrees(1)
176  , gravity(gravity981,Vector3::Zero())
177  {
178  names[0] = "universe"; // Should be "universe joint (trivial)"
179  // FIXME Should the universe joint be a FIXED_JOINT even if it is
180  // in the list of joints ? See comment in definition of
181  // Model::addJointFrame and Model::addBodyFrame
182  addFrame(Frame("universe", 0, 0, SE3::Identity(), FIXED_JOINT));
183  }
184  ~ModelTpl() {} // std::cout << "Destroy model" << std::endl; }
185 
187  template<typename NewScalar>
189  {
191  ReturnType res;
192  res.nq = nq; res.nv = nv;
193  res.njoints = njoints;
194  res.nbodies = nbodies;
195  res.nframes = nframes;
196  res.parents = parents;
197  res.names = names;
198  res.subtrees = subtrees;
199  res.gravity = gravity.template cast<NewScalar>();
200  res.name = name;
201 
202  res.idx_qs = idx_qs;
203  res.nqs = nqs;
204  res.idx_vs = idx_vs;
205  res.nvs = nvs;
206 
207  // Eigen Vectors
208  res.rotorInertia = rotorInertia.template cast<NewScalar>();
209  res.rotorGearRatio = rotorGearRatio.template cast<NewScalar>();
210  res.effortLimit = effortLimit.template cast<NewScalar>();
211  res.velocityLimit = velocityLimit.template cast<NewScalar>();
212  res.lowerPositionLimit = lowerPositionLimit.template cast<NewScalar>();
213  res.upperPositionLimit = upperPositionLimit.template cast<NewScalar>();
214 
215  typename ReturnType::ConfigVectorMap::iterator rit = res.referenceConfigurations.begin();
216  typename ConfigVectorMap::const_iterator it;
217  for (it = referenceConfigurations.begin();
218  it != referenceConfigurations.end(); it++ )
219  {
220  rit->second = it->second.template cast<NewScalar>();
221  rit++;
222  }
223 
224  // reserve vectors
225  res.inertias.resize(inertias.size());
226  res.jointPlacements.resize(jointPlacements.size());
227  res.joints.resize(joints.size());
228  res.frames.resize(frames.size());
229 
231  for(size_t k = 0; k < joints.size(); ++k)
232  {
233  res.inertias[k] = inertias[k].template cast<NewScalar>();
234  res.jointPlacements[k] = jointPlacements[k].template cast<NewScalar>();
235  res.joints[k] = joints[k].template cast<NewScalar>();
236  }
237 
238  for(size_t k = 0; k < frames.size(); ++k)
239  {
240  res.frames[k] = frames[k].template cast<NewScalar>();
241  }
242 
243  return res;
244  }
245 
251  bool operator==(const ModelTpl & other) const
252  {
253  bool res =
254  other.nq == nq
255  && other.nv == nv
256  && other.njoints == njoints
257  && other.nbodies == nbodies
258  && other.nframes == nframes
259  && other.parents == parents
260  && other.names == names
261  && other.subtrees == subtrees
262  && other.gravity == gravity
263  && other.name == name;
264 
265  res &=
266  other.idx_qs == idx_qs
267  && other.nqs == nqs
268  && other.idx_vs == idx_vs
269  && other.nvs == nvs;
270 
271  if(other.referenceConfigurations.size() != referenceConfigurations.size())
272  return false;
273  res &= other.referenceConfigurations == referenceConfigurations;
274  if(!res) return res;
275 
276  if(other.rotorInertia.size() != rotorInertia.size())
277  return false;
278  res &= other.rotorInertia == rotorInertia;
279  if(!res) return res;
280 
281  if(other.rotorGearRatio.size() != rotorGearRatio.size())
282  return false;
283  res &= other.rotorGearRatio == rotorGearRatio;
284  if(!res) return res;
285 
286  if(other.effortLimit.size() != effortLimit.size())
287  return false;
288  res &= other.effortLimit == effortLimit;
289  if(!res) return res;
290 
291  if(other.velocityLimit.size() != velocityLimit.size())
292  return false;
293  res &= other.velocityLimit == velocityLimit;
294  if(!res) return res;
295 
296  if(other.lowerPositionLimit.size() != lowerPositionLimit.size())
297  return false;
298  res &= other.lowerPositionLimit == lowerPositionLimit;
299  if(!res) return res;
300 
301  if(other.upperPositionLimit.size() != upperPositionLimit.size())
302  return false;
303  res &= other.upperPositionLimit == upperPositionLimit;
304  if(!res) return res;
305 
306  for(size_t k = 1; k < inertias.size(); ++k)
307  {
308  res &= other.inertias[k] == inertias[k];
309  if(!res) return res;
310  }
311 
312  for(size_t k = 1; k < other.jointPlacements.size(); ++k)
313  {
314  res &= other.jointPlacements[k] == jointPlacements[k];
315  if(!res) return res;
316  }
317 
318  res &=
319  other.joints == joints
320  && other.frames == frames;
321 
322  return res;
323  }
324 
328  bool operator!=(const ModelTpl & other) const
329  { return !(*this == other); }
330 
353  JointIndex addJoint(const JointIndex parent,
354  const JointModel & joint_model,
355  const SE3 & joint_placement,
356  const std::string & joint_name,
357  const VectorXs & max_effort,
358  const VectorXs & max_velocity,
359  const VectorXs & min_config,
360  const VectorXs & max_config
361  );
362 
380  JointIndex addJoint(const JointIndex parent,
381  const JointModel & joint_model,
382  const SE3 & joint_placement,
383  const std::string & joint_name);
384 
394  int addJointFrame(const JointIndex & joint_index,
395  int previous_frame_index = -1);
396 
406  void appendBodyToJoint(const JointIndex joint_index, const Inertia & Y,
407  const SE3 & body_placement = SE3::Identity());
408 
420  int addBodyFrame (const std::string & body_name,
421  const JointIndex & parentJoint,
422  const SE3 & body_placement = SE3::Identity(),
423  int previousFrame = -1);
424 
436  JointIndex getBodyId(const std::string & name) const;
437 
445  bool existBodyName(const std::string & name) const;
446 
447 
458  JointIndex getJointId(const std::string & name) const;
459 
467  bool existJointName(const std::string & name) const;
468 
482  FrameIndex getFrameId(const std::string & name,
483  const FrameType & type = (FrameType) (JOINT | FIXED_JOINT | BODY | OP_FRAME | SENSOR )) const;
484 
493  bool existFrame(const std::string & name,
494  const FrameType& type = (FrameType) (JOINT | FIXED_JOINT | BODY | OP_FRAME | SENSOR )) const;
495 
504  int addFrame(const Frame & frame);
505 
516  template<typename D>
517  inline bool check(const AlgorithmCheckerBase<D> & checker = AlgorithmCheckerBase<D>()) const
518  { return checker.checkModel(*this); }
519 
521  inline bool check() const;
522 
530  inline bool check(const Data & data) const;
531 
532  protected:
533 
539  void addJointIndexToParentSubtrees(const JointIndex joint_id);
540  };
541 
542 } // namespace pinocchio
543 
544 /* --- Details -------------------------------------------------------------- */
545 /* --- Details -------------------------------------------------------------- */
546 /* --- Details -------------------------------------------------------------- */
547 #include "pinocchio/multibody/model.hxx"
548 
549 #endif // ifndef __pinocchio_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.
container::aligned_vector< SE3 > jointPlacements
Placement (SE3) of the input of joint i regarding to the parent joint output li.
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 supports. supports[j] corresponds to the collection of all joints located on the path betwe...
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 subtrees. subtree[j] corresponds to the subtree supported by the joint j. The first element of subtree[j] is the index of the joint j itself.
TangentVectorType effortLimit
Vector of maximal joint torques.
Specialization of an std::vector with an aligned allocator. This specialization might be used when th...
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]).
container::aligned_vector< Inertia > inertias
Spatial inertias of the body i expressed in the supporting joint frame 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.