pinocchio  2.4.4
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
ModelTpl< _Scalar, _Options, JointCollectionTpl > Struct Template Reference
Inheritance diagram for ModelTpl< _Scalar, _Options, JointCollectionTpl >:
Collaboration diagram for ModelTpl< _Scalar, _Options, JointCollectionTpl >:

Public Types

enum  { Options = _Options }
 
typedef std::map< std::string, ConfigVectorTypeConfigVectorMap
 
typedef VectorXs ConfigVectorType
 Dense vectorized version of a joint configuration vector.
 
typedef DataTpl< Scalar, Options, JointCollectionTpl > Data
 
typedef ForceTpl< Scalar, Options > Force
 
typedef FrameTpl< Scalar, Options > Frame
 
typedef pinocchio::FrameIndex FrameIndex
 
typedef pinocchio::GeomIndex GeomIndex
 
typedef pinocchio::Index Index
 
typedef std::vector< Index > IndexVector
 
typedef InertiaTpl< Scalar, Options > Inertia
 
typedef JointCollectionTpl< Scalar, Options > JointCollection
 
typedef JointDataTpl< Scalar, Options, JointCollectionTpl > JointData
 
typedef pinocchio::JointIndex JointIndex
 
typedef JointModelTpl< Scalar, Options, JointCollectionTpl > JointModel
 
typedef MotionTpl< Scalar, Options > Motion
 
typedef _Scalar Scalar
 
typedef SE3Tpl< Scalar, Options > SE3
 
typedef 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).
 
typedef Eigen::Matrix< Scalar, 3, 1, Options > Vector3
 
typedef Eigen::Matrix< Scalar, Eigen::Dynamic, 1, Options > VectorXs
 

Public Member Functions

 ModelTpl ()
 Default constructor. Builds an empty model with no joints.
 
FrameIndex 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. More...
 
FrameIndex addFrame (const Frame &frame)
 Adds a frame to the kinematic tree. More...
 
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. More...
 
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. More...
 
FrameIndex addJointFrame (const JointIndex &joint_index, int previous_frame_index=-1)
 Add a joint to the frame tree. More...
 
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. More...
 
template<typename NewScalar >
ModelTpl< NewScalar, Options, JointCollectionTpl > cast () const
 
template<typename D >
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. More...
 
bool check () const
 Run check(fusion::list) with DEFAULT_CHECKERS as argument.
 
bool check (const Data &data) const
 Run checkData on data and current model. More...
 
bool existBodyName (const std::string &name) const
 Check if a body given by its name exists. More...
 
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. More...
 
bool existJointName (const std::string &name) const
 Check if a joint given by its name exists. More...
 
FrameIndex getBodyId (const std::string &name) const
 Return the index of a body given by its name. More...
 
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. More...
 
JointIndex getJointId (const std::string &name) const
 Return the index of a joint given by its name. More...
 
bool operator!= (const ModelTpl &other) const
 
bool operator== (const ModelTpl &other) const
 Equality comparison operator. More...
 
typedef PINOCCHIO_ALIGNED_STD_VECTOR (JointModel) JointModelVector
 
typedef PINOCCHIO_ALIGNED_STD_VECTOR (JointData) JointDataVector
 
typedef PINOCCHIO_ALIGNED_STD_VECTOR (Frame) FrameVector
 
 PINOCCHIO_ALIGNED_STD_VECTOR (Inertia) inertias
 Spatial inertias of the body i expressed in the supporting joint frame i.
 
 PINOCCHIO_ALIGNED_STD_VECTOR (SE3) jointPlacements
 Placement (SE3) of the input of joint i regarding to the parent joint output li.
 
- Public Member Functions inherited from Serializable< ModelTpl< _Scalar, _Options, JointCollectionTpl > >
void loadFromBinary (const std::string &filename)
 Loads a Derived object from an binary file.
 
void loadFromString (const std::string &str)
 Loads a Derived object from a string.
 
void loadFromStringStream (std::istringstream &is)
 Loads a Derived object from a stream string.
 
void loadFromText (const std::string &filename)
 Loads a Derived object from a text file.
 
void loadFromXML (const std::string &filename, const std::string &tag_name)
 Loads a Derived object from an XML file.
 
void saveToBinary (const std::string &filename) const
 Saves a Derived object as an binary file.
 
std::string saveToString () const
 Saves a Derived object to a string.
 
void saveToStringStream (std::stringstream &ss) const
 Saves a Derived object to a string stream.
 
void saveToText (const std::string &filename) const
 Saves a Derived object as a text file.
 
void saveToXML (const std::string &filename, const std::string &tag_name) const
 Saves a Derived object as an XML file.
 

Public Attributes

TangentVectorType effortLimit
 Vector of maximal joint torques.
 
FrameVector frames
 Vector of operational frames registered on the model.
 
Motion gravity
 Spatial gravity of the model.
 
std::vector< int > idx_qs
 Starting index of the joint i in the configuration space.
 
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.
 
VectorXs lowerPositionLimit
 Lower joint configuration limit.
 
std::string name
 Model name;.
 
std::vector< std::string > names
 Name of joint i
 
int nbodies
 Number of bodies.
 
int nframes
 Number of operational frames.
 
int njoints
 Number of joints.
 
int nq
 Dimension of the configuration vector representation.
 
std::vector< int > nqs
 Dimension of the joint i configuration subspace.
 
int nv
 Dimension of the velocity vector space.
 
std::vector< int > nvs
 Dimension of the joint i tangent subspace.
 
std::vector< JointIndex > parents
 Joint parent of joint i, denoted li (li==parents[i]).
 
ConfigVectorMap referenceConfigurations
 Map of reference configurations, indexed by user given names.
 
VectorXs rotorGearRatio
 Vector of rotor gear ratio parameters.
 
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. The first element of subtree[j] is the index of the joint j itself.
 
std::vector< IndexVector > supports
 Vector of joint supports. supports[j] corresponds to the collection of all joints located on the path between body j and the root. The last element of supports[j] is the index of the joint j itself.
 
VectorXs upperPositionLimit
 Upper joint configuration limit.
 
TangentVectorType velocityLimit
 Vector of maximal joint velocities.
 

Static Public Attributes

static const Vector3 gravity981
 Default 3D gravity vector (=(0,0,-9.81)).
 

Protected Member Functions

void addJointIndexToParentSubtrees (const JointIndex joint_id)
 Add the joint_id to its parent subtrees. More...
 

Detailed Description

template<typename _Scalar, int _Options, template< typename, int > class JointCollectionTpl>
struct pinocchio::ModelTpl< _Scalar, _Options, JointCollectionTpl >

Definition at line 23 of file fwd.hpp.

Member Function Documentation

◆ addBodyFrame()

FrameIndex 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.

Parameters
[in]body_nameName of the body.
[in]parentJointIndex of the parent joint.
[in]body_placementThe relative placement of the body regarding to the parent joint. Set default to the Identity placement.
[in]previousFrameIndex of the parent frame. If negative, the parent frame is the frame of the parent joint.
Returns
The index of the new frame

◆ addFrame()

FrameIndex addFrame ( const Frame frame)

Adds a frame to the kinematic tree.

Parameters
[in]frameThe frame to add to the kinematic tree.
Returns
Returns the index of the frame if it has been successfully added or if it already exists in the kinematic tree.

◆ addJoint() [1/2]

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.

Remarks
This method does not add a Frame of same name to the vector of frames. Use Model::addJointFrame.
The inertia supported by the joint is set to Zero.
Template Parameters
JointModelDerivedThe type of the joint model.
Parameters
[in]parentIndex of the parent joint.
[in]joint_modelThe joint model.
[in]joint_placementPlacement of the joint inside its parent joint.
[in]joint_nameName of the joint. If empty, the name is random.
[in]max_effortMaximal joint torque.
[in]max_velocityMaximal joint velocity.
[in]min_configLower joint configuration.
[in]max_configUpper joint configuration.
Returns
The index of the new joint.
See also
Model::appendBodyToJoint, Model::addJointFrame

◆ addJoint() [2/2]

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.

Remarks
This method also adds a Frame of same name to the vector of frames.
The inertia supported by the joint is set to Zero.
Template Parameters
JointModelDerivedThe type of the joint model.
Parameters
[in]parentIndex of the parent joint.
[in]joint_modelThe joint model.
[in]joint_placementPlacement of the joint inside its parent joint.
[in]joint_nameName of the joint. If empty, the name is random.
Returns
The index of the new joint.
See also
Model::appendBodyToJoint

◆ addJointFrame()

FrameIndex addJointFrame ( const JointIndex &  joint_index,
int  previous_frame_index = -1 
)

Add a joint to the frame tree.

Parameters
[in]jointIndexIndex of the joint.
[in]frameIndexIndex of the parent frame. If negative, the parent frame is the frame of the parent joint.
Returns
The index of the new frame

◆ addJointIndexToParentSubtrees()

void addJointIndexToParentSubtrees ( const JointIndex  joint_id)
protected

Add the joint_id to its parent subtrees.

Parameters
[in]joint_idThe id of the joint to add to the subtrees

◆ 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.

Parameters
[in]joint_indexIndex of the supporting joint.
[in]YSpatial inertia of the body.
[in]body_placementThe relative placement of the body regarding to the parent joint. Set default to the Identity placement.
See also
Model::addJoint

◆ cast()

ModelTpl<NewScalar,Options,JointCollectionTpl> cast ( ) const
inline
Returns
An expression of *this with the Scalar type casted to NewScalar.

copy into vectors

Definition at line 189 of file model.hpp.

◆ check() [1/2]

bool check ( const AlgorithmCheckerBase< D > &  checker = AlgorithmCheckerBase<D>()) const
inline

Check the validity of the attributes of Model with respect to the specification of some algorithms.

The method is a template so that the checkers can be defined in each algorithms.

Parameters
[in]checkera class, typically defined in the algorithm module, that validates the attributes of model.
Returns
true if the Model is valid, false otherwise.

Definition at line 525 of file model.hpp.

◆ check() [2/2]

bool check ( const Data data) const
inline

Run checkData on data and current model.

Parameters
[in]datato be checked wrt *this.
Returns
true if the data is valid, false otherwise.

◆ existBodyName()

bool existBodyName ( const std::string &  name) const

Check if a body given by its name exists.

Parameters
[in]nameName of the body.
Returns
True if the body exists in the kinematic tree.

◆ 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.

Parameters
[in]nameName of the frame.
[in]typeType of the frame.
Returns
Returns true if the frame exists.

◆ existJointName()

bool existJointName ( const std::string &  name) const

Check if a joint given by its name exists.

Parameters
[in]nameName of the joint.
Returns
True if the joint exists in the kinematic tree.

◆ getBodyId()

FrameIndex getBodyId ( const std::string &  name) const

Return the index of a body given by its name.

Warning
If no body is found, return the number of elements at time T. This can lead to errors if the model is expanded after this method is called (for example to get the id of a parent body)
Parameters
[in]nameName of the body.
Returns
Index of the body.

◆ 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.

See also
Model::existFrame to check if the frame exists or not.
Warning
If no frame is found, returns the size of the vector of Model::frames. This can lead to errors if the model is expanded after this method is called (for example to get the id of a parent frame).
Parameters
[in]nameName of the frame.
[in]typeType of the frame.
Returns
Index of the frame.

◆ getJointId()

JointIndex getJointId ( const std::string &  name) const

Return the index of a joint given by its name.

Warning
If no joint is found, return the number of elements at time T. This can lead to errors if the model is expanded after this method is called (for example to get the id of a parent joint)
Parameters
[in]nameName of the joint.
Returns
Index of the joint.

◆ operator!=()

bool operator!= ( const ModelTpl< _Scalar, _Options, JointCollectionTpl > &  other) const
inline
Returns
true if *this is NOT equal to other.

Definition at line 337 of file model.hpp.

◆ operator==()

bool operator== ( const ModelTpl< _Scalar, _Options, JointCollectionTpl > &  other) const
inline

Equality comparison operator.

Returns
true if *this is equal to other.

Definition at line 250 of file model.hpp.


The documentation for this struct was generated from the following files: