Ask Your Question
0

ompl_planning/GetMotionPlan returns non-valid trajectories

asked 2013-03-16 21:11:04 -0600

updated 2014-01-28 17:15:45 -0600

ngrennan gravatar image

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 ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2013-03-17 00:58:45 -0600

Disregard, as per the construct I had to change the value for my environment_server, use_monitor parameter to true (as I wanted the environment server to use actual tf data from my model) as listed in the fine print here.

But that was still not working as it was currently waiting for the full state of the robot. I had been only pubishing the joint states for the joints I had wanted to control. But I needed to publish all the joint states as listed in my urdf file (ie including joints describing grippers etc) once I did that the environment_server started working. correctly.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

Asked: 2013-03-16 21:11:04 -0600

Seen: 563 times

Last updated: Mar 17 '13