[ros-users] unable to get valid path when calling /ompl_planning/plan_kinematic_path
Sachin Chitta
sachinc at willowgarage.com
Mon Feb 14 18:20:34 UTC 2011
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.
Regards,
Sachin
On Mon, Feb 14, 2011 at 4:41 AM, Felix Messmer <felix_messmer at web.de> wrote:
>
> 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<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):
>
>
> ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
> [ 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
>
> _______________________________________________
> ros-users mailing list
> ros-users at code.ros.org
> https://code.ros.org/mailman/listinfo/ros-users
>
--
Sachin Chitta
Research Scientist
Willow Garage
-------------- next part --------------
An HTML attachment was scrubbed...
URL: <http://lists.ros.org/pipermail/ros-users/attachments/20110214/558a0203/attachment-0003.html>
More information about the ros-users
mailing list