How to fix pr2_kinect_empty_world broken when collision?

edit

What you are seeing is probably the physics solver destabilizing, leading to NaN's in link velocities and eventually segmentation fault of the simulator. For objects with deep interpenetration, there is no known cure in ODE/Gazebo right now.

There is, however, something I am experimenting with that may potentially reduce or eliminate these types of numerical "explosion" by using a type of pose projection correction for interpenetrating bodies. You can see the improved interpenetration correction behavior in the following videos for simple shapes in collision and PR2 in collision. Unfortunately, there is no free lunch in this case, in complex dynamical scenarios for PR2, completely removing the original ODE interpenetration correction introduces limit cycles and errors in overall dynamic behavior of the robot: i.e. see PR2 drift backwards in first 3 seconds of this video. Any comments or suggestions are welcome here.