Some asserts added to RigidBody
some test