- 3.0.2 rigid body dynamics module.
ct::rbd::FixBaseFDSystem< RBDDynamics, ACT_STATE_DIM, EE_ARE_CONTROL_INPUTS > Class Template Reference

A fix base rigid body system that uses forward dynamics. More...

#include <FixBaseFDSystem.h>

Inheritance diagram for ct::rbd::FixBaseFDSystem< RBDDynamics, ACT_STATE_DIM, EE_ARE_CONTROL_INPUTS >:
ct::rbd::FixBaseSystemBase< RBDDynamics, RBDDynamics::NSTATE+ACT_STATE_DIM, RBDDynamics::NJOINTS+EE_ARE_CONTROL_INPUTS *RBDDynamics::N_EE *3 > ct::rbd::RBDSystem< RBDDynamics, false > ct::core::ControlledSystem< STATE_D, CONTROL_D, RBDDynamics::SCALAR > ct::core::System< STATE_DIM, SCALAR >

Public Types

using BASE = FixBaseSystemBase< RBDDynamics, STATE_DIM, CONTROL_DIM >
 
using SCALAR = typename BASE::SCALAR
 
using state_vector_t = typename BASE::state_vector_t
 
using control_vector_t = typename BASE::control_vector_t
 
using JointAcceleration_t = typename BASE::JointAcceleration_t
 
using RigidBodyPose_t = typename BASE::RigidBodyPose_t
 
using ActuatorDynamics_t = ActuatorDynamics< ACTUATOR_STATE_DIM, RBDDynamics::NJOINTS, SCALAR >
 
using FixBaseRobotState_t = FixBaseRobotState< BASE::NJOINTS, ACTUATOR_STATE_DIM, SCALAR >
 
using actuator_state_vector_t = typename FixBaseRobotState_t::actuator_state_vector_t
 
- Public Types inherited from ct::rbd::FixBaseSystemBase< RBDDynamics, RBDDynamics::NSTATE+ACT_STATE_DIM, RBDDynamics::NJOINTS+EE_ARE_CONTROL_INPUTS *RBDDynamics::N_EE *3 >
using Dynamics = RBDDynamics
 
using SCALAR = typename RBDDynamics::SCALAR
 
using Base = core::ControlledSystem< STATE_D, CONTROL_D, SCALAR >
 
using RigidBodyPose_t = tpl::RigidBodyPose< SCALAR >
 
using state_vector_t = ct::core::StateVector< STATE_D, SCALAR >
 
using control_vector_t = core::ControlVector< CONTROL_D, SCALAR >
 
using JointAcceleration_t = JointAcceleration< NJOINTS, SCALAR >
 
- Public Types inherited from ct::core::ControlledSystem< STATE_D, CONTROL_D, RBDDynamics::SCALAR >
typedef std::shared_ptr< ControlledSystem< STATE_D, CONTROL_D, RBDDynamics::SCALAR > > Ptr
 
typedef Base::time_t time_t
 
- Public Types inherited from ct::core::System< STATE_DIM, SCALAR >
typedef SCALAR time_t
 

Public Member Functions

 FixBaseFDSystem (const RigidBodyPose_t &basePose=RigidBodyPose_t())
 constructor More...
 
 FixBaseFDSystem (std::shared_ptr< ActuatorDynamics_t > actuatorDynamics, const RigidBodyPose_t &basePose=RigidBodyPose_t())
 constructor including actuator dynamics More...
 
 FixBaseFDSystem (const FixBaseFDSystem &arg)
 copy constructor More...
 
virtual ~FixBaseFDSystem ()=default
 destructor More...
 
void computeControlledDynamics (const state_vector_t &state, const SCALAR &t, const control_vector_t &controlIn, state_vector_t &derivative) override
 compute the controlled dynamics of the fixed base robotic system More...
 
template<typename T = void>
std::enable_if<(ACT_STATE_DIM > 0), T >::type computeActuatorDynamics (const FixBaseRobotState_t &robotState, const SCALAR &t, const control_vector_t &controlIn, state_vector_t &stateDerivative, control_vector_t &controlOut)
 evaluate the actuator dynamics if actuators are enabled More...
 
template<typename T = void>
std::enable_if<(ACT_STATE_DIM==0), T >::type computeActuatorDynamics (const FixBaseRobotState_t &robotState, const SCALAR &t, const control_vector_t &controlIn, state_vector_t &stateDerivative, control_vector_t &controlOut)
 do nothing if actuators disabled More...
 
virtual FixBaseFDSystem< RBDDynamics, ACT_STATE_DIM, EE_ARE_CONTROL_INPUTS > * clone () const override
 deep cloning More...
 
std::shared_ptr< ActuatorDynamics_tgetActuatorDynamics ()
 get pointer to actuator dynamics More...
 
template<typename T = typename FixBaseRobotState_t::actuator_state_vector_t>
std::enable_if<(ACT_STATE_DIM > 0), T >::type computeConsistentActuatorState (const JointState< BASE::NJOINTS, SCALAR > &jStateRef, const ct::core::ControlVector< BASE::NJOINTS > &torqueRef)
 if actuator dynamics enabled, this method allows to design a consistent actuator state More...
 
template<typename T = typename FixBaseRobotState_t::actuator_state_vector_t>
std::enable_if<(ACT_STATE_DIM > 0), T >::type computeConsistentActuatorState (const JointState< BASE::NJOINTS, SCALAR > &jStateRef)
 if actuator dynamics enabled, this method allows to design a consistent actuator state More...
 
- Public Member Functions inherited from ct::rbd::FixBaseSystemBase< RBDDynamics, RBDDynamics::NSTATE+ACT_STATE_DIM, RBDDynamics::NJOINTS+EE_ARE_CONTROL_INPUTS *RBDDynamics::N_EE *3 >
 FixBaseSystemBase (const RigidBodyPose_t &basePose=RigidBodyPose_t())
 constructor More...
 
 FixBaseSystemBase (const FixBaseSystemBase &arg)
 copy constructor More...
 
virtual ~FixBaseSystemBase ()=default
 destructor More...
 
virtual RBDDynamics & dynamics () override
 get dynamics More...
 
virtual const RBDDynamics & dynamics () const override
 get dynamics (const) More...
 
virtual void computeControlledDynamics (const ct::core::StateVector< STATE_D, SCALAR > &state, const SCALAR &t, const ct::core::ControlVector< CONTROL_D, SCALAR > &controlIn, ct::core::StateVector< STATE_D, SCALAR > &derivative) override=0
 compute the controlled dynamics of the fixed base robotic system More...
 
ct::core::ControlVector< NJOINTScomputeIDTorques (const JointState< NJOINTS, SCALAR > &jState, const JointAcceleration_t &jAcc=JointAcceleration_t(Eigen::Matrix< SCALAR, NJOINTS, 1 >::Zero()))
 compute inverse dynamics torques More...
 
- Public Member Functions inherited from ct::rbd::RBDSystem< RBDDynamics, false >
 RBDSystem ()=default
 
virtual ~RBDSystem ()=default
 
- Public Member Functions inherited from ct::core::ControlledSystem< STATE_D, CONTROL_D, RBDDynamics::SCALAR >
 ControlledSystem (const SYSTEM_TYPE &type=SYSTEM_TYPE::GENERAL)
 
 ControlledSystem (std::shared_ptr< ct::core::Controller< STATE_D, CONTROL_D, RBDDynamics::SCALAR >> controller, const SYSTEM_TYPE &type=SYSTEM_TYPE::GENERAL)
 
 ControlledSystem (const ControlledSystem &arg)
 
virtual ~ControlledSystem ()
 
void setController (const std::shared_ptr< Controller< STATE_D, CONTROL_D, RBDDynamics::SCALAR >> &controller)
 
void getController (std::shared_ptr< Controller< STATE_D, CONTROL_D, RBDDynamics::SCALAR >> &controller) const
 
std::shared_ptr< Controller< STATE_D, CONTROL_D, RBDDynamics::SCALAR > > getController ()
 
virtual void computeDynamics (const StateVector< STATE_D, RBDDynamics::SCALAR > &state, const time_t &t, StateVector< STATE_D, RBDDynamics::SCALAR > &derivative) override
 
virtual void computeControlledDynamics (const StateVector< STATE_D, RBDDynamics::SCALAR > &state, const time_t &t, const ControlVector< CONTROL_D, RBDDynamics::SCALAR > &control, StateVector< STATE_D, RBDDynamics::SCALAR > &derivative)=0
 
ControlVector< CONTROL_D, RBDDynamics::SCALARgetLastControlAction ()
 
- Public Member Functions inherited from ct::core::System< STATE_DIM, SCALAR >
 System (const SYSTEM_TYPE &type=SYSTEM_TYPE::GENERAL)
 
 System (const System &other)
 
virtual ~System ()
 
virtual void computeDynamics (const StateVector< STATE_DIM, SCALAR > &state, const time_t &t, StateVector< STATE_DIM, SCALAR > &derivative)=0
 
SYSTEM_TYPE getType () const
 
virtual bool isSymplectic () const
 

Static Public Attributes

static EIGEN_MAKE_ALIGNED_OPERATOR_NEW const size_t N_EE = RBDDynamics::N_EE
 
static const size_t STATE_DIM = RBDDynamics::NSTATE + ACT_STATE_DIM
 number of end-effectors More...
 
static const size_t CONTROL_DIM = RBDDynamics::NJOINTS + EE_ARE_CONTROL_INPUTS * N_EE * 3
 
static const size_t ACTUATOR_STATE_DIM = ACT_STATE_DIM
 
- Static Public Attributes inherited from ct::rbd::FixBaseSystemBase< RBDDynamics, RBDDynamics::NSTATE+ACT_STATE_DIM, RBDDynamics::NJOINTS+EE_ARE_CONTROL_INPUTS *RBDDynamics::N_EE *3 >
static EIGEN_MAKE_ALIGNED_OPERATOR_NEW const size_t N_EE
 number of end-effectors More...
 
static const size_t NJOINTS
 number of joints More...
 

Additional Inherited Members

- Public Attributes inherited from ct::core::ControlledSystem< STATE_D, CONTROL_D, RBDDynamics::SCALAR >
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef System< STATE_D, RBDDynamics::SCALARBase
 
- Public Attributes inherited from ct::core::System< STATE_DIM, SCALAR >
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef SCALAR S
 
- Protected Attributes inherited from ct::rbd::FixBaseSystemBase< RBDDynamics, RBDDynamics::NSTATE+ACT_STATE_DIM, RBDDynamics::NJOINTS+EE_ARE_CONTROL_INPUTS *RBDDynamics::N_EE *3 >
tpl::RigidBodyPose< SCALARbasePose_
 a "dummy" base pose which sets the robot's "fixed" position in the world More...
 
RBDDynamics dynamics_
 rigid body dynamics container More...
 
- Protected Attributes inherited from ct::core::ControlledSystem< STATE_D, CONTROL_D, RBDDynamics::SCALAR >
std::shared_ptr< Controller< STATE_D, CONTROL_D, RBDDynamics::SCALAR > > controller_
 
ControlVector< CONTROL_D, RBDDynamics::SCALARcontrolAction_
 
- Protected Attributes inherited from ct::core::System< STATE_DIM, SCALAR >
SYSTEM_TYPE type_
 

Detailed Description

template<class RBDDynamics, size_t ACT_STATE_DIM = 0, bool EE_ARE_CONTROL_INPUTS = false>
class ct::rbd::FixBaseFDSystem< RBDDynamics, ACT_STATE_DIM, EE_ARE_CONTROL_INPUTS >

A fix base rigid body system that uses forward dynamics.

A fix base rigid body system that uses forward dynamics. The control input vector is assumed to consist of

  • joint torques and
  • end effector forces expressed in the world frame

The overall state vector is arranged in the order

  • joint positions
  • joint velocities
  • actuator state
Warning
when modelled with RobCoGen, the base pose must be rotated "against gravity" (RobCoGen modeling assumption)

Member Typedef Documentation

◆ BASE

template<class RBDDynamics, size_t ACT_STATE_DIM = 0, bool EE_ARE_CONTROL_INPUTS = false>
using ct::rbd::FixBaseFDSystem< RBDDynamics, ACT_STATE_DIM, EE_ARE_CONTROL_INPUTS >::BASE = FixBaseSystemBase<RBDDynamics, STATE_DIM, CONTROL_DIM>

◆ SCALAR

template<class RBDDynamics, size_t ACT_STATE_DIM = 0, bool EE_ARE_CONTROL_INPUTS = false>
using ct::rbd::FixBaseFDSystem< RBDDynamics, ACT_STATE_DIM, EE_ARE_CONTROL_INPUTS >::SCALAR = typename BASE::SCALAR

◆ state_vector_t

template<class RBDDynamics, size_t ACT_STATE_DIM = 0, bool EE_ARE_CONTROL_INPUTS = false>
using ct::rbd::FixBaseFDSystem< RBDDynamics, ACT_STATE_DIM, EE_ARE_CONTROL_INPUTS >::state_vector_t = typename BASE::state_vector_t

◆ control_vector_t

template<class RBDDynamics, size_t ACT_STATE_DIM = 0, bool EE_ARE_CONTROL_INPUTS = false>
using ct::rbd::FixBaseFDSystem< RBDDynamics, ACT_STATE_DIM, EE_ARE_CONTROL_INPUTS >::control_vector_t = typename BASE::control_vector_t

◆ JointAcceleration_t

template<class RBDDynamics, size_t ACT_STATE_DIM = 0, bool EE_ARE_CONTROL_INPUTS = false>
using ct::rbd::FixBaseFDSystem< RBDDynamics, ACT_STATE_DIM, EE_ARE_CONTROL_INPUTS >::JointAcceleration_t = typename BASE::JointAcceleration_t

◆ RigidBodyPose_t

template<class RBDDynamics, size_t ACT_STATE_DIM = 0, bool EE_ARE_CONTROL_INPUTS = false>
using ct::rbd::FixBaseFDSystem< RBDDynamics, ACT_STATE_DIM, EE_ARE_CONTROL_INPUTS >::RigidBodyPose_t = typename BASE::RigidBodyPose_t

◆ ActuatorDynamics_t

template<class RBDDynamics, size_t ACT_STATE_DIM = 0, bool EE_ARE_CONTROL_INPUTS = false>
using ct::rbd::FixBaseFDSystem< RBDDynamics, ACT_STATE_DIM, EE_ARE_CONTROL_INPUTS >::ActuatorDynamics_t = ActuatorDynamics<ACTUATOR_STATE_DIM, RBDDynamics::NJOINTS, SCALAR>

◆ FixBaseRobotState_t

template<class RBDDynamics, size_t ACT_STATE_DIM = 0, bool EE_ARE_CONTROL_INPUTS = false>
using ct::rbd::FixBaseFDSystem< RBDDynamics, ACT_STATE_DIM, EE_ARE_CONTROL_INPUTS >::FixBaseRobotState_t = FixBaseRobotState<BASE::NJOINTS, ACTUATOR_STATE_DIM, SCALAR>

◆ actuator_state_vector_t

template<class RBDDynamics, size_t ACT_STATE_DIM = 0, bool EE_ARE_CONTROL_INPUTS = false>
using ct::rbd::FixBaseFDSystem< RBDDynamics, ACT_STATE_DIM, EE_ARE_CONTROL_INPUTS >::actuator_state_vector_t = typename FixBaseRobotState_t::actuator_state_vector_t

Constructor & Destructor Documentation

◆ FixBaseFDSystem() [1/3]

template<class RBDDynamics, size_t ACT_STATE_DIM = 0, bool EE_ARE_CONTROL_INPUTS = false>
ct::rbd::FixBaseFDSystem< RBDDynamics, ACT_STATE_DIM, EE_ARE_CONTROL_INPUTS >::FixBaseFDSystem ( const RigidBodyPose_t basePose = RigidBodyPose_t())
inline

constructor

Warning
when using actuator dynamics, the system looses its second order characteristics

◆ FixBaseFDSystem() [2/3]

template<class RBDDynamics, size_t ACT_STATE_DIM = 0, bool EE_ARE_CONTROL_INPUTS = false>
ct::rbd::FixBaseFDSystem< RBDDynamics, ACT_STATE_DIM, EE_ARE_CONTROL_INPUTS >::FixBaseFDSystem ( std::shared_ptr< ActuatorDynamics_t actuatorDynamics,
const RigidBodyPose_t basePose = RigidBodyPose_t() 
)
inline

constructor including actuator dynamics

Warning
when using actuator dynamics, the system looses its second order characteristics

◆ FixBaseFDSystem() [3/3]

template<class RBDDynamics, size_t ACT_STATE_DIM = 0, bool EE_ARE_CONTROL_INPUTS = false>
ct::rbd::FixBaseFDSystem< RBDDynamics, ACT_STATE_DIM, EE_ARE_CONTROL_INPUTS >::FixBaseFDSystem ( const FixBaseFDSystem< RBDDynamics, ACT_STATE_DIM, EE_ARE_CONTROL_INPUTS > &  arg)
inline

copy constructor

Parameters
arginstance of FixBaseFDSystem to be copied.
Note
takes care of explicitly cloning actuatorDynamics, if existent

References ct::rbd::FixBaseFDSystem< RBDDynamics, ACT_STATE_DIM, EE_ARE_CONTROL_INPUTS >::~FixBaseFDSystem().

◆ ~FixBaseFDSystem()

template<class RBDDynamics, size_t ACT_STATE_DIM = 0, bool EE_ARE_CONTROL_INPUTS = false>
virtual ct::rbd::FixBaseFDSystem< RBDDynamics, ACT_STATE_DIM, EE_ARE_CONTROL_INPUTS >::~FixBaseFDSystem ( )
virtualdefault

Member Function Documentation

◆ computeControlledDynamics()

◆ computeActuatorDynamics() [1/2]

template<class RBDDynamics, size_t ACT_STATE_DIM = 0, bool EE_ARE_CONTROL_INPUTS = false>
template<typename T = void>
std::enable_if<(ACT_STATE_DIM > 0), T>::type ct::rbd::FixBaseFDSystem< RBDDynamics, ACT_STATE_DIM, EE_ARE_CONTROL_INPUTS >::computeActuatorDynamics ( const FixBaseRobotState_t robotState,
const SCALAR t,
const control_vector_t controlIn,
state_vector_t stateDerivative,
control_vector_t controlOut 
)
inline

◆ computeActuatorDynamics() [2/2]

template<class RBDDynamics, size_t ACT_STATE_DIM = 0, bool EE_ARE_CONTROL_INPUTS = false>
template<typename T = void>
std::enable_if<(ACT_STATE_DIM == 0), T>::type ct::rbd::FixBaseFDSystem< RBDDynamics, ACT_STATE_DIM, EE_ARE_CONTROL_INPUTS >::computeActuatorDynamics ( const FixBaseRobotState_t robotState,
const SCALAR t,
const control_vector_t controlIn,
state_vector_t stateDerivative,
control_vector_t controlOut 
)
inline

do nothing if actuators disabled

◆ clone()

template<class RBDDynamics, size_t ACT_STATE_DIM = 0, bool EE_ARE_CONTROL_INPUTS = false>
virtual FixBaseFDSystem<RBDDynamics, ACT_STATE_DIM, EE_ARE_CONTROL_INPUTS>* ct::rbd::FixBaseFDSystem< RBDDynamics, ACT_STATE_DIM, EE_ARE_CONTROL_INPUTS >::clone ( ) const
inlineoverridevirtual

◆ getActuatorDynamics()

template<class RBDDynamics, size_t ACT_STATE_DIM = 0, bool EE_ARE_CONTROL_INPUTS = false>
std::shared_ptr<ActuatorDynamics_t> ct::rbd::FixBaseFDSystem< RBDDynamics, ACT_STATE_DIM, EE_ARE_CONTROL_INPUTS >::getActuatorDynamics ( )
inline

get pointer to actuator dynamics

◆ computeConsistentActuatorState() [1/2]

template<class RBDDynamics, size_t ACT_STATE_DIM = 0, bool EE_ARE_CONTROL_INPUTS = false>
template<typename T = typename FixBaseRobotState_t::actuator_state_vector_t>
std::enable_if<(ACT_STATE_DIM > 0), T>::type ct::rbd::FixBaseFDSystem< RBDDynamics, ACT_STATE_DIM, EE_ARE_CONTROL_INPUTS >::computeConsistentActuatorState ( const JointState< BASE::NJOINTS, SCALAR > &  jStateRef,
const ct::core::ControlVector< BASE::NJOINTS > &  torqueRef 
)
inline

if actuator dynamics enabled, this method allows to design a consistent actuator state

Referenced by ct::rbd::FixBaseFDSystem< RBDDynamics, ACT_STATE_DIM, EE_ARE_CONTROL_INPUTS >::computeConsistentActuatorState().

◆ computeConsistentActuatorState() [2/2]

template<class RBDDynamics, size_t ACT_STATE_DIM = 0, bool EE_ARE_CONTROL_INPUTS = false>
template<typename T = typename FixBaseRobotState_t::actuator_state_vector_t>
std::enable_if<(ACT_STATE_DIM > 0), T>::type ct::rbd::FixBaseFDSystem< RBDDynamics, ACT_STATE_DIM, EE_ARE_CONTROL_INPUTS >::computeConsistentActuatorState ( const JointState< BASE::NJOINTS, SCALAR > &  jStateRef)
inline

Member Data Documentation

◆ N_EE

template<class RBDDynamics, size_t ACT_STATE_DIM = 0, bool EE_ARE_CONTROL_INPUTS = false>
EIGEN_MAKE_ALIGNED_OPERATOR_NEW const size_t ct::rbd::FixBaseFDSystem< RBDDynamics, ACT_STATE_DIM, EE_ARE_CONTROL_INPUTS >::N_EE = RBDDynamics::N_EE
static

◆ STATE_DIM

template<class RBDDynamics, size_t ACT_STATE_DIM = 0, bool EE_ARE_CONTROL_INPUTS = false>
const size_t ct::rbd::FixBaseFDSystem< RBDDynamics, ACT_STATE_DIM, EE_ARE_CONTROL_INPUTS >::STATE_DIM = RBDDynamics::NSTATE + ACT_STATE_DIM
static

number of end-effectors

◆ CONTROL_DIM

template<class RBDDynamics, size_t ACT_STATE_DIM = 0, bool EE_ARE_CONTROL_INPUTS = false>
const size_t ct::rbd::FixBaseFDSystem< RBDDynamics, ACT_STATE_DIM, EE_ARE_CONTROL_INPUTS >::CONTROL_DIM = RBDDynamics::NJOINTS + EE_ARE_CONTROL_INPUTS * N_EE * 3
static

◆ ACTUATOR_STATE_DIM

template<class RBDDynamics, size_t ACT_STATE_DIM = 0, bool EE_ARE_CONTROL_INPUTS = false>
const size_t ct::rbd::FixBaseFDSystem< RBDDynamics, ACT_STATE_DIM, EE_ARE_CONTROL_INPUTS >::ACTUATOR_STATE_DIM = ACT_STATE_DIM
static

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