fix gravity compensation in external torque computation

[JohannesPankert]

The joint commands the gazebo plugin receives from the controllers are gravity compensated but the measured efforts at the joints are not.
This commit fixes the computation of the external torques by adding the missing gravity terms.
With the fix, the external wrench published on the topic /panda/franka_state_controller/F_ext is reasonable.

[rickstaa]

@JohannesPankert Good catch! I verified your solution, and it indeed improves the /panda/franka_state_controller/F_ext. I created a pull request on your pull request to also make sure that the real gravity_vector is used in the gravity calculation (see JohannesPankert#1). You first need to rebase on the develop branch so that it includes 4fc4042 before this pull request can be applied. Apart from that, I think adding an entry to the CHANGELOG would also be required by the @frankaemika team for this pull request to be merged.

Before @JohannesPankerts fix:

After @JohannesPankert's fix:

[JohannesPankert]

I merged in the changes proposed by @rickstaa .
@frankaemika, are you planning to merge this? Without this fix, the external F/T estimation is not really useful in simulation.

[rickstaa]

@JohannesPankert I also double-checked to see if the gravity compensation component was not already included in joint->command pointer. But everything seems to work out since the joint->command is set by the controller while joint->effort is coming from Gazebo. I think your pull request makes a lot of sense.

[rickstaa]

I also noticed that tau_ext_hat is not clearly described in the frankastateinterface documentation.

Apart from that, it would be nice to have quick access to the joint torques applied on the joints by gravity (see #207). Merging #198 might be enough to achieve this since it allows people to infer these torques using the franka_state_controller/franka_states message. This, however, should be clear from the documentation.

[marcbone]

@frankaemika, are you planning to merge this? Without this fix, the external F/T estimation is not really useful in simulation.

Hi Johannes,

thank you for your contribution. We are currently quite occupied with other stuff, but I have this PR in my mind and I promise that we will not forget it ;)

[gollth]

Hey @JohannesPankert,

sorry that it took us so long to review and test this. A couple of notes regarding your solution:

1. Your implementation takes this->robot_state_.q from the previous time step to calculate the gravity term. This does essentially delay calculation of tau_ext_hat_filtered by one update.
2. I assume you did the static bool state_initialized = false flag to determine if Gazebo has spit out correct forces/torques. Making this field static, though, can be a problem, since the FrankaHWSim plugin is loaded into the Gazebo process. I would assume that all instances of the FrankaHWSim class will share this property. Converting it in to a member of the class will probably be the safer approach.
3. Our internal control filters tau_ext_hat, which is something we currently don't have in franka gazebo. A configurable filter (off by default) would also be a nice feature.

I already implemented a draft for this and pushed it onto your branch. If that is fine with you, we can merge this PR then. Thanks again for this contribution.

[JohannesPankert]

Hi @gollth
Thanks for the additions to the PR. I am fine with them and I would be happy to see the PR being merged.

[rickstaa]

@JohannesPankert Just a heads up, I found another bug in the calculation for the external torque see #228.

[Yitian-Xian]

Hi, @JohannesPankert , I downloaded the latest version and still found this problem. Could you please help me with this?

[Yitian-Xian]

@JohannesPankert Sorry, I was using Melodic version and experienced this issue. I just tried using Noetic version and then everything is fine. Thanks for your contribution!

[Betancourt20]

Hi,
I was testing the examples with the corresponding corrections and I do not understand something about the force sensor measurements (F_ext) in simulations. In fact, when running the impedance, force, joint velocity, joint position examples and not moving at all the manipulator, I get quite big different measurements. Is this normal?
Thanks in advance

[gollth]

Hi @Betancourt20,
note that our robot does not have a dedicated force sensor at the End-Effector (neither does its simulated counterpart). F_ext is a calculated quantity based on the torques measured on each joint mapped into task space. Refer FrankaStateInterface:

Regarding the noise: Can you post some more detail about the example you where testing, the magnitude of your noise and what you would expect?

[Betancourt20]

Hi @gollth,
Thank you for your quick answer. You are completely right. I wrote quite wrong my comment. So, yes it is the estimation of the external force based on the aforementioned.
I launched the following examples without doing anything else rather than analyzing the $\hat{F}_{ext}$:

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

1. cartesian_impedance_example_controller
   

2. model_example_controller
   

3. joint_position_example_controller
   

I'm sure I missing something here. Thanks for your time.

[rickstaa]

Hi @gollth, Thank you for your quick answer. You are completely right. I wrote quite wrong my comment. So, yes it is the estimation of the external force based on the aforementioned. I launched the following examples without doing anything else rather than analyzing the F^ext:

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

1. cartesian_impedance_example_controller
   
2. model_example_controller
   
3. joint_position_example_controller
   

I'm sure I missing something here. Thanks for your time.

@Betancourt20I've just opened issue #364, addressing a bug in the tau_ext calculation. While this might not directly relate to the issue you're experiencing, especially if you're not sending control commands directly and thus not reaching joint limits, I thought it was worth bringing to your attention.