def main():
rospy.init_node('head_demo')
wait_for_time()
head = fetch_api.Head()
argv = rospy.myargv()
if len(argv) < 2:
print_usage()
return
command = argv[1]
if command == 'look_at':
if len(argv) < 6:
print_usage()
return
frame_id, x, y, z = argv[2], float(argv[3]), float(argv[4]), float(
argv[5])
head.look_at(frame_id, x, y, z)
# rospy.logerr('Not implemented.')
elif command == 'pan_tilt':
if len(argv) < 4:
print_usage()
return
pan, tilt = float(argv[2]), float(argv[3])
head.pan_tilt(pan, tilt)
else:
print_usage()
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):
self.book_data = DatabaseReader()
self.arm_controller = ArmController()
self.location_driver = NavigationManager()
self.torso = fetch_api.Torso()
self.head = fetch_api.Head()
self.cmdline = False
self.cmdline_grab_tray = False
self.cmdline_grab_book = False
# home for sim. could be refactored better
# returnPose = Pose()
# returnPose.position.x = 0.3548
# returnPose.position.y = 0.6489
# returnPose.position.z = 0.0
# returnPose.orientation.x = 0.0
# returnPose.orientation.y = 0.0
# returnPose.orientation.z = 0.14559
# returnPose.orientation.w = .989
# home for real robot as negative book indices
# self.home_pose = returnPose
self.home_pose = self.book_data.library[-1].pose
self.delivery_pose = self.book_data.library[-2].pose
def __init__(self):
self._torso = fetch_api.Torso()
self._head = fetch_api.Head()
self._arm = fetch_api.Arm()
self._gripper = fetch_api.Gripper()
#! /usr/bin/env python
import math
import fetch_api
import rospy
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()
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():
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('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 __init__(self):
self._pose_ctrl = PoseController()
self._head = fetch_api.Head()
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