[ompl-users] path constraints & strange arm movement

[Dirkx, N.J. <n.j...@st...>]

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 what extent can the planners deal with path constraints?
	Is there a planner that is recommended if I want to use path constraints?
		

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.
       Is there a way to suppress these movements or to punish joint states that have a large distance to, say, the ideal path?

Thanks

Nic