Fixed missing IK solver initialization of KinematicGroupInstructionInfo

tesseract-robotics · Sep 11, 2023 · 3edff64 · 3edff64
1 parent 31dbed5
commit 3edff64
Showing 1 changed file with 7 additions and 1 deletion.
diff --git a/tesseract_motion_planners/simple/src/interpolation.cpp b/tesseract_motion_planners/simple/src/interpolation.cpp
@@ -114,7 +114,7 @@ KinematicGroupInstructionInfo::KinematicGroupInstructionInfo(const MoveInstructi
     throw std::runtime_error("InstructionInfo, working frame is empty!");
 
   // Get Previous Instruction Kinematics
-  manip = request.env->getKinematicGroup(mi.manipulator);
+  manip = request.env->getKinematicGroup(mi.manipulator, mi.manipulator_ik_solver);
 
   // Get Previous Instruction TCP and Working Frame
   working_frame = mi.working_frame;
@@ -1150,6 +1150,12 @@ Eigen::VectorXd getClosestJointSolution(const KinematicGroupInstructionInfo& inf
         }
       }
     }
+    if (jp_final.size() == 0)
+      CONSOLE_BRIDGE_logError("getClosestJointSolution: No solution satisfied position limits");
+  }
+  else
+  {
+    CONSOLE_BRIDGE_logError("getClosestJointSolution: No solutions found");
   }
   return jp_final;
 }