wbc
|
Velocity-based implementation of the WBC Scene. It sets up and solves the following problem: More...
#include <VelocitySceneQP.hpp>
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 | 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. | |
![]() | |
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< TaskPtr > | tasks |
std::vector< ConstraintPtr > | constraints |
HierarchicalQP | hqp |
bool | configured |
types::JointCommand | solver_output_joints |
Eigen::VectorXd | solver_output |
![]() | |
RobotModelPtr | robot_model |
QPSolverPtr | solver |
Static Protected Attributes | |
static SceneRegistry< VelocitySceneQP > | reg |
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
wbc::VelocitySceneQP::VelocitySceneQP | ( | RobotModelPtr | robot_model, |
QPSolverPtr | solver, | ||
const double | dt ) |
WbcVelocityScene.
robot_model | Pointer to the robot model |
solver | Solver used to solver the qp optimization problem |
|
inlinevirtual |
|
virtual |
Configure the WBC scene. Create tasks and sort them by priority given the task config.
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.
|
inline |
Return the current value of hessian regularizer.
|
inline |
Get current solver output in raw values.
|
inline |
setHessianRegularizer
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 |
|
virtual |
Solve the given optimization problem.
Implements wbc::Scene.
|
virtual |
Update the wbc scene.
Implements wbc::Scene.
|
protected |
|
protected |
|
protected |
|
protected |
|
staticprotected |
|
protected |
|
protected |
|
protected |