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__
std::vector< int > idx_vs
Starting index of the joint i in the tangent configuration space.
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.
bool check() const
Run check(fusion::list) with DEFAULT_CHECKERS as argument.
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.
VectorXs ConfigVectorType
Dense vectorized version of a joint configuration vector.
JointIndex getBodyId(const std::string &name) const
Return the index of a body given by its name.
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.
ModelTpl< NewScalar, Options, JointCollectionTpl > cast() const
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.
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.
JointIndex getJointId(const std::string &name) const
Return the index of a joint given by its name.
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.
int nframes
Number of operational frames.
bool existJointName(const std::string &name) const
Check if a joint given by its name exists.
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...
VectorXs rotorGearRatio
Vector of rotor gear ratio parameters.
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.
FrameType
Enum on the possible types of frame.
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.
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.
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.
int nbodies
Number of bodies.
bool operator!=(const ModelTpl &other) const
int nq
Dimension of the configuration vector representation.
std::vector< int > idx_qs
Starting index of the joint i in the configuration space.
bool operator==(const ModelTpl &other) const
Equality comparison operator.
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.