diff --git a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp b/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp index 2ae7f289e84e730e229e30f045e0781756d7568c..b7fcdb2df4bdb2278050a16bceb3c425b58dfe81 100644 --- a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp +++ b/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}; }