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< Inertia > | inertias |
Spatial inertias of the body i expressed in the supporting joint frame i. | |
container::aligned_vector< SE3 > | jointPlacements |
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< Frame > | frames |
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... | |
|
inline |
Add a body to the frame tree.
[in] | body_name | Name of the body. |
[in] | parentJoint | Index of the parent joint. |
[in] | body_placement | The relative placement of the body regarding to the parent joint. Set default to the Identity placement. |
[in] | previousFrame | Index of the parent frame. If negative, the parent frame is the frame of the parent joint. |
Definition at line 156 of file model.hxx.
Referenced by Model::Model().
|
inline |
Adds a frame to the kinematic tree.
[in] | frame | The frame to add to the kinematic tree. |
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.
JointModelDerived | The type of the joint model. |
[in] | parent | Index of the parent joint. |
[in] | joint_model | The joint model. |
[in] | joint_placement | Placement of the joint inside its parent joint. |
[in] | joint_name | Name of the joint. If empty, the name is random. |
[in] | max_effort | Maximal joint torque. |
[in] | max_velocity | Maximal joint velocity. |
[in] | min_config | Lower joint configuration. |
[in] | max_config | Upper joint configuration. |
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.
JointModelDerived | The type of the joint model. |
[in] | parent | Index of the parent joint. |
[in] | joint_model | The joint model. |
[in] | joint_placement | Placement of the joint inside its parent joint. |
[in] | joint_name | Name of the joint. If empty, the name is random. |
|
inline |
Add a joint to the frame tree.
[in] | jointIndex | Index of the joint. |
[in] | frameIndex | Index of the parent frame. If negative, the parent frame is the frame of the parent joint. |
Definition at line 133 of file model.hxx.
Referenced by Model::Model().
|
inlineprotected |
Add the joint_id to its parent subtrees.
[in] | joint_id | The id of the joint to add to the subtrees |
Definition at line 233 of file model.hxx.
Referenced by Model::check().
|
inline |
Append a body to a given joint of the kinematic tree.
[in] | joint_index | Index of the supporting joint. |
[in] | Y | Spatial inertia of the body. |
[in] | body_placement | The relative placement of the body regarding to the parent joint. Set default to the Identity placement. |
Definition at line 147 of file model.hxx.
References InertiaBase< Derived >::se3Action().
Referenced by Model::Model().
|
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.
[in] | checker | a class, typically defined in the algorithm module, that validates the attributes of model. |
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().
|
inline |
Run checkData on data and current model.
[in] | data | to be checked wrt *this. |
Definition at line 146 of file check.hxx.
References se3::checkData().
|
inline |
Check if a body given by its name exists.
[in] | name | Name of the body. |
Definition at line 175 of file model.hxx.
Referenced by Model::Model().
|
inline |
Checks if a frame given by its name exists.
[in] | name | Name of the frame. |
[in] | type | Type of the frame. |
Definition at line 212 of file model.hxx.
Referenced by Model::Model().
|
inline |
Check if a joint given by its name exists.
[in] | name | Name of the joint. |
Definition at line 189 of file model.hxx.
Referenced by Model::Model().
|
inline |
Return the index of a body given by its name.
[in] | name | Name of the body. |
Definition at line 170 of file model.hxx.
Referenced by Model::Model().
|
inline |
Returns the index of a frame given by its name.
[in] | name | Name of the frame. |
[in] | type | Type of the frame. |
Definition at line 200 of file model.hxx.
Referenced by Model::Model().
|
inline |
Return the index of a joint given by its name.
[in] | name | Name of the joint. |
Definition at line 181 of file model.hxx.
Referenced by Model::Model().
|
inline |
Get the name of a joint given by its index.
[in] | index | Index of the joint. |
Definition at line 194 of file model.hxx.
Referenced by Model::Model().