A floating base rigid body system that uses forward dynamics. The input vector is assumed to consist of joint torques and end-effector forces expressed in the world. More...

#include <ProjectedFDSystem.h>

Inheritance diagram for ct::rbd::ProjectedFDSystem< RBDDynamics, QUAT_INTEGRATION >:

Public Types
using	Dynamics = RBDDynamics

using	Kinematics = typename RBDDynamics::Kinematics_t

typedef RBDDynamics::SCALAR	SCALAR

typedef core::StateVector< STATE_DIM, SCALAR >	StateVector

typedef core::ControlVector< CONTROL_DIM, SCALAR >	ControlVector

typedef core::ControlledSystem< RBDDynamics::RBDState_t::NSTATE+QUAT_INTEGRATION, RBDDynamics::NJOINTS >	Base

Public Types inherited from ct::core::ControlledSystem< RBDDynamics::NSTATE+QUAT_INTEGRATION, RBDDynamics::NJOINTS, RBDDynamics::SCALAR >
typedef std::shared_ptr< ControlledSystem< RBDDynamics::NSTATE+QUAT_INTEGRATION, RBDDynamics::NJOINTS, 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
	ProjectedFDSystem (typename RBDDynamics::EE_in_contact_t eeInContact=true)

virtual	~ProjectedFDSystem ()

virtual RBDDynamics &	dynamics () override

virtual const RBDDynamics &	dynamics () const override

virtual void	computeControlledDynamics (const StateVector &state, const SCALAR &t, const ControlVector &control, StateVector &derivative) override

void	setEEInContact (typename RBDDynamics::EE_in_contact_t &eeInContact)

RBDDynamics::RBDState_t	RBDStateFromVector (const StateVector &state)

template<bool T>
RBDDynamics::RBDState_t	RBDStateFromVectorImpl (const StateVector &state, typename std::enable_if< T, bool >::type=true)

template<bool T>
RBDDynamics::RBDState_t	RBDStateFromVectorImpl (const StateVector &state, typename std::enable_if<!T, bool >::type=true)

template<bool T>
StateVector	toStateDerivative (const typename RBDDynamics::RBDAcceleration_t &acceleration, const typename RBDDynamics::RBDState_t &state, typename std::enable_if< T, bool >::type=true)

template<bool T>
StateVector	toStateDerivative (const typename RBDDynamics::RBDAcceleration_t &acceleration, const typename RBDDynamics::RBDState_t &state, typename std::enable_if<!T, bool >::type=true)

virtual ProjectedFDSystem< RBDDynamics, QUAT_INTEGRATION > *	clone () const override

Public Member Functions inherited from ct::rbd::RBDSystem< RBDDynamics, QUAT_INTEGRATION >
	RBDSystem ()=default

virtual	~RBDSystem ()=default

Public Member Functions inherited from ct::core::ControlledSystem< RBDDynamics::NSTATE+QUAT_INTEGRATION, RBDDynamics::NJOINTS, RBDDynamics::SCALAR >
	ControlledSystem (const SYSTEM_TYPE &type=SYSTEM_TYPE::GENERAL)

	ControlledSystem (std::shared_ptr< ct::core::Controller< RBDDynamics::NSTATE+QUAT_INTEGRATION, RBDDynamics::NJOINTS, RBDDynamics::SCALAR >> controller, const SYSTEM_TYPE &type=SYSTEM_TYPE::GENERAL)

	ControlledSystem (const ControlledSystem &arg)

virtual	~ControlledSystem ()

void	setController (const std::shared_ptr< Controller< RBDDynamics::NSTATE+QUAT_INTEGRATION, RBDDynamics::NJOINTS, RBDDynamics::SCALAR >> &controller)

void	getController (std::shared_ptr< Controller< RBDDynamics::NSTATE+QUAT_INTEGRATION, RBDDynamics::NJOINTS, RBDDynamics::SCALAR >> &controller) const

std::shared_ptr< Controller< RBDDynamics::NSTATE+QUAT_INTEGRATION, RBDDynamics::NJOINTS, RBDDynamics::SCALAR > >	getController ()

virtual void	computeDynamics (const StateVector< RBDDynamics::NSTATE+QUAT_INTEGRATION, RBDDynamics::SCALAR > &state, const time_t &t, StateVector< RBDDynamics::NSTATE+QUAT_INTEGRATION, RBDDynamics::SCALAR > &derivative) override

virtual void	computeControlledDynamics (const StateVector< RBDDynamics::NSTATE+QUAT_INTEGRATION, RBDDynamics::SCALAR > &state, const time_t &t, const ControlVector< RBDDynamics::NJOINTS, RBDDynamics::SCALAR > &control, StateVector< RBDDynamics::NSTATE+QUAT_INTEGRATION, RBDDynamics::SCALAR > &derivative)=0

ControlVector< RBDDynamics::NJOINTS, RBDDynamics::SCALAR >	getLastControlAction ()

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 const size_t	N_EE = RBDDynamics::N_EE

static const size_t	STATE_DIM = RBDDynamics::NSTATE + QUAT_INTEGRATION

static const size_t	CONTROL_DIM = RBDDynamics::NJOINTS

Additional Inherited Members
Public Attributes inherited from ct::core::ControlledSystem< RBDDynamics::NSTATE+QUAT_INTEGRATION, RBDDynamics::NJOINTS, RBDDynamics::SCALAR >
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef System< RBDDynamics::NSTATE+QUAT_INTEGRATION, RBDDynamics::SCALAR >	Base

Public Attributes inherited from ct::core::System< STATE_DIM, SCALAR >
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef SCALAR	S

Protected Attributes inherited from ct::core::ControlledSystem< RBDDynamics::NSTATE+QUAT_INTEGRATION, RBDDynamics::NJOINTS, RBDDynamics::SCALAR >
std::shared_ptr< Controller< RBDDynamics::NSTATE+QUAT_INTEGRATION, RBDDynamics::NJOINTS, RBDDynamics::SCALAR > >	controller_

ControlVector< RBDDynamics::NJOINTS, RBDDynamics::SCALAR >	controlAction_

Protected Attributes inherited from ct::core::System< STATE_DIM, SCALAR >
SYSTEM_TYPE	type_

Detailed Description

template<class RBDDynamics, bool QUAT_INTEGRATION = false>
class ct::rbd::ProjectedFDSystem< RBDDynamics, QUAT_INTEGRATION >

A floating base rigid body system that uses forward dynamics. The input vector is assumed to consist of joint torques and end-effector forces expressed in the world.

This class creates a ct::core::ControlledSystem based on ProjectedDynamics to make it compatible with ct::core functionality such as a ct::core::Integrator. Please note that there are multiple choices of transforming a Rigid Body Dynamics system into an ODE used for integration and (optimal) control. This particular modelling is explained below. For other choices see FloatingBaseFDSystem and FixBaseFDSystem.

The goal of this class is to transform a Rigid Body Dynamics system into an ODE of the form $\dot{x} = f(x,u)$ using projected forward dynamics as implemented in ProjectedDynamics. To do so, we define the state as

$x = [ {}_W q_B ~ {}_W p_B ~ \theta_J ~ {}_B \omega_B ~ {}_B v_B ~ \dot{\theta}_J ]^T$

where ${}_W q_B$ is the base orientation and ${}_W p_B$ is the base position, both expressed the world frame. ${}_B \omega_B$ is the local angular velocity of the base and ${}_B v_B$ is the linear velocity of the base, both are expressed in the local base coordinate frame. Hence, the velocities are NOT direct derivatives of the position/orientation. $\theta_J$ and $\dot{theta}_J$ are the joint angles and joint velocities, respectively.

The input vector $u = \tau$ contains the joint torques. We further assume the base is unactuated.

Given the Projected Rigid Body Dynamics

${}_B \ddot{q}_c = P M^{-1} (J^T_c \lambda + S^T \tau - G - C)$

where ${}_B \ddot{q}_c = [ {}_B \dot{\omega}_B ~ {}_B \dot{v}_B ~ \ddot{\theta}_J ]^T$ are the generalized coordinates expressed in the base frame.

The system dynamics then become:

$\begin{aligned} \dot{x} &= [ {}_W \dot{q}_B ~ {}_W \dot{p}_B ~ \dot{\theta}_J ~ {}_B \dot{\omega}_B ~ {}_B \dot{v}_B ~ \ddot{\theta}_J ]^T \\ &= [ H_{WB} {}_B \omega_B ~ R_{WB} {}_B v_B ~ \dot{\theta}_J ~ P M^{-1} (J^T_c \lambda + S^T \tau - G - C)]^T \end{aligned}$