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 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 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 __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(): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('straight_gripper_motion_demo') wait_for_time() moveit_robot = moveit_commander.RobotCommander() group = moveit_commander.MoveGroupCommander('arm') def on_shutdown(): group.stop() moveit_commander.roscpp_shutdown() rospy.on_shutdown(on_shutdown) # Set the torso height before running this demo. #torso = fetch_api.Torso() #torso.set_height(0.4) pose1 = PoseStamped() pose1.header.frame_id = 'base_link' pose1.pose.position.x = 0.502 pose1.pose.position.y = -0.510 pose1.pose.position.z = 1.084 pose1.pose.orientation.x = -0.498 pose1.pose.orientation.y = 0.508 pose1.pose.orientation.z = 0.475 pose1.pose.orientation.w = 0.519 pose2 = PoseStamped() pose2.header.frame_id = 'base_link' pose2.pose.position.x = 0.502 pose2.pose.position.y = 0.510 pose2.pose.position.z = 1.084 pose2.pose.orientation.x = 0.522 pose2.pose.orientation.y = -0.484 pose2.pose.orientation.z = -0.499 pose2.pose.orientation.w = -0.494 gripper_poses = [pose1, pose2] arm = fetch_api.Arm() while not rospy.is_shutdown(): for pose_stamped in gripper_poses: error = arm.straight_move_to_pose(group, pose_stamped, jump_threshold=2.0) if error is not None: rospy.logerr(error) rospy.sleep(0.25) moveit_commander.roscpp_shutdown()
def main(): rospy.init_node('access_gripper_teleop') wait_for_time() arm = fetch_api.Arm() move_group = MoveGroupCommander("arm") status_publisher = rospy.Publisher('/access_teleop/arm_status', String, queue_size=1) gripper_publisher = rospy.Publisher('/access_teleop/gripper_pixels', PX, queue_size=1) info_pubs = [] for camera_name in camera_names: info_pubs.append([ camera_name, rospy.Publisher(camera_name + '/camera_info', camera_info_messages.CameraInfo, queue_size=10) ]) tb = TransformBroadcaster() camera_model = PinholeCameraModel() move_by_delta = MoveByDelta(arm, move_group) move_by_delta.start() move_by_absolute = MoveByAbsolute(arm, move_group, status_publisher) move_by_absolute.start() move_and_orient = MoveAndOrient(arm, move_group) move_and_orient.start() orient = Orient(arm, move_group) orient.start() wrist_roll = WristRoll(arm, move_group) wrist_roll.start() rate = rospy.Rate(10) while not rospy.is_shutdown(): publish_camera_transforms(tb) publish_camera_info(info_pubs) publish_gripper_pixels(camera_model, move_group, gripper_publisher) rate.sleep()
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 main(): rospy.init_node('arm_demo') wait_for_time() argv = rospy.myargv() DISCO_POSES = [[ -1.605528329547318, 1.41720603380179, 2.018610841968549, 1.5522558117738399, -1.5635699410855368, 0.7653977094751401, -1.3914909133500242 ]] #torso = fetch_api.Torso() #torso.set_height(fetch_api.Torso.MAX_HEIGHT) arm = fetch_api.Arm() for vals in DISCO_POSES: arm.move_to_joints(fetch_api.ArmJoints.from_list(vals))
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(): rospy.init_node('hallucinated_demo') wait_for_time() argv = rospy.myargv() torso = fetch_api.Torso() torso.set_height(0.4) start = PoseStamped() start.header.frame_id = 'base_link' start.pose = Pose(orientation=Quaternion(0, 0, 0, 1)) start.pose.position.x = 0.5 start.pose.position.y = 0.5 start.pose.position.z = 0.75 arm = fetch_api.Arm() arm.move_to_pose(start) success = [] reader = ArTagReader() sub = rospy.Subscriber( '/ar_pose_marker', AlvarMarkers, reader.callback) # Subscribe to AR tag poses, use reader.callback while len(reader.markers) == 0: rospy.sleep(0.1) for marker in reader.markers: #TODO: get the pose to move to #goal.pose = Pose(orientation=Quaternion(0,0,0,1)) moveTo = PoseStamped() moveTo.header.frame_id = 'base_link' moveTo.pose.position = marker.pose.pose.position moveTo.pose.position.x -= GRIPPER_OFFSET moveTo.pose.orientation = Quaternion(0, 0, 0, 1) error = arm.move_to_pose(moveTo, allowed_planning_time=30.0) if error is None: rospy.loginfo('Moved to marker {}'.format(marker.id)) success.append(marker.id) else: rospy.logwarn('Failed to move to marker {}'.format(marker.id)) if len(success) > 1: rospy.loginfo('Moved to markers listed above') else: rospy.logerr('Failed to move to any markers!')
def main(): rospy.init_node('arm_demo') wait_for_time() argv = rospy.myargv() DISCO_POSES = [[1.5, -0.6, 3.0, 1.0, 3.0, 1.0, 3.0], [0.8, 0.75, 0.0, -2.0, 0.0, 2.0, 0.0], [-0.8, 0.0, 0.0, 2.0, 0.0, -2.0, 0.0], [-1.5, 1.1, -3.0, -0.5, -3.0, -1.0, -3.0], [-0.8, 0.0, 0.0, 2.0, 0.0, -2.0, 0.0], [0.8, 0.75, 0.0, -2.0, 0.0, 2.0, 0.0], [1.5, -0.6, 3.0, 1.0, 3.0, 1.0, 3.0]] torso = fetch_api.Torso() torso.set_height(fetch_api.Torso.MAX_HEIGHT) arm = fetch_api.Arm() for vals in DISCO_POSES: arm.move_to_joints(fetch_api.ArmJoints.from_list(vals))
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('cart_arm_demo') wait_for_time() argv = rospy.myargv() pose1 = PoseStamped() pose1.header.frame_id = 'base_link' pose1.pose.position.x = 0.042 pose1.pose.position.y = 0.384 pose1.pose.position.z = 1.826 pose1.pose.orientation.x = 0.173 pose1.pose.orientation.y = -0.693 pose1.pose.orientation.z = -0.242 pose1.pose.orientation.w = 0.657 pose2 = PoseStamped() pose2.header.frame_id = 'base_link' pose2.pose.position.x = 0.047 pose2.pose.position.y = 0.545 pose2.pose.position.z = 1.822 pose2.pose.orientation.x = -0.274 pose2.pose.orientation.y = -0.701 pose2.pose.orientation.z = 0.173 pose2.pose.orientation.w = 0.635 gripper_poses = [pose1, pose2] arm = fetch_api.Arm() def on_shutdown(): arm.cancel_all_goals() rospy.on_shutdown(on_shutdown) while not rospy.is_shutdown(): for pose_stamped in gripper_poses: error = arm.move_to_pose(pose_stamped, replan=True, execution_timeout=2) if error is not None: rospy.logerr(error) rospy.sleep(1)
def main(): rospy.init_node('cart_arm_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) header = Header() header.frame_id = 'base_link' # Create desired poses pose1 = Pose(position=Point(0.042, 0.384, 1.826), orientation=Quaternion(0.173, -0.693, -0.242, 0.657)) pose2 = Pose(position=Point(0.047, 0.545, 1.822), orientation=Quaternion(-0.274, -0.701, 0.173, 0.635)) pose1_st = PoseStamped(header, pose1) pose2_st = PoseStamped(header, pose2) gripper_poses = [pose1_st, pose2_st] while True: # Move the arm to each position! for pose in gripper_poses: error = arm.move_to_pose(pose) # Deal with potential errors if error is not None: rospy.logerr(error) rospy.sleep(1)
def main(): rospy.init_node('limb_ar_demo') wait_for_time() start = PoseStamped() start.header.frame_id = 'base_link' start.pose.position.x = 0.5 start.pose.position.y = 0.5 start.pose.position.z = 0.75 arm = fetch_api.Arm() arm.move_to_pose(start) reader = ArTagReader() sub = rospy.Subscriber('ar_pose_marker', AlvarMarkers, callback=reader.callback) rate = rospy.Rate(10) while not rospy.is_shutdown(): while len(reader.markers) == 0: rospy.sleep(0.1) # for marker in reader.markers: # pose = marker.pose # pose.header.frame_id = marker.header.frame_id # error = arm.move_to_pose(pose) # if error is None: # rospy.loginfo('Moved to marker {}'.format(marker.id)) # return # else: # rospy.logwarn('Failed to move to marker {}'.format(marker.id)) # rospy.logerr('Failed to move to any markers!') for marker in reader.markers: pose = marker.pose pose.header.frame_id = marker.header.frame_id print(pose) rate.sleep()
def main(): rospy.init_node('arm_demo') wait_for_time() # moveit_commander.roscpp_initialize(sys.argv) # moveit_robot = moveit_commander.RobotCommander() # moveit_group = moveit_commander.MoveGroupCommander('arm') torso = fetch_api.Torso() torso.set_height(fetch_api.Torso.MAX_HEIGHT) arm = fetch_api.Arm() # DISCO_POSES = [[1.5, -0.6, 3.0, 1.0, 3.0, 1.0, 3.0], # [0.8, 0.75, 0.0, -2.0, 0.0, 2.0, 0.0]] # # [-0.8, 0.0, 0.0, 2.0, 0.0, -2.0, 0.0], # # [-1.5, 1.1, -3.0, -0.5, -3.0, -1.0, -3.0], # # [-0.8, 0.0, 0.0, 2.0, 0.0, -2.0, 0.0], # # [0.8, 0.75, 0.0, -2.0, 0.0, 2.0, 0.0], # # [1.5, -0.6, 3.0, 1.0, 3.0, 1.0, 3.0]] # for vals in DISCO_POSES: # arm.move_to_joints(fetch_api.ArmJoints.from_list(vals)) goal = [('shoulder_pan_joint', 1.5), ('shoulder_lift_joint', -0.6), ('upperarm_roll_joint', 3.0), ('elbow_flex_joint', 1.0), ('forearm_roll_joint', 3.0), ('wrist_flex_joint', 1.0), ('wrist_roll_joint', 3.0)] error = arm.move_to_joint_goal(goal, allowed_planning_time=120.0, execution_timeout=rospy.Duration(60.0), num_planning_attempts=5, replan=True, replan_attempts=5) if error is not None: arm.cancel_all_goals() rospy.logerr('Failed: {}'.format(error)) else: rospy.loginfo('Succeeded')
def create_gripper_marker(x, z, pose): # Create marker for gripper base base_marker = Marker() base_marker.type = Marker.MESH_RESOURCE base_marker.mesh_resource = GRIPPER_MESH base_marker.pose.position.x += x + GRIPPER_X_OFFSET base_marker.pose.position.z += z arm = fetch_api.Arm() if arm.compute_ik(create_pose_stamped(pose)): base_marker.color = GREEN else: base_marker.color = RED # Create marker for left gripper prong left_marker = copy.deepcopy(base_marker) left_marker.mesh_resource = L_FINGER_MESH left_marker.scale.y = FINGER_Y_SCALE # Create marker for right gripper prong right_marker = copy.deepcopy(left_marker) right_marker.mesh_resource = R_FINGER_MESH return [base_marker, left_marker, right_marker]
def main(): rospy.init_node('hallucinate_demo') wait_for_time() start = PoseStamped() start.header.frame_id = 'base_link' start.pose.position.x = 0.5 start.pose.position.y = 0.5 start.pose.position.z = 0.75 arm = fetch_api.Arm() arm.move_to_pose(start) reader = ArTagReader() # Subscribe to AR tag poses, use reader.callback sub = rospy.Subscriber('/ar_pose_marker', AlvarMarkers, reader.callback) while len(reader.markers) == 0: print 'sleep' rospy.sleep(0.1) successes = [] for marker in reader.markers: print marker ps = PoseStamped() ps.pose.position = marker.pose.pose.position ps.header.frame_id = 'base_link' ps.pose.orientation = Quaternion(0, 0, 0, 1) ps.pose.position.x -= 0.3 error = arm.move_to_pose(ps, allowed_planning_time=30) if error is None: rospy.loginfo('Moved to marker {}'.format(marker.id)) successes.append(marker.id) else: rospy.logwarn('Failed to move to marker {}'.format(marker.id)) if len(successes) > 0: rospy.loginfo('Moved to markers {}!'.format(successes)) else: rospy.logerr('Fail to move to any marker!')
def main(): rospy.init_node('gripper_teleop') wait_for_time() # Added by Xinyi: # Initialize Settings for the Test (Part 1) # Raise the torso to allow arm movement torso = fetch_api.Torso() torso.set_height(fetch_api.Torso.MAX_HEIGHT) # Set arm joints # Order: body ---> gripper # b: blue joint # g: gray joint # order: [b, g, b, g, b, g, b] # OPTION 1: Follow given trajectory arm = fetch_api.Arm() arm_initial_poses = [1.0, 1.25, 1.0, -2.25, -0.3, 1.0, 0.0] arm.move_to_joints(fetch_api.ArmJoints.from_list(arm_initial_poses)) # for recording bag file # arm_viz_poses = [1.0, 1.25, 1.0, -2.25, 2.25, 2.25, 0.0] # arm.move_to_joints(fetch_api.ArmJoints.from_list(arm_viz_poses)) # OPTION 2: Use motion planning # INITIAL_POSES = [ # ("shoulder_pan_joint", 1.0), ("shoulder_lift_joint", 1.2), ("upperarm_roll_joint", 1.0), ("elbow_flex_joint", -2.0), # ("forearm_roll_joint", -0.3), ("wrist_flex_joint", 1.2), ("wrist_roll_joint", 0.0)] # arm.move_to_joint_goal(INITIAL_POSES, replan=True) # INITIAL_POSES = PoseStamped() # INITIAL_POSES.header.frame_id = 'base_link' # INITIAL_POSES.pose.position.x = 0.6 # INITIAL_POSES.pose.position.y = -0.1 # INITIAL_POSES.pose.position.z = 0.3 # INITIAL_POSES.pose.orientation.w = 1 # error = arm.move_to_pose(INITIAL_POSES, replan=True) # if error is not None: # arm.cancel_all_goals() # rospy.logerr('FAIL TO MOVE TO INITIAL_POSES {}'.format(error)) # else: # rospy.loginfo('MOVED TO INITIAL_POSES') # Set the base position base = fetch_api.Base() # OPTION 1: Start from origin # move Fetch from the origin to the table # base.go_forward(1.6, 0.5) # value (distance in meters) # base.turn(math.pi / 3) # value (angle in degrees) # base.go_forward(3.3, 0.5) # value (distance in meters) # base.turn(-math.pi / 3) # OPTION 2: Start right in front of the table base.align_with_x_axis_pos() base.go_forward(0.3, 0.5) # Avoid occlusion # OPTION 1: Freeze point cloud # freeze_pub = rospy.Publisher('/access_teleop/freeze_cloud', Bool, queue_size=5) # rospy.sleep(0.5) # publish freeze point cloud # freeze_pub.publish(Bool(data=True)) # OPTION 2: Record a bag file of the surrounding head = fetch_api.Head() # tilt the head from -0.35 to 0.85 # for i in range(6): # head.look_at("base_link", HEAD_POSE[0], HEAD_POSE[1], -0.35 + i * 0.2) # os.system("rosrun perception save_cloud world $(rospack find access_teleop)/bags/") # rospy.set_param("bag_file_refreshed", "true") # (end) move_group = MoveGroupCommander("arm") status_publisher = rospy.Publisher('/access_teleop/arm_status', String, queue_size=1) gripper_publisher = rospy.Publisher('/access_teleop/gripper_pixels', PX, queue_size=1) info_pubs = [] for camera_name in camera_names: info_pubs.append([camera_name, rospy.Publisher(camera_name + '/camera_info', camera_info_messages.CameraInfo, queue_size=1)]) # Added by Xinyi # Debug: visualize camera positions vis_pub = rospy.Publisher('visualization_marker', Marker, queue_size=5) rospy.sleep(0.5) # (end) tb = TransformBroadcaster() camera_model = PinholeCameraModel() move_by_delta = MoveByDelta(arm, move_group) move_by_delta.start() move_by_absolute = MoveByAbsolute(arm, move_group, status_publisher) move_by_absolute.start() move_and_orient = MoveAndOrient(arm, move_group) move_and_orient.start() orient = Orient(arm, move_group) orient.start() # Added by Xinyi wrist_roll = WristRoll(arm, move_group) wrist_roll.start() base_switch_task = BaseSwitchTask(base) base_switch_task.start() head_tilt = HeadTilt(head) head_tilt.start() # Initialize Settings for the Test (Part 2) # Add the first test object to Gazebo os.system("$(rospack find access_teleop)/scripts/switch_object.sh " + "NONE" + " " + str(MODELS[0])) # Move arm joints to positions for test arm_test_poses = [1.0, 1.25, 1.0, -2.25, -0.3, 1.0, 0.0] arm.move_to_joints(fetch_api.ArmJoints.from_list(arm_test_poses)) # Adjust the head to look at the table # OPTION 1: Tilt by angle # head.pan_tilt(0, math.pi / 2) # OPTION 2: Look at a point in space head.look_at("base_link", HEAD_POSE[0], HEAD_POSE[1], HEAD_POSE[2]) rospy.sleep(0.5) # (end) rate = rospy.Rate(200) while not rospy.is_shutdown(): publish_camera_transforms(tb, vis_pub) publish_camera_info(info_pubs) publish_gripper_pixels(camera_model, move_group, gripper_publisher) # publish freeze point cloud # freeze_pub.publish(Bool(data=True)) # # get the current model position and publish a marker of current model position # model_state = rospy.ServiceProxy('gazebo/get_model_state', GetModelState) # model_pos = model_state(MODELS[current_model_idx], 'base_link') # if model_pos.success: # marker = Marker( # type=Marker.SPHERE, # id=current_model_idx, # pose=Pose(Point(model_pos.pose.position.x, model_pos.pose.position.y, model_pos.pose.position.z), # Quaternion(model_pos.pose.orientation.x, model_pos.pose.orientation.y, model_pos.pose.orientation.z, model_pos.pose.orientation.w)), # scale=Vector3(0.05, 0.05, 0.05), # header=Header(frame_id='base_link'), # color=ColorRGBA(1.0, 0.5, 1.0, 0.5)) # vis_pub.publish(marker) rate.sleep()
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 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('arm_obstacle_demo') wait_for_time() planning_scene = PlanningSceneInterface('base_link') # Create table obstacle planning_scene.removeCollisionObject('table') table_size_x = 0.5 table_size_y = 1 table_size_z = 0.03 table_x = 0.8 table_y = 0 table_z = 0.6 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 torso = fetch_api.Torso() torso.set_height(fetch_api.Torso.MAX_HEIGHT) arm = fetch_api.Arm() def shutdown(): arm.cancel_all_goals() rospy.on_shutdown(shutdown) # and add an orientation constraint to pose 2: 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 kwargs = { 'allowed_planning_time': 100, 'execution_timeout': 100, 'num_planning_attempts': 15, 'replan': True, 'replan_attempts': 10 } # Before moving to the first pose planning_scene.removeAttachedObject('tray') error = arm.move_to_pose(pose1, **kwargs) # If the robot reaches the first pose successfully, then "attach" an object there # Of course, you would have to close the gripper first and ensure that you grasped the object properly 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) kwargs['orientation'] = oc error = arm.move_to_pose(pose2, **kwargs) if error is not None: rospy.logerr('Pose 2 failed: {}'.format(error)) else: rospy.loginfo('Pose 2 succeeded') planning_scene.removeCollisionObject('table') planning_scene.removeCollisionObject('divider') # At the end of your program planning_scene.removeAttachedObject('tray')
def main(): rospy.init_node('arm_obstacle_demo') wait_for_time() argv = rospy.myargv() torso = fetch_api.Torso() torso.set_height(fetch_api.Torso.MAX_HEIGHT) 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 planning_scene = PlanningSceneInterface('base_link') # Create table obstacle planning_scene.removeCollisionObject('table') table_size_x = 0.5 table_size_y = 1 table_size_z = 0.03 table_x = 0.8 table_y = 0 table_z = 0.6 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) planning_scene.removeAttachedObject('tray') arm = fetch_api.Arm() def shutdown(): arm.cancel_all_goals() rospy.on_shutdown(shutdown) kwargs = { 'allowed_planning_time': 20, 'execution_timeout': 40, 'num_planning_attempts': 50, 'replan': False, } error = arm.move_to_pose(pose1, **kwargs) 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) #kwargs['orientation_constraint'] = oc error = arm.move_to_pose(pose2, **kwargs) if error is not None: rospy.logerr('Pose 2 failed: {}'.format(error)) else: rospy.loginfo('Pose 2 succeeded') planning_scene.removeCollisionObject('table') planning_scene.removeCollisionObject('divider') # At the end of your program planning_scene.removeAttachedObject('tray') planning_scene.clear()
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')
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 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 __init__(self): self._torso = fetch_api.Torso() self._head = fetch_api.Head() self._arm = fetch_api.Arm() self._gripper = fetch_api.Gripper()
def __init__(self): self._grip = fetch_api.Gripper() self._arm = fetch_api.Arm() pass