Class iHQP

• Defined in File iHQP.h

Inheritance Relationships

Base Type

• public OpenSoT::Solver< Eigen::MatrixXd, Eigen::VectorXd > (Template Class Solver)

Class Documentation

class iHQP : public OpenSoT::Solver<Eigen::MatrixXd, Eigen::VectorXd>

The iHQP class implement a solver that accept a Stack of Tasks with Bounds and Constraints.

Public Types

typedef std::shared_ptr<iHQP> Ptr

typedef MatrixPiler VectorPiler

Public Functions

iHQP(OpenSoT::AutoStack &stack_of_tasks, const double eps_regularisation = DEFAULT_EPS_REGULARISATION, const solver_back_ends be_solver = solver_back_ends::qpOASES)

iHQP constructor of the problem using the AutoStack this constructor permits to use user regularisation feature (inserting it inside the AutoStack)

Parameters

• stack_of_tasks – data structure which will contain tasks, constraints and user defined regularisation NOTICE that the user defined regularisation will be applied to all the stack levels

• eps_regularisation – regularisation factor used inside the QP solver (BackEnd)

• be_solver –

Throws

exception – if the stack can not be initialized

iHQP(OpenSoT::AutoStack &stack_of_tasks, const double eps_regularisation, const std::vector<solver_back_ends> be_solver)

iHQP(Stack &stack_of_tasks, const double eps_regularisation = DEFAULT_EPS_REGULARISATION, const solver_back_ends be_solver = solver_back_ends::qpOASES)

iHQP constructor of the problem

Parameters

• stack_of_tasks – a vector of tasks

• eps_regularisation – regularisation factor

Throws

exception – if the stack can not be initialized

iHQP(Stack &stack_of_tasks, const double eps_regularisation, const std::vector<solver_back_ends> be_solver)

iHQP(Stack &stack_of_tasks, ConstraintPtr bounds, const double eps_regularisation = DEFAULT_EPS_REGULARISATION, const solver_back_ends be_solver = solver_back_ends::qpOASES)

iHQP constructor of the problem

Parameters

• stack_of_tasks – a vector of tasks

• bounds – a vector of bounds passed to all the stacks

• eps_regularisation – regularisation factor

Throws

exception – if the stack can not be initialized

iHQP(Stack &stack_of_tasks, ConstraintPtr bounds, const double eps_regularisation, const std::vector<solver_back_ends> be_solver)

iHQP(Stack &stack_of_tasks, ConstraintPtr bounds, ConstraintPtr globalConstraints, const double eps_regularisation = DEFAULT_EPS_REGULARISATION, const solver_back_ends be_solver = solver_back_ends::qpOASES)

iHQP constructor of the problem

Parameters

• stack_of_tasks – a vector of tasks

• bounds – a vector of bounds passed to all the stacks

• globalConstraints – a vector of constraints passed to all the stacks

• eps_regularisation – regularisation factor

Throws

exception – if the stack can not be initialized

iHQP(Stack &stack_of_tasks, ConstraintPtr bounds, ConstraintPtr globalConstraints, const double eps_regularisation, const std::vector<solver_back_ends> be_solver)

inline ~iHQP()

virtual bool solve(Eigen::VectorXd &solution)

solve a stack of tasks

Parameters

solution – vector

Returns

true if all the stack is solved

inline unsigned int getNumberOfTasks()

getNumberOfTasks

Returns

lenght of the stack

bool setOptions(const unsigned int i, const boost::any &opt)

setOptions set option to a particular task

Parameters

• i – number of stack to set the option

• opt – options for task i

Returns

false if i-th problem does not exists or does not succeed

bool getOptions(const unsigned int i, boost::any &opt)

getOptions return the options of the i-th qp problem

Parameters

• i – number of stack to get the option

• opt – a data structure which has to be converted to particular structure used by the BackEnd implementation

Returns

false if i-th problem does not exists

bool getObjective(const unsigned int i, double &val)

getObjective return the value of the objective function at the optimum for the i-th qp problem

Parameters

• i – number of stack to get the value of the objective function

• val – value of the objective function at the optimum

Returns

false if i-th problem does not exists

void setActiveStack(const unsigned int i, const bool flag)

setActiveStack select a stack to do not solve

Parameters

• i – stack index

• flag – true or flase

void activateAllStacks()

activateAllStacks activate all stacks

std::string getBackEndName(const unsigned int i)

getBackEndName retrieve the name of the solver

Returns

a string with the name of the solver, right now: “qpOASES” “OSQP” “????”

bool setEpsRegularisation(const double eps, const unsigned int i)

setEpsRegularisation OVERWRITES the actual eps regularisation factor of a specific level of the stack

Parameters

• eps – reguralisation factor

• i – stack level

Returns

false if eps < 0 or i no in the stack

bool setEpsRegularisation(const double eps)

setEpsRegularisation OVERWRITES the actual eps regularisation factor for all the level of the stack

Parameters

eps – reguralisation factor

Returns

false if eps < 0

bool getBackEnd(const unsigned int i, BackEnd::Ptr &back_end)

getBackEnd retrieve the back-end associated to the i-th qp problem

Parameters

• i – priority level

• back_end –

Returns

false if the level does not exists

Protected Functions

virtual void _log(XBot::MatLogger2::Ptr logger, const std::string &prefix)

_log implement this on the solver to log data

Parameters

• logger – a pointer to a MatLogger

• prefix – used to log variables

bool prepareSoT(const std::vector<solver_back_ends> be_solver)

prepareSoT initialize the complete stack

Returns

true if stack is correctly initialized

void computeCostFunction(const TaskPtr &task, Eigen::MatrixXd &H, Eigen::VectorXd &g)

computeCostFunction compute a cost function for velocity control: F = ||Jdq - v||

Parameters

• task – to get Jacobian and reference

• H – Hessian matrix computed as J’J

• g – reference vector computed as J’v

void computeOptimalityConstraint(const TaskPtr &task, BackEnd::Ptr &problem, Eigen::MatrixXd &A, Eigen::VectorXd &lA, Eigen::VectorXd &uA)

computeOptimalityConstraint compute optimality constraint for velocity control: Jj*dqj = Jj*dqi

Parameters

• task – to get Jacobian of the previous task

• problem – to get solution of the previous task

• A – constraint matrix

• lA – lower bounds

• uA – upper bounds

Protected Attributes

vector<OpenSoT::constraints::Aggregated> constraints_task

vector<BackEnd::Ptr> _qp_stack_of_tasks

_qp_stack_of_tasks vector of QPOases Problem

vector<bool> _active_stacks

double _epsRegularisation

_epsRegularisation regularisation factor for dumped least squares

Eigen::MatrixXd H

Eigen::VectorXd g

Eigen::MatrixXd Hr

Eigen::VectorXd gr

OpenSoT::Task<Eigen::MatrixXd, Eigen::VectorXd>::TaskPtr _regularisation_task

MatrixPiler A

VectorPiler lA

VectorPiler uA

Eigen::VectorXd l

Eigen::VectorXd u

std::vector<Eigen::MatrixXd> tmp_A

std::vector<Eigen::VectorXd> tmp_lA

std::vector<Eigen::VectorXd> tmp_uA

std::vector<solver_back_ends> _be_solver

Protected Static Attributes

static const std::string _IHQP_CONSTRAINTS_PLUS_

static const std::string _IHQP_CONSTRAINTS_OPTIMALITY_