changed check grasp reachability to accept grasp rather than psoe