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
