Wocra Controller based on LQP solver for the ocra framework. More...

#include <WocraController.h>

Inheritance diagram for wocra::WocraController:

Collaboration diagram for wocra::WocraController:

Classes
struct	Pimpl

Public Member Functions
	WocraController (const std::string &ctrlName, std::shared_ptr< Model > innerModel, std::shared_ptr< OneLevelSolver > innerSolver, bool useReducedProblem)

virtual	~WocraController ()
	Destructor of Wocra controller. More...

std::shared_ptr< Model >	getModel ()

std::shared_ptr< OneLevelSolver >	getSolver ()

bool	isUsingReducedProblem ()

void	setVariableMinimizationWeights (double w_ddq, double w_tau, double w_fc)

void	takeIntoAccountGravity (bool useGrav)

void	writePerformanceInStream (std::ostream &myOstream, bool addCommaAtEnd) const

std::string	getPerformances () const

void	addConstraint (ocra::LinearConstraint &constraint) const

void	removeConstraint (ocra::LinearConstraint &constraint) const

void	addConstraint (ocra::ControlConstraint &constraint) const

void	removeConstraint (ocra::ControlConstraint &constraint) const

Public Member Functions inherited from ocra::Controller
virtual	~Controller ()=0

void	printInfo (int level, const std::string &filename)

void	setMaxJointTorques (const Eigen::VectorXd &tau_max)

const Eigen::VectorXd &	getMaxJointTorques () const

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

void	addTasks (const std::vector< std::shared_ptr< Task >> &tasks)

void	removeTask (const std::string &taskName)

void	removeTasks (const std::vector< std::string > tasks)

std::vector< std::string >	getTaskNames ()

std::string	getTaskPortName (const std::string &taskName)

std::vector< std::string >	getTaskPortNames ()

void	addContactSet (const ContactSet &contacts)

std::shared_ptr< Task >	getTask (const std::string &name)

const std::shared_ptr< Task >	getTask (const std::string &name) const

const std::map< std::string, std::shared_ptr< Task > > &	getTasks () const

void	computeOutput (Eigen::VectorXd &tau)
	Computation of output torques based on the tasks added to the controller. More...

const Eigen::VectorXd &	computeOutput ()

std::shared_ptr< Task >	createContactTask (const std::string &name, PointContactFeature::Ptr feature, double mu, double margin) const
	Creates a contact task. More...

std::shared_ptr< Task >	createTask (const std::string &name, Feature::Ptr feature, Feature::Ptr featureDes) const
	Generic task creation. More...

std::shared_ptr< Task >	createTask (const std::string &name, Feature::Ptr feature) const

void	enableErrorHandling ()

void	disableErrorHandling ()

bool	isErrorHandlingEnabled () const

const std::string &	getErrorMessage () const

void	clearErrorFlag ()

int	getErrorFlag () const

void	setMaxJointTorqueNorm (double maxTau)

double	getMaxJointTorqueNorm () const

void	setFixedLinkForOdometry (std::string newFixedLink)

void	setUseOdometry (bool useOdometry)

void	getFixedLinkForOdometry (std::string &currentFixedLink)

void	setContactState (int isInLeftSupport, int isInRightSupport)

void	getContactState (int &leftSupport, int &rightSupport)

Public Member Functions inherited from ocra::NamedInstance
	NamedInstance (const std::string &name)

const std::string &	getName () const

virtual	~NamedInstance ()

Protected Member Functions
virtual void	doComputeOutput (Eigen::VectorXd &tau)

virtual void	doAddTask (std::shared_ptr< Task > task)

virtual void	doAddContactSet (const ContactSet &contacts)

virtual std::shared_ptr< Task >	doCreateTask (const std::string &name, Feature::Ptr feature, Feature::Ptr featureDes) const

virtual std::shared_ptr< Task >	doCreateTask (const std::string &name, Feature::Ptr feature) const

virtual std::shared_ptr< Task >	doCreateContactTask (const std::string &name, PointContactFeature::Ptr feature, double mu, double margin) const

Protected Member Functions inherited from ocra::Controller
	Controller (const std::string &name, Model &model)

const std::vector< std::shared_ptr< Task > > &	getActiveTasks () const

void	setErrorFlag (int eflag)

void	setErrorMessage (const std::string &msg)

virtual void	doSetMaxJointTorques (const Eigen::VectorXd &tauMax)

Additional Inherited Members
Public Types inherited from ocra::Controller
enum	ErrorFlag { SUCCESS = 0, CRITICAL_ERROR = 1, STATIC_EQ_LOSS = 2, DYN_EQ_LOSS = 4, INSTABILITY = 8 + 1, OTHER = 16 }
	Error handling. More...

Detailed Description

Wocra Controller based on LQP solver for the ocra framework.

This controller is a generic optimization-based whole-body dynamic one, which can deal with tasks transitions. The multi-task management and their sequencing is done with a weighting strategy. A mechanical system presents different control issues regarding its limitations and physical constraints which are passed to the optimization problem as equality and inequality constraints. The controller will correct the error between actual and desired states by, for example, minimizing an error resulting from the quadratic norm of their difference. This error will be minimized through a Linear Quadratic Program (LQP), which is composed by a quadratic cost function with linear constraints.

The following are types of internal and external physical constraints of the system, describing a robot and its interaction with the environment:

Equations of motion (with interactions) (See ocra::FullDynamicEquationFunction).
Actuation limits.
- Torque limits.
- Acceleration limits.
- Velocity limits.
- Joint limits.
Obstacle avoidance.
Interaction with the environment:
- Bilateral closure.
- Unilateral frictional contact.

All the previous constraints are linear except for the unilateral frictional contacts, which represent the constraint of the contact forces lying in the Coulomb friction cone and is nonlinear, but at least quadratic. To deal with these kinds of constraints the WOCRA controller will rely on the LQP formulation, since it is simpler and more suitable for the real-time dynamic control of humanoid robots.

Wocra will address the low-level control of the robot through the concept of tasks, i.e. the control of one or more DoF of the system towards objectives which are generally the minimization of tracking errors. Tasks are composed of two main elements, the subset of the controlled DoF and their desired goals (For more details on tasks, see ()). An example of a task function is the norm of an error.

Different tasks can be mixed in wocra to achieve a desired high-level motion. These tasks will be subject to the aforementioned equality and inequality constraints, which motivate the use of an LQP, which solves the following problem:

$\begin{align*} \underset{\x}{\text{min}} &\; \frac{1}{2} \left( \x\tp P \x + \q\tp\x + r \right) \\ s.t. &:\; \G\x \leq h \\ &\; A\x = \b \end{align*}$

Where $ x $ is the vector to optimize, $ P $ , $ q $ and $ r $ represent the quadratic cost function, $ G $ , $ h $ define the inequality constraints and $ A $ , $ b $ define the equality constraints, which are the concatenation of the different constraints previously mentioned. The controller thus builds these matrices according to the tasks and the constraints acting on the robot and, when the solution is found, extracts the input torque vector $\tau$ to actuate the system. $ x $ can be replaced by $\left[ \ddq \tp \; f_c \tp \; \tau \tp \right] \tp$ which we also denote by the dynamic variable $\X = [\ddq\tp \av]$ , where $\av$ is further referred to as the action variable.

But the story does not end here. In particular, Wocra will use a weighting strategy which associates each task with a coefficient that sets its importance with respect to the others (a task with a higher weights gets a higher priority). The first point is to set these coefficients as real values. As a consequence, priorities are not strict and all tasks are achieved according to the trade-off defined by the weights. Given a set of $ n $ tasks and their related weights $(T_i(\q, \dq, \X), \omega_i) \; i \in [1...n]$ one solves their weighted sum subject to the concatenation of the constraints. The task $ T_0 $ is dedicated to the minimization of the whole optimization variable. This is required when the control problem has many solutions, in order to ensure the uniqueness and more specifically to provide a solution that minimizes the input torque vector $\tau$ . $ T_0 $ has a very small weight with respect to the others $0 < \omega_0 << 1$ to limit the induced error. This program is solved only one time per call.

The algorithm consists then in finding $\X_i^*$ , the solution of the problem:

$\begin{align*} \underset{\X}{\text{min}} &\; \frac{1}{2} \left( \sum_{i=1}^{n} (\omega_i^2 T_i (\q, \dq, \X)) + \omega_0^2 T_0(\q, \dq, \X) \right) \\ s.t.: &\; G \X \leq \h \\ &\; \A \X = \b \end{align*}$

[salini2012Thesis].

Note: Put proper link to the Tasks class and document it well based on Salini's thesis.

Definition at line 101 of file WocraController.h.

Constructor & Destructor Documentation

wocra::WocraController::WocraController	(	const std::string &	ctrlName,
		std::shared_ptr< Model >	innerModel,
		std::shared_ptr< OneLevelSolver >	innerSolver,
		bool	useReducedProblem
	)

Initializes Wocra controller.

During the instantiation of this class a WocraController::Pimpl object is created. This object, in turn, will specify the constraints (robot dynamics) and objectives ( $\ddq, \torque, \force_c$ ) for the solver, as well as its relative minimization weights and will take into account gravity.

Parameters

ctrlName	The name of the controller
innerModel	The internal model of the robot one wants to control
innerSolver	The internal solver one wants to use to make the quadratic optimization
useReducedProblem	Tell if the redundant problem is considered (unknown variable is $[ \ddq, \torque, \force_c ]$ ), or is the reduced problem (non-redundant) is considred (unknown variable is $[ \torque, \force_c ]$ )

Definition at line 80 of file WocraController.cpp.

wocra::WocraController::~WocraController ( )

virtual

Destructor of Wocra controller.

It disconnects all the tasks connected to the controller, then it disconnects the inner objectives that minimize the problem variables, and finally it disconnects the dynamic equation constraint (if needed).

Definition at line 116 of file WocraController.cpp.

Member Function Documentation

void wocra::WocraController::addConstraint ( ocra::LinearConstraint & constraint ) const

Adds a Linear constraint that is equivalent for the full & reduced problems.

Parameters

Linear constraint.

Definition at line 181 of file WocraController.cpp.

void wocra::WocraController::addConstraint ( ocra::ControlConstraint & constraint ) const

Adds a Linear constraint that has different expressions, depending on the problem type. In this case, the constraint needs to be connected to the controller to get the matrices for the problem reduction. See wocra::ControlConstraint for more info.

Parameters

constraint Constraint.

Definition at line 191 of file WocraController.cpp.

void wocra::WocraController::doAddContactSet ( const ContactSet & contacts )

protectedvirtual

Internal implementation inside the addContactSet method.

Parameters

contacts The contact set to add in the controller.

Todo:: this method has not been implemented!!!

Implements ocra::Controller.

Definition at line 214 of file WocraController.cpp.

void wocra::WocraController::doAddTask ( std::shared_ptr< Task > task )

protectedvirtual

Internal implementation inside the addTask method.

Parameters

task	The task to add in the controller.

Implements ocra::Controller.

Definition at line 203 of file WocraController.cpp.

void wocra::WocraController::doComputeOutput ( Eigen::VectorXd & tau )

protectedvirtual

Computes the output of the controller.

Here, the controller solves the optimization problem depending on the tasks and constraints, and the result is set in the variable of the problem, either $x = [ \ddq \; \tau \; \force_c ]$ or $x = [ \tau \; \force_c ]$ . The torque variable $\tau$ is finally applied to the robot.

Parameters

tau	The torque variable, which is the output of our problem.

Implements ocra::Controller.

Definition at line 278 of file WocraController.cpp.

std::shared_ptr< Task > wocra::WocraController::doCreateContactTask	(	const std::string &	name,
		PointContactFeature::Ptr	feature,
		double	mu,
		double	margin
	)		const

protectedvirtual

Internal implementation inside the createContactTask method.

Parameters

name	The task name, a unique identifier
feature	The contact point feature of the robot one wants to control
mu	The friction cone coefficient $\mu$ such as $\Vert \force_t \Vert < \mu \force_n$
margin	The margin inside the friction cone

Returns: The pointer to the new created contact task

This method is called by the higher level methods createWocraContactTask(const std::string&, PointContactFeature::Ptr, , double, double) const and is the concrete implementation required by the ocra::Controller class.

Implements ocra::Controller.

Definition at line 266 of file WocraController.cpp.

std::shared_ptr< Task > wocra::WocraController::doCreateTask	(	const std::string &	name,
		Feature::Ptr	feature,
		Feature::Ptr	featureDes
	)		const

protectedvirtual

Internal implementation inside the createTask method.

Parameters

name	The task name, a unique identifier
feature	The part of the robot one wants to control (full state, frame, CoM,...)
featureDes	The desired state one wants to reach, depends on the feature argument

Returns: The pointer to the new created task

This method is called by the higher level methods createWocraTask(const std::string&, const Feature&, const Feature&) const and is the concrete implementation required by the ocra Controller class.

Create an WocraTask.

Returns: an WocraTask instance that is a dynamic_cast of the task returned by ocra::Controller::createTask(const std::string& name, Feature::Ptr feature, Feature::Ptr featureDes) const Create an WocraTask.; an WocraTask instance that is a dynamic_cast of the task returned by ocra::Controller::createTask(const std::string& name, Feature::Ptr feature) const Create an WocraContactTask.; an WocraTask instance that is a dynamic_cast of the task returned by ocra::Controller::createContactTask(const std::string& name, PointContactFeature::Ptr feature, double mu, double margin) const

Implements ocra::Controller.

Definition at line 256 of file WocraController.cpp.

std::shared_ptr< Task > wocra::WocraController::doCreateTask	(	const std::string &	name,
		Feature::Ptr	feature
	)		const

protectedvirtual

Internal implementation inside the createTask method.

Parameters

name	The task name, a unique identifier
feature	The part of the robot one wants to control (full state, frame, CoM,...)

Returns: The pointer to the new created task

This method is called by the higher level methods #createWocraTask(const std::string&, const Feature&) const and is the concrete implementation required by the ocra Controller class.

Implements ocra::Controller.

Definition at line 261 of file WocraController.cpp.

std::shared_ptr< Model > wocra::WocraController::getModel ( )

Returns the inner model.

Returns: inner model used to construct this controller instance

Definition at line 145 of file WocraController.cpp.

std::string wocra::WocraController::getPerformances ( ) const

Get information about performances through a string.

Information are saved in a JSON way (http://www.json.org/). It returns a dictionnary of the form:

{
   "performance_info_1": [0.01, 0.02, 0.01, ... , 0.03, 0.01],
   ...
   "performance_info_n": [0.01, 0.02, 0.01, ... , 0.03, 0.01]
}

where performance_info are:

controller_update_tasks
controller_solve_problem
solver_prepare
solver_solve

See wocra::WocraController::writePerformanceInStream(std::ostream&, bool) const and wocra::OneLevelSolver::writePerformanceInStream(std::ostream&, bool).

Definition at line 321 of file WocraController.cpp.

std::shared_ptr< OneLevelSolver > wocra::WocraController::getSolver ( )

Returns the inner solver.

Returns: Inner solver used to construct this controller instance.

Definition at line 153 of file WocraController.cpp.

bool wocra::WocraController::isUsingReducedProblem ( )

Returns: true if variable of reduced problem ( $[ \torque \; \force_c ]$ ) is considered, or false if it uses the variable of the full one ( $[ \ddq \; \torque \; \force_c ]$ ).

Definition at line 161 of file WocraController.cpp.

void wocra::WocraController::removeConstraint ( ocra::LinearConstraint & constraint ) const

Removes a Linear constraint that is equivalent for the full & reduced problems.

Parameters

Linear Constraint to remove.

Definition at line 186 of file WocraController.cpp.

void wocra::WocraController::removeConstraint ( ocra::ControlConstraint & constraint ) const

Removes a Linear constraint that has different expressions, depending on the problem type. In this case, the constraint needs to be disconnected from the controller.

Parameters

constraint Constraint.

Definition at line 197 of file WocraController.cpp.

void wocra::WocraController::setVariableMinimizationWeights	(	double	w_ddq,
		double	w_tau,
		double	w_fc
	)

Sets weights for the objectives that minimize the norm of the problem variables: $[ \ddq \; \torque \; \force_c ]$ .

Parameters

w_ddq	weight for the minimization of $\ddq$ .
w_tau	weight for the minimization of $\torque$ .
w_fc	weight for the minimization of $\force_c$ .

Definition at line 169 of file WocraController.cpp.

void wocra::WocraController::takeIntoAccountGravity ( bool useGrav )

Whether to take into account gavity in the dynamic equation of motion.

Parameters

useGrav true if gravity is enable, false otherwise.

Definition at line 176 of file WocraController.cpp.

void wocra::WocraController::writePerformanceInStream	(	std::ostream &	myOstream,
		bool	addCommaAtEnd
	)		const

Writes information about controller performances in a string stream.

Parameters

outstream	the output stream where to write the performances information
addCommaAtEnd	If true, add a comma at the end of the stream. If false, it means that this is the end of the json file, nothing will be added after that.

See wocra::WocraController::getPerformances() to know more. Here it saves:

controller_update_tasks
controller_solve_problem
solver_prepare
solver_solve

Definition at line 309 of file WocraController.cpp.

The documentation for this class was generated from the following files:

Classes

Public Member Functions

Protected Member Functions

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation