From 5970c309b8f3809fc79cce2661d2e2215e9859f3 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Thu, 17 Oct 2024 09:13:51 -0500 Subject: [PATCH] Fix KinematicGroupInstructionInfo and JointGroupInstructionInfo extract cartesian pose --- tesseract_motion_planners/simple/src/interpolation.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/tesseract_motion_planners/simple/src/interpolation.cpp b/tesseract_motion_planners/simple/src/interpolation.cpp index d4ba6124ea..73836099ac 100644 --- a/tesseract_motion_planners/simple/src/interpolation.cpp +++ b/tesseract_motion_planners/simple/src/interpolation.cpp @@ -113,7 +113,10 @@ Eigen::Isometry3d JointGroupInstructionInfo::calcCartesianPose(const Eigen::Vect Eigen::Isometry3d JointGroupInstructionInfo::extractCartesianPose(bool in_world) const { if (!instruction.getWaypoint().isCartesianWaypoint()) - throw std::runtime_error("Instruction waypoint type is not a CartesianWaypoint, unable to extract cartesian pose!"); + { + const auto& jp = getJointPosition(instruction.getWaypoint()); + return calcCartesianPose(jp, in_world); + } if (in_world) return working_frame_transform * instruction.getWaypoint().as().getTransform(); @@ -184,7 +187,10 @@ Eigen::Isometry3d KinematicGroupInstructionInfo::calcCartesianPose(const Eigen:: Eigen::Isometry3d KinematicGroupInstructionInfo::extractCartesianPose(bool in_world) const { if (!instruction.getWaypoint().isCartesianWaypoint()) - throw std::runtime_error("Instruction waypoint type is not a CartesianWaypoint, unable to extract cartesian pose!"); + { + const auto& jp = getJointPosition(instruction.getWaypoint()); + return calcCartesianPose(jp, in_world); + } if (in_world) return working_frame_transform * instruction.getWaypoint().as().getTransform();