PILZ: Throw if IK solver doesn't exist (#2082) (#2921)

moveit · Aug 2, 2024 · d583c04 · d583c04
1 parent d26ec06
commit d583c04
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 1 deletion.
diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp
@@ -34,6 +34,8 @@
 
 #include <pilz_industrial_motion_planner/trajectory_generator_lin.h>
 
+#include <pilz_industrial_motion_planner/tip_frame_getter.h>
+
 #include <cassert>
 #include <sstream>
 #include <time.h>
@@ -75,7 +77,7 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin
   // goal given in joint space
   if (!req.goal_constraints.front().joint_constraints.empty())
   {
-    info.link_name = robot_model_->getJointModelGroup(req.group_name)->getSolverInstance()->getTipFrame();
+    info.link_name = getSolverTipFrame(robot_model_->getJointModelGroup(req.group_name));
 
     if (req.goal_constraints.front().joint_constraints.size() !=
         robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames().size())

diff --git a/...ners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp b/...ners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp
@@ -220,6 +220,10 @@ TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIKSolver)
   const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_);
   const kinematics::KinematicsBaseConstPtr& solver = jmg->getSolverInstance();
 
+  if (!solver)
+  {
+    throw("No IK solver configured for group '" + planning_group_ + "'");
+  }
   // robot state
   moveit::core::RobotState rstate(robot_model_);