From 44f3f88f76e4b51863388d5cef38f6de34aa57d4 Mon Sep 17 00:00:00 2001
From: armar-user <armar6@kit.edu>
Date: Mon, 25 Jul 2022 20:01:00 +0200
Subject: [PATCH] fix: TrajectoryFollowingController: returing twist in local
frame
---
.../TrajectoryFollowingController.cpp | 12 +++++++++++-
1 file changed, 11 insertions(+), 1 deletion(-)
diff --git a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp b/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp
index 2ae7f289..b7fcdb2d 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};
}
--
GitLab