[bug] Computation of F_ext seems to be wrong in simulation.

[JohannesPankert]

When starting the simulation with the command provided in the tutorial, a large external wrench is reported when the robot is not in contact.

Launch command:

roslaunch franka_gazebo panda.launch x:=-0.5 \
    world:=$(rospack find franka_gazebo)/world/stone.sdf \
    controller:=cartesian_impedance_example_controller \
    rviz:=true

Measured wrench:

rostopic echo /panda/franka_state_controller/F_ext
...
header: 
  seq: 207
  stamp: 
    secs: 7
    nsecs:  78000000
  frame_id: "panda_K"
wrench: 
  force: 
    x: 15.404927762579728
    y: 0.12111203520518202
    z: 34.97186307726185
  torque: 
    x: -0.7130799816326103
    y: -5.507311471991857
    z: 0.03923425854899071

The measured wrench changes when the robot is in contact with the environment. The cartesian_impedance_example_controller works fine and has no visible tracking error. After calling the set_load service and attaching a fake mass to the robot, the controller has a tracking error, and F_ext changes as well

[gollth]

Hi @JohannesPankert,

thanks for the report. The whole issue might be related to Gazebo Joints Move by Themselves. The potentially bugged implementation of Gazebo's GetForceTorque() function will also mess up the calculation of F_ext.

It seems that the cartesian_impedance_example_controller is "strong" enough to cope with this error.

We keep investigating the issue, however, because of the intricacy of the bug, there is no estimate how long that will take. Sorry

[JohannesPankert]

Hi @gollth, I investigated the problem a bit more and I found the issue. There is a problem in the calculation of the external torques in the gazebo plugin. The joint commands coming from the controller are gravity compensated but the measured torques in gazebo are not. I propose the following fix that solves the issue for me: #198

[gollth]

Fixed in #198