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()
↧