[bug] Incorrect external torque calculation in franka_gazebo

[rickstaa]

I have identified a potential issue in the external torque calculation performed within the franka_gazebo package. In the current code:

franka_ros/franka_gazebo/src/franka_hw_sim.cpp

Line 682 in d439fc7

double tau_ext = joint->effort - joint->command + joint->gravity;

there is a discrepancy in how control commands sent by the user are handled.

The problem arises from the fact that the control command provided by the user is utilized in the external torque calculation. However, when joint limits are reached, Gazebo does not apply this torque; instead, it clamps it. Consequently, the calculation must be modified to account for this clamping behaviour, setting the torque to zero when joint limits are reached.

To address this issue, I have submitted #228.

Steps to Reproduce the Issue

To observe the issue:

1. Clone the rickstaa/franka_ros/show_tau_ext_bug branch.
2. Install ROS system dependencies using the command: rosdep install --from-path src --ignore-src -r -y --skip-keys libfranka.
3. Build and source the Catkin workspace.
4. Launch the Panda simulation with the command roslaunch franka_gazebo panda.launch physics:=dart use_gripper:=false.
5. Plot the external torque using the command: rqt_plot /franka_state_controller/franka_states/tau_ext_hat_filtered.
6. Start the arm effort dynamic reconfigure server with: rosrun franka_gazebo joint_effort_dynamic_reconfigure_server.py.
7. Launch the RQT GUI tool: rosrun rqt_gui rqt_gui -s reconfigure.
8. Start the joint effort controllers by running: roslaunch franka_gazebo load_arm_effort_controller.launch.
9. Send an effort command to panda_joint1 higher than the collision threshold (e.g., 20) and observe that the internal controller effort is added to the external torque.

Note

You can also use the rosrun franka_gazebo print_collision_info.py command to view information about detected collisions.

To test the fix:

You can use the test_tau_ext_bug branch to test that after #228 is merged, this issue no longer occurs.

TODOs

• Merge the pull request at fix(franka_gazebo): fix tau_ext internal controller bug #228 to address this issue.