wbc::VelocitySceneQP Class Reference

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

#include <VelocitySceneQP.hpp>

Inheritance diagram for wbc::VelocitySceneQP:

Public Member Functions
	VelocitySceneQP (RobotModelPtr robot_model, QPSolverPtr solver, const double dt)
	WbcVelocityScene.
virtual	~VelocitySceneQP ()
virtual bool	configure (const std::vector< TaskPtr > &tasks)
	Configure the WBC scene. Create tasks and sort them by priority given the task config.
virtual const HierarchicalQP &	update ()
	Update the wbc scene.
virtual const types::JointCommand &	solve (const HierarchicalQP &hqp)
	Solve the given optimization problem.
void	setVelocityRegularization (const double reg)
	Set velocity regularization term.
Public Member Functions inherited from wbc::Scene
	Scene (RobotModelPtr robot_model, QPSolverPtr solver, const double dt)
	~Scene ()
RobotModelPtr	getRobotModel ()
	Return the current robot model.
QPSolverPtr	getSolver ()
	Return the current solver.
const Eigen::VectorXd &	getSolverOutputRaw () const
	Get current solver output in raw values.
const std::vector< types::Wrench > &	getContactWrenches ()
	Get estimated contact wrenches.

Protected Attributes
std::vector< TaskPtr >	tasks
std::vector< ConstraintPtr >	constraints
HierarchicalQP	hqp
bool	configured
types::JointCommand	solver_output_joints
double	velocity_regularization
Protected Attributes inherited from wbc::Scene
RobotModelPtr	robot_model
QPSolverPtr	solver
Eigen::VectorXd	solver_output
std::vector< types::Wrench >	contact_wrenches

Static Protected Attributes
static SceneRegistry< VelocitySceneQP >	reg

Detailed Description

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

\[ \begin{array}{ccc} minimize & \| \mathbf{J}_w\dot{\mathbf{q}} - \mathbf{v}_d\|_2& \\ \mathbf{\dot{q}} & & \\ & & \\ s.t. & \mathbf{J}_{c,i}\dot{\mathbf{q}}=0, \, \forall i & \\ & \dot{\mathbf{q}}_{m} \leq \dot{\mathbf{q}} \leq \dot{\mathbf{q}}_{M} & \\ \end{array} \]

In contrast to the VelocityScene class, the tasks are formulated within the cost function instead of modeling them as tasks. The problem is solved with respect to a number of rigid contacts \(\mathbf{J}_{c,i}\dot{\mathbf{q}}=0, \, \forall i \) and under consideration of the joint velocity limits of the robot.

\(\dot{\mathbf{q}}\) - Vector of robot joint velocities
\(\mathbf{v}_{d}\) - Desired Spatial velocities 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
\(\dot{\mathbf{q}}_{m},\dot{\mathbf{q}}_{M}\) - Joint velocity limits
\(\mathbf{J}_{c,i}\) - Contact Jcaobian of i-th contact point

Constructor & Destructor Documentation

VelocitySceneQP()

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

WbcVelocityScene.

Parameters

robot_model	Pointer to the robot model
solver	Solver used to solver the qp optimization problem

~VelocitySceneQP()

virtual wbc::VelocitySceneQP::~VelocitySceneQP ( )

inlinevirtual

Member Function Documentation

configure()

bool wbc::VelocitySceneQP::configure ( const std::vector< TaskPtr > & tasks )

virtual

Configure the WBC scene. Create tasks and sort them by priority given the task config.

Parameters

tasks

Tasks used in optimization function. Size has to be > 0. All tasks have to be valid. See tasks and TaskConfig.hpp for more details.

Implements wbc::Scene.

setVelocityRegularization()

void wbc::VelocitySceneQP::setVelocityRegularization ( const double reg )

inline

Set velocity regularization term.

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 types::JointCommand & wbc::VelocitySceneQP::solve ( const HierarchicalQP & hqp )

virtual

Solve the given optimization problem.

Returns

Solver output as joint velocity command

Implements wbc::Scene.

update()

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

virtual

Update the wbc scene.

Implements wbc::Scene.

Member Data Documentation

configured

bool wbc::VelocitySceneQP::configured

protected

constraints

std::vector< ConstraintPtr > wbc::VelocitySceneQP::constraints

protected

hqp

HierarchicalQP wbc::VelocitySceneQP::hqp

protected

reg

SceneRegistry< VelocitySceneQP > wbc::VelocitySceneQP::reg

staticprotected

solver_output_joints

types::JointCommand wbc::VelocitySceneQP::solver_output_joints

protected

tasks

std::vector< TaskPtr > wbc::VelocitySceneQP::tasks

protected

velocity_regularization

double wbc::VelocitySceneQP::velocity_regularization

protected

---

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

• src/scenes/velocity_qp/VelocitySceneQP.hpp
• src/scenes/velocity_qp/VelocitySceneQP.cpp