Constraining the joint in the urdf instead for faster results