Fix KinematicGroupInstructionInfo and JointGroupInstructionInfo extra…

…ct cartesian pose
tesseract-robotics · Oct 17, 2024 · 9df7378 · 9df7378
1 parent e6f96b1
commit 9df7378
Showing 1 changed file with 8 additions and 2 deletions.
diff --git 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<CartesianWaypointPoly>().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<CartesianWaypointPoly>().getTransform();