Acceleration-based implementation of the WBC Scene. It sets up and solves the following problem: More...
#include <AccelerationSceneReducedTSID.hpp>
Public Member Functions | |
| AccelerationSceneReducedTSID (RobotModelPtr robot_model, QPSolverPtr solver, const double dt) | |
| virtual | ~AccelerationSceneReducedTSID () |
| 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 and return the (updated) optimization problem. | |
| virtual const types::JointCommand & | solve (const HierarchicalQP &hqp) |
| Solve the given optimization problem. | |
| const std::vector< types::Wrench > & | getContactWrenches () |
| Get estimated contact wrenches. | |
| 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 Member Functions | |
| bool | contactsHaveChanged (const std::vector< types::Contact > &old_contacts, const std::vector< types::Contact > &new_contacts) |
Protected Attributes | |
| Eigen::VectorXd | robot_acc |
| Eigen::VectorXd | solver_output_acc |
| std::vector< types::Wrench > | contact_wrenches |
| double | hessian_regularizer |
| std::vector< types::Contact > | contacts |
| std::vector< TaskPtr > | tasks |
| std::vector< ConstraintPtr > | constraints |
| 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< AccelerationSceneReducedTSID > | reg |
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()
|
inlinevirtual |
Member Function Documentation
◆ configure()
|
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.
◆ contactsHaveChanged()
|
inlineprotected |
◆ getContactWrenches()
|
inline |
Get estimated contact wrenches.
◆ getHessianRegularizer()
|
inline |
Return the current value of hessian regularizer.
◆ getSolverOutputRaw()
|
inline |
Get current solver output in raw values.
◆ setHessianRegularizer()
|
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()
|
virtual |
Solve the given optimization problem.
- Returns
- Solver output as joint acceleration command
Implements wbc::Scene.
◆ 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.
Member Data Documentation
◆ configured
|
protected |
◆ constraints
|
protected |
◆ contact_wrenches
|
protected |
◆ contacts
|
protected |
◆ hessian_regularizer
|
protected |
◆ hqp
|
protected |
◆ reg
|
staticprotected |
◆ robot_acc
|
protected |
◆ solver_output
|
protected |
◆ solver_output_acc
|
protected |
◆ solver_output_joints
|
protected |
◆ tasks
|
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
Generated by
1.13.2