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, uint dim_contact=3, bool use_spatial_acc_bias=true) | |
| 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. | |
| void | setAccelerationPenalty (const double reg) |
| Set acceleration regularization term. | |
| void | setContactWrenchPenalty (const double reg) |
| Set contact Wrench regularization term. | |
| void | setAccelerationDeltaPenalty (const double w) |
| Set weight for penalizing the difference between consecutive joint accelerations (solver output), i.e., the term \( w\|\ddot{\mathbf{q}} - \ddot{\mathbf{q}}_{prev}\|_2^2 \) is added to the cost function. This smoothes the solver output over time, which is helpful e.g. on real robots with noisy state estimation. Higher values give smoother, but less reactive motion. Default is 0 (disabled). | |
| void | setContactWrenchDeltaPenalty (const double w) |
| Set weight for penalizing the difference between consecutive contact wrenches (solver output), i.e., the term \( w\|\mathbf{f} - \mathbf{f}_{prev}\|_2^2 \) is added to the cost function. This smoothes the contact force distribution over time, which is helpful e.g. on real robots with noisy state estimation, where the force distribution may otherwise jump between the contact points. Higher values give smoother, but less reactive force distributions. Default is 0 (disabled). | |
| void | setFrictionConeSlackPenalty (const double w) |
| Soften the contact surface friction cone constraint using a single slack variable \(s_i\) per contact, i.e., the hard constraint \(\mathbf{A}_i\mathbf{f}_i \leq \mathbf{0}\) is replaced by \(\mathbf{A}_i\mathbf{f}_i \leq s_i\mathbf{1}, s_i \geq 0\) and the term \( w\sum_i s_i^2 \) is added to the cost function. This avoids hard active-set switching (chattering) and infeasibility due to noisy state estimates on a real robot, at the cost of allowing small friction cone violations. Higher values approximate the hard constraint more closely. Only available for surface contacts (dim_contact == 6). Default is 0 (hard constraint, disabled). | |
| 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 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::Contact > | contacts |
| std::vector< TaskPtr > | tasks |
| std::vector< ConstraintPtr > | constraints |
| HierarchicalQP | hqp |
| bool | configured |
| types::JointCommand | solver_output_joints |
| uint | dim_contact |
| bool | use_spatial_acc_bias |
| double | acceleration_penalty |
| double | contact_wrench_penalty |
| double | acceleration_delta_penalty |
| double | wrench_delta_penalty |
| double | friction_cone_slack_penalty |
| std::shared_ptr< ContactsFrictionSurfaceConstraint > | friction_surface_constraint |
| 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< 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, | ||
| uint | dim_contact = 3, | ||
| bool | use_spatial_acc_bias = true ) |
◆ ~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 |
◆ setAccelerationDeltaPenalty()
|
inline |
Set weight for penalizing the difference between consecutive joint accelerations (solver output), i.e., the term \( w\|\ddot{\mathbf{q}} - \ddot{\mathbf{q}}_{prev}\|_2^2 \) is added to the cost function. This smoothes the solver output over time, which is helpful e.g. on real robots with noisy state estimation. Higher values give smoother, but less reactive motion. Default is 0 (disabled).
◆ setAccelerationPenalty()
|
inline |
Set acceleration 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.
◆ setContactWrenchDeltaPenalty()
|
inline |
Set weight for penalizing the difference between consecutive contact wrenches (solver output), i.e., the term \( w\|\mathbf{f} - \mathbf{f}_{prev}\|_2^2 \) is added to the cost function. This smoothes the contact force distribution over time, which is helpful e.g. on real robots with noisy state estimation, where the force distribution may otherwise jump between the contact points. Higher values give smoother, but less reactive force distributions. Default is 0 (disabled).
◆ setContactWrenchPenalty()
|
inline |
Set contact Wrench 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
◆ setFrictionConeSlackPenalty()
| void wbc::AccelerationSceneReducedTSID::setFrictionConeSlackPenalty | ( | const double | w | ) |
Soften the contact surface friction cone constraint using a single slack variable \(s_i\) per contact, i.e., the hard constraint \(\mathbf{A}_i\mathbf{f}_i \leq \mathbf{0}\) is replaced by \(\mathbf{A}_i\mathbf{f}_i \leq s_i\mathbf{1}, s_i \geq 0\) and the term \( w\sum_i s_i^2 \) is added to the cost function. This avoids hard active-set switching (chattering) and infeasibility due to noisy state estimates on a real robot, at the cost of allowing small friction cone violations. Higher values approximate the hard constraint more closely. Only available for surface contacts (dim_contact == 6). Default is 0 (hard constraint, disabled).
◆ 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
◆ acceleration_delta_penalty
|
protected |
◆ acceleration_penalty
|
protected |
◆ configured
|
protected |
◆ constraints
|
protected |
◆ contact_wrench_penalty
|
protected |
◆ contacts
|
protected |
◆ dim_contact
|
protected |
◆ friction_cone_slack_penalty
|
protected |
◆ friction_surface_constraint
|
protected |
◆ hqp
|
protected |
◆ reg
|
staticprotected |
◆ robot_acc
|
protected |
◆ solver_output_acc
|
protected |
◆ solver_output_joints
|
protected |
◆ tasks
|
protected |
◆ use_spatial_acc_bias
|
protected |
◆ wrench_delta_penalty
|
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.17.0