Merge pull request #215 from Gauss-Robotics/fix/cartesian_admittance_…

…task Allow desired twists in velocity/CartesianAdmittance
ADVRHumanoids · Aug 2, 2024 · 3e114c4 · 3e114c4
2 parents 50ce7e9 + d92c6a7
commit 3e114c4
Showing 1 changed file with 3 additions and 1 deletion.
diff --git a/src/tasks/velocity/CartesianAdmittance.cpp b/src/tasks/velocity/CartesianAdmittance.cpp
@@ -69,7 +69,9 @@ void CartesianAdmittance::_update()
     _wrench_filt = Eigen::Vector6d::Map(_filter.process(_tmp).data(), CHANNELS);
 
 
-    _desiredTwist = _C.asDiagonal()*_wrench_filt;
+    // Adding to the desired twist allows to still use a desired twist additionally
+    // given by Cartesian::setReference(..., const Eigen::Vector6d& desiredTwist)
+    _desiredTwist += _C.asDiagonal()*_wrench_filt;
 
     Cartesian::_update();
 }