BipedalLocomotion::ContinuousDynamicalSystem::FloatingBaseDynamicsWithCompliantContacts class

FloatingBaseDynamicalSystem describes a floating base dynamical system.

The FloatingBaseDynamicalSystem inherits from a generic DynamicalSystem where:

  • DynamicalSystem::StateType is described by an std::tuple containing:
    • Eigen::Vector6d: the base velocity expressed in mixed representation;
    • Eigen::VectorXd: the joint velocities [in rad/s];
    • Eigen::Vector3d: position of the base w.r.t. the inertial frame
    • Eigen::Matrix3d: rotation matrix ${} ^ I R _ {b}$ . Matrix that transform a vector whose coordinates are expressed in the base frame in the inertial frame;
    • Eigen::VectorXd: the joint positions [in rad].
  • DynamicalSystem::StateDerivativeType is described by an std::tuple containing:
    • Eigen::Vector6d: the base acceleration expressed in mixed representation;
    • Eigen::VectorXd: the joint accelerations [in rad/s^2];
    • Eigen::Vector3d: base velocity w.r.t. the inertial frame;
    • Eigen::Matrix3d: rate of change of the rotation matrix ${} ^ I \dot{R} _ {b}$ . whose coordinates are expressed in the base frame in the inertial frame;
    • Eigen::VectorXd: the joint velocities [in rad/s].
  • DynamicalSystem::InputType is described by an std::tuple containing:
    • Eigen::VectorXd: the joint torques [in Nm];
    • std::vector<CompliantContactWrench>: List of contact wrenches.

Base classes

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

Public functions

auto initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler) -> bool
Initialize the FloatingBaseDynamicsWithCompliantContacts system.
auto setRobotModel(const iDynTree::Model& model) -> bool
Set the model of the robot.
auto setMassMatrixRegularization(const Eigen::Ref<const Eigen::MatrixXd>& matrix) -> bool
Set the mass matrix regularization term.
auto dynamics(const std::chrono::nanoseconds& time, StateDerivative& stateDerivative) -> bool
Computes the floating based system 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.

Function documentation

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

Initialize the FloatingBaseDynamicsWithCompliantContacts system.

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

bool BipedalLocomotion::ContinuousDynamicalSystem::FloatingBaseDynamicsWithCompliantContacts::setRobotModel(const iDynTree::Model& model)

Set the model of the robot.

Parameters
model an iDynTree robot model.
Returns true in case of success, false otherwise.

bool BipedalLocomotion::ContinuousDynamicalSystem::FloatingBaseDynamicsWithCompliantContacts::setMassMatrixRegularization(const Eigen::Ref<const Eigen::MatrixXd>& matrix)

Set the mass matrix regularization term.

Parameters
matrix the regularization term for the mass matrix. @notice Calling this function is not mandatory. Call it only if you want to add a regularization term.
Returns true in case of success, false otherwise.

i.e. $\bar{M} = M + M _ {reg}$ . Where $M$ is the mass matrix and $M_{reg}$ is the matrix regularization term.

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

Computes the floating based system 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::FloatingBaseDynamicsWithCompliantContacts::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::FloatingBaseDynamicsWithCompliantContacts::getState() const

Get the state to the dynamical system.

Returns the current state of the dynamical system

bool BipedalLocomotion::ContinuousDynamicalSystem::FloatingBaseDynamicsWithCompliantContacts::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.