Clean up MultiDofJoint - move function definition out of class body