dual_arm_planning_api

h1. Planning API functions description

h2. 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. 

<pre>

roslaunch dual_arm_planning_api dual_arm_planning_api_simu.launch

</pre>

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.* 

<pre>

roslaunch dual_arm_planning_api dual_arm_planning_api.launch

</pre>

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:

<pre>

rosservice call /dual_arm_planning_api/init "{}"

</pre>

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.

<pre>

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"

</pre>

First you can notice that i use rostopic pub instead of rosaction call. The reason is that with rostopic pub we can have autocompletion in the terminal (with tab) which is very useful when we have a lot of arguments.

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:

<pre>

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"

</pre>