Ask Your Question
2

unable to get valid path when calling /ompl_planning/plan_kinematic_path

asked 2011-02-14 16:13:28 -0500

Tully gravatar image

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<motion_planning_msgs::getmotionplan>("/ompl_planning/plan_kinematic_path");

motion_planning_msgs::GetMotionPlan::Request req; motion_planning_msgs::GetMotionPlan::Response res;

std::vector<std::string> 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):

~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 'kinematic::LBKPIECE[LBKPIECEkConfig2cob]', with priority 11 volume was set normalization 0 val 0.537644 normalization 1 val -1.20871 normalization 2 val 0.204787 normalization 3 val 0.943959 normalization 4 val -0.0503284 normalization 5 val 0.00951776 normalization 6 val -0.00680026 normalization 0 val 0.537644 normalization 1 val -1.20871 normalization 2 val 0.204787 normalization 3 val 0.943959 normalization 4 val -0.0503284 normalization 5 val 0.00951776 normalization 6 val -0.00680026 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 -2.0843, 2.0843 -2.957, 2.957 -2.0843, 2.0843 -2.957, 2.957 -2.0844, 2.0844 -2.957, 2.957 Path constraints: 0 kinematic constraints

touch between sdh_thumb_2_link and sdh_thumb_3_link touch between sdh_finger_23_link and sdh_thumb_3_link touch between sdh_finger_12_link and sdh_finger_13_link touch between sdh_palm_link and sdh_thumb_2_link ... //some more allowed collisions //not a single collision is not allowed!!! ... touch between arm_1_link and arm_2_link touch between arm_0_link and arm_1_link touch between torso_tray_link and base_link touch between head_cover_link and torso_upper_neck_tilt_link 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) spent 0.0222179 seconds 0.0222179; Average planning time: 0.0222179 'kinematic::LBKPIECE[LBKPIECEkConfig2cob]' is 10 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

What irritates me is that "Ompl says ok".......because ... (more)

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
2

answered 2011-02-14 16:14:10 -0500

tfoote gravatar image

You have to give it some allowed planning time. Right now, planning time will default to 0 since you have commented this line:

//req.motion_plan_request.allowed_planning_time=ros::Duration(50.0);

The ros output for this case does need fixing though. Please open a ticket against me to that effect.

edit flag offensive delete link more

Comments

I commented that out since I didn't know what value to set here. I guess 50.0 is a little too pessimistic...;-) Also, I opened a ticket: https://code.ros.org/trac/ros-pkg/ticket/4787
tfoote gravatar imagetfoote ( 2011-02-14 16:14:30 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

Asked: 2011-02-14 16:13:28 -0500

Seen: 712 times

Last updated: Feb 14 '11