fix: TrajectoryFollowingController: returing twist in local frame

source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp

@@ -203,7 +203,17 @@ namespace armarx::navigation::traj_ctrl
         ARMARX_VERBOSE << deactivateSpam(1) << "Twist limited " << twistLimited.linear.transpose();
         ARMARX_VERBOSE << deactivateSpam(1) << "Twist angular " << twistLimited.angular.transpose();
 
-        return TrajectoryControllerResult{.twist = twistLimited,
+        // convert to the robot's base frame
+        const core::Pose global_T_robot(robot->getGlobalPose());
+
+        const auto& twistGlobal = twistLimited;
+
+        core::Twist twistLocal;
+        twistLocal.linear = global_T_robot.linear().inverse() * twistGlobal.linear;
+        // TODO if not in 2D, then this must be changed!
+        twistLocal.angular = twistGlobal.angular;
+
+        return TrajectoryControllerResult{.twist = twistLocal,
                                           .dropPoint = projectedPose.projection};
     }