Better debug output for IK tip frames (moveit#603)

otamachan · Sep 21, 2017 · 27ac0c6 · 27ac0c6
1 parent 127d262
commit 27ac0c6
Showing 1 changed file with 6 additions and 1 deletion.
diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp
@@ -1562,7 +1562,12 @@ bool moveit::core::RobotState::setFromIK(const JointModelGroup* jmg, const Eigen
     // Make sure one of the tip frames worked
     if (!found_valid_frame)
     {
-      logError("moveit.robot_state: Cannot compute IK for query pose reference frame '%s'", pose_frame.c_str());
+      logError("moveit.robot_state: Cannot compute IK for query %u pose reference frame '%s'", i, pose_frame.c_str());
+      // Debug available tip frames
+      std::stringstream ss;
+      for (solver_tip_id = 0; solver_tip_id < solver_tip_frames.size(); ++solver_tip_id)
+        ss << solver_tip_frames[solver_tip_id] << ", ";
+      logError("Available tip frames: [%s]", ss.str().c_str());
       return false;
     }