add some test for rigid body systems

fix a few minor issues