Bugfix in WholeBodyDynamicsDevice filtering

Co-authored-by: Prashanth Ramadoss <[email protected]>
robotology · Mar 9, 2022 · 5329f02 · 5329f02
1 parent 7058fe2
commit 5329f02
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp
@@ -2203,7 +2203,7 @@ void WholeBodyDynamicsDevice::filterSensorsAndRemoveSensorOffsets()
 
         // Run the filter
         Eigen::Ref<const Eigen::VectorXd> outputFt = filters.forcetorqueFilters[ft]->filt(filters.bufferEigen6);
-        iDynTree::toEigen(filteredFTMeasure.getLinearVec3()) = outputFt.tail<3>();
+        iDynTree::toEigen(filteredFTMeasure.getLinearVec3()) = outputFt.head<3>();
         iDynTree::toEigen(filteredFTMeasure.getAngularVec3()) = outputFt.tail<3>();
 
         filteredSensorMeasurements.setMeasurement(iDynTree::SIX_AXIS_FORCE_TORQUE,ft,filteredFTMeasure);