From 075be8c76314e4b86c9f201708996c0bc2c203a6 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Mon, 27 Feb 2023 19:54:26 +0100
Subject: [PATCH] potential fix
---
.../ArticulatedObjectGeometricPlanningHelper.cpp | 7 +++++++
1 file changed, 7 insertions(+)
diff --git a/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp b/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp
index d149caed3..92b02612d 100644
--- a/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp
+++ b/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp
@@ -43,6 +43,9 @@ namespace simox::geometric_planning
const auto node = articulatedObject->getRobotNode(nodeName);
REQUIRE(node != nullptr);
+ const auto global_T_object_root = articulatedObject->getGlobalPose();
+ articulatedObject->setGlobalPose(Eigen::Matrix4f::Identity());
+
simox::geometric_planning::ArticulatedObjectGeometricPlanningHelper helper(
articulatedObject);
@@ -84,6 +87,10 @@ namespace simox::geometric_planning
const auto parametricPath = helper.getPathForNode(node->getName(), joint->getName());
+ // reset global pose
+ articulatedObject->setGlobalPose(global_T_object_root);
+
+
return parametricPath;
}
--
GitLab