return key speed = .2 turn = 1 def vels(speed, turn): return "currently:\tspeed %s\tturn %s " % (speed, turn) if __name__ == "__main__": settings = termios.tcgetattr(sys.stdin) rospy.init_node('fetch_teleop_key') base = fetch_api.Base() x = 0 th = 0 status = 0 count = 0 target_speed = 0 target_turn = 0 control_speed = 0 control_turn = 0 try: print msg print vels(speed, turn) while (1): key = getKey() if key in moveBindings.keys():
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('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 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