Planning API functions description¶

Master branch¶

The whole CM2P stack can be launched with a simple launch file. The launch files are available in this package (dual_arm_planning_api/launch).
There is two of them. One is for launching CM2P in simulation mode.

roslaunch dual_arm_planning_api dual_arm_planning_api_simu.launch

The other is for launching CM2P with the real robots. The two instances of CM2C must be running beforehand so that the controllers are spawned.

roslaunch dual_arm_planning_api dual_arm_planning_api.launch

Now the dual_arm_planning_api node is running. It is necessary to run the following rosservice in another terminal to finish the initialization of the node:

rosservice call /dual_arm_planning_api/init "{}" 

This takes a few seconds to load the environment, the frames etc. You should see all of this pop in Rviz.
After that the node is ready to take commands.

We can start with a left pick or a right pick.

rostopic pub /dual_arm_planning_api/PickLeft/goal dual_arm_planning_api/PickActionGoal "header:
  seq: 0
  stamp:
    secs: 0
    nsecs: 0
  frame_id: ''
goal_id:
  stamp:
    secs: 0
    nsecs: 0
  id: ''
goal:
  planning_time: 10.0
  pre_grasp_approach_direction:
    x: 0.0
    y: 0.0
    z: 1.0
  pre_grasp_approach_distance: 0.1" 

rostopic pub /dual_arm_planning_api/PickRight/goal dual_arm_planning_api/PickActionGoal "header:
  seq: 0
  stamp:
    secs: 0
    nsecs: 0
  frame_id: ''
goal_id:
  stamp:
    secs: 0
    nsecs: 0
  id: ''
goal:
  planning_time: 10.0
  pre_grasp_approach_direction:
    x: 0.0
    y: 0.0
    z: 1.0
  pre_grasp_approach_distance: 0.1" 

First you can notice that i use rostopic pub instead of rosaction call. The reason is that with rostopic pub we can have auto-completion in the terminal (with tab) which is very useful when we have a lot of arguments. You will have it with rosservices as well, but not with rosactions.
But most importantly you will notice that there is no pose defined for the grasp frame. This is because there is one defined during the initialization for each arm.
But you can change it whenever you want with the following service:

rosservice call /dual_arm_planning_api/SetLeftGrasp  "position:
  x: 0.0
  y: 0.0
  z: 0.0
orientation:
  x: 0.0
  y: 0.0
  z: 0.0" 

You can notice that the frames (left_arm_desired_Grasp or right_arm_desired_Grasp) are updated in Rviz after calling the service.
The SetLeftGrasp (respectively SetRightGrasp) service must be called however before the PickLeft (respectively PickRight) action.
You can use PickLeft or PickRight in the order you want. The first pick will be a default moveit! pick and the second one a dual-arm pick.
You don't have to worry about that as you will use PickLeft (respectively PickRight) for both.

Now that the object is picked by both arms, we can start to think about the place.
First we have to define the new location of the object after the place. For that we call the following service:

rosservice call  /dual_arm_planning_api/SetObjectPose "position:
  x: 0.0
  y: 0.0
  z: 0.4
orientation:
  x: 0.0
  y: 0.0
  z: 0.0" 

With this service we define the pose of the dynamic object (after place) in the frame of the static object.
You can notice that the frames are updated in Rviz after calling the service. This is really useful to check the validity of the new pose before planning.
Once this is done we can plan a place().

This is done by calling the following action:

rostopic pub /dual_arm_planning_api/PlanPlace/goal dual_arm_planning_api/PlanPlaceActionGoal "header:
  seq: 0
  stamp:
    secs: 0
    nsecs: 0
  frame_id: ''
goal_id:
  stamp:
    secs: 0
    nsecs: 0
  id: ''
goal:
  planning_time: 400.0
  start_translation_direction:
    x: 0.0
    y: 0.0
    z: 1.0
  start_translation_distance: 0.1" 

It only takes the start translation (translation at the beginning of the place) and the planning time as arguments.
In the example above we add a translation of 10cm in the Z direction. This is defined in the frame of the object.
We use a planning time of 400seconds. Once a solution is found, the remaining time will be used for path simplification.

Once the solution is found and the path simplified, you should be able to see in Rviz the planned trajectories displayed in purple.
After checking the validity of the trajectories you can execute the place() by calling the following action:

rosaction call /dual_arm_planning_api/Place "{}"