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 
14 
18 
20 
22 
23 #include <map>
24 #include <iterator>
25 
26 namespace pinocchio
27 {
28  template<
29  typename NewScalar,
30  typename Scalar,
31  int Options,
32  template<typename, int> class JointCollectionTpl>
33  struct CastType<NewScalar, ModelTpl<Scalar, Options, JointCollectionTpl>>
34  {
36  };
37 
38  template<typename _Scalar, int _Options, template<typename, int> class JointCollectionTpl>
39  struct traits<ModelTpl<_Scalar, _Options, JointCollectionTpl>>
40  {
41  typedef _Scalar Scalar;
42  };
43 
44  template<typename _Scalar, int _Options, template<typename, int> class JointCollectionTpl>
45  struct ModelTpl
46  : serialization::Serializable<ModelTpl<_Scalar, _Options, JointCollectionTpl>>
47  , NumericalBase<ModelTpl<_Scalar, _Options, JointCollectionTpl>>
48  {
49  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
50 
51  typedef _Scalar Scalar;
52  enum
53  {
54  Options = _Options
55  };
56 
57  typedef JointCollectionTpl<Scalar, Options> JointCollection;
59 
65 
70  typedef std::vector<Index> IndexVector;
71 
74 
75  typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModel) JointModelVector;
76  typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointData) JointDataVector;
77 
78  typedef PINOCCHIO_ALIGNED_STD_VECTOR(Frame) FrameVector;
79 
80  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> VectorXs;
81  typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
82 
83  typedef PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) InertiaVector;
84  typedef PINOCCHIO_ALIGNED_STD_VECTOR(SE3) SE3Vector;
85 
88 
90  typedef std::map<std::string, ConfigVectorType> ConfigVectorMap;
91 
96 
98  int nq;
99 
101  int nv;
102 
104  int njoints;
105 
107  int nbodies;
108 
110  int nframes;
111 
113  InertiaVector inertias;
114 
116  SE3Vector jointPlacements;
117 
119  JointModelVector joints;
120 
122  std::vector<int> idx_qs;
123 
125  std::vector<int> nqs;
126 
128  std::vector<int> idx_vs;
129 
131  std::vector<int> nvs;
132 
135  std::vector<JointIndex> parents;
136 
139  std::vector<IndexVector> children;
140 
142  std::vector<std::string> names;
143 
146 
150 
153 
156 
159 
162 
165 
168 
171 
174 
176  FrameVector frames;
177 
183  std::vector<IndexVector> supports;
184 
188  std::vector<IndexVector> subtrees;
189 
192 
194  static const Vector3 gravity981;
195 
197  std::string name;
198 
201  : nq(0)
202  , nv(0)
203  , njoints(1)
204  , nbodies(1)
205  , nframes(0)
206  , inertias(1, Inertia::Zero())
207  , jointPlacements(1, SE3::Identity())
208  , joints(1)
209  , idx_qs(1, 0)
210  , nqs(1, 0)
211  , idx_vs(1, 0)
212  , nvs(1, 0)
213  , parents(1, 0)
214  , children(1)
215  , names(1)
216  , supports(1, IndexVector(1, 0))
217  , subtrees(1)
218  , gravity(gravity981, Vector3::Zero())
219  {
220  names[0] = "universe"; // Should be "universe joint (trivial)"
221  // FIXME Should the universe joint be a FIXED_JOINT even if it is
222  // in the list of joints ? See comment in definition of
223  // Model::addJointFrame and Model::addBodyFrame
224  addFrame(Frame("universe", 0, 0, SE3::Identity(), FIXED_JOINT));
225  }
226 
232  template<typename S2, int O2>
233  explicit ModelTpl(const ModelTpl<S2, O2> & other)
234  {
235  *this = other.template cast<Scalar>();
236  }
237 
239  template<typename NewScalar>
240  typename CastType<NewScalar, ModelTpl>::type cast() const;
241 
247  bool operator==(const ModelTpl & other) const;
248 
252  bool operator!=(const ModelTpl & other) const
253  {
254  return !(*this == other);
255  }
256 
277  const JointIndex parent,
278  const JointModel & joint_model,
279  const SE3 & joint_placement,
280  const std::string & joint_name);
281 
292  const JointIndex parent,
293  const JointModel & joint_model,
294  const SE3 & joint_placement,
295  const std::string & joint_name,
296  const VectorXs & max_effort,
297  const VectorXs & max_velocity,
298  const VectorXs & min_config,
299  const VectorXs & max_config);
300 
309  const JointIndex parent,
310  const JointModel & joint_model,
311  const SE3 & joint_placement,
312  const std::string & joint_name,
313  const VectorXs & max_effort,
314  const VectorXs & max_velocity,
315  const VectorXs & min_config,
316  const VectorXs & max_config,
317  const VectorXs & friction,
318  const VectorXs & damping);
319 
329  FrameIndex addJointFrame(const JointIndex & joint_index, int previous_frame_index = -1);
330 
341  void appendBodyToJoint(
342  const JointIndex joint_index,
343  const Inertia & Y,
344  const SE3 & body_placement = SE3::Identity());
345 
359  const std::string & body_name,
360  const JointIndex & parentJoint,
361  const SE3 & body_placement = SE3::Identity(),
362  int parentFrame = -1);
363 
375  FrameIndex getBodyId(const std::string & name) const;
376 
384  bool existBodyName(const std::string & name) const;
385 
396  JointIndex getJointId(const std::string & name) const;
397 
405  bool existJointName(const std::string & name) const;
406 
421  const std::string & name,
422  const FrameType & type = (FrameType)(JOINT | FIXED_JOINT | BODY | OP_FRAME | SENSOR)) const;
423 
432  bool existFrame(
433  const std::string & name,
434  const FrameType & type = (FrameType)(JOINT | FIXED_JOINT | BODY | OP_FRAME | SENSOR)) const;
435 
448  FrameIndex addFrame(const Frame & frame, const bool append_inertia = true);
449 
460  template<typename D>
461  bool check(const AlgorithmCheckerBase<D> & checker = AlgorithmCheckerBase<D>()) const
462  {
463  return checker.checkModel(*this);
464  }
465 
471  std::vector<bool> hasConfigurationLimit();
472 
478  std::vector<bool> hasConfigurationLimitInTangent();
479 
481  bool check() const;
482 
490  bool check(const Data & data) const;
491 
492  protected:
499  };
500 
501 } // namespace pinocchio
502 
503 /* --- Details -------------------------------------------------------------- */
504 /* --- Details -------------------------------------------------------------- */
505 /* --- Details -------------------------------------------------------------- */
506 #include "pinocchio/multibody/model.hxx"
507 
508 #if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
509  #include "pinocchio/multibody/model.txx"
510 #endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
511 
512 #endif // ifndef __pinocchio_multibody_model_hpp__
pinocchio::InertiaTpl< Scalar, Options >
pinocchio::FrameIndex
Index FrameIndex
Definition: multibody/fwd.hpp:28
common_symbols.type
type
Definition: common_symbols.py:35
fwd.hpp
pinocchio::ModelTpl::names
std::vector< std::string > names
Name of the joints.
Definition: multibody/model.hpp:142
pinocchio::NumericalBase
Definition: fwd.hpp:89
pinocchio::ModelTpl::frames
FrameVector frames
Vector of operational frames registered on the model.
Definition: multibody/model.hpp:176
pinocchio::DataTpl
Definition: context/generic.hpp:25
pinocchio::AlgorithmCheckerBase
CRTP class describing the API of the checkers.
Definition: algorithm/check.hpp:22
pinocchio::ModelTpl::existJointName
bool existJointName(const std::string &name) const
Check if a joint given by its name exists.
pinocchio::ModelTpl::SE3
SE3Tpl< Scalar, Options > SE3
Definition: multibody/model.hpp:60
pinocchio::ModelTpl::jointPlacements
SE3Vector jointPlacements
Vector of joint placements: placement of a joint i wrt its parent joint frame.
Definition: multibody/model.hpp:116
pinocchio::ModelTpl::GeomIndex
pinocchio::GeomIndex GeomIndex
Definition: multibody/model.hpp:68
lambdas.parent
parent
Definition: lambdas.py:21
pinocchio::ModelTpl::lowerPositionLimit
ConfigVectorType lowerPositionLimit
Lower joint configuration limit.
Definition: multibody/model.hpp:170
pinocchio::ModelTpl::existBodyName
bool existBodyName(const std::string &name) const
Check if a body given by its name exists.
pinocchio::ModelTpl::ModelTpl
ModelTpl()
Default constructor. Builds an empty model with no joints.
Definition: multibody/model.hpp:200
aligned-vector.hpp
pinocchio::Options
Options
Definition: joint-configuration.hpp:1082
pinocchio::ModelTpl::cast
CastType< NewScalar, ModelTpl >::type cast() const
pinocchio::ModelTpl::Motion
MotionTpl< Scalar, Options > Motion
Definition: multibody/model.hpp:61
pinocchio::ModelTpl::operator==
bool operator==(const ModelTpl &other) const
Equality comparison operator.
pinocchio::ModelTpl::operator!=
bool operator!=(const ModelTpl &other) const
Definition: multibody/model.hpp:252
pinocchio::FrameType
FrameType
Enum on the possible types of frames.
Definition: multibody/frame.hpp:31
pinocchio::SE3Tpl< Scalar, Options >
pinocchio::SENSOR
@ SENSOR
sensor frame: defined in a sensor element
Definition: multibody/frame.hpp:38
serializable.hpp
setup.data
data
Definition: cmake/cython/setup.in.py:48
inertia.hpp
pinocchio::python::Scalar
context::Scalar Scalar
Definition: admm-solver.cpp:29
pinocchio::ModelTpl::joints
JointModelVector joints
Vector of joint models.
Definition: multibody/model.hpp:119
pinocchio::JointDataTpl
Definition: multibody/joint/fwd.hpp:162
pinocchio::ModelTpl::existFrame
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.
pinocchio::ModelTpl::Data
DataTpl< Scalar, Options, JointCollectionTpl > Data
Definition: multibody/model.hpp:58
motion.hpp
pinocchio::ModelTpl::name
std::string name
Model name.
Definition: multibody/model.hpp:197
pinocchio::ModelTpl::addJoint
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.
pinocchio::ModelTpl::hasConfigurationLimit
std::vector< bool > hasConfigurationLimit()
Check if joints have configuration limits.
pinocchio::ModelTpl::rotorInertia
TangentVectorType rotorInertia
Vector of rotor inertia parameters.
Definition: multibody/model.hpp:152
pinocchio::ModelTpl::check
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: multibody/model.hpp:461
pinocchio::FrameTpl
A Plucker coordinate frame attached to a parent joint inside a kinematic tree.
Definition: multibody/frame.hpp:55
pinocchio::Index
PINOCCHIO_COMPILER_DIAGNOSTIC_POP typedef std::size_t Index
Definition: multibody/fwd.hpp:22
pinocchio::ModelTpl::inertias
InertiaVector inertias
Vector of spatial inertias supported by each joint.
Definition: multibody/model.hpp:113
pinocchio::ModelTpl::JointIndex
pinocchio::JointIndex JointIndex
Definition: multibody/model.hpp:67
pinocchio::ModelTpl::Force
ForceTpl< Scalar, Options > Force
Definition: multibody/model.hpp:62
pinocchio::ModelTpl::referenceConfigurations
ConfigVectorMap referenceConfigurations
Map of reference configurations, indexed by user given names.
Definition: multibody/model.hpp:145
pinocchio::ModelTpl::Inertia
InertiaTpl< Scalar, Options > Inertia
Definition: multibody/model.hpp:63
pinocchio::ModelTpl::subtrees
std::vector< IndexVector > subtrees
Vector of joint subtrees. subtree[j] corresponds to the subtree supported by the joint j....
Definition: multibody/model.hpp:188
pinocchio::ModelTpl::nqs
std::vector< int > nqs
Vector of dimension of the joint configuration subspace.
Definition: multibody/model.hpp:125
pinocchio::ModelTpl::PINOCCHIO_ALIGNED_STD_VECTOR
typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModel) JointModelVector
pinocchio::ModelTpl::addBodyFrame
FrameIndex addBodyFrame(const std::string &body_name, const JointIndex &parentJoint, const SE3 &body_placement=SE3::Identity(), int parentFrame=-1)
Add a body to the frame tree.
pinocchio::ModelTpl::nvs
std::vector< int > nvs
Dimension of the *i*th joint tangent subspace.
Definition: multibody/model.hpp:131
pinocchio::ModelTpl::appendBodyToJoint
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.
pinocchio::ModelTpl::children
std::vector< IndexVector > children
Vector of children index. Chidren of the i*th joint, denoted *mu(i) corresponds to the set (i==parent...
Definition: multibody/model.hpp:139
reachable-workspace-with-collisions.parentJoint
parentJoint
Definition: reachable-workspace-with-collisions.py:48
pinocchio::ModelTpl::idx_qs
std::vector< int > idx_qs
Vector of starting index of the *i*th joint in the configuration space.
Definition: multibody/model.hpp:122
pinocchio::ModelTpl::TangentVectorType
VectorXs TangentVectorType
Dense vectorized version of a joint tangent vector (e.g. velocity, acceleration, etc)....
Definition: multibody/model.hpp:95
pinocchio::ModelTpl::IndexVector
std::vector< Index > IndexVector
Definition: multibody/model.hpp:70
pinocchio::ModelTpl::JointData
JointDataTpl< Scalar, Options, JointCollectionTpl > JointData
Definition: multibody/model.hpp:73
joint-generic.hpp
pinocchio::ModelTpl::getFrameId
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.
se3.hpp
pinocchio::ModelTpl::hasConfigurationLimitInTangent
std::vector< bool > hasConfigurationLimitInTangent()
Check if joints have configuration limits.
pinocchio::CastType< NewScalar, ModelTpl< Scalar, Options, JointCollectionTpl > >::type
ModelTpl< NewScalar, Options, JointCollectionTpl > type
Definition: multibody/model.hpp:35
pinocchio::JointModelTpl
Definition: multibody/joint/fwd.hpp:155
pinocchio::ForceTpl< Scalar, Options >
pinocchio::ModelTpl::FrameIndex
pinocchio::FrameIndex FrameIndex
Definition: multibody/model.hpp:69
pinocchio::ModelTpl::rotorGearRatio
TangentVectorType rotorGearRatio
Vector of rotor gear ratio parameters.
Definition: multibody/model.hpp:155
pinocchio::ModelTpl::armature
VectorXs armature
Vector of armature values expressed at the joint level This vector may contain the contribution of ro...
Definition: multibody/model.hpp:149
pinocchio::ModelTpl::VectorXs
Eigen::Matrix< Scalar, Eigen::Dynamic, 1, Options > VectorXs
Definition: multibody/model.hpp:80
pinocchio::ModelTpl::Scalar
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
Definition: multibody/model.hpp:51
pinocchio::ModelTpl::addJointIndexToParentSubtrees
void addJointIndexToParentSubtrees(const JointIndex joint_id)
Add the joint_id to its parent subtrees.
pinocchio::ModelTpl::nframes
int nframes
Number of operational frames.
Definition: multibody/model.hpp:110
pinocchio::ModelTpl::velocityLimit
TangentVectorType velocityLimit
Vector of maximal joint velocities.
Definition: multibody/model.hpp:167
pinocchio::ModelTpl::nbodies
int nbodies
Number of bodies.
Definition: multibody/model.hpp:107
pinocchio::ModelTpl::friction
TangentVectorType friction
Vector of joint friction parameters.
Definition: multibody/model.hpp:158
pinocchio::ModelTpl::addJointFrame
FrameIndex addJointFrame(const JointIndex &joint_index, int previous_frame_index=-1)
Add a joint to the frame tree.
contact-cholesky.frame
frame
Definition: contact-cholesky.py:24
pinocchio::ModelTpl::gravity
Motion gravity
Spatial gravity of the model.
Definition: multibody/model.hpp:191
pinocchio::JOINT
@ JOINT
joint frame: attached to the child body of a joint (a.k.a. child frame)
Definition: multibody/frame.hpp:34
pinocchio::ModelTpl::gravity981
static const Vector3 gravity981
Default 3D gravity vector (=(0,0,-9.81)).
Definition: multibody/model.hpp:194
pinocchio::ModelTpl::getJointId
JointIndex getJointId(const std::string &name) const
Return the index of a joint given by its name.
pinocchio::ModelTpl::Vector3
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
Definition: multibody/model.hpp:81
pinocchio::ModelTpl::Index
pinocchio::Index Index
Definition: multibody/model.hpp:66
pinocchio::OP_FRAME
@ OP_FRAME
operational frame: user-defined frames that are defined at runtime
Definition: multibody/frame.hpp:33
append-urdf-model-with-another-model.joint_name
string joint_name
Definition: append-urdf-model-with-another-model.py:33
append-urdf-model-with-another-model.joint_placement
joint_placement
Definition: append-urdf-model-with-another-model.py:29
pinocchio::ModelTpl::damping
TangentVectorType damping
Vector of joint damping parameters.
Definition: multibody/model.hpp:161
pinocchio::ModelTpl::nv
int nv
Dimension of the velocity vector space.
Definition: multibody/model.hpp:101
pinocchio::ModelTpl::getBodyId
FrameIndex getBodyId(const std::string &name) const
Return the index of a body given by its name.
pinocchio::ModelTpl::ConfigVectorMap
std::map< std::string, ConfigVectorType > ConfigVectorMap
Map between a string (key) and a configuration vector.
Definition: multibody/model.hpp:90
pinocchio::SE3Tpl< Scalar, Options >::Identity
static SE3Tpl Identity()
Definition: spatial/se3-tpl.hpp:136
pinocchio::FIXED_JOINT
@ FIXED_JOINT
fixed joint frame: joint frame but for a fixed joint
Definition: multibody/frame.hpp:35
frame.hpp
pinocchio::ModelTpl::supports
std::vector< IndexVector > supports
Vector of joint supports. supports[j] corresponds to the vector of indices of the joints located on t...
Definition: multibody/model.hpp:183
pinocchio::ModelTpl::check
bool check() const
Run check(fusion::list) with DEFAULT_CHECKERS as argument.
Definition: default-check.hpp:23
pinocchio::ModelTpl::addFrame
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...
pinocchio::JointIndex
Index JointIndex
Definition: multibody/fwd.hpp:26
pinocchio::ModelTpl::effortLimit
TangentVectorType effortLimit
Vector of maximal joint torques.
Definition: multibody/model.hpp:164
pinocchio::ModelTpl::parents
std::vector< JointIndex > parents
Vector of parent joint indexes. The parent of joint i, denoted li, corresponds to li==parents[i].
Definition: multibody/model.hpp:135
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:71
pinocchio::ModelTpl::Options
@ Options
Definition: multibody/model.hpp:54
pinocchio::ModelTpl::Frame
FrameTpl< Scalar, Options > Frame
Definition: multibody/model.hpp:64
append-urdf-model-with-another-model.body_placement
body_placement
Definition: append-urdf-model-with-another-model.py:39
pinocchio::traits< ModelTpl< _Scalar, _Options, JointCollectionTpl > >::Scalar
_Scalar Scalar
Definition: multibody/model.hpp:41
pinocchio::ModelTpl::ConfigVectorType
VectorXs ConfigVectorType
Dense vectorized version of a joint configuration vector.
Definition: multibody/model.hpp:87
pinocchio::ModelTpl::idx_vs
std::vector< int > idx_vs
Starting index of the *i*th joint in the tangent configuration space.
Definition: multibody/model.hpp:128
pinocchio::MotionTpl< Scalar, Options >
append-urdf-model-with-another-model.joint_id
joint_id
Definition: append-urdf-model-with-another-model.py:34
pinocchio::CastType
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type....
Definition: fwd.hpp:99
pinocchio::ModelTpl
Definition: context/generic.hpp:20
pinocchio::ModelTpl::ModelTpl
ModelTpl(const ModelTpl< S2, O2 > &other)
Copy constructor by casting.
Definition: multibody/model.hpp:233
pinocchio::ModelTpl::upperPositionLimit
ConfigVectorType upperPositionLimit
Upper joint configuration limit.
Definition: multibody/model.hpp:173
force.hpp
pinocchio::BODY
@ BODY
body frame: attached to the collision, inertial or visual properties of a link
Definition: multibody/frame.hpp:36
pinocchio::GeomIndex
Index GeomIndex
Definition: multibody/fwd.hpp:27
fwd.hpp
pinocchio::ModelTpl::nq
int nq
Dimension of the configuration vector representation.
Definition: multibody/model.hpp:98
pinocchio::serialization::Serializable
Definition: serialization/serializable.hpp:16
pinocchio::ModelTpl::JointCollection
JointCollectionTpl< Scalar, Options > JointCollection
Definition: multibody/model.hpp:57
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27
pinocchio::ModelTpl::njoints
int njoints
Number of joints.
Definition: multibody/model.hpp:104
pinocchio::ModelTpl::JointModel
JointModelTpl< Scalar, Options, JointCollectionTpl > JointModel
Definition: multibody/model.hpp:72


pinocchio
Author(s):
autogenerated on Sun Dec 22 2024 03:41:11