Finish off RigidBody graph building

* Added type/safety checks for object-group building
* Added code for constraints building