Debug in RigidBodyDriver