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

#include <AccelerationSceneReducedTSID.hpp>

Inheritance diagram for wbc::AccelerationSceneReducedTSID:

Public Member Functions
	AccelerationSceneReducedTSID (RobotModelPtr robot_model, QPSolverPtr solver, const double dt)

virtual	~AccelerationSceneReducedTSID ()

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

const base::samples::Wrenches &	getContactWrenches ()
	Get estimated contact wrenches.

void	setHessianRegularizer (const double reg)
	setHessianRegularizer

double	getHessianRegularizer ()
	Return the current value of hessian regularizer.

const base::VectorXd &	getSolverOutputRaw () const

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::VectorXd	solver_output_acc

base::samples::Wrenches	contact_wrenches

double	hessian_regularizer

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< AccelerationSceneReducedTSID >	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 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}},\mathbf{\tau},\mathbf{f} & & \\ s.t. & \mathbf{H}\mathbf{\ddot{q}} - \mathbf{S}^T\mathbf{\tau} - \mathbf{J}_c^T\mathbf{f} = -\mathbf{h} & \\ & \mathbf{J}_{c,i}\mathbf{\ddot{q}} = -\dot{\mathbf{J}}_{c,i}\dot{\mathbf{q}}, \, \forall i& \\ & \mathbf{\tau}_m \leq \mathbf{\tau} \leq \mathbf{\tau}_M& \\ \end{array} \]

\(\ddot{\mathbf{q}}\) - Vector of robot joint accelerations
\(\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 = \mathbf{W}\mathbf{J}\) - Weighted task Jacobians
\(\mathbf{W}\) - Diagonal task weight matrix
\(\mathbf{H}\) - Joint space inertia matrix
\(\mathbf{S}\) - Selection matrix
\(\mathbf{\tau}\) - actuation forces/torques
\(\mathbf{h}\) - bias forces/torques
\(\mathbf{f}\) - external forces
\(\mathbf{J}_{c,i}\) - Contact Jacobian of i-th contact point
\(\dot{\mathbf{J}}\dot{\mathbf{q}}\) - Acceleration bias
\(\mathbf{\tau}_m,\mathbf{\tau}_M\) - Joint force/torque limits

The implementation is close to the task-space-inverse dynamics (TSID) method: https://andreadelprete.github.io/teaching/tsid/1_tsid_theory.pdf. It computes the required joint space accelerations \(\ddot{\mathbf{q}}\), torques \(\mathbf{\tau}\) and contact wrenches \(\mathbf{f}\), required to achieve the given task space accelerations \(\mathbf{v}_{d}\) under consideration of the equations of motion (eom), rigid contacts and joint force/torque limits. Note that onyl a single hierarchy level is allowed here, prioritization can be achieved by assigning suitable task weights \(\mathbf{W}\).

Constructor & Destructor Documentation

◆ AccelerationSceneReducedTSID()

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

◆ ~AccelerationSceneReducedTSID()

virtual wbc::AccelerationSceneReducedTSID::~AccelerationSceneReducedTSID ( )

inlinevirtual

Member Function Documentation

◆ createTask()

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

protectedvirtual

brief Create a task and add it to the WBC scene

Implements wbc::Scene.

◆ getContactWrenches()

const base::samples::Wrenches & wbc::AccelerationSceneReducedTSID::getContactWrenches ( )

inline

Get estimated contact wrenches.

◆ getHessianRegularizer()

double wbc::AccelerationSceneReducedTSID::getHessianRegularizer ( )

inline

Return the current value of hessian regularizer.

◆ getSolverOutputRaw()

const base::VectorXd & wbc::AccelerationSceneReducedTSID::getSolverOutputRaw ( ) const

inline

◆ setHessianRegularizer()

void wbc::AccelerationSceneReducedTSID::setHessianRegularizer ( const double reg )

inline

setHessianRegularizer

Parameters

reg	This value is added to the diagonal of the Hessian matrix inside the QP to reduce the risk of infeasibility. Default is 1e-8

◆ solve()

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

virtual

Solve the given optimization problem.

Returns: Solver output as joint acceleration command

Implements wbc::Scene.

◆ update()

const HierarchicalQP & wbc::AccelerationSceneReducedTSID::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::AccelerationSceneReducedTSID::updateTasksStatus ( )

virtual

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

Implements wbc::Scene.

Member Data Documentation

◆ contact_wrenches

base::samples::Wrenches wbc::AccelerationSceneReducedTSID::contact_wrenches

protected

◆ hessian_regularizer

double wbc::AccelerationSceneReducedTSID::hessian_regularizer

protected

◆ reg

SceneRegistry< AccelerationSceneReducedTSID > wbc::AccelerationSceneReducedTSID::reg

staticprotected

◆ robot_acc

base::VectorXd wbc::AccelerationSceneReducedTSID::robot_acc

protected

◆ solver_output_acc

base::VectorXd wbc::AccelerationSceneReducedTSID::solver_output_acc

protected

◆ stamp

base::Time wbc::AccelerationSceneReducedTSID::stamp

protected

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

src/scenes/acceleration_reduced_tsid/AccelerationSceneReducedTSID.hpp
src/scenes/acceleration_reduced_tsid/AccelerationSceneReducedTSID.cpp

Public Member Functions

Protected Member Functions

Protected Attributes

Static Protected Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ AccelerationSceneReducedTSID()

◆ ~AccelerationSceneReducedTSID()

Member Function Documentation

◆ createTask()

◆ getContactWrenches()

◆ getHessianRegularizer()

◆ getSolverOutputRaw()

◆ setHessianRegularizer()

◆ solve()

◆ update()

◆ updateTasksStatus()

Member Data Documentation

◆ contact_wrenches

◆ hessian_regularizer

◆ reg

◆ robot_acc

◆ solver_output_acc

◆ stamp