pinocchio  UNKNOWN
Model Struct Reference
Collaboration diagram for Model:
[legend]

Public Types

typedef se3::JointIndex JointIndex
 
typedef se3::GeomIndex GeomIndex
 
typedef se3::FrameIndex FrameIndex
 
typedef std::vector< Index > IndexVector
 

Public Member Functions

 Model ()
 Default constructor. Builds an empty model with no joints.
 
template<typename JointModelDerived >
JointIndex addJoint (const JointIndex parent, const JointModelBase< JointModelDerived > &joint_model, const SE3 &joint_placement, const std::string &joint_name, const Eigen::VectorXd &max_effort, const Eigen::VectorXd &max_velocity, const Eigen::VectorXd &min_config, const Eigen::VectorXd &max_config)
 Add a joint to the kinematic tree with given bounds. More...
 
template<typename JointModelDerived >
JointIndex addJoint (const JointIndex parent, const JointModelBase< JointModelDerived > &joint_model, const SE3 &joint_placement, const std::string &joint_name)
 Add a joint to the kinematic tree with infinite bounds. More...
 
int addJointFrame (const JointIndex &jointIndex, int frameIndex=-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...
 
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. More...
 
JointIndex getBodyId (const std::string &name) const
 Return the index of a body given by its name. More...
 
bool existBodyName (const std::string &name) const
 Check if a body given by its name exists. More...
 
JointIndex getJointId (const std::string &name) const
 Return the index of a joint given by its name. More...
 
bool existJointName (const std::string &name) const
 Check if a joint given by its name exists. More...
 
PINOCCHIO_DEPRECATED const std::string & getJointName (const JointIndex index) const
 Get the name of a joint given by its index. 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...
 
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...
 
int addFrame (const Frame &frame)
 Adds a frame to the kinematic tree. More...
 
template<typename D >
bool check (const AlgorithmCheckerBase< D > &checker=AlgorithmCheckerBase< D >()) const
 
bool check () const
 Run check(fusion::list) with DEFAULT_CHECKERS as argument.
 
bool check (const Data &data) const
 

Public Attributes

EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef se3::Index Index
 
int nq
 Dimension of the configuration vector representation.
 
int nv
 Dimension of the velocity vector space.
 
int njoints
 Number of joints.
 
int nbodies
 Number of bodies.
 
int nframes
 Number of operational frames.
 
container::aligned_vector< Inertiainertias
 Spatial inertias of the body i expressed in the supporting joint frame i.
 
container::aligned_vector< SE3jointPlacements
 Placement (SE3) of the input of joint i regarding to the parent joint output li.
 
JointModelVector joints
 Model of joint i, encapsulated in a JointModelAccessor.
 
std::vector< JointIndex > parents
 Joint parent of joint i, denoted li (li==parents[i]).
 
std::vector< std::string > names
 Name of joint i
 
Eigen::VectorXd neutralConfiguration
 Vector of joint's neutral configurations.
 
Eigen::VectorXd rotorInertia
 Vector of rotor inertia parameters.
 
Eigen::VectorXd rotorGearRatio
 Vector of rotor gear ratio parameters.
 
Eigen::VectorXd effortLimit
 Vector of maximal joint torques.
 
Eigen::VectorXd velocityLimit
 Vector of maximal joint velocities.
 
Eigen::VectorXd lowerPositionLimit
 Lower joint configuration limit.
 
Eigen::VectorXd upperPositionLimit
 Upper joint configuration limit.
 
container::aligned_vector< Frameframes
 Vector of operational frames registered on the model.
 
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.
 
Motion gravity
 Spatial gravity of the model.
 
std::string name
 Model name;.
 

Static Public Attributes

static const Eigen::Vector3d 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

Definition at line 39 of file model.hpp.

Member Function Documentation

int addBodyFrame ( const std::string &  body_name,
const JointIndex &  parentJoint,
const SE3 body_placement = SE3::Identity(),
int  previousFrame = -1 
)
inline

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 or -1 in case of error.

Definition at line 156 of file model.hxx.

Referenced by Model::Model().

int addFrame ( const Frame frame)
inline

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, -1 otherwise.

Definition at line 219 of file model.hxx.

References FrameTpl< _Scalar, _Options >::name, and FrameTpl< _Scalar, _Options >::type.

Referenced by Model::Model().

Model::JointIndex addJoint ( const JointIndex  parent,
const JointModelBase< JointModelDerived > &  joint_model,
const SE3 joint_placement,
const std::string &  joint_name,
const Eigen::VectorXd &  max_effort,
const Eigen::VectorXd &  max_velocity,
const Eigen::VectorXd &  min_config,
const Eigen::VectorXd &  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

Definition at line 57 of file model.hxx.

References se3::nq(), and se3::nv().

Referenced by Model::Model().

Model::JointIndex addJoint ( const JointIndex  parent,
const JointModelBase< JointModelDerived > &  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

Definition at line 117 of file model.hxx.

int addJointFrame ( const JointIndex &  jointIndex,
int  frameIndex = -1 
)
inline

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 or -1 in case of error.

Definition at line 133 of file model.hxx.

Referenced by Model::Model().

void addJointIndexToParentSubtrees ( const JointIndex  joint_id)
inlineprotected

Add the joint_id to its parent subtrees.

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

Definition at line 233 of file model.hxx.

Referenced by Model::check().

void appendBodyToJoint ( const JointIndex  joint_index,
const Inertia Y,
const SE3 body_placement = SE3::Identity() 
)
inline

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

Definition at line 147 of file model.hxx.

References InertiaBase< Derived >::se3Action().

Referenced by Model::Model().

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 331 of file model.hpp.

References Model::addJointIndexToParentSubtrees(), and Model::check().

Referenced by se3::aba(), se3::ccrba(), se3::centerOfMass(), se3::computeABADerivatives(), se3::computeAllTerms(), se3::computeCollisions(), se3::computeCoriolisMatrix(), se3::computeDistances(), se3::computeForwardKinematicsDerivatives(), se3::computeGeneralizedGravity(), se3::computeGeneralizedGravityDerivatives(), se3::computeJointJacobians(), se3::computeJointJacobiansTimeVariation(), se3::computeMinverse(), se3::computeRNEADerivatives(), se3::crba(), se3::crbaMinimal(), se3::dccrba(), se3::emptyForwardPass(), se3::forwardDynamics(), se3::forwardKinematics(), se3::framesForwardKinematics(), se3::getComFromCrba(), se3::getFrameAcceleration(), se3::getFrameJacobian(), se3::getFrameJacobianTimeVariation(), se3::getFrameVelocity(), se3::getJacobianComFromCrba(), se3::getJointAccelerationDerivatives(), se3::getJointJacobian(), se3::getJointJacobianTimeVariation(), se3::getJointVelocityDerivatives(), se3::impulseDynamics(), se3::jacobianCenterOfMass(), se3::jointJacobian(), se3::kineticEnergy(), se3::nonLinearEffects(), se3::potentialEnergy(), se3::rnea(), se3::updateFramePlacements(), se3::updateGeometryPlacements(), and se3::updateGlobalPlacements().

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.

Definition at line 146 of file check.hxx.

References se3::checkData().

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

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.

Definition at line 175 of file model.hxx.

Referenced by Model::Model().

bool existFrame ( const std::string &  name,
const FrameType type = (FrameType) (JOINT | FIXED_JOINT | BODY | OP_FRAME | SENSOR ) 
) const
inline

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.

Definition at line 212 of file model.hxx.

Referenced by Model::Model().

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

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.

Definition at line 189 of file model.hxx.

Referenced by Model::Model().

Model::JointIndex getBodyId ( const std::string &  name) const
inline

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.

Definition at line 170 of file model.hxx.

Referenced by Model::Model().

Model::FrameIndex getFrameId ( const std::string &  name,
const FrameType type = (FrameType) (JOINT | FIXED_JOINT | BODY | OP_FRAME | SENSOR ) 
) const
inline

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.

Definition at line 200 of file model.hxx.

Referenced by Model::Model().

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

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
name Name of the joint.

Definition at line 181 of file model.hxx.

Referenced by Model::Model().

const std::string & getJointName ( const JointIndex  index) const
inline

Get the name of a joint given by its index.

Parameters
[in]indexIndex of the joint.
Returns
Name of the joint.

Definition at line 194 of file model.hxx.

Referenced by Model::Model().


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