Re: [ompl-users] path constraints & strange arm movement

[<is...@gm...>]

Hello Nic,

I am cc'ing Sachin Chitta (from Willow Garage), in case he has additional  
comments. Responses are inline.

On Apr 28, 2011 7:20am, "Dirkx, NJ" <nj...@st...> wrote:
> Hi there
> ,


> I am using OMPL planners for a 7DOF robotic arm (non-pr2) with ROS. More  
> specific, I have set it up according to the pr2_arm_navigation stack, so  
> I am using the move_arm node and the ompl_ros_interface.



> The entire chain from perception to execution is working, I only ran into  
> two problems:



> 1.Path constraints:

> As soon as I add path constraints on the orientation of the end effector  
> the planners fail to find a path. It doesn't matter which planner I  
> choose, they all show the same behaviour. Even if the constraints are set  
> really large (say 2.5 rad), they don't succeed in finding a solution.

> The following code snippet shows how I added the path constraints:



> //set path constraints

> goalB.motion_plan_request.path_constraints.orientation_constraints.resize(1);

> goalB.motion_plan_request.path_constraints.orientation_constraints[0].header.frame_id  
> = "base_link";

> goalB.motion_plan_request.path_constraints.orientation_constraints[0].header.stamp  
> =  
> ros::Time::now();

> goalB.motion_plan_request.path_constraints.orientation_constraints[0].link_name  
> = "hand_right";



> geometry_msgs::Quaternion quat_msg =  
> tf::createQuaternionMsgFromRollPitchYaw(0.0, 0.0, 0.0);



> goalB.motion_plan_request.path_constraints.orientation_constraints[0].orientation.x  
> =  
> quat_msg.x;

> goalB.motion_plan_request.path_constraints.orientation_constraints[0].orientation.y  
> =  
> quat_msg.y;

> goalB.motion_plan_request.path_constraints.orientation_constraints[0].orientation.z  
> =  
> quat_msg.z;

> goalB.motion_plan_request.path_constraints.orientation_constraints[0].orientation.w  
> =  
> quat_msg.w;



> goalB.motion_plan_request.path_constraints.orientation_constraints[0].type  
> =  
> motion_planning_msgs::OrientationConstraint::HEADER_FRAME;

> goalB.motion_plan_request.path_constraints.orientation_constraints[0].absolute_roll_tolerance  
> =  
> 1.5;

> goalB.motion_plan_request.path_constraints.orientation_constraints[0].absolute_pitch_tolerance  
> =  
> 1.5;

> goalB.motion_plan_request.path_constraints.orientation_constraints[0].absolute_yaw_tolerance  
> =  
> 1.5;



> My question are:

> Is this the right way to give in path constraints?

To me it seems the setup is correct, but it has changed a bit since I last  
used it. Perhaps Sachin can comment if there is something wrong.

 From the behaviour you describe for the planners, it sounds like they are  
unable to find valid states. Do you by any chance have ros::console  
information from OMPL? It should appear in your log and should say things  
like "No solution found after ... seconds" and
".... created ... states". I'm curious about the number of states created  
by the planners. If this is low (under 10) it means there is something  
wrong with the evaluation of validity, and perhaps in your input.



> To what extent can the planners deal with path constraints?


The planners rely on sampling the state space. If a constraint will reduce  
the volume of the valid space to 0 within the 7-dimensional state space for  
the robot, the methods currently employed by the planners will not work. If  
you keep the volume above 0, the probability of finding a solution is  
proportional to the ratio of the valid volume with respect to the total  
volume.

> Is there a planner that is recommended if I want to use path constraints?

It really depends on the problem you are solving. I don't think I can say  
this with certainty. I would avoid lazy planners perhaps. So... if SBL or  
LBKPIECE1 or LazyRRT appear to be slower, they probably will be  
consistently slower.
My top choices would be KPIECE1 and RRTConnect. OMPL just got a new planner  
(BKPIECE1), which I think will soon be available through ROS as well. That  
will perhaps be a good choice as well.





> 2.Large/strange arm movement

> The planned planned path to the end pose very seems to be very indirect/  
> inefficient. The arm moves through a large part of the joint space before  
> ending up on the right spot, resulting in very exotic arm movements. All  
> planners show similar behaviour.

This is due to the randomized nature of the algorithms. Adding constraints  
typically makes matters worse, from this point of view :(

> Is there a way to suppress these movements or to punish joint states that  
> have a large distance to, say, the ideal path?

We just added some smoothing code in OMPL, but it is not yet part of ROS.  
This will still not give you an optimal path, but it should produce more  
natural looking paths.


Ioan



> Thanks



> Nic

> ------------------------------------------------------------------------------

> WhatsUp Gold - Download Free Network Management Software

> The most intuitive, comprehensive, and cost-effective network

> management toolset available today. Delivers lowest initial

> acquisition cost and overall TCO of any competing solution.

> http://p.sf.net/sfu/whatsupgold-sd

> _______________________________________________

> ompl-users mailing list

> omp...@li...

> https://lists.sourceforge.net/lists/listinfo/ompl-users