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

QPInverseKinematics is a concrete class and implements an integration base inverse kinematics.

The inverse kinematics is here implemented as Quadratic Programming (QP) problem. The user should set the desired task with the method QPInverseKinematics::addTask. Each task has a given priority. Currently we support only priority equal to 0 or 1. If the task priority is set to 0 the task will be considered as hard task, thus treated as an equality constraint. If the priority is equal to 1 the task will be embedded in the cost function. The class is also able to treat inequality constraints. A possible usage of the IK can be found in "Romualdi et al. A Benchmarking of DCM Based Architectures for Position and Velocity Controlled Walking of Humanoid Robots" https://doi.org/10.1109/HUMANOIDS.2018.8625025 Here you can find an example of the QPInverseKinematics class used as velocity controller or IK

Velocity Control

Here you can find an example of the QPFixedBaseInverseKinematics interface used as a velocity controller.

Inverse Kinematics

If you want to use QPInverseKinematics as IK you need to integrate the output velocity. System::FloatingBaseSystemKinematics and System::Integrator classes can be used to integrate the output of the IK taking into account the geometrical structure of the configuration space ( )

Base classes

class IntegrationBasedIK: IntegrationBasedIK implements the interface for the integration base inverse kinematics.

Derived classes

class QPFixedBaseInverseKinematics: QPFixedBaseInverseKinematics is specialization of QPInverseKinematics class in the case of fixed base system.

Public static functions

static auto build(std::weak_ptr<const ParametersHandler::IParametersHandler> handler, std::shared_ptr<iDynTree::KinDynComputations> kinDyn) -> IntegrationBasedIKProblem: Build the inverse kinematics solver.

Constructors, destructors, conversion operators

QPInverseKinematics(): Constructor.
~QPInverseKinematics() virtual: Destructor.

Public functions

auto addTask(std::shared_ptr<Task> task, const std::string& taskName, std::size_t priority, std::shared_ptr<const System::WeightProviderPort> weightProvider = nullptr) -> bool override: Add a linear task in the solver.
auto addTask(std::shared_ptr<Task> task, const std::string& taskName, std::size_t priority, Eigen::Ref<const Eigen::VectorXd> weight) -> bool override: Add a linear task in the solver.
auto setTaskWeight(const std::string& taskName, std::shared_ptr<const System::WeightProviderPort> weightProvider) -> bool override: Set the weightProvider associated to an already existing task.
auto setTaskWeight(const std::string& taskName, Eigen::Ref<const Eigen::VectorXd> weight) -> bool override: Set the weight associated to an already existing task.
auto getTaskWeightProvider(const std::string& taskName) const -> std::weak_ptr<const System::WeightProviderPort> override: Get the weightProvider associated to an already existing task.
auto finalize(const System::VariablesHandler& handler) -> bool override: Finalize the IK.
auto initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler) -> bool override: Initialize the inverse kinematics algorithm.
auto getTaskNames() const -> std::vector<std::string> override: Get a vector containing the name of the tasks.
auto isOutputValid() const -> bool override: Return true if the content of get is valid.
auto advance() -> bool override: Solve the inverse kinematics.
auto getOutput() const -> const State& override: Get the outcome of the optimization problem.
auto getTask(const std::string& name) const -> std::weak_ptr<Task> override: Get a specific task.
auto toString() const -> std::string override: Return the description of the InverseKinematics problem.
auto getRawSolution() const -> Eigen::Ref<const Eigen::VectorXd> override: Return the vector representing the entire solution of the QPInverseKinematics.

Function documentation

static IntegrationBasedIKProblem BipedalLocomotion::IK::QPInverseKinematics::build(std::weak_ptr<const ParametersHandler::IParametersHandler> handler, std::shared_ptr<iDynTree::KinDynComputations> kinDyn)

Build the inverse kinematics solver.

Parameters
handler	pointer to the IParametersHandler interface.
kinDyn	a pointer to an iDynTree::KinDynComputations object that will be shared among all the tasks.
Returns	an IntegrationBasedIKProblem. In case of issues an invalid IntegrationBasedIKProblem will be returned.

Note

the following parameters are required by the class

Group	Parameter Name	Type	Description	Mandatory
	`tasks`	`vector<string>`	Vector containing the list of the tasks considered in the IK.	Yes
`IK`	`robot_velocity_variable_name`	`string`	Name of the variable contained in `VariablesHandler` describing the generalized robot velocity	Yes
`IK`	`verbosity`	`bool`	Verbosity of the solver. Default value `false`	No

Where the generalized robot velocity is a vector containing the base spatialvelocity (expressed in mixed representation) and the joint velocities. For each task listed in the parameter tasks the user must specify all the parameters required by the task itself but robot_velocity_variable_name since is already specified in the IK group. Moreover the following parameters are required for each task.

Group	Parameter Name	Type	Description	Mandatory
`TASK_NAME`	`type`	`string`	String representing the type of the task. The string should match the name of the C++ class.	Yes
`TASK_NAME`	`priority`	`int`	Priority associated to the task. (Check QPInverseKinematics::addTask for further information)	Yes
`TASK_NAME`	`weight_provider_type`	`string`	String representing the type of the weight provider. The string should match the name of the C++ class. It is required only if the task is low priority. The default value in case of low priority task (`priority = 1`) is `ConstantWeightProvider`	No

Given the weight type specified by weight_provider_type, the user must specify all the parameters required by the provider in the TASK_NAME group handler TASK_NAME is a placeholder for the name of the task contained in the tasks list.

The following ini file presents an example of the configuration that can be used to build the IK

tasks                           ("COM_TASK", "RIGHT_FOOT_TASK", "LEFT_FOOT_TASK", "TORSO_TASK", "JOINT_REGULARIZATION_TASK")

[IK]
robot_velocity_variable_name    robot_velocity

[COM_TASK]
type                            CoMTask
kp_linear                       2.0
mask                            (true, true, true)
priority                        0

[RIGHT_FOOT_TASK]
type                            SE3Task
frame_name                      r_sole
kp_linear                       7.0
kp_angular                      5.0
priority                        0

[LEFT_FOOT_TASK]
type                            SE3Task
frame_name                      l_sole
kp_linear                       7.0
kp_angular                      5.0
priority                        0


[JOINT_REGULARIZATION_TASK]
type                            JointTrackingTask
kp                              (5.0, 5.0, 5.0,
                                 5.0, 5.0, 5.0, 5.0,
                                 5.0, 5.0, 5.0, 5.0,
                                 5.0, 5.0, 5.0, 5.0, 5.0, 5.0,
                                 5.0, 5.0, 5.0, 5.0, 5.0, 5.0)
priority                        1
weight_provider_type            ConstantWeightProvider
weight                          (1.0, 1.0, 1.0,
                                 2.0, 2.0, 2.0, 2.0,
                                 2.0, 2.0, 2.0, 2.0,
                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
                                 1.0, 1.0, 1.0, 1.0, 1.0, 1.0)

[include TORSO_TASK "./tasks/torso.ini"]

Where the file ./tasks/torso.ini contains the definition of a low priority task whose weight is a BipedalLocomotion::ContinuousDynamicalSystem::MultiStateWeightProvider. Since MultiStateWeightProvider requires the definition of subgroups, an additional file is suggested as explained in: https://github.com/robotology/yarp/discussions/2563

type                            SO3Task
frame_name                      chest
kp_angular                      5.0

weight_provider_type            MultiStateWeightProvider

states                          ("STANCE", "WALKING")
sampling_time                   0.01
settling_time                   3.0

[STANCE]
name                            stance
weight                          (0.1, 0.1, 0.1)

[WALKING]
name                            walking
weight                          (5.0, 5.0, 5.0)

bool BipedalLocomotion::IK::QPInverseKinematics::addTask(std::shared_ptr<Task> task, const std::string& taskName, std::size_t priority, std::shared_ptr<const System::WeightProviderPort> weightProvider = nullptr) override

Add a linear task in the solver.

Parameters
task	pointer to a given linear task
taskName	unique name associated to the task.
priority	Priority associated to the task. The lower the number the higher the priority.
weightProvider	Weight provider associated to the task. This parameter is optional. The default value is an object that does not contain any value. The user may avoid to pass a provider only if the priority of the task is equal to 0.
Returns	true if the task has been added to the solver.

bool BipedalLocomotion::IK::QPInverseKinematics::addTask(std::shared_ptr<Task> task, const std::string& taskName, std::size_t priority, Eigen::Ref<const Eigen::VectorXd> weight) override

Add a linear task in the solver.

Parameters
task	pointer to a given linear task
taskName	unique name associated to the task.
priority	Priority associated to the task. The lower the number the higher the priority.
weight	Weight associated to the task.
Returns	true if the task has been added to the solver.

bool BipedalLocomotion::IK::QPInverseKinematics::setTaskWeight(const std::string& taskName, std::shared_ptr<const System::WeightProviderPort> weightProvider) override

Set the weightProvider associated to an already existing task.

Parameters
taskName	name associated to the task
weightProvider	new weight provider associated to the task.
Returns	true if the weight has been updated

bool BipedalLocomotion::IK::QPInverseKinematics::setTaskWeight(const std::string& taskName, Eigen::Ref<const Eigen::VectorXd> weight) override

Set the weight associated to an already existing task.

Parameters
taskName	name associated to the task
weight	new Weight associated to the task. A constant weight is assumed.
Returns	true if the weight has been updated

std::weak_ptr<const System::WeightProviderPort> BipedalLocomotion::IK::QPInverseKinematics::getTaskWeightProvider(const std::string& taskName) const override

Get the weightProvider associated to an already existing task.

Parameters
taskName	name associated to the task
Returns	a weak pointer to the weightProvider. If the task does not exist the pointer is not lockable

bool BipedalLocomotion::IK::QPInverseKinematics::finalize(const System::VariablesHandler& handler) override

Finalize the IK.

Parameters
handler	parameter handler.
Returns	true in case of success, false otherwise.

bool BipedalLocomotion::IK::QPInverseKinematics::initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler) override

Initialize the inverse kinematics algorithm.

Parameters
handler	pointer to the IParametersHandler interface.
Returns	True in case of success, false otherwise.

Note

the following parameters are required by the class

Parameter Name	Type	Description	Mandatory
`robot_velocity_variable_name`	`string`	Name of the variable contained in `VariablesHandler` describing the generalized robot velocity	Yes
`verbosity`	`bool`	Verbosity of the solver. Default value `false`	No

Where the generalized robot velocity is a vector containing the base spatial velocity (expressed in mixed representation) and the joint velocities.

std::vector<std::string> BipedalLocomotion::IK::QPInverseKinematics::getTaskNames() const override

Get a vector containing the name of the tasks.

Returns	an std::vector containing all the names associated to the tasks

bool BipedalLocomotion::IK::QPInverseKinematics::advance() override

Solve the inverse kinematics.

Returns	true in case of success and false otherwise.

const State& BipedalLocomotion::IK::QPInverseKinematics::getOutput() const override

Get the outcome of the optimization problem.

Returns	the state of the inverse kinematics.

std::weak_ptr<Task> BipedalLocomotion::IK::QPInverseKinematics::getTask(const std::string& name) const override

Get a specific task.

Parameters
name	name associated to the task.
Returns	a weak ptr associated to an existing task in the IK. If the task does not exist a nullptr is returned.

std::string BipedalLocomotion::IK::QPInverseKinematics::toString() const override

Return the description of the InverseKinematics problem.

Returns	a string containing the description of the solver.

Eigen::Ref<const Eigen::VectorXd> BipedalLocomotion::IK::QPInverseKinematics::getRawSolution() const override

Return the vector representing the entire solution of the QPInverseKinematics.

Returns	a vector containing the solution of the optimization problem