Panda simulation sometimes breaks when controlling gripper when the DART physics engine is used

[rickstaa]

Describe the bug

Although the gravity compensation is more stable when using the DART simulator, it leads to a less stable model when the gripper is controlled. This is likely caused by:

• Incorrect gripper PID gains (see [bug] Gazebo gripper control is broken frankaemika/franka_ros#172).
• A bug in the franka_gripper_sim gazebo plugin.
• An upstream bug in the Dart simulator.

Reproduction steps

1. Start the franka_gazebo simulation with the Dart physics engine roslaunch franka_gazebo panda.launch physics:=dart.
2. Send a gripper open command to the /franka_gripper/gripper_action/goal topic.
3. Send a gripper close command to the /franka_gripper/gripper_action/goal topic.
4. Repeat till the robot or gripper blows up.
5. Change to the ODE physics engine and see if the problem does not exist.

Expected behaviour

The robot and or gripper should not blow up.

Screenshots / Live demo link

System information

• OS: Ubuntu 20.04
• ROS version: noetic
• Panda gazebo version: v2.15.4

Additional context

Crash Log

[/gazebo] [ WARN] [WallTime: 1702479055.003819334, 56.693000000]: Zero Jacobian close to singularity
[/gazebo] [ WARN] [WallTime: 1702479055.003859996, 56.693000000]: Body Jacobian close to singularity
gzclient: /build/ogre-1.9-kiU5_5/ogre-1.9-1.9.0+dfsg1/OgreMain/include/OgreAxisAlignedBox.h:251: void Ogre::AxisAlignedBox::setExtents(const Ogre::Vector3&, const Ogre::Vector3&): Assertion `(min.x <= max.x && min.y <= max.y && min.z <= max.z) && "The minimum corner of the box must be less than or equal to maximum corner"' failed.
Error [BoxedLcpConstraintSolver.cpp:291] [BoxedLcpConstraintSolver] The solution of LCP includes NAN values:    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0 -nan -nan. We're setting it zero for safety. Consider using more robust solver such as PGS as a secondary solver. If this happens even with PGS solver, please report this as a bug.
[/gazebo] [ WARN] [WallTime: 1702479055.392792112, 56.695000000]: Command for panda_joint1is not finite, won't send to robot
[/gazebo] [ WARN] [WallTime: 1702479055.392819819, 56.695000000]: Command for panda_joint2is not finite, won't send to robot
[/gazebo] [ WARN] [WallTime: 1702479055.392839563, 56.695000000]: Command for panda_joint3is not finite, won't send to robot
[/gazebo] [ WARN] [WallTime: 1702479055.392854891, 56.695000000]: Command for panda_joint4is not finite, won't send to robot
[/gazebo] [ WARN] [WallTime: 1702479055.392870780, 56.695000000]: Command for panda_joint5is not finite, won't send to robot
[/gazebo] [ WARN] [WallTime: 1702479055.392884700, 56.695000000]: Command for panda_joint6is not finite, won't send to robot
[/gazebo] [ WARN] [WallTime: 1702479055.392898526, 56.695000000]: Command for panda_joint7is not finite, won't send to robot
[/gazebo] [ WARN] [WallTime: 1702479055.946697535, 56.695000000]: Command for panda_joint1is not finite, won't send to robot
[/gazebo] [ WARN] [WallTime: 1702479055.946727930, 56.695000000]: Command for panda_joint2is not finite, won't send to robot
[/gazebo] [ WARN] [WallTime: 1702479055.946736296, 56.695000000]: Command for panda_joint3is not finite, won't send to robot
[/gazebo] [ WARN] [WallTime: 1702479055.946743953, 56.696000000]: Command for panda_joint4is not finite, won't send to robot
[/gazebo] [ WARN] [WallTime: 1702479055.946761668, 56.696000000]: Command for panda_joint5is not finite, won't send to robot
[/gazebo] [ WARN] [WallTime: 1702479055.946770413, 56.696000000]: Command for panda_joint6is not finite, won't send to robot
[/gazebo] [ WARN] [WallTime: 1702479055.946779049, 56.696000000]: Command for panda_joint7is not finite, won't send to robot
Aborted (core dumped)
[gazebo_gui-3] process has died [pid 558186, exit code 134, cmd /opt/ros/noetic/lib/gazebo_ros/gzclient __name:=gazebo_gui __log:=/home/ricks/.ros/log/e109211c-99c6-11ee-a705-33d127d3cc02/gazebo_gui-3.log].
log file: /home/ricks/.ros/log/e109211c-99c6-11ee-a705-33d127d3cc02/gazebo_gui-3*.log
[/gazebo] [ WARN] [WallTime: 1702479056.514490499, 56.697000000]: Command for panda_joint1is not finite, won't send to robot
[/gazebo] [ WARN] [WallTime: 1702479056.514527948, 56.697000000]: Command for panda_joint2is not finite, won't send to robot
[/gazebo] [ WARN] [WallTime: 1702479056.514539736, 56.697000000]: Command for panda_joint3is not finite, won't send to robot
[/gazebo] [ WARN] [WallTime: 1702479056.514554096, 56.697000000]: Command for panda_joint4is not finite, won't send to robot
[/gazebo] [ WARN] [WallTime: 1702479056.514565378, 56.697000000]: Command for panda_joint5is not finite, won't send to robot
[/gazebo] [ WARN] [WallTime: 1702479056.514579582, 56.697000000]: Command for panda_joint6is not finite, won't send to robot
[/gazebo] [ WARN] [WallTime: 1702479056.514594592, 56.697000000]: Command for panda_joint7is not finite, won't send to robot
Segmentation fault (core dumped)
[gazebo-2] process has died [pid 558182, exit code 139, cmd /opt/ros/noetic/lib/gazebo_ros/gzserver -u -e dart worlds/empty.world __name:=gazebo __log:=/home/ricks/.ros/log/e109211c-99c6-11ee-a705-33d127d3cc02/gazebo-2.log].
log file: /home/ricks/.ros/log/e109211c-99c6-11ee-a705-33d127d3cc02/gazebo-2*.log

TODOs

• Tune the gripper PID gains The franka_gazebo gripperAction action Service implementation should be improved #33.