From 78745a51858a8c79caecadc00b46cdbf13ac9b0c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Hawo=20H=C3=B6fer?= <uuujt@student.kit.edu>
Date: Thu, 9 Mar 2023 10:27:25 +0100
Subject: [PATCH] also fix translational pose
---
.../ArticulatedObjectGeometricPlanningHelper.cpp | 8 +++++++-
1 file changed, 7 insertions(+), 1 deletion(-)
diff --git a/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp b/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp
index 69ae26e28..dabe7e348 100644
--- a/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp
+++ b/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp
@@ -269,10 +269,16 @@ namespace simox::geometric_planning
subpart->registerRobotNode(jointReferenceNode);
CHECK(jointReferenceNode->initialize(subpart->getRootNode()));
+
// ARMARX_DEBUG << "registered robot node `" << jointReferenceNode->getName() << "`";
+ Eigen::Isometry3f global_T_node(node->getGlobalPose());
+ Eigen::Isometry3f global_T_jointReference(jointReferenceNode->getGlobalPose());
+
+ Eigen::Isometry3f jointReference_T_node = global_T_jointReference.inverse() * global_T_node;
+ // jointRef_T_node = jointRef_T_global * global_T_node
return ParametricPath(
- jointReferenceNode, std::make_unique<Line>(parameterRange), Pose::Identity());
+ jointReferenceNode, std::make_unique<Line>(parameterRange), jointReference_T_node);
}
ParametricPath
--
GitLab