def main(): rospy.init_node('erase_whiteboard_demo') wait_for_time() argv = rospy.myargv() ERASE_POSES = [[-0.239, -0.433, 1.401, 0.758, -2.997, 0.565, 1.742], [-0.530, -0.423, 1.438, 0.714, -2.983, 0.266, 1.689], [-0.251, -0.400, 1.469, 0.755, -2.993, 0.576, 1.667], [-0.556, -0.395, 1.437, 0.726, -2.982, 0.181, 1.690]] torso = fetch_api.Torso() torso.set_height(0.4) display = fetch_api.Display() display.display_msg("Hello! I am going to erase the whiteboard for you!") time.sleep(7) gripper = fetch_api.Gripper() gripper.close() time.sleep(2) arm = fetch_api.Arm() torso.set_height(0.0) for vals in ERASE_POSES: arm.move_to_joints(fetch_api.ArmJoints.from_list(vals)) time.sleep(5) display.display_msg("All done!") time.sleep(2)
def execute(self): arm = fetch_api.Arm() gripper = fetch_api.Gripper() for action in self.actions: if action.action == ActionSaver.MOVE: # Compute the pose of the wrist in base_link ar = fetch_api.pose2matrix(action.arPose) ar2wrist = fetch_api.pose2transform(action.arPose, action.wristPose, True) # Save the ar marker's id and load it wrist = np.dot(fetch_api.pose2matrix(filter(lambda x: x.id == action.arId, self.markers)[0].pose.pose), ar2wrist) # Navigate the arm there kwargs = { 'allowed_planning_time': 50, 'execution_timeout': 40, 'num_planning_attempts': 30, 'replan': False, } pose_stamped = PoseStamped() pose_stamped.header.frame_id = "base_link" pose_stamped.pose = fetch_api.matrix2pose(wrist) arm.move_to_pose(pose_stamped, **kwargs) # Wait? elif action.action == ActionSaver.OPEN: gripper.open() elif action.action == ActionSaver.CLOSE: gripper.close()
def __init__(self, program_file=PROGRAM_FILE): # TODO: Either implement behavior that fixes programs when markers change # or only let this callback run once self._markers_sub = rospy.Subscriber(SUB_NAME, Marker, callback=self._markers_callback) self._curr_markers = None self._tf_listener = tf.TransformListener() self._arm = fetch_api.Arm() self._gripper = fetch_api.Gripper() self._torso = fetch_api.Torso() self._controller_client = actionlib.SimpleActionClient( '/query_controller_states', QueryControllerStatesAction) self._program_file = program_file self._programs = self._read_in_programs() self._joint_reader = JointStateReader() mat = tft.identity_matrix() mat[:, 0] = np.array([0, 0, -1, 0]) mat[:, 2] = np.array([1, 0, 0, 0]) o = tft.quaternion_from_matrix(mat) self._constraint_pose = Pose(orientation=Quaternion(*o)) oc = OrientationConstraint() oc.header.frame_id = 'base_link' oc.link_name = 'gripper_link' oc.orientation = self._constraint_pose.orientation oc.weight = 1.0 oc.absolute_z_axis_tolerance = 1.0 oc.absolute_x_axis_tolerance = 1.0 oc.absolute_y_axis_tolerance = 1.0 self._constraint = None
def __init__(self): self._drink_orders = Queue() self._is_working = False # Initialize Navigation Server. self._nav_server = NavigationServer() self._nav_server.loadMarkers() # Initialize Arm and Gripper self._gripper = fetch_api.Gripper() self._arm = fetch_api.Arm() rospy.logdebug('Gripper and Arm instantiated.'); self._arm_server = ArmServer() # Startup Fetch ARM self._controller_client = actionlib.SimpleActionClient('/query_controller_states', QueryControllerStatesAction) rospy.sleep(0.5) goal = QueryControllerStatesGoal() state = ControllerState() state.name = 'arm_controller/follow_joint_trajectory' state.state = ControllerState.RUNNING goal.updates.append(state) self._controller_client.send_goal(goal) # Wait for Fetch ARM rospy.loginfo('Waiting for arm to start.') self._controller_client.wait_for_result() rospy.loginfo('Arm has started..') # Instantiate subscription model with front-end self._drink_order_subscriber = rospy.Subscriber('/drink_order', DrinkOrder, self.drink_order_handler) self._drink_status_publisher = rospy.Publisher('/drink_status', DrinkStatus, queue_size=1)
def handle_put_box(req): """ Robot arrived at station, drop the box. """ global put_poses # Remove the planning floor, since we don't want it to get in the way remove_floor() torso = fetch_api.Torso() arm = fetch_api.Arm() gripper = fetch_api.Gripper() base = fetch_api.Base() # Drop the box print("Dropping the box") move_pose(arm, DROP) rospy.sleep(1.0) gripper.open() rospy.sleep(0.5) # Move to carry pose move_pose(arm, CARRY) # Move back print("Dropped box, backing up") before_pos = robot_pose.position after_pos = robot_pose.position while distance(before_pos, after_pos) < 0.50: after_pos = robot_pose.position base.move(-0.1, 0.0) return 0
def main(): rospy.init_node('interactive_gripper_demo') im_server = InteractiveMarkerServer('gripper_im_server', q_size=10) arm = fetch_api.Arm() gripper = fetch_api.Gripper() teleop = GripperTeleop(arm, gripper, im_server) #auto_pick = AutoPickTeleop(arm, gripper, im_server) rospy.spin()
def main(): global pose_marker_server global nav_server global target_pose_pub global pose_names_pub global current_pose current_pose = geometry_msgs.msg.Pose( orientation=geometry_msgs.msg.Quaternion(0, 0, 0, 1)) rospy.init_node('action_node') wait_for_time() pose_marker_server = InteractiveMarkerServer('simple_marker') pose_names_pub = rospy.Publisher('/pose_names', PoseNames, queue_size=10, latch=True) nav_server = NavigationServer() # Create nav_server.loadMarkers() arm_server = ArmServer() message = PoseNames() message.names = nav_server.pose_names_list pose_names_pub.publish(message) reader = ArTagReader() sub = rospy.Subscriber('/ar_pose_marker', AlvarMarkers, reader.callback) controller_client = actionlib.SimpleActionClient( '/query_controller_states', QueryControllerStatesAction) rospy.sleep(0.5) goal = QueryControllerStatesGoal() state = ControllerState() state.name = 'arm_controller/follow_joint_trajectory' state.state = ControllerState.RUNNING goal.updates.append(state) controller_client.send_goal(goal) print 'Waiting for arm to start.' controller_client.wait_for_result() print 'Arm has been started.' gripper = fetch_api.Gripper() arm = fetch_api.Arm() print 'Arm and gripper instantiated.' # handle user actions user_actions_sub = rospy.Subscriber('/user_actions', UserAction, handle_user_actions) arm_service = rospy.Service('barbot/arm_to_pose', ArmToPose, arm_server.findGlass) rospy.spin()
def execute(self): arm = fetch_api.Arm() gripper = fetch_api.Gripper() for action in self.actions: if action.action == ActionSaver.MOVE: # Compute the pose of the wrist in base_link ar = fetch_api.pose2matrix(action.arPose) ar2wrist = fetch_api.pose2transform(action.arPose, action.wristPose, True) # Save the ar marker's id and load it wrist = np.dot( fetch_api.pose2matrix( filter(lambda x: x.id == action.arId, self.markers)[0].pose.pose), ar2wrist) # Navigate the arm there kwargs = { 'allowed_planning_time': 50, 'execution_timeout': 40, 'num_planning_attempts': 30, 'replan': False, } pose_stamped = PoseStamped() pose_stamped.header.frame_id = "base_link" pose_stamped.pose = fetch_api.matrix2pose(wrist) marker = Marker() marker.header.frame_id = "base_link" marker.header.stamp = rospy.Time() marker.id = 0 marker.type = Marker.ARROW marker.action = Marker.ADD marker.pose.position.x = pose_stamped.pose.position.x marker.pose.position.y = pose_stamped.pose.position.y marker.pose.position.z = pose_stamped.pose.position.z marker.pose.orientation.x = pose_stamped.pose.orientation.x marker.pose.orientation.y = pose_stamped.pose.orientation.y marker.pose.orientation.z = pose_stamped.pose.orientation.z marker.pose.orientation.w = pose_stamped.pose.orientation.w marker.scale.x = 0.2 marker.scale.y = 0.05 marker.scale.z = 0.05 marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 0.0 self.viz_pub.publish(marker) arm.move_to_pose(pose_stamped, **kwargs) # Wait? elif action.action == ActionSaver.OPEN: gripper.open() elif action.action == ActionSaver.CLOSE: gripper.close()
def __init__(self): self.actions = [] self.reader = ArTagReader() rospy.sleep(0.1) sub = rospy.Subscriber('/ar_pose_marker', AlvarMarkers, self.reader.callback) rospy.sleep(0.1) self.tf_listener = tf.TransformListener() self._controller_client = actionlib.SimpleActionClient( '/query_controller_states', QueryControllerStatesAction) self.gripper = fetch_api.Gripper()
def main(): rospy.init_node('gripper_im_server_node') arm = fetch_api.Arm() gripper = fetch_api.Gripper() im_server = InteractiveMarkerServer('gripper_im_server', q_size=2) auto_pick_server = InteractiveMarkerServer('auto_pick_im_server', q_size=2) teleop = GripperTeleop(arm, gripper, im_server) auto_pick = AutoPickTeleop(arm, gripper, auto_pick_server) teleop.start() auto_pick.start() rospy.spin()
def main(): global nav_server global WORKING global arm_server global orders global sound_handler sound_handler = SoundClient() orders = [] WORKING = None rospy.init_node('barbot_controller_node') wait_for_time() nav_server = NavigationServer() nav_server.loadMarkers() gripper = fetch_api.Gripper() arm = fetch_api.Arm() arm_server = ArmServer() print 'Arm server instantiated.' # MOVE TO MICHAEL_NODE controller_client = actionlib.SimpleActionClient( '/query_controller_states', QueryControllerStatesAction) rospy.sleep(0.5) goal = QueryControllerStatesGoal() state = ControllerState() state.name = 'arm_controller/follow_joint_trajectory' state.state = ControllerState.RUNNING goal.updates.append(state) controller_client.send_goal(goal) print 'Waiting for arm to start.' controller_client.wait_for_result() arm_server.lookup() arm_server.set_init_poses() arm_server.set_prepose() # nav_server.goToMarker(BAR_TABLE) print 'going home' nav_server.goToMarker(HOME) print 'please start to order now' sound_handler.say('You may now order.') # # handle user actions drink_order_sub = rospy.Subscriber('/drink_order', DrinkOrder, handle_user_actions) signal.signal(signal.SIGINT, signal_handler) while True: query_orders() rospy.sleep(0.1)
def __init__(self): # TODO: Either implement behavior that fixes programs when markers change # or only let this callback run once self._markers_sub = rospy.Subscriber(SUB_NAME, AlvarMarkers, callback=self._markers_callback) self._programs = self._read_in_programs() self._curr_markers = None self._tf_listener = tf.TransformListener() self._arm = fetch_api.Arm() self._gripper = fetch_api.Gripper() self._controller_client = actionlib.SimpleActionClient( '/query_controller_states', QueryControllerStatesAction)
def main(): rospy.init_node('gripper_teleop') wait_for_time() arm = fetch_api.Arm() gripper = fetch_api.Gripper() im_server = InteractiveMarkerServer('gripper_im_server', q_size=10) teleop = GripperTeleop(arm, gripper, im_server) teleop.start() pp_im_server = InteractiveMarkerServer('pick_place_im_server', q_size=10) pick_place = PickPlaceTeleop(arm, gripper, pp_im_server) pick_place.start() rospy.spin()
def __init__(self): self._grip = fetch_api.Gripper() self._base = fetch_api.Base() self._arm = fetch_api.Arm() self._torso = fetch_api.Torso() self._torso.set_height(0.4) self._head = fetch_api.Head() self._head.pan_tilt(0, 0.0) self.actions = None try: self.actions = pickle.load(open(PICKLE_FILE, "rb")) print '{} loaded.'.format(PICKLE_FILE) except: print '{} could not be loaded.'.format(PICKLE_FILE) self._reader = ArTagReader() self._sub = rospy.Subscriber('/ar_pose_marker', AlvarMarkers, self._reader.callback)
def __init__(self): # set arm and gripper, and fiducial id self.arm = fetch_api.Arm() self.gripper = fetch_api.Gripper() # Will need to change this to something real at some point #self.target_id = None# target_id self.planning_scene = PlanningSceneInterface('base_link') # First, load the poses for the fiducial insert movement self.sequence = pickle.load(open(INSERT_GRASP_POSES, "rb")) self.gripper_open = True # # Init the reader for the tags self.reader = ArTagReader() self.sub = rospy.Subscriber('/ar_pose_marker', AlvarMarkers, self.reader.callback)
def main(): # Initialize the interactive marker server for the gripper rospy.init_node('gripper_demo') im_server = InteractiveMarkerServer('gripper_im_server', q_size=2) auto_pick_im_server = InteractiveMarkerServer('auto_pick_im_server', q_size=2) down_im_server = InteractiveMarkerServer('down_gripper_im_server', q_size=2) arm = fetch_api.Arm() gripper = fetch_api.Gripper() teleop = GripperTeleop(arm, gripper, im_server) auto_pick = AutoPickTeleop(arm, gripper, auto_pick_im_server) down_teleop = GripperTeleopDown(arm, gripper, down_im_server) teleop.start() auto_pick.start() down_teleop.start() rospy.spin()
def main(): rospy.init_node('gripper_demo') wait_for_time() argv = rospy.myargv() if len(argv) < 2: print_usage() return command = argv[1] gripper = fetch_api.Gripper() effort = gripper.MAX_EFFORT if command == 'close' and len(argv) > 2: effort = float(argv[2]) if command == 'open': gripper.open() elif command == 'close': gripper.close(effort) else: print_usage()
if __name__ == '__main__': rospy.init_node('motion_demo') base = fetch_api.Base() base.go_forward(1.5) print('Move Base Forward 1.5 meters') head = fetch_api.Head() head.pan_tilt(0, math.pi / 4) head.pan_tilt(0, -math.pi / 2) print('Head looks down 45 degrees') print('Head looks up 90 degrees') head.pan_tilt(0, 0) head.pan_tilt(math.pi / 2, 0) head.pan_tilt(0, 0) print('Head looks left 90 degrees') head.pan_tilt(-math.pi / 2, 0) print('Head looks right 90 degrees') head.pan_tilt(0, 0) print('Head looks forward') torso = fetch_api.Torso() torso.set_height(0.4) print('Raise torso to the maximum') arm = fetch_api.Arm() arm.move_to_joints(fetch_api.ArmJoints.from_list([0, 0, 0, 0, 0, 0, 0])) print('Move arm to joint values [0,0,0,0,0,0,0]') gripper = fetch_api.Gripper() gripper.close() print('Close the gripper') gripper.open() print('Open the gripper')
def __init__(self): self._torso = fetch_api.Torso() self._head = fetch_api.Head() self._arm = fetch_api.Arm() self._gripper = fetch_api.Gripper()
def main(): pose_actions = None # Get the actions from the pickel file. argv = rospy.myargv() if len(argv) < 2: usage() return fileName = argv[1] try: pose_actions = pickle.load(open(fileName, "rb")) print '{} loaded.'.format(fileName) except: print '{} could not be loaded.'.format(fileName) usage() return rospy.init_node("load_file_actions") wait_for_time() print 'Time has begun.' # Start the arm. reader = ArTagReader() sub = rospy.Subscriber('/ar_pose_marker', AlvarMarkers, reader.callback) controller_client = actionlib.SimpleActionClient('/query_controller_states', QueryControllerStatesAction) rospy.sleep(1.0) goal = QueryControllerStatesGoal() state = ControllerState() state.name = 'arm_controller/follow_joint_trajectory' state.state = ControllerState.RUNNING goal.updates.append(state) controller_client.send_goal(goal) print 'Waiting for arm to start.' controller_client.wait_for_result() print 'Arm has been started.' gripper = fetch_api.Gripper() arm = fetch_api.Arm() print 'Arm and gripper instantiated.' rospy.sleep(1.0) count = 0 # Run through each of the actions for pose_action in pose_actions: print 'Performing action.' if pose_action.actionType == PoseExecutable.OPEN: print 'Opening the gripper' gripper.open() elif pose_action.actionType == PoseExecutable.CLOSE: print 'Closing the gripper' gripper.close() elif pose_action.actionType == PoseExecutable.MOVETO: count += 1 print 'Moving to location.' pose_stamped = PoseStamped() pose_stamped.header.frame_id = "base_link" if pose_action.relativeFrame == 'base_link': pose_stamped.pose = pose_action.pose else: for marker in reader.markers: if pose_action.relativeFrame == marker.id: wrist2 = makeMatrix(pose_action.pose) tag = makeMatrix(pose_action.arPose) tag2 = tf.transformations.inverse_matrix(tag) result = np.dot(tag2, wrist2) result2 = np.dot(makeMatrix(marker.pose.pose), result) pose_stamped = PoseStamped() pose_stamped.header.frame_id = "base_link" pose_stamped.pose = transform_to_pose(result2) error = arm.move_to_pose(pose_stamped, allowed_planning_time=40, num_planning_attempts=20) if fileName == PICKLE_FILE_DISPENSE and count == 2: print 'Dispensing!' rospy.sleep(DISPENSE_TIME) if error is not None: print 'Error moving to {}.'.format(pose_action.pose) else: print 'invalid command {}'.format(pose_action.action)
def __init__(self): print("create ActionRunner") # rospy.init_node('action_runner') wait_for_time() # get gripper self.gripper = fetch_api.Gripper() print("done with gripper") self.arm = fetch_api.Arm() print("done with arm") self.base = fetch_api.Base() print("done with base") self.reader = self.ArTagReader() self.markers = {} # get initial position of markers... it will continue updating in background reachable = False rospy.sleep(0.5) while len(self.reader.markers) == 0: # TODO[LOW PRIORITY]: implement looking for the AR tag by tilting its head up and down print("waiting for marker") self.base.turn(0.6) print("im about to sleep") rospy.sleep(1.5) print("i had a nap") print("found my marker!") markers = self.reader.markers print("Original marker pose is " + str(markers[0])) debug_marker_pose = PoseStamped(pose=markers[0].pose.pose) debug_marker_pose.header.stamp = rospy.Time.now() debug_marker_pose.header.frame_id = "/base_link" draw_debug_marker(debug_marker_pose, [0, 1, 0, 0.5]) original_pose_stamped = dot_poses( lookup_transform("/odom", "/base_link"), markers[0].pose.pose) print("Adjusting orientation") number_of_turns = 0 while not reachable: if number_of_turns > 5: print("Turned enough, I guess...") break while len(self.reader.markers) == 0: pass # m = markers[0] m = deepcopy(self.reader.markers[0]) pose_stamped = PoseStamped(pose=m.pose.pose) pose_stamped.header.frame_id = "/base_link" pose_stamped.header.stamp = rospy.Time.now() turn = compute_turn(deepcopy(pose_stamped.pose)) print("\tComputed turn:" + str(turn)) if abs(turn) > 0.07: self.base.turn(turn) number_of_turns += 1 rospy.sleep(1.5) print("\tExecuted turn") # markers = self.reader.markers if abs(compute_turn(m.pose.pose)) <= MIN_TURN_RADS: print("\tComputed turn was less than " + str(MIN_TURN_RADS) + ": " + str(abs(compute_turn(m.pose.pose)))) print("...good enough") break print("Re-Orientation complete!") print("Adjusting forward position...") if self.arm.compute_ik(pose_stamped): print("\tMarker is already reachable") reachable = True else: reachable = False print("\tMarker is too far away from the robot") computed_forward = compute_dist(pose_stamped.pose) - 1 print("\tComputed forward distance is " + str(computed_forward)) self.base.go_forward(computed_forward) print("\tMoving forward by " + str(computed_forward)) print("Moved to location") # move gripper #target_pose = PoseStamped(pose=original_pose_stamped.pose) #target_pose = PoseStamped(pose=markers[0].pose.pose) original_pose_stamped.pose.position.x -= 0.25 original_pose_stamped.pose.orientation.x = 0 original_pose_stamped.pose.orientation.y = 0.7 original_pose_stamped.pose.orientation.z = 0 original_pose_stamped.pose.orientation.w = -0.7 target_pose = dot_poses(lookup_transform("/base_link", "/odom"), original_pose_stamped.pose) target_pose.header.frame_id = "/base_link" target_pose.header.stamp = rospy.Time.now() print("Marker pose after moving is " + str(target_pose)) draw_debug_marker(target_pose, [0, 0, 1, 0.5]) if self.arm.compute_ik(target_pose): self.success = True print("reachable") # FINISHED: the gripper moves significantly slower by changing MoveGroup's # MotionPlanRequest's max_velocity_scaling factor. print("move_to_pose result: " + str(self.arm.move_to_pose(target_pose))) else: print("not reachable :(") self.success = False
def main(): rospy.init_node('arm_obstacle_demo') wait_for_time() argv = rospy.myargv() # Create the arm and safety measures arm = fetch_api.Arm() def shutdown(): arm.cancel_all_goals() return rospy.on_shutdown(shutdown) planning_scene = PlanningSceneInterface(frame='base_link') planning_scene.clear() # add table to the scene planning_scene.removeCollisionObject('table') table_size_x = 0.5 table_size_x = 0.5 table_size_y = 1 table_size_z = 0.03 table_x = 0.8 table_y = 0 table_z = 0.4 planning_scene.addBox('table', table_size_x, table_size_y, table_size_z, table_x, table_y, table_z) # Create divider obstacle planning_scene.removeCollisionObject('divider') size_x = 0.3 size_y = 0.01 size_z = 0.4 x = table_x - (table_size_x / 2) + (size_x / 2) y = 0 z = table_z + (table_size_z / 2) + (size_z / 2) planning_scene.addBox('divider', size_x, size_y, size_z, x, y, z) pose1 = PoseStamped() pose1.header.frame_id = 'base_link' pose1.pose.position.x = 0.5 pose1.pose.position.y = -0.3 pose1.pose.position.z = 0.75 pose1.pose.orientation.w = 1 pose2 = PoseStamped() pose2.header.frame_id = 'base_link' pose2.pose.position.x = 0.5 pose2.pose.position.y = 0.3 pose2.pose.position.z = 0.75 pose2.pose.orientation.w = 1 oc = OrientationConstraint() oc.header.frame_id = 'base_link' oc.link_name = 'wrist_roll_link' oc.orientation.w = 1 oc.absolute_x_axis_tolerance = 0.1 oc.absolute_y_axis_tolerance = 0.1 oc.absolute_z_axis_tolerance = 3.14 oc.weight = 1.0 kwargs1 = { 'allowed_planning_time': 15, 'execution_timeout': 10, 'num_planning_attempts': 5, 'replan': False, } kwargs2 = { 'allowed_planning_time': 15, 'execution_timeout': 10, 'num_planning_attempts': 5, 'replan': False, 'orientation_constraint': oc } gripper = fetch_api.Gripper() planning_scene.removeAttachedObject('tray') while True: # Before moving to the first pose error = arm.move_to_pose(pose1, **kwargs1) if error is not None: rospy.logerr('Pose 1 failed: {}'.format(error)) else: rospy.loginfo('Pose 1 succeeded') frame_attached_to = 'gripper_link' frames_okay_to_collide_with = [ 'gripper_link', 'l_gripper_finger_link', 'r_gripper_finger_link' ] planning_scene.attachBox('tray', 0.3, 0.07, 0.01, 0.05, 0, 0, frame_attached_to, frames_okay_to_collide_with) planning_scene.setColor('tray', 1, 0, 1) planning_scene.sendColors() rospy.sleep(1) error = arm.move_to_pose(pose2, **kwargs2) if error is not None: rospy.logerr('Pose 2 failed: {}'.format(error)) else: rospy.loginfo('Pose 2 succeeded') planning_scene.removeAttachedObject('tray')
def main(): rospy.init_node('coke_can_node') wait_for_time() # controls of Fetch ##### TODO: Uncomment these if nessecary # arm = fetch_api.Arm() # arm_joints = fetch_api.ArmJoints() # torso = fetch_api.Torso() head = fetch_api.Head() fetch_gripper = fetch_api.Gripper() move_base_client = actionlib.SimpleActionClient('move_base', MoveBaseAction) move_base_client.wait_for_server() rospy.sleep(0.5) ########## TODO: Uncomment this # shutdown handler # def shutdown(): # arm.cancel_all_goals() # rospy.on_shutdown(shutdown) # move base to the position near coke can goal = MoveBaseGoal() goal.target_pose.header.frame_id = "map" goal.target_pose.header.stamp = rospy.Time.now() goal.target_pose.pose.position.x = 1.51860149414 goal.target_pose.pose.position.y = -4.3 # -3.90194324852 goal.target_pose.pose.orientation.z = -0.55 goal.target_pose.pose.orientation.w = 0.84 move_base_client.send_goal(goal) wait = move_base_client.wait_for_result() if not wait: rospy.logerr("Action server not available!") rospy.signal_shutdown("Action server not available!") else: # move_base_client.get_result() print "Ready to pick up the coke can!" # move head head.pan_tilt(0, 0.7) print "Head moved!" ##### TODO: write code for moving the arm to pick up the coke can # pose = PoseStamped() # pose.header.frame_id = 'base_link' # pose.pose.position.x = 0.8 # pose.pose.position.y = 0 # pose.pose.position.z = 0.75 # pose.pose.orientation.w = 1 # kwargs = { # 'allowed_planning_time': 100, # 'execution_timeout': 100, # 'num_planning_attempts': 1, # 'replan_attempts': 5, # 'replan': True, # 'orientation_constraint': None # } # error = arm.move_to_pose(pose, **kwargs) # if error is not None: # rospy.logerr('Pose failed: {}'.format(error)) # else: # rospy.loginfo('Pose succeeded') # print "Arm moved!" # move base to the position near the shelf goal = MoveBaseGoal() goal.target_pose.header.frame_id = "map" goal.target_pose.header.stamp = rospy.Time.now() goal.target_pose.pose.position.x = 4.50320185696 goal.target_pose.pose.position.y = -4.2 goal.target_pose.pose.orientation.z = -0.554336505677 goal.target_pose.pose.orientation.w = 0.832292639926 move_base_client.send_goal(goal) wait = move_base_client.wait_for_result() if not wait: rospy.logerr("Action server not available!") rospy.signal_shutdown("Action server not available!") else: # move_base_client.get_result() print "Ready to place the coke can!"
def main(): rospy.init_node('create_pbd_action') wait_for_time() # Check to see the proper args were given argv = rospy.myargv() if len(argv) < 2: print_usage() return save_file_name = argv[1] # This is the list of poses that we will save sequence = [] # The Arm and the gripper on the robot gripper = fetch_api.Gripper() arm = fetch_api.Arm() gripper_open = True # Init a a tfListener for reading the gripper pose listener = tf.TransformListener() rospy.sleep(rospy.Duration.from_sec(1)) # Init the reader for the tags reader = ArTagReader() sub = rospy.Subscriber('/ar_pose_marker', AlvarMarkers, reader.callback) # Step 1: Relax the arm if not IN_SIM: controller_client = actionlib.SimpleActionClient( '/query_controller_states', QueryControllerStatesAction) # Sleep for a second to make sure the client starts up rospy.sleep(1.0) # The rest of this code is given in the lab goal = QueryControllerStatesGoal() state = ControllerState() state.name = 'arm_controller/follow_joint_trajectory' state.state = ControllerState.STOPPED goal.updates.append(state) controller_client.send_goal(goal) controller_client.wait_for_result() # Get the user interface going print_user_options() running = True while running: user_input = raw_input("") if user_input == "save_pose": # Get the current pose with respect to the base_link current_gripper_pose = get_current_gripper_pose(listener) # Ask the user which frame they would like to save the pose to # Eg: base_link, tag 1, tag 2 print "Please input the frame you would like to save the pose in. The options are:" print " base_link" for marker in reader.markers: print " %s" % marker.id frame = raw_input("") # Check the frame the pose should be saved in pose = Pose() frame_id = frame if frame == "base_link": pose = current_gripper_pose frame_id = 'base_link' else: for marker in reader.markers: if frame == str(marker.id): # First, we need to turn the poses into transform matrixes pose_in_base_matrix = pose_to_transform( current_gripper_pose) tag_in_base_matrix = pose_to_transform( marker.pose.pose) # Then take the inverse of the tag in the base fram inv_matrix = np.linalg.inv(tag_in_base_matrix) # Then take the dot product pose_to_save = np.dot(inv_matrix, pose_in_base_matrix) # The target pose is then transformed back into a pose object pose = transform_to_pose(pose_to_save) frame_id = str(marker.id) # Create and append the PBD_Pose pbd_pose = PBD_Pose(pose, gripper_open, frame) sequence.append(pbd_pose) if user_input == "open_gripper": gripper.open() gripper_open = True if user_input == "close_gripper": gripper.close() gripper_open = False if user_input == "save_program": pickle.dump(sequence, open(save_file_name, "wb")) if user_input == "help": print_user_options() if user_input == "quit": running = False
def main(): rospy.init_node('pour_scene') wait_for_time() target_name = 'cup1' x_pose = 0.329202820349 y_pose = -0.01 z_pose = 0.060 x_ori, y_ori, z_ori, w_ori = cvt_e2q(0, 0, 0) x, y, z, _ = getState('cup2') head = fetch_api.Head() arm = fetch_api.Arm() torso = fetch_api.Torso() torso.set_height(fetch_api.Torso.MAX_HEIGHT) head.look_at('base_link', x, y, z) sess = tensorflow.Session() model = load_model(sess) x, y, z, _ = getState(target_name) # observation test get_observation() move_arm_to(arm, x_pose, y + y_pose, z + z_pose, x_ori, y_ori, z_ori, w_ori) x, y, z, _ = getState('table1') base = fetch_api.Base() if x > 1: base.go_forward(0.6, speed=0.2) # collision in motion planning planning_scene = PlanningSceneInterface('base_link') planning_scene.clear() length_table = 1 width_table = 1 height_table = 0.04 x_table, y_table, z_table, _ = getState('table1') z_table = z + 0.7 planning_scene.addBox('table', length_table, width_table, height_table, x_table, y_table, z_table) length_box = 0.05 width_box = 0.05 height_box = 0.2 x, y, z, _ = getState('cup1') x_box = x y_box = y z_box = z planning_scene.addBox('mug_1', length_box, width_box, height_box, x_box, y_box, z_box) length_box = 0.03 width_box = 0.05 height_box = 0.2 x, y, z, _ = getState('cup2') x_box = x y_box = y z_box = z planning_scene.addBox('mug_2', length_box, width_box, height_box, x_box, y_box, z_box) # the initial position of gripper is (-0.5, 0, 0), and # the final position of gripper is (-0.5+x_pose, y_pose, z_pose). x, y, z, _ = getState(target_name) move_arm_to(arm, x - 0.5 + x_pose, y + y_pose, z + z_pose, x_ori, y_ori, z_ori, w_ori) planning_scene.removeCollisionObject('mug_1') # planning_scene.removeCollisionObject('mug_2') # planning_scene.removeCollisionObject('mug') # planning_scene.removeCollisionObject('table') gripper = fetch_api.Gripper() effort = gripper.MAX_EFFORT gripper.close(effort) x, y, z, _ = getState(target_name) move_arm_to(arm, x - 0.5 + x_pose, y + y_pose, z + z_pose + 0.06, x_ori, y_ori, z_ori, w_ori) x, y, z, _ = getState('cup2') head.look_at('base_link', x, y, z) for _ in xrange(50): obs = get_observation() act = model.choose_action(obs) print('obs: {}, action: {}'.format(obs, act)) execute_action(act, arm) time.sleep(0.5)
def main(): rospy.init_node("book_grasp_procedure") wait_for_time() # First, load the poses for the fiducial insert movement sequence = pickle.load( open(INSERT_GRASP_POSES, "rb") ) gripper = fetch_api.Gripper() arm = fetch_api.Arm() gripper_open = True # # Init the reader for the tags reader = ArTagReader() sub = rospy.Subscriber('/ar_pose_marker', AlvarMarkers, reader.callback) rospy.sleep(0.5) # Init the planning scene for collisions planning_scene = PlanningSceneInterface('base_link') print "waiting for service...." rospy.wait_for_service('get_spines') print "found service!" get_spine_poses = rospy.ServiceProxy('get_spines', GetSpineLocations) response = get_spine_poses() planning_scene.addBox('surface', (response.surface_x_size - 0.17), (response.surface_y_size), response.surface_z_size, response.surface_pose.position.x, response.surface_pose.position.y, response.surface_pose.position.z) gripper.open() gripper_open = True target_marker_pose = None check = 0 print "Searching for fiducial...." while target_marker_pose == None and check < 100: # If the fiducial was not seen, try again rospy.sleep(0.1) check += 1 for marker in reader.markers: if TARGET_ID == marker.id: target_marker_pose = marker.pose.pose if target_marker_pose == None: print "Failed to find fiducial" print "Surface position z" print response.surface_pose.position.z print "target marker position z" print target_marker_pose.position.z # This is the same as the pbd action stuff, not making any changes at the moment for pbd_pose in sequence: move_pose = PoseStamped() move_pose.header.frame_id = 'base_link' if pbd_pose.frame == 'base_link': move_pose.pose = pbd_pose.pose else: # for marker in reader.markers: # if TARGET_ID == marker.id: print "Calculating pose relative to marker...." # Transform the pose to be in the base_link frame pose_in_tag_frame = pose_to_transform(pbd_pose.pose) #tag_in_base_frame = pose_to_transform(marker.pose.pose) tag_in_base_frame = pose_to_transform(target_marker_pose) target_matrix = np.dot(tag_in_base_frame, pose_in_tag_frame) target_pose = transform_to_pose(target_matrix) move_pose.pose = target_pose rospy.sleep(1) err = arm.move_to_pose(move_pose, replan=True) print "Error in move to pose: ", err # Check the gripper to open/close if pbd_pose.gripper_open != gripper_open: if gripper_open == True: gripper.close() gripper_open = False else: gripper.open() gripper_open = True print "Take this opportunity to change to a different mock point cloud, if necessary" user_input = raw_input("Press enter to continue") # After this we calculate the spine_pose closest to the fiducial. I will test that if I can get the service call working target_fiducial = None check = 0 print "Searching for fiducial" while target_fiducial == None and check < 100: # If the fiducial was not seen, try again rospy.sleep(0.1) check += 1 for marker in reader.markers: if marker.id == TARGET_ID: target_fiducial = marker if target_fiducial == None: print "Failed to find fiducial" print target_fiducial.id # Now, we make a request to the perception side of things spine_poses = [] # code for checking check = 0 found_good_pose = False closest_pose = None print "waiting for service...." rospy.wait_for_service('get_spines') print "found service!" print "Searching for a good grasp pose...." try: while check < 20 and found_good_pose == False: get_spine_poses = rospy.ServiceProxy('get_spines', GetSpineLocations) response = get_spine_poses() spine_poses = response.spine_poses min_dist = float('inf') for pose in spine_poses: distance = calculate_euclidean_distance(pose, target_fiducial.pose.pose) if distance < min_dist: min_dist = distance closest_pose = pose check += 1 if closest_pose.position.x > target_fiducial.pose.pose.position.x and closest_pose.position.y < (target_fiducial.pose.pose.position.y + 0.025) and closest_pose.position.y > (target_fiducial.pose.pose.position.y - 0.025): found_good_pose = True # debugging line # for pose in spine_poses: # print pose except rospy.ServiceException, e: print "Service call failed: %s"%e
def handle_grab_box(req): """ Given a req, grabs the shoe box at the shoe shelf """ global markers global grab_poses global viz_pub global robot_pose global planning # Attach the floor before doing any movements attach_floor() arm = fetch_api.Arm() gripper = fetch_api.Gripper() torso = fetch_api.Torso() head = fetch_api.Head() base = fetch_api.Base() # Prepare for grabbing the box print("Preparing to Grab Box, setting height + arm") # Move the arm, gripper, toros and head to initial position gripper.open() beginHeight = 0.1 # Navigate the arm to prepare move_pose(arm, PREPARE) # Move torso higher if on top shelf: if req.ar_id == 6 or req.ar_id == 4: beginHeight = 0.2 torso.set_height(beginHeight) head.pan_tilt(*SHELF_HEAD_POSE) rospy.sleep(3.0) # Wait 5 seconds until markers are updated target_marker = None for i in range(5): print("Waiting for the markers... {}/3".format(i)) if any(markers): print([marker.id for marker in markers]) target_marker = filter(lambda x: x.id == req.ar_id, markers) if any(target_marker): break if i == 4: print("Didn't find any marker") move_dist(0.1, -0.1) torso.set_height(torso.MAX_HEIGHT) move_pose(arm, CARRY) rospy.sleep(1.0) return 1 rospy.sleep(1.0) print[marker.id for marker in markers] print("Target: {}".format(req.ar_id)) if not any(target_marker): print("Didn't find the target marker") markers = [] move_dist(0.1, -0.1) torso.set_height(torso.MAX_HEIGHT) move_pose(arm, CARRY) rospy.sleep(1.0) return 1 # Navigate the gripper print("Navigating arm to the target box") for action in grab_poses: if action[0] == 'MOVE': arPose = action[1] wristPose = action[2] # Compute the pose of the wrist in base_link ar = fetch_api.pose2matrix(arPose) ar2wrist = fetch_api.pose2transform(arPose, wristPose, True) wrist = np.dot(fetch_api.pose2matrix(target_marker[0].pose.pose), ar2wrist) print(target_marker[0].pose.header.frame_id) # Navigate the arm there kwargs = { 'allowed_planning_time': 50, 'execution_timeout': 40, 'num_planning_attempts': 30, 'replan': False, } pose_stamped = PoseStamped() pose_stamped.header.frame_id = "base_link" pose_stamped.pose = fetch_api.matrix2pose(wrist) mp_marker = Marker() mp_marker.header.frame_id = "base_link" mp_marker.header.stamp = rospy.Time() mp_marker.id = 0 mp_marker.type = Marker.ARROW mp_marker.action = Marker.ADD mp_marker.ns = 'motion_plan_goal' mp_marker.pose.position.x = pose_stamped.pose.position.x mp_marker.pose.position.y = pose_stamped.pose.position.y mp_marker.pose.position.z = pose_stamped.pose.position.z mp_marker.pose.orientation.x = pose_stamped.pose.orientation.x mp_marker.pose.orientation.y = pose_stamped.pose.orientation.y mp_marker.pose.orientation.z = pose_stamped.pose.orientation.z mp_marker.pose.orientation.w = pose_stamped.pose.orientation.w mp_marker.scale.x = 0.2 mp_marker.scale.y = 0.05 mp_marker.scale.z = 0.05 mp_marker.color.a = 1.0 mp_marker.color.r = 1.0 mp_marker.color.g = 0.0 mp_marker.color.b = 0.0 viz_pub.publish(mp_marker) error = arm.move_to_pose(pose_stamped, **kwargs) if error is None: rospy.loginfo('Moved to the target marker') else: rospy.logwarn('Failed to move to the target marker') move_dist(0.1, -0.1) torso.set_height(torso.MAX_HEIGHT) move_pose(arm, CARRY) return 1 rospy.sleep(1.0) elif action[0] == 'OPEN': gripper.open() elif action[0] == 'CLOSE': gripper.close() gripper.close() print("Grabbed the box, moving torso up") torso.set_height(beginHeight + 0.1) # Back up the base, set torso back down and head back up head.pan_tilt(*MOVING_HEAD_POSE) move_dist(0.19, -0.1) torso.set_height(torso.MAX_HEIGHT) # Set arm to initial position move_pose(arm, CARRY) # Empty markers for the next call markers = [] return 0
def __init__(self): self._grip = fetch_api.Gripper() self._arm = fetch_api.Arm() pass