planar sqp works
fixed beam truncation bug with is_inside
fixed truncate bug
preliminary icra 2014 planar results