[Explicit Compliance and Safety][2/3] Add optional feedforward compensation torques to DynamicConstraint in TVM and Tasks backends#501
Open
mathieu-celerier wants to merge 6 commits intojrl-umi3218:masterfrom
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.
for more information, see https://pre-commit.ci
…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
mathieu-celerier
force-pushed
the
topic/add-compensation-torques-dynamic-constraint
branch
from
41e9a8e to
80e828f
Compare
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
2 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 extends the previous torque propagation framework by introducing feedforward compensation torques into the
DynamicConstraintfor both the TVM and Tasks backends.For the TVM backend, a new boolean flag is added to
mc_tvm::DynamicFunctionto enable external torque disturbance compensation. The flag defaults tofalseto avoid breaking existing behavior. When enabled, feedforward compensation torques are retrieved directly withinDynamicFunctionfromRobot, usingmc_tvm::Robot::tauCompensation()when available, or falling back to the estimated external torques otherwise.For the Tasks backend, a similar boolean flag is added to
DynamicConstraintto activate disturbance compensation, also defaulted tofalse.The feedforward torques are explicitly passed as
Eigen::VectorXdto Tasks’MotionConstrandMotionSpringConstr, usingmc_rbdyn::Robot::compensationTorques()when provided, ormc_rbdyn::Robot::externalTorques()as a fallback.Together, these changes enable optional feedforward disturbance rejection in the solver while preserving backward compatibility.
This PR include the content/commits of #500 and extend it.
It is also dependent on this PR in Tasks.