Dear community, I feel a little desperate since I don't know what I'm doing wrong here: From within our grasp_planner, I want to call the "/plan_kinematic_path" service from the ompl package. Here is how I set it all up: ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ ros::service::waitForService("/ompl_planning/plan_kinematic_path"); ros::ServiceClient client = n.serviceClient("/ompl_planning/plan_kinematic_path"); motion_planning_msgs::GetMotionPlan::Request req; motion_planning_msgs::GetMotionPlan::Response res; std::vector names(7); names[0] = "arm_1_joint"; names[1] = "arm_2_joint"; names[2] = "arm_3_joint"; names[3] = "arm_4_joint"; names[4] = "arm_5_joint"; names[5] = "arm_6_joint"; names[6] = "arm_7_joint"; req.motion_plan_request.goal_constraints.joint_constraints.resize(names.size()); for (unsigned int i = 0 ; i < names.size(); ++i) { req.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = names[i]; req.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0; req.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.1; req.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.1; } //req.motion_plan_request.planner_id=""; req.motion_plan_request.group_name="arm"; req.motion_plan_request.num_planning_attempts=1; //req.motion_plan_request.allowed_planning_time=ros::Duration(50.0); ROS_INFO("calling service..."); if (client.call(req, res)) { ROS_INFO("Result: %d", res.error_code.val); } else { ROS_ERROR("Failed to call service plan_kinematic_path"); return 1; } ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ I.e. I just want a plan that brings the arm back into its home position at (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0). The output from the planner is the following (with Debub output): ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ [ INFO] [1297684313.476421959, 56.604000000]: Received request for planning [ INFO] [1297684313.476537217, 56.604000000]: Selected motion planner: 'kinematic::LBKPIECE[LBKPIECEkConfig2cob]', with priority 11 [DEBUG] [1297684313.476605128, 56.604000000]: No workspace bounding volume was set [ INFO] [1297684313.479051337, 56.604000000]: ompl planning for group arm [DEBUG] [1297684313.479230006, 56.604000000]: Start states before normalization 0 val 0.537644 [DEBUG] [1297684313.479279888, 56.604000000]: Start states before normalization 1 val -1.20871 [DEBUG] [1297684313.479314884, 56.604000000]: Start states before normalization 2 val 0.204787 [DEBUG] [1297684313.479346079, 56.604000000]: Start states before normalization 3 val 0.943959 [DEBUG] [1297684313.479377832, 56.604000000]: Start states before normalization 4 val -0.0503284 [DEBUG] [1297684313.479408952, 56.604000000]: Start states before normalization 5 val 0.00951776 [DEBUG] [1297684313.479439600, 56.604000000]: Start states before normalization 6 val -0.00680026 [DEBUG] [1297684313.479473040, 56.604000000]: Start states after normalization 0 val 0.537644 [DEBUG] [1297684313.479504849, 56.604000000]: Start states after normalization 1 val -1.20871 [DEBUG] [1297684313.479534850, 56.604000000]: Start states after normalization 2 val 0.204787 [DEBUG] [1297684313.479564637, 56.604000000]: Start states after normalization 3 val 0.943959 [DEBUG] [1297684313.479594670, 56.604000000]: Start states after normalization 4 val -0.0503284 [DEBUG] [1297684313.479625155, 56.604000000]: Start states after normalization 5 val 0.00951776 [DEBUG] [1297684313.479655881, 56.604000000]: Start states after normalization 6 val -0.00680026 [DEBUG] [1297684313.480120098, 56.604000000]: Kinematic state space settings: - dimension = 7 - start states: 0.537644 -1.20871 0.204787 0.943959 -0.0503284 0.00951776 -0.00680026 Goal state, threshold = 1e-12, memory address = 0x7fb9b40adb00, state = 0 0 0 0 0 0 0 Joint constraints: [-0.1, 0.1] [-0.1, 0.1] [-0.1, 0.1] [-0.1, 0.1] [-0.1, 0.1] [-0.1, 0.1] [-0.1, 0.1] - bounding box: [-2.957, 2.957](0.05914) [-2.0843, 2.0843](0.041686) [-2.957, 2.957](0.05914) [-2.0843, 2.0843](0.041686) [-2.957, 2.957](0.05914) [-2.0844, 2.0844](0.0416879) [-2.957, 2.957](0.05914) Path constraints: 0 kinematic constraints [DEBUG] [1297684313.480187558, 56.604000000]: Goal threshold is 1e-12 [DEBUG] [1297684313.480328546, 56.604000000]: Collision but allowed touch between sdh_thumb_2_link and sdh_thumb_3_link [DEBUG] [1297684313.480372852, 56.604000000]: Collision but allowed touch between sdh_finger_23_link and sdh_thumb_3_link [DEBUG] [1297684313.480404417, 56.604000000]: Collision but allowed touch between sdh_finger_12_link and sdh_finger_13_link [DEBUG] [1297684313.480434052, 56.604000000]: Collision but allowed touch between sdh_palm_link and sdh_thumb_2_link ... //some more allowed collisions //not a single collision is not allowed!!! ... [DEBUG] [1297684313.507005162, 56.607000000]: Collision but allowed touch between arm_1_link and arm_2_link [DEBUG] [1297684313.507287845, 56.607000000]: Collision but allowed touch between arm_0_link and arm_1_link [DEBUG] [1297684313.507593130, 56.607000000]: Collision but allowed touch between torso_tray_link and base_link [DEBUG] [1297684313.507913948, 56.607000000]: Collision but allowed touch between head_cover_link and torso_upper_neck_tilt_link [DEBUG] [1297684313.508219712, 56.607000000]: Collision but allowed touch between torso_lower_neck_tilt_link and torso_upper_neck_tilt_link Info: LBKPIECE1: Starting with 2 states Info: LBKPIECE1: Created 2 (1 start + 1 goal) states in 2 cells (1 start + 1 goal) [DEBUG] [1297684313.508655728, 56.607000000]: [Failure] Motion planner spent 0.0222179 seconds [DEBUG] [1297684313.508999801, 56.607000000]: Total planning time: 0.0222179; Average planning time: 0.0222179 [ INFO] [1297684313.509273713, 56.607000000]: Ompl says ok [DEBUG] [1297684313.509664645, 56.607000000]: New motion priority for 'kinematic::LBKPIECE[LBKPIECEkConfig2cob]' is 10 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ What irritates me is that "Ompl says ok".......because it's not...=) Still the solution trajectory is empty and the error_code.val is 0. I can use the "/plan_kinematic_path" service successfully when it is called from within the move_arm_action! But since I don't want the robot to move at this point, I just want to call the planner and execute the trajectory later. I also checked how the move_arm_action_server sets up the /plan_kinematic_path request and I couldn't find a difference. Is there anything that I missed while setting the request? How else can I get the solution trajectory to a joint_goal without executing the movement? Thanks for your help! Best, Felix