Fixed RobotPose.changeEndEffectorFrame

TimSchneider42 · TimSchneider42 · commit 7d928b5007a8 · 2024-05-27T19:31:41.000+02:00
diff --git a/include/franky/robot_pose.hpp b/include/franky/robot_pose.hpp
@@ -39,7 +39,7 @@ class RobotPose {
   }
 
   [[nodiscard]] inline RobotPose changeEndEffectorFrame(const Affine &transform) const {
-    return {end_effector_pose_ * transform.inverse(), elbow_position_};
+    return {end_effector_pose_ * transform, elbow_position_};
   }
 
   [[nodiscard]] inline RobotPose with_elbow_position(const std::optional<double> elbow_position) const {

Original file line number	Diff line number	Diff line change
`@@ -39,7 +39,7 @@ class RobotPose {`
`39`	`39`	`}`
`40`	`40`
`41`	`41`	`[[nodiscard]] inline RobotPose changeEndEffectorFrame(const Affine &transform) const {`
`42`		`- return {end_effector_pose_ * transform.inverse(), elbow_position_};`
	`42`	`+ return {end_effector_pose_ * transform, elbow_position_};`
`43`	`43`	`}`
`44`	`44`
`45`	`45`	`[[nodiscard]] inline RobotPose with_elbow_position(const std::optional<double> elbow_position) const {`