wbc::AccelerationScene Class Reference

Acceleration-based implementation of the WBC Scene. It sets up and solves the following optimization problem: More...

#include <AccelerationScene.hpp>

Inheritance diagram for wbc::AccelerationScene:

Public Member Functions
	AccelerationScene (RobotModelPtr robot_model, QPSolverPtr solver, const double dt)
virtual	~AccelerationScene ()
virtual const HierarchicalQP &	update ()
	Update the wbc scene and return the (updated) optimization problem.
virtual const base::commands::Joints &	solve (const HierarchicalQP &hqp)
	Solve the given optimization problem.
virtual const TasksStatus &	updateTasksStatus ()
	evaluateTasks Evaluate the fulfillment of the tasks given the current robot state and the solver output
Public Member Functions inherited from wbc::Scene
	Scene (RobotModelPtr robot_model, QPSolverPtr solver, const double dt)
	~Scene ()
virtual bool	configure (const std::vector< TaskConfig > &config)
	Configure the WBC scene. Create tasks and sort them by priority.
void	setReference (const std::string &task_name, const base::samples::Joints &ref)
	Set reference input for a joint space task.
void	setReference (const std::string &task_name, const base::samples::RigidBodyStateSE3 &ref)
	Set reference input for a cartesian space task.
void	setTaskWeights (const std::string &task_name, const base::VectorXd &weights)
	Set Task weights input for a task.
void	setTaskActivation (const std::string &task_name, double activation)
	Set Task activation for a task.
TaskPtr	getTask (const std::string &name)
	Return a Particular task. Throw if the task does not exist.
bool	hasTask (const std::string &name)
	True in case the given task exists.
const TasksStatus &	getTasksStatus () const
	Returns all tasks as vector.
void	getHierarchicalQP (HierarchicalQP &_hqp)
	Return tasks sorted by priority for the solver.
const base::commands::Joints &	getSolverOutput () const
	Get current solver output.
const base::VectorXd &	getSolverOutputRaw () const
	Get current solver output in raw values.
void	setJointWeights (const JointWeights &weights)
	set Joint weights by given name
const JointWeights &	getJointWeights () const
	Get Joint weights as Named vector.
const JointWeights &	getActuatedJointWeights () const
	Get Joint weights as Named vector.
RobotModelPtr	getRobotModel ()
	Return the current robot model.
QPSolverPtr	getSolver ()
	Return the current solver.
std::vector< TaskConfig >	getWbcConfig ()
	Return task configuration.

Protected Member Functions
virtual TaskPtr	createTask (const TaskConfig &config)
Protected Member Functions inherited from wbc::Scene
void	clearTasks ()
	Delete all tasks and free memory.

Protected Attributes
base::VectorXd	robot_acc
base::Time	stamp
Protected Attributes inherited from wbc::Scene
RobotModelPtr	robot_model
QPSolverPtr	solver
std::vector< std::vector< TaskPtr > >	tasks
std::vector< std::vector< ConstraintPtr > >	constraints
TasksStatus	tasks_status
HierarchicalQP	hqp
std::vector< int >	n_task_variables_per_prio
bool	configured
base::commands::Joints	solver_output_joints
JointWeights	joint_weights
JointWeights	actuated_joint_weights
std::vector< TaskConfig >	wbc_config
base::VectorXd	solver_output

Static Protected Attributes
static SceneRegistry< AccelerationScene >	reg

Additional Inherited Members
Static Public Member Functions inherited from wbc::Scene
static void	sortTaskConfig (const std::vector< TaskConfig > &config, std::vector< std::vector< TaskConfig > > &sorted_config)
	Sort task config by the priorities of the tasks.
static uint	getNTaskVariables (const std::vector< TaskConfig > &config)
	Return number of tasks per priority, given the task config.
static std::vector< int >	getNTaskVariablesPerPrio (const std::vector< TaskConfig > &config)
	Return number of tasks per priority, given the task config.

Detailed Description

Acceleration-based implementation of the WBC Scene. It sets up and solves the following optimization problem:

\[ \begin{array}{ccc} minimize & \| \mathbf{J}_w\ddot{\mathbf{q}} - \dot{\mathbf{v}}_d + \dot{\mathbf{J}}\dot{\mathbf{q}}\|_2\\ \mathbf{\ddot{q}} & & \\ \end{array} \]

The tasks are all formulated within the cost function, i.e., the problem is unconstrained. Soft prioritization can be achieved using task weights, as \(\mathbf{J}_w=\mathbf{W}\mathbf{J}\).

\(\ddot{\mathbf{q}}\) - Vector of robot joint accelerations
\(\dot{\mathbf{v}}_{d}\) - Desired spatial accelerations of all tasks stacked in a vector
\(\mathbf{J}\) - Task Jacobians of all tasks stacked in a single matrix
\(\mathbf{J}_w\) - Weighted task Jacobians
\(\mathbf{W}\) - Diagonal task weight matrix
\(\dot{\mathbf{J}}\dot{\mathbf{q}}\) - Acceleration bias

Constructor & Destructor Documentation

AccelerationScene()

wbc::AccelerationScene::AccelerationScene	(	RobotModelPtr	robot_model,
		QPSolverPtr	solver,
		const double	dt )

~AccelerationScene()

virtual wbc::AccelerationScene::~AccelerationScene ( )

inlinevirtual

Member Function Documentation

createTask()

TaskPtr wbc::AccelerationScene::createTask ( const TaskConfig & config )

protectedvirtual

brief Create a task and add it to the WBC scene

Implements wbc::Scene.

solve()

const base::commands::Joints & wbc::AccelerationScene::solve ( const HierarchicalQP & hqp )

virtual

Solve the given optimization problem.

Returns

Solver output as joint acceleration command

Implements wbc::Scene.

update()

const HierarchicalQP & wbc::AccelerationScene::update ( )

virtual

Update the wbc scene and return the (updated) optimization problem.

Parameters

ctrl_output

Control solution that fulfill the given tasks as good as possible

Implements wbc::Scene.

updateTasksStatus()

const TasksStatus & wbc::AccelerationScene::updateTasksStatus ( )

virtual

evaluateTasks Evaluate the fulfillment of the tasks given the current robot state and the solver output

Implements wbc::Scene.

Member Data Documentation

reg

SceneRegistry< AccelerationScene > wbc::AccelerationScene::reg

staticprotected

robot_acc

base::VectorXd wbc::AccelerationScene::robot_acc

protected

stamp

base::Time wbc::AccelerationScene::stamp

protected

---

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

• src/scenes/acceleration/AccelerationScene.hpp
• src/scenes/acceleration/AccelerationScene.cpp