From 6347d348d08c68eaf9803bffe9dd08742daaab3f Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Fri, 11 Nov 2016 09:07:02 -0700 Subject: [PATCH] Improved RobotState feedback for setFromIK() (#342) --- moveit_core/robot_state/src/robot_state.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index f3d28134b2..c5101624dc 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -1614,13 +1614,13 @@ bool moveit::core::RobotState::setFromIKSubgroups(const JointModelGroup *jmg, co // Error check if (poses_in.size() != sub_groups.size()) { - logError("Number of poses must be the same as number of sub-groups"); + logError("Number of poses (%u) must be the same as number of sub-groups (%u)", poses_in.size(), sub_groups.size()); return false; } if (tips_in.size() != sub_groups.size()) { - logError("Number of tip names must be the same as number of sub-groups"); + logError("Number of tip names (%u) must be same as number of sub-groups (%u)", tips_in.size(), sub_groups.size()); return false; } @@ -1707,7 +1707,8 @@ bool moveit::core::RobotState::setFromIKSubgroups(const JointModelGroup *jmg, co if (pose_frame != solver_tip_frame) { - logError("Cannot compute IK for query pose reference frame '%s'", pose_frame.c_str()); + logError("Cannot compute IK for query pose reference frame '%s', desired: '%s'", pose_frame.c_str(), + solver_tip_frame.c_str()); return false; } }