From 0b89a3e4743367055b829303b03b74268419e2f3 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Tue, 1 Aug 2023 12:10:08 +0200
Subject: [PATCH] Fix using cloned nodes instead of original nodes to get the
correct local trafo (for real)
---
VirtualRobot/RobotFactory.cpp | 4 ++--
1 file changed, 2 insertions(+), 2 deletions(-)
diff --git a/VirtualRobot/RobotFactory.cpp b/VirtualRobot/RobotFactory.cpp
index 1764a0e67..feb7ac101 100644
--- a/VirtualRobot/RobotFactory.cpp
+++ b/VirtualRobot/RobotFactory.cpp
@@ -1358,8 +1358,8 @@ namespace VirtualRobot
RobotNodePtr clonedLink =
originalLink->clone(flattenedRobot, cloneChildren, clonedBody);
- Eigen::Matrix4f global_T_body = clonedBody->getGlobalPose();
- Eigen::Matrix4f global_T_link = clonedLink->getGlobalPose();
+ Eigen::Matrix4f global_T_body = originalBody->getGlobalPose();
+ Eigen::Matrix4f global_T_link = originalLink->getGlobalPose();
Eigen::Matrix4f body_T_link = simox::math::inverted_pose(global_T_body) * global_T_link;
--
GitLab