BipedalLocomotion::IK::GravityTask class
#include <BipedalLocomotion/IK/GravityTask.h>

The GravityTask class aligns the z axis of a frame to a desired gravity direction.

The task assumes perfect control of the robot velocity $\nu$ that contains the base linear and angular velocity expressed in mixed representation and the joint velocities. The task represents the following equation

\[ \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \end{bmatrix} J_\omega \nu = -k \begin{bmatrix} 0 &-1 & 0 \\ 1 & 0 & 0 \end{bmatrix} {} ^ W R_f {} ^ f g ^ * + {} ^ W R_f {} ^ f \omega_{ff} \]

where $J_\omega$ is the angular part of the frame jacobian. $k$ is the controller gain. ${} ^ W R_f$ is the frame rotation. ${} ^ W g ^ *$ is the desired z axis direction expressed in the frame coordinates. ${} ^ W \omega_{ff}$ is the feed-forward velocity expressed in the frame coordinates. The explanation is as follows.

We define ${}^wR_b$ as the orientation of the frame $b$ we want to move, and ${}^b\hat{V}$ as the unitary vector fixed in frame $b$ that we want to align to ${}^wz = e_3 = [0 0 1]^\top$.

We define the following Lyapunov function:

\[ V = 1 - \cos(\theta) \geq 0. \]

Since $ a^\top b = ||a|| ~||b|| \cos(\theta) $ , we have:

\[ V = 1 - \left({}^wR_b{}^b\hat{V}\right)^\top e_3, \]

given that both vectors are unitary.

The time derivative of the Lyapunov function is as follows:

\[ \dot{V} = - \left({}^w\dot{R}_b{}^b\hat{V}\right)^\top e_3, \]

where $ {}^w\dot{R}_b = S({}^w\omega){}^w{R}_b $ , with $ S(\cdot) $ being a skew symmetric matrix, and $ {}^w\omega \in \mathbb{R}^3 $ is the right-trivialized angular velocity. Hence,

\[ \dot{V} = - \left(S({}^w\omega){}^w{R}_b{}^b\hat{V}\right)^\top e_3. \]

Exploiting $ S(\cdot)^\top = -S(\cdot) $ , we have:

\[ \dot{V} = {}^b\hat{V}^\top{}^w{R}_b^\top S({}^w\omega) e_3, \]

and $ S(a)b = -S(b)a $ :

\[ \dot{V} = -{}^b\hat{V}^\top{}^w{R}_b^\top S(e_3){}^w\omega. \]

Then, if we choose:

\[ {}^w\omega = k \left({}^b\hat{V}^\top{}^w{R}_b^\top S(e_3)\right)^\top + \lambda e_3, \]

with $ k, \lambda \in \mathbb{R}^3 $ , we have that $ \dot{V} \leq 0 $ . Then,

\[ {}^w\omega = -k S(e_3) {}^w{R}_b{}^b\hat{V} + \lambda e_3. \]

We can introduce the right-trivialized Jacobian $ {}^wJ $ such that $ {}^wJ\dot{\nu} = {}^w\omega $ :

\[ {}^wJ\dot{\nu} = -k S(e_3) {}^w{R}_b{}^b\hat{V} + \lambda e_3. \]

We can filter out the rotation around the world Z-axis, thus obtaining:

\[ \begin{bmatrix} e_1 & e_2 \end{bmatrix}^\top {}^wJ\dot{\nu} = -k \begin{bmatrix} 0 & -1 & 0\\ 1 & 0 & 0 \end{bmatrix} {}^w{R}_b{}^b\hat{V}. \]

Base classes

struct IKLinearTask
IKLinearTask specializes a LinearTask in the case of Inverse Kinematics.

Public functions

auto initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler) -> bool override
Initialize the task.
auto setKinDyn(std::shared_ptr<iDynTree::KinDynComputations> kinDyn) -> bool override
Set the kinDynComputations object.
auto setVariablesHandler(const System::VariablesHandler& variablesHandler) -> bool override
Set the set of variables required by the task.
auto update() -> bool override
Update the content of the task.
auto setDesiredGravityDirectionInTargetFrame(const Eigen::Ref<const Eigen::Vector3d> desiredGravityDirection) -> bool
Set the desired gravity direction expressed the target frame.
auto setFeedForwardVelocityInTargetFrame(const Eigen::Ref<const Eigen::Vector3d> feedforwardVelocity) -> bool
Set the feedforward angular velocity expressed in the target frame.
auto setSetPoint(const Eigen::Ref<const Eigen::Vector3d> desiredGravityDirection, const Eigen::Ref<const Eigen::Vector3d> feedforwardVelocity = Eigen::Vector3d::Zero()) -> bool
Set the desired gravity direction expressed the target frame and the feedforward velocity.
auto size() const -> std::size_t override
Get the size of the task.
auto type() const -> Type override
The DistanceTask is an equality task.
auto isValid() const -> bool override
Determines the validity of the objects retrieved with getA() and getB()

Function documentation

bool BipedalLocomotion::IK::GravityTask::initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler) override

Initialize the task.

Parameters
paramHandler pointer to the parameters handler.
Returns True in case of success, false otherwise.

bool BipedalLocomotion::IK::GravityTask::setKinDyn(std::shared_ptr<iDynTree::KinDynComputations> kinDyn) override

Set the kinDynComputations object.

Parameters
kinDyn pointer to a kinDynComputations object.
Returns True in case of success, false otherwise.

bool BipedalLocomotion::IK::GravityTask::setVariablesHandler(const System::VariablesHandler& variablesHandler) override

Set the set of variables required by the task.

Parameters
variablesHandler reference to a variables handler.
Returns True in case of success, false otherwise.

The variables are stored in the System::VariablesHandler.

bool BipedalLocomotion::IK::GravityTask::update() override

Update the content of the task.

Returns True in case of success, false otherwise.

bool BipedalLocomotion::IK::GravityTask::setDesiredGravityDirectionInTargetFrame(const Eigen::Ref<const Eigen::Vector3d> desiredGravityDirection)

Set the desired gravity direction expressed the target frame.

Parameters
desiredGravityDirection The desired gravity direction.
Returns True in case of success, false otherwise.

The input is normalized, unless the norm is too small.

bool BipedalLocomotion::IK::GravityTask::setFeedForwardVelocityInTargetFrame(const Eigen::Ref<const Eigen::Vector3d> feedforwardVelocity)

Set the feedforward angular velocity expressed in the target frame.

Parameters
feedforwardVelocity The desired feedforward velocity in rad/s.
Returns True in case of success, false otherwise. Only the first two components are used.

bool BipedalLocomotion::IK::GravityTask::setSetPoint(const Eigen::Ref<const Eigen::Vector3d> desiredGravityDirection, const Eigen::Ref<const Eigen::Vector3d> feedforwardVelocity = Eigen::Vector3d::Zero())

Set the desired gravity direction expressed the target frame and the feedforward velocity.

Parameters
desiredGravityDirection The desired gravity direction in target frame.
feedforwardVelocity The desired feedforward velocity in rad/s expressed in the target frame.
Returns True in case of success, false otherwise.

The desiredGravityDirection is normalized, unless the norm is too small. Only the first two components of the feedforwardVelocity are used. This is equivalent of using setDesiredGravityDirectionInTargetFrame and setFeedForwardVelocityInTargetFrame

std::size_t BipedalLocomotion::IK::GravityTask::size() const override

Get the size of the task.

Returns the size of the task.

(I.e the number of rows of the vector b)

Type BipedalLocomotion::IK::GravityTask::type() const override

The DistanceTask is an equality task.

Returns the size of the task.

bool BipedalLocomotion::IK::GravityTask::isValid() const override

Determines the validity of the objects retrieved with getA() and getB()

Returns True if the objects are valid, false otherwise.