reeds_shepp works with steve lavalle's code

do thorough testing