BipedalLocomotion::ContinuousDynamicalSystem::CentroidalDynamics class
#include <BipedalLocomotion/ContinuousDynamicalSystem/CentroidalDynamics.h>

CentroidalDynamics describes the centroidal dynamics of a multi-body system.

The centroidal momentum ${}_{\bar{G}} h ^\top= \begin{bmatrix} {}_{\bar{G}} h^{p\top} & {}_{\bar{G}} h^{\omega\top} \end{bmatrix} \in \mathbb{R}^6$ is the aggregate linear and angular momentum of each link of the robot referred to the robot center of mass (CoM). The vectors ${}_{\bar{G}} h^p \in \mathbb{R}^3$ and ${}_{\bar{G}} h^\omega * \in \mathbb{R}^3$ are the linear and angular momentum, respectively. The coordinates of ${}_{\bar{G}} h$ are expressed w.r.t. a frame centered in the robot CoM and oriented as the inertial frame $\mathcal{I}$ The CentroidalDynamics class describes the dynamics of the centroidal momentum and of the CoM. Indeed the time derivative of the centroidal momentum depends on the external contact wrenches acting on the system, thus leading to:

\[ {}_{\bar{G}} \dot{h} = \sum_{i = 1}^{n_c} \begin{bmatrix} I_3 & 0_3 \ (p_{\mathcal{C}_i} - p_{\text{CoM}})^\wedge & I_3 \end{bmatrix} \mathrm{f}_i + m\bar{g} \]

where $\bar{g}^\top = \begin{bmatrix} g^\top & 0_{1\times3} \end{bmatrix}$ , $m$ is the mass of the robot and $n_c$ the number of active contacts. The relation between the linear momentum ${}_{\bar{G}} h^{p}$ and the robot CoM velocity is linear and depends on the robot mass $m$ , i.e. ${}_{\bar{G}} h^{p} = m v_{\text{CoM}}$ .

The CentroidalDynamics inherits from a generic DynamicalSystem where the State is described by a BipedalLocomotion::GenericContainer::named_tuple

NameTypeDescription
com_posEigen::Vector3dPosition of the CoM written in the inertial frame
com_velEigen::Vector3dVelocity of the CoM written in the inertial frame
angular_momentumEigen::Vector3dThe centroidal angular momentum whose coordinates are written respect the inertial frame.

The StateDerivative is described by a BipedalLocomotion::GenericContainer::named_tuple

NameTypeDescription
com_velEigen::Vector3dVelocity of the CoM written in the inertial frame
com_accEigen::Vector3dAcceleration of the CoM written in the inertial frame
angular_momentum_rateEigen::Vector3dThe centroidal angular momentum rate of change written respect the inertial frame.

The Input is described by a BipedalLocomotion::GenericContainer::named_tuple

NameTypeDescription
contactsstd::unordered_map<std::string, BipedalLocomotion::Contacts::DiscreteGeometryContact>List of contact where each force applied in the discrete geometry contact is expressed with respect to the inertial frame
external_wrenchstd::optional<BipedalLocomotion::Math::Wrenchd>Optional wrench applied to the robot center of mass. The coordinates are expressed in the inertial frame

Base classes

template<class _Derived>
class DynamicalSystem<CentroidalDynamics>
DynamicalSystem defines a continuous time dynamical system, i.e.

Public functions

auto initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler) -> bool
Initialize the CentroidalDynamics system.
auto dynamics(const std::chrono::nanoseconds& time, StateDerivative& stateDerivative) -> bool
Computes the centroidal momentum dynamics.
auto setState(const State& state) -> bool
Set the state of the dynamical system.
auto getState() const -> const State&
Get the state to the dynamical system.
auto setControlInput(const Input& controlInput) -> bool
Set the control input to the dynamical system.
void setGravityVector(const Eigen::Ref<const Eigen::Vector3d>& gravity)
Set the gravity vector of the dynamical system.

Function documentation

bool BipedalLocomotion::ContinuousDynamicalSystem::CentroidalDynamics::initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler)

Initialize the CentroidalDynamics system.

Parameters
handler pointer to the parameter handler.
Returns true in case of success/false otherwise.

bool BipedalLocomotion::ContinuousDynamicalSystem::CentroidalDynamics::dynamics(const std::chrono::nanoseconds& time, StateDerivative& stateDerivative)

Computes the centroidal momentum dynamics.

Parameters
time the time at witch the dynamics is computed.
stateDerivative
Returns true in case of success, false otherwise.

It return $f(x, u, t)$ .

bool BipedalLocomotion::ContinuousDynamicalSystem::CentroidalDynamics::setState(const State& state)

Set the state of the dynamical system.

Parameters
state tuple containing a const reference to the state elements.
Returns true in case of success, false otherwise.

const State& BipedalLocomotion::ContinuousDynamicalSystem::CentroidalDynamics::getState() const

Get the state to the dynamical system.

Returns the current state of the dynamical system

bool BipedalLocomotion::ContinuousDynamicalSystem::CentroidalDynamics::setControlInput(const Input& controlInput)

Set the control input to the dynamical system.

Parameters
controlInput the value of the control input used to compute the system dynamics.
Returns true in case of success, false otherwise.

void BipedalLocomotion::ContinuousDynamicalSystem::CentroidalDynamics::setGravityVector(const Eigen::Ref<const Eigen::Vector3d>& gravity)

Set the gravity vector of the dynamical system.

Parameters
gravity the value of the gravity vector used in the system dynamics.
Returns true in case of success, false otherwise.