Open mathieu-celerier opened 5 months ago
Need some fixes for now, will update.
@mathieu-celerier Thank you for the Pull Request for the new feature. I have a few questions.
compensateExternalForces
option of DynamicsConstraint
is not supported in Tasks. Shouldn't a warning message be output when this is specified if the solver backend is Tasks?
This PR allows the QP to be aware of the external forces and compensate for them to ensure all constraints and objectives. Compliance of a task, if necessary, is achieved by uncompensating the external forces by adding to the task's reference acceleration the accelerations produced by these external forces.
Detailed description of the additions
Robot
with two new methodstauExternal()
andalphaDExternal()
. Add the update of external torque and acceleration as both output and update of TVM in Robot to prevent recomputation for multiple tasks with theupdateEF()
method.DynamicFunction
to include the compensation of external forces in `b` and a boolean in the constructor to activate or not this feature.DynamicsConstraint
,initialize
, andinitialize_tvm
constructors to include a boolean that allows activation of the compensation feature by forwarding it to theDynamicFunction
.CompliantPostureTask
andCompliantEndEffectorTask
to be used with this QP as it will naturally compensate external forces. Each task is a derivation of their non-compliant versions and adds getter and setter as well as datastore entries to control if a task should be compliant or not. Compliance for the tasks is achieved by adding the acceleration due to the external forces to the reference acceleration of the task.