[Explicit Compliance and Safety] Introduce CBF formulation for collision and joint constraints - #531
Open
bastien-muraccioli wants to merge 10 commits into
Open
Conversation
…heir equivalent joint accelerations to `Robot` Add the new members `externalTorques_`, `compensationTorques_`, `exteralTorquesAcc_` and `compensationTorquesAcc_` to `mc_rbdyn::Robot`. Allong with their setters and getters `setExternalTorques`, `setCompensationTorques`, `setExternalTorquesAcc`, `setCompensationTorquesAcc`. Note that the compensation torques and associated equivalent accelerations are set as std::optional. This is intended for later use for external forces compensation and explicit compliance.
…asks backend and closedLoop feedback type in both TVM and Tasks Since Tasks backend doesn't benefit from a computation graph, the joint acceleration equivalent to the external/compensation torques should be computed once before hand for later use by tasks in the solver. For both Tasks and TVM, the information of external torques in robot are updated from realRobot.
…raint` in the Tasks backend Add an additional boolean argument to `DynamicConstraint` to enable compensation of external torques disturbance estimation. The boolean is defaulted to false to prevent breaking change. The feedforward compensation torques are passed as an `Eigen::VectorXd` to Tasks' `MotionConstr` and `MotionSpringConstr`. The compensation torques are obtained from `mc_rbdyn::Robot::compensationTorques()` if available or `mc_rbdyn::Robot::externalTorques()` otherwise.
…raint` in the TVM backend Add an additional boolean argument to `mc_tvm::DynamicFunction` to enable compensation of external torques disturbance estimation. The boolean is defaulted to false to prevent breaking change. The feedforward compensation torques are obtained directly in the `DynamicFunction` using `Robot`. The compensation torques are obtained from `mc_tvm::Robot::tauCompensation()` if available or `mc_rbdyn::Robot::tauExternal()` otherwise.
…setExternalTorques funtion of Tasks MotionConstr
…ckend New compliant alternative for tasks are: - CompliantPostureTask - CompliantPositionTask - CompliantOrientationTask - CompliantEndEffectorTask
Impacted tasks: - CompliantPostureTask - CompliantPositionTask - CompliantOrientationTask - CompliantEndEffectorTask
Contributor
|
Tick the box to add this pull request to the merge queue (same as
|
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
3 participants
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
This PR introduces Control Barrier Function (CBF) constraints for collision avoidance and joint position/velocity limits, targeting closed-loop torque-controlled robots.
The standard velocity damper formulation can become fragile in closed-loop settings, where sensor noise and feedback effects may lead to constraint violations.
This PR reformulates these constraints using a CBF-based approach, improving robustness and safety during execution.
This includes:
The implementation follows the approach described in:
Safe Execution of RL Policies via Acceleration-based CBF-QP Constraint Enforcement for Real-World Robotic Deployments
The CBF formulation is only supported by the TVM backend and requires the following PR to be merged first:
Add closed-loop velocity damper#59
This PR builds upon and extends the work from #500, #501, and #502.