tsid  1.8.0
Efficient Task Space Inverse Dynamics for Multi-body Systems based on Pinocchio
tsid::robots::RobotWrapper Class Reference

Wrapper for a robot based on pinocchio. More...

#include <tsid/robots/robot-wrapper.hpp>

Collaboration diagram for tsid::robots::RobotWrapper:

Public Types

enum  e_RootJointType { FIXED_BASE_SYSTEM = 0 , FLOATING_BASE_SYSTEM = 1 }
 
typedef pinocchio::Model Model
 
typedef pinocchio::Data Data
 
typedef pinocchio::Motion Motion
 
typedef pinocchio::Frame Frame
 
typedef pinocchio::SE3 SE3
 
typedef math::Vector Vector
 
typedef math::Vector3 Vector3
 
typedef math::Vector6 Vector6
 
typedef math::Matrix Matrix
 
typedef math::Matrix3x Matrix3x
 
typedef math::RefVector RefVector
 
typedef math::ConstRefVector ConstRefVector
 
typedef enum tsid::robots::RobotWrapper::e_RootJointType RootJointType
 

Public Member Functions

 RobotWrapper (const std::string &filename, const std::vector< std::string > &package_dirs, bool verbose=false)
 
 RobotWrapper (const std::string &filename, const std::vector< std::string > &package_dirs, const pinocchio::JointModelVariant &rootJoint, bool verbose=false)
 
TSID_DEPRECATED RobotWrapper (const Model &m, bool verbose=false)
 
 RobotWrapper (const Model &m, RootJointType rootJoint, bool verbose=false)
 
virtual ~RobotWrapper ()=default
 
virtual int nq () const
 
virtual int nq_actuated () const
 
virtual int nv () const
 
virtual int na () const
 
virtual bool is_fixed_base () const
 
const Modelmodel () const
 Accessor to model. More...
 
Modelmodel ()
 
void computeAllTerms (Data &data, const Vector &q, const Vector &v) const
 
const Vectorrotor_inertias () const
 
const Vectorgear_ratios () const
 
bool rotor_inertias (ConstRefVector rotor_inertias)
 
bool gear_ratios (ConstRefVector gear_ratios)
 
void com (const Data &data, RefVector com_pos, RefVector com_vel, RefVector com_acc) const
 
const Vector3com (const Data &data) const
 
const Vector3com_vel (const Data &data) const
 
const Vector3com_acc (const Data &data) const
 
const Matrix3xJcom (const Data &data) const
 
const Matrixmass (const Data &data)
 
const VectornonLinearEffects (const Data &data) const
 
const SE3position (const Data &data, const Model::JointIndex index) const
 
const Motionvelocity (const Data &data, const Model::JointIndex index) const
 
const Motionacceleration (const Data &data, const Model::JointIndex index) const
 
void jacobianWorld (const Data &data, const Model::JointIndex index, Data::Matrix6x &J) const
 
void jacobianLocal (const Data &data, const Model::JointIndex index, Data::Matrix6x &J) const
 
SE3 framePosition (const Data &data, const Model::FrameIndex index) const
 
void framePosition (const Data &data, const Model::FrameIndex index, SE3 &framePosition) const
 
Motion frameVelocity (const Data &data, const Model::FrameIndex index) const
 
Motion frameVelocityWorldOriented (const Data &data, const Model::FrameIndex index) const
 
void frameVelocity (const Data &data, const Model::FrameIndex index, Motion &frameVelocity) const
 
Motion frameAcceleration (const Data &data, const Model::FrameIndex index) const
 
Motion frameAccelerationWorldOriented (const Data &data, const Model::FrameIndex index) const
 
void frameAcceleration (const Data &data, const Model::FrameIndex index, Motion &frameAcceleration) const
 
Motion frameClassicAcceleration (const Data &data, const Model::FrameIndex index) const
 
Motion frameClassicAccelerationWorldOriented (const Data &data, const Model::FrameIndex index) const
 
void frameClassicAcceleration (const Data &data, const Model::FrameIndex index, Motion &frameAcceleration) const
 
void frameJacobianWorld (Data &data, const Model::FrameIndex index, Data::Matrix6x &J) const
 
void frameJacobianLocal (Data &data, const Model::FrameIndex index, Data::Matrix6x &J) const
 
const Data::Matrix6x & momentumJacobian (const Data &data) const
 
Vector3 angularMomentumTimeVariation (const Data &data) const
 
void setGravity (const Motion &gravity)
 

Public Attributes

EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef math::Scalar Scalar
 

Protected Member Functions

void init ()
 
void updateMd ()
 

Protected Attributes

Model m_model
 Robot model. More...
 
std::string m_model_filename
 
bool m_verbose
 
int m_nq_actuated
 
int m_na
 
bool m_is_fixed_base
 
Vector m_rotor_inertias
 
Vector m_gear_ratios
 
Vector m_Md
 
Matrix m_M
 diagonal part of inertia matrix due to rotor inertias More...
 

Detailed Description

Wrapper for a robot based on pinocchio.

Member Typedef Documentation

◆ ConstRefVector

◆ Data

typedef pinocchio::Data tsid::robots::RobotWrapper::Data

◆ Frame

typedef pinocchio::Frame tsid::robots::RobotWrapper::Frame

◆ Matrix

◆ Matrix3x

◆ Model

typedef pinocchio::Model tsid::robots::RobotWrapper::Model

◆ Motion

typedef pinocchio::Motion tsid::robots::RobotWrapper::Motion

◆ RefVector

◆ RootJointType

◆ SE3

typedef pinocchio::SE3 tsid::robots::RobotWrapper::SE3

◆ Vector

◆ Vector3

◆ Vector6

Member Enumeration Documentation

◆ e_RootJointType

Enumerator
FIXED_BASE_SYSTEM 
FLOATING_BASE_SYSTEM 

Constructor & Destructor Documentation

◆ RobotWrapper() [1/4]

tsid::robots::RobotWrapper::RobotWrapper ( const std::string &  filename,
const std::vector< std::string > &  package_dirs,
bool  verbose = false 
)

◆ RobotWrapper() [2/4]

tsid::robots::RobotWrapper::RobotWrapper ( const std::string &  filename,
const std::vector< std::string > &  package_dirs,
const pinocchio::JointModelVariant &  rootJoint,
bool  verbose = false 
)

◆ RobotWrapper() [3/4]

tsid::robots::RobotWrapper::RobotWrapper ( const Model m,
bool  verbose = false 
)

◆ RobotWrapper() [4/4]

tsid::robots::RobotWrapper::RobotWrapper ( const Model m,
RootJointType  rootJoint,
bool  verbose = false 
)

◆ ~RobotWrapper()

virtual tsid::robots::RobotWrapper::~RobotWrapper ( )
virtualdefault

Member Function Documentation

◆ acceleration()

const Motion & tsid::robots::RobotWrapper::acceleration ( const Data data,
const Model::JointIndex  index 
) const

◆ angularMomentumTimeVariation()

Vector3 tsid::robots::RobotWrapper::angularMomentumTimeVariation ( const Data data) const

◆ com() [1/2]

const Vector3 & tsid::robots::RobotWrapper::com ( const Data data) const

◆ com() [2/2]

void tsid::robots::RobotWrapper::com ( const Data data,
RefVector  com_pos,
RefVector  com_vel,
RefVector  com_acc 
) const

◆ com_acc()

const Vector3 & tsid::robots::RobotWrapper::com_acc ( const Data data) const

◆ com_vel()

const Vector3 & tsid::robots::RobotWrapper::com_vel ( const Data data) const

◆ computeAllTerms()

void tsid::robots::RobotWrapper::computeAllTerms ( Data data,
const Vector q,
const Vector v 
) const

◆ frameAcceleration() [1/2]

Motion tsid::robots::RobotWrapper::frameAcceleration ( const Data data,
const Model::FrameIndex  index 
) const

◆ frameAcceleration() [2/2]

void tsid::robots::RobotWrapper::frameAcceleration ( const Data data,
const Model::FrameIndex  index,
Motion frameAcceleration 
) const

◆ frameAccelerationWorldOriented()

Motion tsid::robots::RobotWrapper::frameAccelerationWorldOriented ( const Data data,
const Model::FrameIndex  index 
) const

◆ frameClassicAcceleration() [1/2]

Motion tsid::robots::RobotWrapper::frameClassicAcceleration ( const Data data,
const Model::FrameIndex  index 
) const

◆ frameClassicAcceleration() [2/2]

void tsid::robots::RobotWrapper::frameClassicAcceleration ( const Data data,
const Model::FrameIndex  index,
Motion frameAcceleration 
) const

◆ frameClassicAccelerationWorldOriented()

Motion tsid::robots::RobotWrapper::frameClassicAccelerationWorldOriented ( const Data data,
const Model::FrameIndex  index 
) const

◆ frameJacobianLocal()

void tsid::robots::RobotWrapper::frameJacobianLocal ( Data data,
const Model::FrameIndex  index,
Data::Matrix6x &  J 
) const

◆ frameJacobianWorld()

void tsid::robots::RobotWrapper::frameJacobianWorld ( Data data,
const Model::FrameIndex  index,
Data::Matrix6x &  J 
) const

◆ framePosition() [1/2]

SE3 tsid::robots::RobotWrapper::framePosition ( const Data data,
const Model::FrameIndex  index 
) const

◆ framePosition() [2/2]

void tsid::robots::RobotWrapper::framePosition ( const Data data,
const Model::FrameIndex  index,
SE3 framePosition 
) const

◆ frameVelocity() [1/2]

Motion tsid::robots::RobotWrapper::frameVelocity ( const Data data,
const Model::FrameIndex  index 
) const

◆ frameVelocity() [2/2]

void tsid::robots::RobotWrapper::frameVelocity ( const Data data,
const Model::FrameIndex  index,
Motion frameVelocity 
) const

◆ frameVelocityWorldOriented()

Motion tsid::robots::RobotWrapper::frameVelocityWorldOriented ( const Data data,
const Model::FrameIndex  index 
) const

◆ gear_ratios() [1/2]

const Vector & tsid::robots::RobotWrapper::gear_ratios ( ) const

◆ gear_ratios() [2/2]

bool tsid::robots::RobotWrapper::gear_ratios ( ConstRefVector  gear_ratios)

◆ init()

void tsid::robots::RobotWrapper::init ( )
protected

◆ is_fixed_base()

bool tsid::robots::RobotWrapper::is_fixed_base ( ) const
virtual

◆ jacobianLocal()

void tsid::robots::RobotWrapper::jacobianLocal ( const Data data,
const Model::JointIndex  index,
Data::Matrix6x &  J 
) const

◆ jacobianWorld()

void tsid::robots::RobotWrapper::jacobianWorld ( const Data data,
const Model::JointIndex  index,
Data::Matrix6x &  J 
) const

◆ Jcom()

const Matrix3x & tsid::robots::RobotWrapper::Jcom ( const Data data) const

◆ mass()

const Matrix & tsid::robots::RobotWrapper::mass ( const Data data)

◆ model() [1/2]

Model & tsid::robots::RobotWrapper::model ( )

◆ model() [2/2]

const Model & tsid::robots::RobotWrapper::model ( ) const

Accessor to model.

Returns
a const reference on the model.

◆ momentumJacobian()

const Data::Matrix6x & tsid::robots::RobotWrapper::momentumJacobian ( const Data data) const

◆ na()

int tsid::robots::RobotWrapper::na ( ) const
virtual

◆ nonLinearEffects()

const Vector & tsid::robots::RobotWrapper::nonLinearEffects ( const Data data) const

◆ nq()

int tsid::robots::RobotWrapper::nq ( ) const
virtual

◆ nq_actuated()

int tsid::robots::RobotWrapper::nq_actuated ( ) const
virtual

◆ nv()

int tsid::robots::RobotWrapper::nv ( ) const
virtual

◆ position()

const SE3 & tsid::robots::RobotWrapper::position ( const Data data,
const Model::JointIndex  index 
) const

◆ rotor_inertias() [1/2]

const Vector & tsid::robots::RobotWrapper::rotor_inertias ( ) const

◆ rotor_inertias() [2/2]

bool tsid::robots::RobotWrapper::rotor_inertias ( ConstRefVector  rotor_inertias)

◆ setGravity()

void tsid::robots::RobotWrapper::setGravity ( const Motion gravity)

◆ updateMd()

void tsid::robots::RobotWrapper::updateMd ( )
protected

◆ velocity()

const Motion & tsid::robots::RobotWrapper::velocity ( const Data data,
const Model::JointIndex  index 
) const

Member Data Documentation

◆ m_gear_ratios

Vector tsid::robots::RobotWrapper::m_gear_ratios
protected

◆ m_is_fixed_base

bool tsid::robots::RobotWrapper::m_is_fixed_base
protected

number of actuators (nv for fixed-based, nv-6 for floating-base robots)

◆ m_M

Matrix tsid::robots::RobotWrapper::m_M
protected

diagonal part of inertia matrix due to rotor inertias

◆ m_Md

Vector tsid::robots::RobotWrapper::m_Md
protected

◆ m_model

Model tsid::robots::RobotWrapper::m_model
protected

Robot model.

◆ m_model_filename

std::string tsid::robots::RobotWrapper::m_model_filename
protected

◆ m_na

int tsid::robots::RobotWrapper::m_na
protected

dimension of the configuration space of the actuated DoF (nq for fixed-based, nq-7 for floating-base robots)

◆ m_nq_actuated

int tsid::robots::RobotWrapper::m_nq_actuated
protected

◆ m_rotor_inertias

Vector tsid::robots::RobotWrapper::m_rotor_inertias
protected

◆ m_verbose

bool tsid::robots::RobotWrapper::m_verbose
protected

◆ Scalar

EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef math::Scalar tsid::robots::RobotWrapper::Scalar

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