Non-zero initial velocities in the ODE physics engine

[rickstaa]

I'm not sure if this is an issue with Gazebo, the ODE physics engine or a bug in my code, so please let me know if I'm posting this in the wrong place.

I want to use the gravity compensator included in the franka_gazebo package. This gravity compensator uses the KDL library to get the torques caused by gravity on the robot joints. These torques are then compensated by adding an opposite but equal torque to the control command (see https://github.com/frankaemika/franka_ros/blob/8efef6235b969c68ec1f8d5bc85026e4bca9cf19/franka_gazebo/src/franka_hw_sim.cpp#L325).

Unfortunately, there appears to be a significant drift in the joint positions when using this gravity compensator. I have been working with frankaemika to get rid of this drift (see frankaemika/franka_ros#160 (comment) for a quick overview). We found that this drift is noticeable in the ODE physics engine. The drift is significantly reduced when we switch to the DART physics engine. The difference in drift seems too big to be caused by small numerical differences. I, however, was not able to find the direct cause.

System information

OS: Ubuntu 20.04
ROS: ros-noetic-desktop-full/focal,now 1.5.0-1focal.20211124.035539 amd64
GAZEBO: 11.9.1

❗ Please test this with Gazebo 11. There seems to be a bug in Gazebo 9 that prevents the gravity compensation from working (see frankaemika/franka_ros#160 (comment)). ❗

How to reproduce

I created a small example repository with a small stick robot to show the behaviour in action.

1. Clone the example repository into a catkin workspace.
2. Install the ROS dependencies.
3. Build the workspace.
4. Run the stick example while using the ODE physics engine roslaunch gazebo_bug_reproduce stick.launch physics:=ode .
5. See the drift.
6. Run the stick example using the DART physics engine roslaunch gazebo_bug_reproduce stick.launch physics:=dart.
7. See no drift.

Investigation

Ode

When starting the stick simulation using the ODE physics engine, we get the following behaviour:

Show plots

The gravity compensation torques are:

Show plots

The joint velocities are:

Show plots

The starting velocities are:

Show plots

DART

When starting the stick simulation using the DART physics engine, we get the following behaviour:

Show plots

The gravity compensation torques are:

Show plots

The joint velocities are:

Show plots

The starting velocities are:

Show plots

When we simulate for a longer time the drift also occurs in the DART simulator:

Show plots

Increase joint damping

When increasing the joint damping from 0.0 to 0.003 the drift in the DART physics engine is insignificant. Where in the ODE physics engine, it is significantly reduced. The example branch that includes the joint damping can be found here.

ODE

Show plots

DART

Show plots

Current conclusion

It looks like the initial velocity primarily causes the drift. This initial velocity is significantly more prominent in the ODE physics engine than the DART engine. The DART engine also presents the drift but takes longer to manifest. This drift is reduced by adding joint damping. However, when doing this, the drift stays present in the ODE engine while it is removed in the DART engine. This is caused by the bigger initial velocity seen in the ODE engine.

Possibly related issues

• Joint velocities are high when the joint link is in contact and the joint is not rotating #622