regarsp_pose working. add collision checking.