dual_arm_planning_api

h1. Cooperative Manipulation Planning

!cm2p_laas.png!

h2. Installation process

- Install ROS Melodic

- Install ROS Moveit! and ompl packages. http://docs.ros.org/en/melodic/api/moveit_tutorials/html/doc/getting_started/getting_started.html

- Create a ROS catkin workspace for CM2P. Inside you will clone the following repositories:

- Clone https://redmine.laas.fr/projects/dual_pandas_moveit_config and be on laas_setup branch. 

- Clone http://wiki.ros.org/manipulation_msgs and http://wiki.ros.org/household_objects_database_msgs. Basically these are kinetic repos that do not exist in melodic. 

But we can put them in the catkin workspace and it will work fine. It is to avoid compatibility problems with moveit_ompl_planning_interface (if i rememember well) so install them before.

- Clone moveit_ompl_planning_interface. The original repository and documentation can be found here: https://github.com/KavrakiLab/moveit_ompl_planning_interface

However you should clone this repo: https://redmine.laas.fr/projects/moveit_ompl_planning_interface

It is the code compatible with ros melodic, dual_arm_worker melodic branch, dual_arm_planning_api etc. 

- Clone dual_arm_worker, melodic branch: https://redmine.laas.fr/projects/dual_arm_worker_private/repository  

Installation tutorial is available here: https://homepages.laas.fr/jcortes/DualArmManipTuto/

However you should skip the moveit_ompl_planning_interface step described in this tutorial and use the package in the step above. 

- Finally clone this package, the dual_arm_plannin_api.

You are encouraged however to build the workspace everytime you add a package. Built it in release mode.

h2. Software description 

h3. The dual_arm_worker project

The documentation can be found here and it is well detailed: 

https://homepages.laas.fr/jcortes/DualArmManipTuto/

https://hal.laas.fr/hal-02327826/document

The changes made to the code for pro-act (ie melodic branch) only concerns compatibility issues between the different ROS/Linux releases etc. Very few bugs corrected. So the documentation is up to date.

h3. The dual_pandas_moveit_config package

This package was made from scratch for the pro-act project. It was generated thanks to the moveit! setup assistant: http://docs.ros.org/en/melodic/api/moveit_tutorials/html/doc/setup_assistant/setup_assistant_tutorial.html

To make it compatible with the dual_arm_worker project it is necessary to configure it properly. You can find more information here: https://homepages.laas.fr/jcortes/DualArmManipTuto/

This is actually one of the most important package as you can set a lot of parameters in the generated configuration files (planners settings, kinematic solvers, etc). To make it compatible with the dual_arm_project, it was necessary to add a fake torso in the .urdf of the robot. The reason is that the dual_arm_worker project was originally intended for robots like the pr2, and the work was not finished to make it compatible with two independent arms. So the laziest solution was to add a fake torso in the dual_pandas.urdf.xacro file. 

Quick overview of the main files (most of them are auto generated by the moveit_setup_assistant):

<pre>

</pre>

- *ros_controllers.yaml* : This file doesn't spawn the controllers, instead it list the controllers that should be available. Basically CM2C is spawning the controllers, and for CM2P to know what are these controllers they are declared in this file. This file is to list the real controllers (those spawned by CM2C) when working with the real robots. Not so important in our case as we don't link directly the pick() and place() outputs to the controllers in the dual_arm context.

- *dual_pandas.srdf* : Interesting things here: you can disable collision checking between some robot links of your choice, and also declare some "poses". For example:

<pre>

    <group_state name="open_gripper_1" group="left_gripper">

        <joint name="panda_1_finger_joint1" value="0.038" />

        <joint name="panda_1_finger_joint2" value="0.038" />

    </group_state>

</pre>

You can then go to these poses easily via the rviz panel for example, or feed them as inputs of some moveit! functionalities like move().

- *ompl_planning.yaml* : This file regroup settings for your planners. 

<pre>

  # range = 5 seems close to optimal here for bitrrt.

  BiTRRTkConfigDualArm:

    plugin: ompl_interface/DualArmGeometricPlanningContext

    type: geometric::BiTRRTCustom

    range: 5

</pre>

For the dual_arm planners like bitrrt and trrt, i found the range parameter very impactful on the planing time.

- *dual_arm_manipulation.yaml* : In this file you will find the OJMSS_jump_factor parameter. This parameter was the most important one for the success of the cooperative manipulation plans. 

By lowering this parameter, you will encounter less joint flips during the planning (flips of shoulder/elbow/wrist). This is crucial when the two arms hold an object and form a closed kinematic chain.

This parameter should be as close to 1.0 as possible (1.0 <=OJMSS_jump_factor<=1.1).

- *kinematics.yaml* : This is where you declare what kinematic solver is used for each arm. There is different options available, i found that track_ik was the best as we could set the solve_type setting to distance which result in solutions of better quality. The problem however is that all the available solvers are numerical, and not efficient. I didn't manage to generate working ik_fast plugins (well known analytical solver) for both the arms. 

Actually the planning time for a dual_arm_place() is on average 200seconds. If we could use analytical solvers instead it will probably lead to planning time inferiors to 10 seconds. So this is one of the major upgrade that could be done to this work.