diff --git a/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp b/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp index d149caed3a25294d59c55b61cd6d19294dcfc8b9..92b02612dd327bc9b80a09f5ec166e047139ccf4 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; }