Loading [MathJax]/extensions/tex2jax.js
wbc
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:
wbc::Scene

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 HierarchicalQPupdate ()
 Update the wbc scene.
 
virtual const types::JointCommandsolve (const HierarchicalQP &hqp)
 Solve the given optimization problem.
 
void setHessianRegularizer (const double reg)
 setHessianRegularizer
 
double getHessianRegularizer ()
 Return the current value of hessian regularizer.
 
const Eigen::VectorXd & getSolverOutputRaw () const
 Get current solver output in raw values.
 
- 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.
 

Protected Attributes

double hessian_regularizer
 
std::vector< TaskPtrtasks
 
std::vector< ConstraintPtrconstraints
 
HierarchicalQP hqp
 
bool configured
 
types::JointCommand solver_output_joints
 
Eigen::VectorXd solver_output
 
- Protected Attributes inherited from wbc::Scene
RobotModelPtr robot_model
 
QPSolverPtr solver
 

Static Protected Attributes

static SceneRegistry< VelocitySceneQPreg
 

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_modelPointer to the robot model
solverSolver 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
tasksTasks 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.

◆ getHessianRegularizer()

double wbc::VelocitySceneQP::getHessianRegularizer ( )
inline

Return the current value of hessian regularizer.

◆ getSolverOutputRaw()

const Eigen::VectorXd & wbc::VelocitySceneQP::getSolverOutputRaw ( ) const
inline

Get current solver output in raw values.

◆ setHessianRegularizer()

void wbc::VelocitySceneQP::setHessianRegularizer ( const double reg)
inline

setHessianRegularizer

Parameters
regThis 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

◆ hessian_regularizer

double wbc::VelocitySceneQP::hessian_regularizer
protected

◆ hqp

HierarchicalQP wbc::VelocitySceneQP::hqp
protected

◆ reg

SceneRegistry< VelocitySceneQP > wbc::VelocitySceneQP::reg
staticprotected

◆ solver_output

Eigen::VectorXd wbc::VelocitySceneQP::solver_output
protected

◆ solver_output_joints

types::JointCommand wbc::VelocitySceneQP::solver_output_joints
protected

◆ tasks

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

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