From d92c6a7e252189b6ae614555c6a75ef615f1babd Mon Sep 17 00:00:00 2001 From: Sinan Barut Date: Thu, 1 Aug 2024 17:38:50 +0200 Subject: [PATCH] Allow desired twists in velocity/CartesianAdmittance This was not possible before due to overwriting of desired twists by the force feedback --- src/tasks/velocity/CartesianAdmittance.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/tasks/velocity/CartesianAdmittance.cpp b/src/tasks/velocity/CartesianAdmittance.cpp index 56a8ee11..56062352 100644 --- 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(); }