BipedalLocomotion::TSID::QPTSID class
#include <BipedalLocomotion/TSID/QPTSID.h>

QPTSID is a concrete class and implements a task space inverse dynamics.

The TSID is here implemented as Quadratic Programming (QP) problem. The user should set the desired task with the method QPTSID::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, Velocity and Torque-Controlled Humanoid Robots" https://doi.org/10.1142/S0219843619500348

Base classes

class TaskSpaceInverseDynamics: TaskSpaceInverseDynamics implements the interface for the task space inverse dynamics.

Derived classes

class QPFixedBaseTSID: QPFixedBaseTSID is specialization of QPTSID class in the case of fixed base system.

Constructors, destructors, conversion operators

QPTSID(): Constructor.
~QPTSID() 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 getTaskNames() const -> std::vector<std::string> override: Get a vector containing the name of the tasks.
auto getTask(const std::string& name) const -> std::weak_ptr<Task> override: Get a specific task.
auto initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler) -> bool override: Initialize the TSID algorithm.
auto finalize(const System::VariablesHandler& handler) -> bool override: Finalize the TSID.
auto advance() -> bool override: Solve the fixed base TSID.
auto getOutput() const -> const State& override: Get the outcome of the optimization problem.
auto isOutputValid() const -> bool override: Return true if the content of get is valid.
auto toString() const -> std::string override: Return the description of the TSID problem.
auto getRawSolution() const -> Eigen::Ref<const Eigen::VectorXd> override: Return the vector representing the entire solution of the QPTSID.

Function documentation

bool BipedalLocomotion::TSID::QPTSID::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::TSID::QPTSID::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::TSID::QPTSID::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::TSID::QPTSID::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::TSID::QPTSID::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

std::vector<std::string> BipedalLocomotion::TSID::QPTSID::getTaskNames() const override

Get a vector containing the name of the tasks.

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

std::weak_ptr<Task> BipedalLocomotion::TSID::QPTSID::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 TSID. If the task does not exist a nullptr is returned.

bool BipedalLocomotion::TSID::QPTSID::initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler) override

Initialize the TSID algorithm.

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

Note

the following parameters are required by the class

Parameter Name	Type	Description	Mandatory
`robot_acceleration_variable_name`	`string`	Name of the variable contained in `VariablesHandler` describing the generalized robot acceleration	Yes
`joint_torques_variable_name`	`string`	Name of the variable contained in `VariablesHandler` describing the robot torque	Yes
`contact_wrench_variables_name`	`vector<string>`	List of the variables associated to the contact wrenches in the `VariablesHandler`	Yes
`verbosity`	`bool`	Verbosity of the solver. Default value `false`	No

Where the generalized robot acceleration is a vector containing the base acceleration (expressed in mixed representation) and the joint accelerations,

bool BipedalLocomotion::TSID::QPTSID::finalize(const System::VariablesHandler& handler) override

Finalize the TSID.

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

bool BipedalLocomotion::TSID::QPTSID::advance() override

Solve the fixed base TSID.

Returns	true in case of success and false otherwise.

const State& BipedalLocomotion::TSID::QPTSID::getOutput() const override

Get the outcome of the optimization problem.

Returns	the state of the TSID.

Eigen::Ref<const Eigen::VectorXd> BipedalLocomotion::TSID::QPTSID::getRawSolution() const override

Return the vector representing the entire solution of the QPTSID.

Returns	a vector containing the solution of the optimization problem