Add PinJoint tests
Add TODOs