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