Add tests TODO
Add PinJoint constructor
Fix constructor