ompl_planning/GetMotionPlan returns non-valid trajectories

edit

Hi All

As per above, I am trying to call ompl_planning/GetMotionPlan service from the ompl_planning node for arm navigation.

I'm not using a move_arm action but am calling the ompl_planning node directly. I'm trying to keep it as simple as possible and just want ompl to generate a trajectory for me. I am using ROSJAVA though I dont think that should affect the message traffic.

Noting issues listed with incorrect parameters here The list of parameters I am loading in my request is here:

motionplanrequest.getMotionPlanRequest().setAllowedPlanningTime(Duration.fromMillis(5000));
                            motionplanrequest.getMotionPlanRequest().setExpectedPathDuration(Duration.fromMillis(500));
                            motionplanrequest.getMotionPlanRequest().setExpectedPathDt(Duration.fromMillis(50)); //effectively mean that the returned plan will be one interation long
                            motionplanrequest.getMotionPlanRequest().setNumPlanningAttempts(1);
                            motionplanrequest.getMotionPlanRequest().setGroupName("r_arm");
                            motionplanrequest.getMotionPlanRequest().setPlannerId("");
                            //update the GetMotionPlan Contraints with the new pose goal 
                            PositionConstraint Goalpos = node.getTopicMessageFactory().newFromType(PositionConstraint._TYPE);
                            OrientationConstraint Goalo = node.getTopicMessageFactory().newFromType(OrientationConstraint._TYPE);

                            Goalpos.getPosition().setX(desiredx);
                            Goalpos.getPosition().setY(desiredy);
                            Goalpos.getPosition().setZ(desiredz);
                            Goalpos.getHeader().setFrameId("base_link");
                            Goalpos.setLinkName("Tool");
                            double[] dims = {0.02, 0.02, 0.02};

                            Goalpos.getConstraintRegionShape().setType(Shape.BOX);
                            Goalpos.getConstraintRegionShape().setDimensions(dims);
                            Goalpos.getConstraintRegionOrientation().setW(1);
                            Goalpos.setWeight(1);

                            Goalo.getHeader().setStamp(node.getCurrentTime());
                            Goalo.getHeader().setFrameId("base_link");
                            Goalo.setLinkName("Tool");
                            Goalo.setAbsolutePitchTolerance(0.04);
                            Goalo.setAbsoluteRollTolerance(0.04);
                            Goalo.setAbsoluteYawTolerance(0.04);
                            Goalo.getOrientation().setW(desiredquat.getW());
                            Goalo.getOrientation().setX(desiredquat.getX());
                            Goalo.getOrientation().setY(desiredquat.getY());
                            Goalo.getOrientation().setZ(desiredquat.getZ());
                            Goalo.setWeight(1);

                            motionplanrequest.getMotionPlanRequest().getGoalConstraints().getPositionConstraints().add(Goalpos);
                            motionplanrequest.getMotionPlanRequest().getGoalConstraints().getOrientationConstraints().add(Goalo);

The nodes that are available are listed here, they are basically those available from the warehouse viewer wizard, minus the warehouse viewer itself:

HMM_version2_move_r_arm (move_arm/move_arm_simple_action)
 HMM_version2_r_arm_kinematics (arm_kinematics_constraint_aware/arm_kinematics_constraint_aware)
environment_server (planning_environment/environment_server)
interpolated_ik_node_left (interpolated_ik_motion_planner/interpolated_ik_motion_planner.py)
interpolated_ik_node_right (interpolated_ik_motion_planner/interpolated_ik_motion_planner.py)
mongo (mongodb/wrapper.py)
ompl_planning (ompl_ros_interface/ompl_ros)
planning_scene_validity_server (planning_environment/planning_scene_validity_server)
rob_st_pub (robot_state_publisher/state_publisher)
rviz_warehouse_viewer (rviz/rviz)
trajectory_filter_server (trajectory_filter_server/trajectory_filter_server)

plus I have my ROSJAVA node that is trying to call ompl_planning/GetMotionPlan. When It has called the service is immediately publishes the trajectory to my trajectory controller node (this works with properly formatted trajectories). I believe the trajectory is empty because the topic contains the following messages that would have been the result of the service response:

   header: 
  seq: 20046
  stamp: 
    secs: 0
    nsecs: 0
  frame_id: ''
joint_names: ['joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7']
points: []

As for other errors, there are none listed, I did have issues with a non-available planning scene however the HMM_version2_move_r_arm node cleared that up. Additionally Rviz shows my custom arm moving as it does in gazebo. Below is the command output from the launch file that handles the majority of arm_navigation_nodes:

 [INFO] [WallTime: 1363504840.808273] [2221.336500] ik_utilities: waiting for IK services to be there
[INFO] [WallTime: 1363504840.853969] [2221.339800] ik_utilities: waiting for IK services to be there
[ INFO] [1363504845.675942901, 2222.123500000]: waitForService: Service [/register_planning_scene] has not been advertised, waiting...
[ INFO] [1363504845.695581190, 2222.128200000]: Environment server started
[ INFO] [1363504845.785783314, 2222.143600000]: waitForService: Service [/register_planning_scene] is now available.
[ INFO] [1363504845 ...