ompl_planning returns invalid plans

edit

Hi,

I've been using the OMPL planner for the PR2 arm in fairly constrained situations (trying to pick items out of a cabinet) and it keeps returning invalid plans. Specifically, it returns a plan that hits an obstacle in the environment. Usually when this is the case it prints out a message that the "plan appears to have become invalidated already". When I check the validity of the plan using the planning_environment trajectory validity checker, it is indeed invalid. If I actually run the plan, the arm will collide with an obstacle. The environment is not dynamic; the obstacles the arm collides with were present and in the collision map during planning.

At the moment, I am simply re-planning when and invalid plan is returned and usually it will eventually find a valid plan, but I have had over 50 invalid plans returned. Each plan takes some computation time and then more time for the validity check, making for an unacceptably long planning time even when a valid plan is eventually found. The return from the planner is always 0 (whether the plan is valid or not). I tried using the pick and place pickup action for the same goal and observed the same behavior; the plans returned are usually invalid and the pickup action usually fails.

I give the planner 45 seconds, which it never uses all of and often some ordered collisions, but never the ones that invalidate the plan. I've reproduced the full call below.

Has anyone else seen this? Is there something simple I am overlooking?

Thanks, Jenny

motion_planning_msgs::GetMotionPlan mprsrv;

mprsrv.request.motion_plan_request.group_name =
    handDescription().armGroup(req.pickup_goal.arm_name);
mprsrv.request.motion_plan_request.num_planning_attempts = 1;
mprsrv.request.motion_plan_request.allowed_planning_time = 
    ros::Duration(45.0);

//I set mprsrv.request.motion_plan_request.goal_constraints, which is long and
//dependent on the arguments to the function so I've omitted it
mprsrv.request.motion_plan_request.ordered_collision_operations = 
    collisionOperationsForGrasp(req.pickup_goal, req.names);

//I set the starting state equal to the robot's current state using get_robot_state

mprsrv.request.motion_plan_request.workspace_parameters.
    workspace_region_pose.header.stamp = ros::Time::now();

ros::ServiceClient planner = 
   n.serviceClient<motion_planning_msgs::GetMotionPlan>
  ("/ompl_planning/plan_kinematic_path");

ROS_INFO("Waiting for planning service"); planner.waitForExistence(); ROS_INFO("Planning"); bool goodcall = planner.call(mprsrv); ROS_INFO("Finished planning");