Using moveit_commander to determine reachability

edit

I'm currently using moveit_commander to generate positions and trajectories for a 7DOF Schunk arm given different sets of cartesian and quarternian.

I want to figure out the reachable region of the arm given a specified orientation of the end effector. Code is shown below, is there any way to intercept the output of group.plan() or some other bit to determine whether a position is within reach (i.e. before it crashes out if the position is out of range)? My only option at the moment is to launch and quit ROS over and over again, specifying a slightly different location each time.

I'm in the process of turning this function into a service, once completed I'd like to be able to call it with a range of points and fixed quarterian and fill up an array based on whether each point is 'reachable' i.e. whether moveit_commander could find a set of orientations for that position.

def move_group_schunk_lofi():


  moveit_commander.roscpp_initialize(sys.argv)
  rospy.init_node('schunk_lofi_moveit_config',
                  anonymous=True)
  robot = moveit_commander.RobotCommander()
  scene = moveit_commander.PlanningSceneInterface()
  group = moveit_commander.MoveGroupCommander("arm1")
  display_trajectory_publisher = rospy.Publisher(
                                      '/move_group/display_planned_path',
                                      moveit_msgs.msg.DisplayTrajectory,
                                      queue_size = 10)
  home = group.get_current_pose().pose
  pose_target = geometry_msgs.msg.Pose()

  pose_target.orientation.x =  0.27980739
  pose_target.orientation.y =  0.88048882
  pose_target.orientation.z =  0.11590166
  pose_target.orientation.w =  -0.36471093


  pose_target.position.x = -0.20
  pose_target.position.y = 0.40
  pose_target.position.z = 0.35

  group.set_pose_target(pose_target)
  plan1 = group.plan()

  poslist = plan1.joint_trajectory.points
  orientations = (poslist[-1].positions)
  print(poslist[-1].positions)

  moveit_commander.roscpp_shutdown()