Update Leg to fix function positionToAngle