6 #ifndef __pinocchio_multibody_model_hpp__
7 #define __pinocchio_multibody_model_hpp__
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"
15 #include "pinocchio/multibody/fwd.hpp"
16 #include "pinocchio/multibody/frame.hpp"
17 #include "pinocchio/multibody/joint/joint-generic.hpp"
19 #include "pinocchio/container/aligned-vector.hpp"
21 #include "pinocchio/serialization/serializable.hpp"
32 template<
typename,
int>
33 class JointCollectionTpl>
39 template<
typename _Scalar,
int _Options,
template<
typename,
int>
class JointCollectionTpl>
42 typedef _Scalar Scalar;
45 template<
typename _Scalar,
int _Options,
template<
typename,
int>
class JointCollectionTpl>
48 ,
NumericalBase<ModelTpl<_Scalar, _Options, JointCollectionTpl>>
50 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
52 typedef _Scalar Scalar;
58 typedef JointCollectionTpl<Scalar, Options> JointCollection;
67 typedef pinocchio::Index Index;
68 typedef pinocchio::JointIndex JointIndex;
69 typedef pinocchio::GeomIndex GeomIndex;
70 typedef pinocchio::FrameIndex FrameIndex;
71 typedef std::vector<Index> IndexVector;
76 typedef PINOCCHIO_ALIGNED_STD_VECTOR(
JointModel) JointModelVector;
77 typedef PINOCCHIO_ALIGNED_STD_VECTOR(
JointData) JointDataVector;
79 typedef PINOCCHIO_ALIGNED_STD_VECTOR(
Frame) FrameVector;
81 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> VectorXs;
82 typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
84 typedef PINOCCHIO_ALIGNED_STD_VECTOR(
Inertia) InertiaVector;
85 typedef PINOCCHIO_ALIGNED_STD_VECTOR(
SE3) SE3Vector;
221 names[0] =
"universe";
233 template<
typename S2,
int O2>
236 *
this = other.template cast<Scalar>();
240 template<
typename NewScalar>
255 return !(*
this == other);
278 const JointIndex parent,
280 const SE3 & joint_placement,
281 const std::string & joint_name);
293 const JointIndex parent,
295 const SE3 & joint_placement,
296 const std::string & joint_name,
297 const VectorXs & max_effort,
298 const VectorXs & max_velocity,
299 const VectorXs & min_config,
300 const VectorXs & max_config);
310 const JointIndex parent,
312 const SE3 & joint_placement,
313 const std::string & joint_name,
314 const VectorXs & max_effort,
315 const VectorXs & max_velocity,
316 const VectorXs & min_config,
317 const VectorXs & max_config,
330 FrameIndex
addJointFrame(
const JointIndex & joint_index,
int previous_frame_index = -1);
343 const JointIndex joint_index,
345 const SE3 & body_placement = SE3::Identity());
360 const std::string & body_name,
361 const JointIndex & parentJoint,
362 const SE3 & body_placement = SE3::Identity(),
363 int parentFrame = -1);
422 const std::string &
name,
434 const std::string &
name,
464 return checker.checkModel(*
this);
507 #include "pinocchio/multibody/model.hxx"
509 #if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
510 #include "pinocchio/multibody/model.txx"
Main pinocchio namespace.
FrameType
Enum on the possible types of frames.
@ BODY
body frame: attached to the collision, inertial or visual properties of a link
@ SENSOR
sensor frame: defined in a sensor element
@ OP_FRAME
operational frame: user-defined frames that are defined at runtime
@ JOINT
joint frame: attached to the child body of a joint (a.k.a. child frame)
@ FIXED_JOINT
fixed joint frame: joint frame but for a fixed joint
CRTP class describing the API of the checkers.
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type....
A Plucker coordinate frame attached to a parent joint inside a kinematic tree.
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, const VectorXs &friction, const VectorXs &damping)
TangentVectorType velocityLimit
Vector of maximal joint velocities.
TangentVectorType friction
Vector of joint friction parameters.
std::vector< int > idx_vs
Starting index of the *i*th joint in the tangent configuration space.
FrameIndex addJointFrame(const JointIndex &joint_index, int previous_frame_index=-1)
Add a joint to the frame tree.
std::vector< IndexVector > subtrees
Vector of joint subtrees. subtree[j] corresponds to the subtree supported by the joint j....
std::vector< IndexVector > supports
Vector of joint supports. supports[j] corresponds to the vector of indices of the joints located on t...
std::vector< bool > hasConfigurationLimit()
Check if joints have configuration limits.
Motion gravity
Spatial gravity of the model.
FrameIndex getBodyId(const std::string &name) const
Return the index of a body given by its name.
bool operator==(const ModelTpl &other) const
Equality comparison operator.
ConfigVectorType upperPositionLimit
Upper joint configuration limit.
TangentVectorType rotorGearRatio
Vector of rotor gear ratio parameters.
ModelTpl()
Default constructor. Builds an empty model with no joints.
bool operator!=(const ModelTpl &other) const
InertiaVector inertias
Vector of spatial inertias supported by each joint.
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.
std::vector< int > idx_qs
Vector of starting index of the *i*th joint in the configuration space.
SE3Vector jointPlacements
Vector of joint placements: placement of a joint i wrt its parent joint frame.
int nframes
Number of operational frames.
VectorXs TangentVectorType
Dense vectorized version of a joint tangent vector (e.g. velocity, acceleration, etc)....
JointModelVector joints
Vector of joint models.
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.
bool check() const
Run check(fusion::list) with DEFAULT_CHECKERS as argument.
std::vector< int > nqs
Vector of dimension of the joint configuration subspace.
std::vector< IndexVector > children
Vector of children index. Chidren of the i*th joint, denoted *mu(i) corresponds to the set (i==parent...
ModelTpl(const ModelTpl< S2, O2 > &other)
Copy constructor by casting.
TangentVectorType rotorInertia
Vector of rotor inertia parameters.
TangentVectorType effortLimit
Vector of maximal joint torques.
int nq
Dimension of the configuration vector representation.
std::vector< JointIndex > parents
Vector of parent joint indexes. The parent of joint i, denoted li, corresponds to li==parents[i].
int nbodies
Number of bodies.
CastType< NewScalar, ModelTpl >::type cast() const
std::string name
Model name.
std::vector< std::string > names
Name of the joints.
JointIndex getJointId(const std::string &name) const
Return the index of a joint given by its name.
int nv
Dimension of the velocity vector space.
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.
bool check(const Data &data) const
Run checkData on data and current model.
ConfigVectorMap referenceConfigurations
Map of reference configurations, indexed by user given names.
FrameVector frames
Vector of operational frames registered on the model.
void addJointIndexToParentSubtrees(const JointIndex joint_id)
Add the joint_id to its parent subtrees.
bool existBodyName(const std::string &name) const
Check if a body given by its name exists.
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.
std::vector< int > nvs
Dimension of the *i*th joint tangent subspace.
VectorXs armature
Vector of armature values expressed at the joint level This vector may contain the contribution of ro...
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.
static const Vector3 gravity981
Default 3D gravity vector (=(0,0,-9.81)).
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)
int njoints
Number of joints.
VectorXs ConfigVectorType
Dense vectorized version of a joint configuration vector.
std::map< std::string, ConfigVectorType > ConfigVectorMap
Map between a string (key) and a configuration vector.
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.
bool existJointName(const std::string &name) const
Check if a joint given by its name exists.
TangentVectorType damping
Vector of joint damping parameters.
ConfigVectorType lowerPositionLimit
Lower joint configuration limit.
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...
std::vector< bool > hasConfigurationLimitInTangent()
Check if joints have configuration limits.
Common traits structure to fully define base classes for CRTP.