#include <mc_solver/TasksQPSolver.h>

Inheritance diagram for mc_solver::TasksQPSolver:

Collaboration diagram for mc_solver::TasksQPSolver:

Public Member Functions
	TasksQPSolver (mc_rbdyn::RobotsPtr robots, double timeStep)

	TasksQPSolver (double timeStep)

	~TasksQPSolver () final=default

void	addTask (tasks::qp::Task *task)

void	addTask (std::shared_ptr< tasks::qp::Task > task)

void	removeTask (tasks::qp::Task *task)

void	setContacts (ControllerToken, const std::vector< mc_rbdyn::Contact > &contacts) final

template<typename... Fun>
void	addConstraint (tasks::qp::ConstraintFunction< Fun... > *constraint)

template<typename... Fun>
void	removeConstraint (tasks::qp::ConstraintFunction< Fun... > *constraint)

std::pair< int, const tasks::qp::BilateralContact & >	contactById (const tasks::qp::ContactId &id) const

Eigen::VectorXd	lambdaVec (int cIndex) const

const sva::ForceVecd	desiredContactForce (const mc_rbdyn::Contact &id) const final

void	updateNrVars ()

void	updateNrVars (const mc_rbdyn::Robots &robots)

void	updateConstrSize ()

tasks::qp::SolverData &	data () noexcept

const tasks::qp::SolverData &	data () const noexcept

tasks::qp::QPSolver &	solver () noexcept

const tasks::qp::QPSolver &	solver () const noexcept

double	solveTime () final

double	solveAndBuildTime () final

const Eigen::VectorXd &	result () const

void	addTask (mc_tasks::MetaTask *task)

void	addTask (std::shared_ptr< mc_tasks::MetaTask > task)

void	removeTask (mc_tasks::MetaTask *task)

void	removeTask (std::shared_ptr< mc_tasks::MetaTask > task)

Public Member Functions inherited from mc_solver::QPSolver
	QPSolver (mc_rbdyn::RobotsPtr robots, double timeStep, Backend backend)

	QPSolver (double timeStep, Backend backend)

virtual	~QPSolver ()=default

Backend	backend () const noexcept

void	addConstraintSet (ConstraintSet &cs)

template<typename T >
void	addConstraintSet (const std::unique_ptr< T > &ptr)

void	removeConstraintSet (ConstraintSet &cs)

template<typename T >
void	removeConstraintSet (const std::unique_ptr< T > &ptr)

void	addTask (mc_tasks::MetaTask *task)

void	addTask (std::shared_ptr< mc_tasks::MetaTask > task)

void	removeTask (mc_tasks::MetaTask *task)

void	removeTask (std::shared_ptr< mc_tasks::MetaTask > task)

void	setContacts (const std::vector< mc_rbdyn::Contact > &contacts={})

const std::vector< mc_rbdyn::Contact > &	contacts () const noexcept

const std::vector< mc_solver::ConstraintSet * > &	constraints () const noexcept

const std::vector< mc_tasks::MetaTask * > &	tasks () const noexcept

bool	run (FeedbackType fType=FeedbackType::None)

const mc_rbdyn::Robot &	robot () const

mc_rbdyn::Robot &	robot ()

mc_rbdyn::Robot &	robot (unsigned int idx)

const mc_rbdyn::Robot &	robot (unsigned int idx) const

const mc_rbdyn::Robot &	env () const

mc_rbdyn::Robot &	env ()

const mc_rbdyn::Robots &	robots () const

mc_rbdyn::Robots &	robots ()

const mc_rbdyn::Robots &	realRobots () const

mc_rbdyn::Robots &	realRobots ()

double	dt () const

void	logger (std::shared_ptr< mc_rtc::Logger > logger)

std::shared_ptr< mc_rtc::Logger >	logger () const

void	gui (std::shared_ptr< mc_rtc::gui::StateBuilder > gui)

std::shared_ptr< mc_rtc::gui::StateBuilder >	gui () const

void	controller (mc_control::MCController *ctl) noexcept

const mc_control::MCController *	controller () const noexcept

mc_control::MCController *	controller () noexcept

Static Public Member Functions
static TasksQPSolver &	from_solver (QPSolver &solver) noexcept

static const TasksQPSolver &	from_solver (const QPSolver &solver) noexcept

Static Public Member Functions inherited from mc_solver::QPSolver
static Backend	context_backend ()

static void	context_backend (Backend backend)

Additional Inherited Members
Public Types inherited from mc_solver::QPSolver
enum	Backend { Backend::Unset, Backend::Tasks, Backend::TVM }

Protected Member Functions inherited from mc_solver::QPSolver
void	addTaskToGUI (mc_tasks::MetaTask *task)

Protected Attributes inherited from mc_solver::QPSolver
Backend	backend_

mc_rbdyn::RobotsPtr	robots_p

mc_rbdyn::RobotsPtr	realRobots_p

double	timeStep

std::vector< mc_rbdyn::Contact >	contacts_

std::vector< mc_tasks::MetaTask * >	metaTasks_

std::vector< std::shared_ptr< void > >	shPtrTasksStorage

std::vector< mc_solver::ConstraintSet * >	constraints_

std::shared_ptr< mc_rtc::Logger >	logger_ = nullptr

std::shared_ptr< mc_rtc::gui::StateBuilder >	gui_ = nullptr

mc_control::MCController *	controller_ = nullptr

Detailed Description

This implements the QPSolver interface for the Tasks backend

This solver can accepts tasks and constraints from mc_rtc and Tasks types

Constructor & Destructor Documentation

◆ TasksQPSolver() [1/2]

mc_solver::TasksQPSolver::TasksQPSolver	(	mc_rbdyn::RobotsPtr	robots,
		double	timeStep
	)

inline

◆ TasksQPSolver() [2/2]

mc_solver::TasksQPSolver::TasksQPSolver ( double timeStep )

inline

◆ ~TasksQPSolver()

mc_solver::TasksQPSolver::~TasksQPSolver ( )

finaldefault

Member Function Documentation

◆ addConstraint()

template<typename... Fun>

void mc_solver::TasksQPSolver::addConstraint ( tasks::qp::ConstraintFunction< Fun... > * constraint )

inline

Add a constraint function to the solver

Parameters

constraint Pointer to the ConstraintFunction. QPSolver does not take ownserhip of this pointer and the caller should make sure the object remains valid until it is removed from the solver

◆ addTask() [1/4]

void mc_solver::QPSolver::addTask

Add a task to the solver

Adding the same task multiple times has no effect.

Parameters

task	Pointer to an mc_tasks::MetaTask, QPSolver does not take ownership of this pointer. The MetaTask update function will be automatically called before the optimization is solved.

◆ addTask() [2/4]

void mc_solver::QPSolver::addTask

inline

Add a task to the solver, ownership is shared with the solver

◆ addTask() [3/4]

void mc_solver::TasksQPSolver::addTask ( std::shared_ptr< tasks::qp::Task > task )

inline

Add a task to the solver, ownership is shared with the solver

◆ addTask() [4/4]

void mc_solver::TasksQPSolver::addTask ( tasks::qp::Task * task )

Add a task to the solver

Parameters

task	Pointer to the added task, QPSolver does not take ownership of this pointer and the caller should make sure the object remains valid until it is removed from the solver

◆ contactById()

std::pair<int, const tasks::qp::BilateralContact &> mc_solver::TasksQPSolver::contactById ( const tasks::qp::ContactId & id ) const

Gives access to the tasks::qp::BilateralContact entity in the solver from a contact id

Parameters

id	The contact id of the contact

Returns: The tasks:qp::BilateralContact entity from the solver if id is valid, otherwise, the first element of the pair is -1 and the reference is invalid

◆ data() [1/2]

const tasks::qp::SolverData& mc_solver::TasksQPSolver::data ( ) const

inlinenoexcept

Returns the internal QP solver data (const)

◆ data() [2/2]

tasks::qp::SolverData& mc_solver::TasksQPSolver::data ( )

inlinenoexcept

Returns the internal QP solver data

◆ desiredContactForce()

const sva::ForceVecd mc_solver::TasksQPSolver::desiredContactForce ( const mc_rbdyn::Contact & id ) const

finalvirtual

Desired resultant of contact force in robot surface frame

Parameters

contact Contact for which the force is desired. This contact must be one of the active contacts in the solver.

Returns: Contact force in robot surface frame

Implements mc_solver::QPSolver.

◆ from_solver() [1/2]

static const TasksQPSolver& mc_solver::TasksQPSolver::from_solver ( const QPSolver & solver )

inlinestaticnoexcept

Helper to get a TasksQPSolver from a QPSolver instance

The caller should make sure the cast is valid by checking the QPSolver backend.

In debug the program will abort otherwise, in release UB abounds

◆ from_solver() [2/2]

static TasksQPSolver& mc_solver::TasksQPSolver::from_solver ( QPSolver & solver )

inlinestaticnoexcept

Helper to get a TasksQPSolver from a QPSolver instance

The caller should make sure the cast is valid by checking the QPSolver backend.

In debug the program will abort otherwise, in release UB abounds

◆ lambdaVec()

Eigen::VectorXd mc_solver::TasksQPSolver::lambdaVec ( int cIndex ) const

Gives access to a part to lambdaVec given a contact index

Parameters

cIndex The index of the contact

Returns: The lambdaVec associated

◆ removeConstraint()

template<typename... Fun>

void mc_solver::TasksQPSolver::removeConstraint ( tasks::qp::ConstraintFunction< Fun... > * constraint )

inline

Remove a constraint function from the solver

Parameters

constraint Pointer to the constraint that will be removed. It is not destroyed afterwards

◆ removeTask() [1/3]

void mc_solver::QPSolver::removeTask

Remove a task from the solver

Removing a task that is not in the solver has no effect.

Parameters

task	Pointer to an mc_tasks::MetaTask. The task will not be updated anymore and memory should be released by the task's owner.

◆ removeTask() [2/3]

void mc_solver::QPSolver::removeTask

inline

Remove a task from the solver which was shared with the solver

◆ removeTask() [3/3]

void mc_solver::TasksQPSolver::removeTask ( tasks::qp::Task * task )

Remove a task from the solver

Parameters

task	Pointer to the removed task. The task is not deleted after being removed

◆ result()

const Eigen::VectorXd& mc_solver::TasksQPSolver::result ( ) const

◆ setContacts()

void mc_solver::TasksQPSolver::setContacts	(	ControllerToken	,
		const std::vector< mc_rbdyn::Contact > &	contacts
	)

finalvirtual

Implements mc_solver::QPSolver.

◆ solveAndBuildTime()

double mc_solver::TasksQPSolver::solveAndBuildTime ( )

finalvirtual

Returns the building and solving time in ms

Implements mc_solver::QPSolver.

◆ solver() [1/2]

const tasks::qp::QPSolver& mc_solver::TasksQPSolver::solver ( ) const

inlinenoexcept

Access the internal QP solver (const)

◆ solver() [2/2]

tasks::qp::QPSolver& mc_solver::TasksQPSolver::solver ( )

inlinenoexcept

Access the internal QP solver

◆ solveTime()

double mc_solver::TasksQPSolver::solveTime ( )

finalvirtual

Returns the solving time in ms

Implements mc_solver::QPSolver.

◆ updateConstrSize()

void mc_solver::TasksQPSolver::updateConstrSize ( )

Update constraints matrix sizes

Note: This is mainly provided to allow safe usage of raw constraint from Tasks rather than those wrapped in this library, you probably do not need to call this

◆ updateNrVars() [1/2]

void mc_solver::TasksQPSolver::updateNrVars ( )

Update number of variables

This should be called when/if you add new robots into the scene after the solver initialization, this is a costly operation.

◆ updateNrVars() [2/2]

void mc_solver::TasksQPSolver::updateNrVars ( const mc_rbdyn::Robots & robots )

Update nr vars in tasks and constraints

The documentation for this struct was generated from the following file:

include/mc_solver/TasksQPSolver.h

Public Member Functions

Static Public Member Functions

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ TasksQPSolver() [1/2]

◆ TasksQPSolver() [2/2]

◆ ~TasksQPSolver()

Member Function Documentation

◆ addConstraint()

◆ addTask() [1/4]

◆ addTask() [2/4]

◆ addTask() [3/4]

◆ addTask() [4/4]

◆ contactById()

◆ data() [1/2]

◆ data() [2/2]

◆ desiredContactForce()

◆ from_solver() [1/2]

◆ from_solver() [2/2]

◆ lambdaVec()

◆ removeConstraint()

◆ removeTask() [1/3]

◆ removeTask() [2/3]

◆ removeTask() [3/3]

◆ result()

◆ setContacts()

◆ solveAndBuildTime()

◆ solver() [1/2]

◆ solver() [2/2]

◆ solveTime()

◆ updateConstrSize()

◆ updateNrVars() [1/2]

◆ updateNrVars() [2/2]