dual_arm_planning_api

h1. Cooperation Manipulation Control

h2. Franka_ros

To operate the pandas it is necessary to have real-time kernels on the machines running the control stacks. It is the case with the *kukarm* and *truyere* workstation.

You should have ROS (melodic version ideally) installed. You will need to also to install *franka_ros* and *libfranka* .

<pre>

sudo apt-get install ros-melodic-franka-ros

sudo apt-get install ros-melodic-libfranka

</pre>

You are strongly advised to read the franka documentation to begin with. See https://redmine.laas.fr/projects/dual_arm_exp/wiki/Franka_documentation_and_operating_the_Pandas and https://frankaemika.github.io/docs/franka_ros.html.

!ros-architecture.png!

Basically franka_ros is a collection of package integrating libfranka into ros_control. There is packages for hardware abstraction, to control the grippers, for robots description etc.

*Important* : libfranka must have the same versions running on the workstations (kukarm, truyere etc) and on the panda controllers (FCI). Otherwise there will be compatibility issues.

h2. CM2C

h3. Installation

Once franka_ros is installed you can create a catkin workspace in your home directory to install CM2C. See http://wiki.ros.org/catkin/Tutorials/create_a_workspace.

CM2C is available here on the mp2_ros repository:

https://redmine.laas.fr/projects/m2p2/repository

Then you only have to compile your catkin workspace and CM2C can be launched. 

You might need to install different ROS packages missing on the system. 

*Important* : your catkin workspace must be built in release mode! By default it might be in debug mode, and CM2C might not be able to run at the desired frequency for real time usage.

<pre>

catkin build -DCMAKE_BUILD_TYPE=Release

</pre>

Information on how to use CM2C can be found a bit further in the wiki: https://redmine.laas.fr/projects/dual_arm_exp/wiki/Control_API_functions_description 

h3. The code

All the important code is located in a single file: admittance_joint_trajectory_controller_impl.h. This is where you will find the admittance control law implementation etc.

h3. CM2P Inputs

CM2C spawns follow_joint_trajectory action servers with the following names:

<pre>

/panda_1_controller/follow_joint_trajectory

/panda_2_controller/follow_joint_trajectory

</pre>

CM2P send the trajectories to these servers via the dual_arm_controller_api class.

h3. Admittance parameters Inputs

Those are initialized in admittance_joint_trajectory_controller_impl.h: 

<pre>

// initialize admittance controller parameters

admittance_translational_mass_ = 10.0;

admittance_translational_mass_target_ = 10.0;

admittance_translational_damping_ = 700.

admittance_translational_damping_target_ = 700.0;

admittance_translational_spring_ = 500.0;

admittance_translational_spring_target_ = 500.0;

admittance_rotational_mass_ = 50.0;

admittance_rotational_mass_target_ = 50.0;

admittance_rotational_damping_ = 50.0;

admittance_rotational_damping_target_ = 70.0;

admittance_rotational_spring_ = 50.0;

admittance_rotational_spring_target_ = 70.0;

</pre>

as well as in a configuration file, admittance_pose_param.cfg in the cfg repository.

Originally it was possible to set them dynamically via a rqt_reconfigure GUI, however it was very bugged.

You can set them dynamically by using the Admittance_pose_param/set_parameters service. However it will update it to both arms, so you might need to create a dedicated service for each arm.

h3. From admittance_joint_trajectory_controller to simple joint_trajectory_controller

You might want to not use the admittance gains, and only run a simple joint_trajectory_controller. This can be done by commenting the admittance gains in those 3 lines and compiling after:

<pre>

desired_state_.position[i] = desired_state_.position[i];// - q_dc_[i];

desired_state_.velocity[i] = desired_state_.velocity[i];// - dq_dc_[i];

desired_state_.acceleration[i] = desired_state_.acceleration[i];// - ddq_dc_[i];

</pre>

A reason for that is that we had issues running a place() with the admittance gains. Without it works fine.

h3. External forces estimation

The admittance control law uses the external forces measurements on the end effector to compute the admittance gains for each joint.

h4. The fusedSensorData structure

!fused_sensor_data.png!

I created this structure to hold all the data from the F/T sensors. This structure is declared in admittance_joint_trajectory_controller.h.

The function fuse() is called every cycle in the update() function to add the last data of the active sensor into the fifo and compute the average on a sliding window to remove noise. 

<pre>

  // AVERAGE COMPUTED ON THE FIFO ABOVE, HENCE WITH NOISE SUPPRESSED

  Eigen::Vector3d O_mean_fused_forces_k;  // FORCES ON EEF EXPRESSED IN BASE FRAME

  Eigen::Vector3d O_mean_fused_torques_k; // TORQUES ON EEF EXPRESSED IN BASE FRAME

  Eigen::Vector3d k_mean_fused_forces_k; // FORCES ON EEF EXPRESSED IN EEF FRAME

  Eigen::Vector3d k_mean_fused_torques_k; // TORQUES ON EEF EXPRESSED IN EEF FRAME

</pre>

These average are then used in the control law to compute the admittance gains.

h4. Force/Torque sensors integration

For now we use the pandas Force/Torque sensors (FTS) to estimate the forces and torques at the end effector (EEF). The pandas have FTS at the joints, so the forces at the EEF are estimates. 

If the results are okay, it would be better to dispose of a real FTS placed at the EEF. Those FTS are available at LAAS, we did most of the integration but didn't finish it. 

You will find a wiki page with all the information regarding these FTS here: https://wiki.laas.fr/robots/Panda

From an hardware point of view this is done. The wires have been extended and 3d printed support have been printed to integrate it. You can find the .stl models of these supports by following the link above. 

From a software point of view we have a C++ library developed by Matthieu, the icub-libs. See https://git.openrobots.org/projects/icub-libs.

I developed a ROS wrapper of this library that you can find here: https://redmine.laas.fr/projects/icub_fts_wrapper.

You can put the wrapper in your catkin workspace and build it along with CM2C. Obviously you will have to install the library beforehand.

You can launch the wrapper like so in a terminal:

<pre>

rosrun icub_fts_wrapper icub_fts_wrapper

</pre>

The wrapper will publish on the topic "/icub_fts_wrapper/fts_data" the force/torque sensor data. *As you can see the problem here is that it was made for a single arm and need to be extended to dual arm* by having different topic names etc. A way to do this is to use a launch file with the name of the arm as an argument, and use it to initialize the topic. 

The work on the wrapper is mostly done but there is a bug regarding the resolution on the Y axis to correct as well as the projection of the forces from the FTS frame to the EEF frame. The translation part is trivial but the orientation part might require more efforts.

The wrapper is almost integrated into CM2C. The data structure fusedSensorData implemented in admittance_joint_trajectory_controller.h dispose of a active_sensor variable. By default the active sensor is "Franka", hence we use the pandas sensors. But you can set it after launching CM2C via the service /admittance_joint_trajectory_controller/setFTS. As you can see here again you will need to change the services names as for now we have 1 service for both arms. So instead you will need something like this:

/panda_1_controller/setFTS

/panda_2_controller/setFTS

You will also need in the function update() to read FTS data on the fts_wrapper topics, so for example:

<pre>

"/icub_fts_wrapper_panda_1/fts_data"

"/icub_fts_wrapper_panda_2/fts_data"

</pre>

and to update the fusedSensorData with it. In the code you can see only the data coming from franka sensors are updated. 

!fts_cm2c.png!

So it is needed here to read the data from the icub_wrapper and to add them to the fusedSensorData structure, fts_Data. Those data must be calibrated into the EEF frame. You can do that either in the wrapper or directly here in the update function but it must be done somewhere.

*Troubleshooting* : If you have some errors when trying to run the icub_fts_wrapper node you might need to restart the computer and it should work again.