class UR_Moveit_API: def __init__(self, boundaries=False): self.scene = PlanningSceneInterface("/base_link") self.robot = RobotCommander() self.group_commander = MoveGroupCommander("manipulator") #self.gripper = MoveGroupCommander("gripper") self.group_commander.set_end_effector_link('ee_link') self.get_basic_infomation() if boundaries is True: self.add_bounds() self.joint_state_subscriber = rospy.Subscriber("/joint_states", JointState, self.joint_callback) self.joint_pubs = [ rospy.Publisher('/%s/command' % name, Float64, queue_size=1) for name in CONTROLLER_NAMES ] self.gripper_pub = rospy.Publisher('/gripper_prismatic_joint/command', Float64, queue_size=1) # create a DisplayTrajectory publisher which is used later to publish trajectories for RViz to visualize self.display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) self.reset_subscriber = rospy.Subscriber("/ur/reset", String, self.reset) rospy.sleep(2) def get_basic_infomation(self): # We can get the name of the reference frame for this robot: planning_frame = self.group_commander.get_planning_frame() print("============ Reference frame: %s" % planning_frame, "============") # We can also print the name of the end-effector link for this group: eef_link = self.group_commander.get_end_effector_link() print("============ End effector: %s" % eef_link, "============") # We can get a list of all the groups in the robot: group_names = self.robot.get_group_names() print("============ Robot Groups:", group_names, "============") # Sometimes for debugging it is useful to print the entire state of the # robot: print("============ Printing robot state") print(self.robot.get_current_state(), "============") print("================================================") def joint_callback(self, joint_state): self.joint_state = joint_state def plan_cartesian_path(self, scale=1): # Copy class variables to local variables to make the web tutorials more clear. # In practice, you should use the class variables directly unless you have a good # reason not to. group = self.group_commander waypoints = [] wpose = group.get_current_pose().pose wpose.position.z -= scale * 0.1 # First move up (z) wpose.position.y += scale * 0.2 # and sideways (y) waypoints.append(copy.deepcopy(wpose)) wpose.position.x += scale * 0.1 # Second move forward/backwards in (x) waypoints.append(copy.deepcopy(wpose)) wpose.position.y -= scale * 0.1 # Third move sideways (y) waypoints.append(copy.deepcopy(wpose)) (plan, fraction) = group.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold # Note: We are just planning, not asking move_group to actually move the robot yet: return plan, fraction def display_trajectory(self, plan): # Copy class variables to local variables to make the web tutorials more clear. # In practice, you should use the class variables directly unless you have a good # reason not to. robot = self.robot display_trajectory_publisher = self.display_trajectory_publisher display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = robot.get_current_state() display_trajectory.trajectory.append(plan) # Publish display_trajectory_publisher.publish(display_trajectory) ''' def open_gripper(self, drop=False): plan = self.gripper.plan(GRIPPER_DROP if drop else GRIPPER_OPEN) return self.gripper.execute(plan, wait=True) ''' def add_bounds(self): print("Complete to add_bounds") # size_x, size_y, size_z, x, y, z self.scene.addBox('table', 1.0, 1.0, 1.0, .0, .0, -0.5) # add a cube of 0.1m size, at [1, 0, 0.5] in the base_link frame #self.scene.addCube("my_cube", 0.1, 1, 0, 0.5) # Remove the cube #self.scene.removeCollisionObject("my_cube") def remove_bounds(self): for obj in self.scene.get_objects().keys(): self.scene.remove_world_object(obj) ''' def close_gripper(self): plan = self.gripper.plan(GRIPPER_CLOSED) return self.gripper.execute(plan, wait=True) ''' def get_ik_client(self, request): rospy.wait_for_service('/compute_ik') inverse_ik = rospy.ServiceProxy('/compute_ik', GetPositionIK) ret = inverse_ik(request) if ret.error_code.val != 1: return None return ret.solution.joint_state def get_fk_client(self, header, link_names, robot_state): rospy.wait_for_service('/compute_fk') fk = rospy.ServiceProxy('/compute_fk', GetPositionFK) ret = fk(header, link_names, robot_state) if ret.error_code.val != 1: return None return ret.pose_stamped def orient_to_target(self, x=None, y=None, angle=None): current = self.get_joint_values() if x or y: angle = np.arctan2(y, x) if angle >= 3.1: angle = 3.1 elif angle <= -3.1: angle = -3.1 current[0] = angle plan = self.group_commander.plan(current) return self.group_commander.execute(plan, wait=True) def wrist_rotate(self, angle): rotated_values = self.group_commander.get_current_joint_values() rotated_values[4] = angle - rotated_values[0] if rotated_values[4] > np.pi / 2: rotated_values[4] -= np.pi elif rotated_values[4] < -(np.pi / 2): rotated_values[4] += np.pi plan = self.group_commander.plan(rotated_values) return self.group_commander.execute(plan, wait=True) def get_joint_values(self): return self.group_commander.get_current_joint_values() def get_current_pose(self): return self.group_commander.get_current_pose() def move_to_neutral(self): print('Moving to neutral...') plan = self.group_commander.plan(NEUTRAL_VALUES) return self.group_commander.execute(plan, wait=True) def move_to_home(self): print('Moving to home...') plan = self.group_commander.plan(HOME) return self.group_commander.execute(plan, wait=True) def move_to_up(self): print('Moving to up...') plan = self.group_commander.plan(UP) return self.group_commander.execute(plan, wait=True) def reset(self, data): print("data: ", data) self.move_to_neutral() rospy.sleep(0.5) def orient_to_pregrasp(self, x, y): angle = np.arctan2(y, x) return self.move_to_drop(angle) def move_to_grasp(self, x, y, z, angle, compensate_control_noise=True): if compensate_control_noise: x = (x - CONTROL_NOISE_COEFFICIENT_BETA ) / CONTROL_NOISE_COEFFICIENT_ALPHA y = (y - CONTROL_NOISE_COEFFICIENT_BETA ) / CONTROL_NOISE_COEFFICIENT_ALPHA current_p = self.group_commander.get_current_pose().pose p1 = Pose(position=Point(x=x, y=y, z=z), orientation=DOWN_ORIENTATION) plan, f = self.group_commander.compute_cartesian_path([current_p, p1], 0.001, 0.0) joint_goal = list(plan.joint_trajectory.points[-1].positions) first_servo = joint_goal[0] joint_goal[4] = (angle - first_servo) % np.pi if joint_goal[4] > np.pi / 2: joint_goal[4] -= np.pi elif joint_goal[4] < -(np.pi / 2): joint_goal[4] += np.pi try: plan = self.group_commander.plan(joint_goal) except MoveItCommanderException as e: print('Exception while planning') traceback.print_exc(e) return False return self.group_commander.execute(plan, wait=True) def move_to_vertical(self, z, force_orientation=True, shift_factor=1.0): current_p = self.group_commander.get_current_pose().pose current_angle = self.get_joint_values()[4] orientation = current_p.orientation if force_orientation else None p1 = Pose(position=Point(x=current_p.position.x * shift_factor, y=current_p.position.y * shift_factor, z=z), orientation=orientation) waypoints = [current_p, p1] plan, f = self.group_commander.compute_cartesian_path( waypoints, 0.001, 0.0) if not force_orientation: return self.group_commander.execute(plan, wait=True) else: if len(plan.joint_trajectory.points) > 0: joint_goal = list(plan.joint_trajectory.points[-1].positions) else: return False joint_goal[4] = current_angle plan = self.group_commander.plan(joint_goal) return self.group_commander.execute(plan, wait=True) def move_to_target(self, target): assert len(target) >= 6, 'Invalid target command' for i, pos in enumerate(target): self.joint_pubs[i].publish(pos) def move_to_joint_position(self, joints): """ Adds the given joint values to the current joint values, moves to position """ joint_state = self.joint_state joint_dict = dict(zip(joint_state.name, joint_state.position)) for i in range(len(JOINT_NAMES)): joint_dict[JOINT_NAMES[i]] += joints[i] joint_state = JointState() joint_state.name = JOINT_NAMES joint_goal = [joint_dict[joint] for joint in JOINT_NAMES] joint_goal = np.clip(np.array(joint_goal), JOINT_MIN, JOINT_MAX) joint_state.position = joint_goal header = Header() robot_state = RobotState() robot_state.joint_state = joint_state link_names = ['ee_link'] position = self.get_fk_client(header, link_names, robot_state) target_p = position[0].pose.position x, y, z = target_p.x, target_p.y, target_p.z conditions = [ x <= BOUNDS_LEFTWALL, x >= BOUNDS_RIGHTWALL, y <= BOUNDS_BACKWALL, y >= BOUNDS_FRONTWALL, z <= BOUNDS_FLOOR, z >= 0.15 ] print("Target Position: %0.4f, %0.4f, %0.4f" % (x, y, z)) for condition in conditions: if not condition: return self.move_to_target(joint_goal) rospy.sleep(0.15) def scenario_plan(self): plan = self.group_commander.plan(PREDISCARD_VALUES) self.group_commander.execute(plan, wait=True) plan = self.group_commander.plan(DISCARD_VALUES) self.group_commander.execute(plan, wait=True) #self.open_gripper(drop=True) plan = self.group_commander.plan(PREDISCARD_VALUES) self.group_commander.execute(plan, wait=True) self.move_to_neutral() def execute_plan(self, plan): # Copy class variables to local variables to make the web tutorials more clear. # In practice, you should use the class variables directly unless you have a good # reason not to. group = self.group_commander ## Use execute if you would like the robot to follow ## the plan that has already been computed: group.execute(plan, wait=True)
class UR_Moveit_API: def __init__(self, boundaries=False): self.scene = PlanningSceneInterface("/base_link") self.robot = RobotCommander() self.group_commander = MoveGroupCommander("manipulator") self.gripper = MoveGroupCommander("gripper") self.group_commander.set_end_effector_link('eef_link') self.get_basic_infomation() if boundaries is True: self.add_bounds() self.joint_state_subscriber = rospy.Subscriber("/joint_states", JointState, self.joint_callback) self.joint_pubs = [ rospy.Publisher('/%s/command' % name, Float64, queue_size=1) for name in CONTROLLER_NAMES ] self.gripper_pub = rospy.Publisher('/gripper_prismatic_joint/command', Float64, queue_size=1) # create a DisplayTrajectory publisher which is used later to publish trajectories for RViz to visualize self.display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) self.reset_subscriber = rospy.Subscriber("/ur/reset", String, self.reset) rospy.sleep(2) def get_basic_infomation(self): # We can get the name of the reference frame for this robot: planning_frame = self.group_commander.get_planning_frame() print("============ Reference frame: %s" % planning_frame, "============") # We can also print the name of the end-effector link for this group: eef_link = self.group_commander.get_end_effector_link() print("============ End effector: %s" % eef_link, "============") # We can get a list of all the groups in the robot: group_names = self.robot.get_group_names() print("============ Robot Groups:", group_names, "============") # Sometimes for debugging it is useful to print the entire state of the # robot: print("============ Printing robot state") print(self.robot.get_current_state(), "============") print("================================================") def joint_callback(self, joint_state): self.joint_state = joint_state def open_gripper(self, drop=False): plan = self.gripper.plan(GRIPPER_DROP if drop else GRIPPER_OPEN) return self.gripper.execute(plan, wait=True) def add_bounds(self): print("Complete to add_bounds") # size_x, size_y, size_z, x, y, z self.scene.addBox('table', 1.0, 1.0, 1.0, .0, .0, -0.5) # add a cube of 0.1m size, at [1, 0, 0.5] in the base_link frame #self.scene.addCube("my_cube", 0.1, 1, 0, 0.5) # Remove the cube #self.scene.removeCollisionObject("my_cube") def remove_bounds(self): for obj in self.scene.get_objects().keys(): self.scene.remove_world_object(obj) def close_gripper(self): plan = self.gripper.plan(GRIPPER_CLOSED) return self.gripper.execute(plan, wait=True) def get_ik_client(self, request): rospy.wait_for_service('/compute_ik') inverse_ik = rospy.ServiceProxy('/compute_ik', GetPositionIK) ret = inverse_ik(request) if ret.error_code.val != 1: return None return ret.solution.joint_state def get_fk_client(self, header, link_names, robot_state): rospy.wait_for_service('/compute_fk') fk = rospy.ServiceProxy('/compute_fk', GetPositionFK) ret = fk(header, link_names, robot_state) if ret.error_code.val != 1: return None return ret.pose_stamped def joint_limits(self, header, link_names, robot_state): rospy.wait_for_service('/compute_fk') fk = rospy.ServiceProxy('/compute_fk', GetPositionFK) ret = fk(header, link_names, robot_state) if ret.error_code.val != 1: return None return ret.pose_stamped def eval_grasp(self, threshold=.0001, manual=False): if manual: user_input = None while user_input not in ('y', 'n', 'r'): user_input = raw_input( 'Successful grasp? [(y)es/(n)o/(r)edo]: ') if user_input == 'y': return 1, None elif user_input == 'n': return 0, None else: return -1, None else: current = np.array(self.gripper.get_current_joint_values()) target = np.array(GRIPPER_CLOSED) error = current[0] - target[0] return error > threshold, error def orient_to_target(self, x=None, y=None, angle=None): current = self.get_joint_values() if x or y: angle = np.arctan2(y, x) if angle >= 3.1: angle = 3.1 elif angle <= -3.1: angle = -3.1 current[0] = angle plan = self.group_commander.plan(current) return self.group_commander.execute(plan, wait=True) def wrist_rotate(self, angle): rotated_values = self.group_commander.get_current_joint_values() rotated_values[4] = angle - rotated_values[0] if rotated_values[4] > np.pi / 2: rotated_values[4] -= np.pi elif rotated_values[4] < -(np.pi / 2): rotated_values[4] += np.pi plan = self.group_commander.plan(rotated_values) return self.group_commander.execute(plan, wait=True) def get_joint_values(self): return self.group_commander.get_current_joint_values() def get_current_pose(self): return self.group_commander.get_current_pose() def move_to_neutral(self): print('Moving to neutral...') plan = self.group_commander.plan(NEUTRAL_VALUES) return self.group_commander.execute(plan, wait=True) def move_to_drop(self, angle=None): drop_positions = DROPPING_VALUES[:] if angle: drop_positions[0] = angle plan = self.group_commander.plan(drop_positions) return self.group_commander.execute(plan, wait=True) def move_to_empty(self): plan = self.group_commander.plan(EMPTY_VALUES) return self.group_commander.execute(plan, wait=True) def move_to_reset(self): print('Moving to reset...') plan = self.group_commander.plan(RESET_VALUES) return self.group_commander.execute(plan, wait=True) def reset(self, data): print("data: ", data) self.move_to_reset() rospy.sleep(0.5) def orient_to_pregrasp(self, x, y): angle = np.arctan2(y, x) return self.move_to_drop(angle) def move_to_grasp(self, x, y, z, angle, compensate_control_noise=True): if compensate_control_noise: x = (x - CONTROL_NOISE_COEFFICIENT_BETA ) / CONTROL_NOISE_COEFFICIENT_ALPHA y = (y - CONTROL_NOISE_COEFFICIENT_BETA ) / CONTROL_NOISE_COEFFICIENT_ALPHA current_p = self.group_commander.get_current_pose().pose p1 = Pose(position=Point(x=x, y=y, z=z), orientation=DOWN_ORIENTATION) plan, f = self.group_commander.compute_cartesian_path([current_p, p1], 0.001, 0.0) joint_goal = list(plan.joint_trajectory.points[-1].positions) first_servo = joint_goal[0] joint_goal[4] = (angle - first_servo) % np.pi if joint_goal[4] > np.pi / 2: joint_goal[4] -= np.pi elif joint_goal[4] < -(np.pi / 2): joint_goal[4] += np.pi try: plan = self.group_commander.plan(joint_goal) except MoveItCommanderException as e: print('Exception while planning') traceback.print_exc(e) return False return self.group_commander.execute(plan, wait=True) def move_to_vertical(self, z, force_orientation=True, shift_factor=1.0): current_p = self.group_commander.get_current_pose().pose current_angle = self.get_joint_values()[4] orientation = current_p.orientation if force_orientation else None p1 = Pose(position=Point(x=current_p.position.x * shift_factor, y=current_p.position.y * shift_factor, z=z), orientation=orientation) waypoints = [current_p, p1] plan, f = self.group_commander.compute_cartesian_path( waypoints, 0.001, 0.0) if not force_orientation: return self.group_commander.execute(plan, wait=True) else: if len(plan.joint_trajectory.points) > 0: joint_goal = list(plan.joint_trajectory.points[-1].positions) else: return False joint_goal[4] = current_angle plan = self.group_commander.plan(joint_goal) return self.group_commander.execute(plan, wait=True) def move_to_target(self, target): assert len(target) >= 6, 'Invalid target command' for i, pos in enumerate(target): self.joint_pubs[i].publish(pos) def move_to_joint_position(self, joints): """ Adds the given joint values to the current joint values, moves to position """ joint_state = self.joint_state joint_dict = dict(zip(joint_state.name, joint_state.position)) for i in range(len(JOINT_NAMES)): joint_dict[JOINT_NAMES[i]] += joints[i] joint_state = JointState() joint_state.name = JOINT_NAMES joint_goal = [joint_dict[joint] for joint in JOINT_NAMES] joint_goal = np.clip(np.array(joint_goal), JOINT_MIN, JOINT_MAX) joint_state.position = joint_goal header = Header() robot_state = RobotState() robot_state.joint_state = joint_state link_names = ['eef_link'] position = self.get_fk_client(header, link_names, robot_state) target_p = position[0].pose.position x, y, z = target_p.x, target_p.y, target_p.z conditions = [ x <= BOUNDS_LEFTWALL, x >= BOUNDS_RIGHTWALL, y <= BOUNDS_BACKWALL, y >= BOUNDS_FRONTWALL, z <= BOUNDS_FLOOR, z >= 0.15 ] print("Target Position: %0.4f, %0.4f, %0.4f" % (x, y, z)) for condition in conditions: if not condition: return self.move_to_target(joint_goal) rospy.sleep(0.15) def sweep_arena(self): self.remove_bounds() self.move_to_drop(.8) plan = self.group_commander.plan(TL_CORNER[0]) self.group_commander.execute(plan, wait=True) plan = self.group_commander.plan(TL_CORNER[1]) self.group_commander.execute(plan, wait=True) plan = self.group_commander.plan(L_SWEEP[0]) self.group_commander.execute(plan, wait=True) plan = self.group_commander.plan(L_SWEEP[1]) self.group_commander.execute(plan, wait=True) self.move_to_drop(-.8) plan = self.group_commander.plan(BL_CORNER[0]) self.group_commander.execute(plan, wait=True) plan = self.group_commander.plan(BL_CORNER[1]) self.group_commander.execute(plan, wait=True) self.move_to_drop(-2.45) plan = self.group_commander.plan(BR_CORNER[0]) self.group_commander.execute(plan, wait=True) plan = self.group_commander.plan(BR_CORNER[1]) self.group_commander.execute(plan, wait=True) self.move_to_drop(2.3) plan = self.group_commander.plan(TR_CORNER[0]) self.group_commander.execute(plan, wait=True) plan = self.group_commander.plan(TR_CORNER[1]) self.group_commander.execute(plan, wait=True) self.add_bounds() def discard_object(self): plan = self.group_commander.plan(PREDISCARD_VALUES) self.group_commander.execute(plan, wait=True) plan = self.group_commander.plan(DISCARD_VALUES) self.group_commander.execute(plan, wait=True) self.open_gripper(drop=True) plan = self.group_commander.plan(PREDISCARD_VALUES) self.group_commander.execute(plan, wait=True) self.move_to_neutral()