def test_pour(visualize):
arms = [LEFT_ARM]
#cup_name, bowl_name = 'bluecup', 'bluebowl'
#cup_name, bowl_name = 'cup_7', 'cup_8'
#cup_name, bowl_name = 'cup_7-1', 'cup_7-2'
#cup_name, bowl_name = get_name('bluecup', 1), get_name('bluecup', 2)
cup_name = create_name('bluecup', 1) # bluecup | purple_cup | orange_cup | blue_cup | olive1_cup | blue3D_cup
bowl_name = create_name('bowl', 1) # bowl | steel_bowl
world, table_body = create_world([bowl_name, cup_name, bowl_name], visualize=visualize)
with ClientSaver(world.perception.client):
cup_z = stable_z(world.perception.sim_bodies[cup_name], table_body) + Z_EPSILON
bowl_z = stable_z(world.perception.sim_bodies[bowl_name], table_body) + Z_EPSILON
world.perception.set_pose(cup_name, Point(0.6, 0.2, cup_z), unit_quat())
world.perception.set_pose(bowl_name, Point(0.6, 0.0, bowl_z), unit_quat())
update_world(world, table_body)
init = [
('Contains', cup_name, COFFEE),
#('Graspable', bowl_name),
]
goal = [
('Contains', bowl_name, COFFEE),
]
task = Task(init=init, goal=goal, arms=arms,
empty_arms=True, reset_arms=True, reset_items=True)
return world, task
def collect_push(args, ros_world):
arm = LEFT_ARM
block_name, = get_pour_items(ros_world)
goal_pos2d = np.random.uniform(*POSE2D_RANGE)[:2]
init = [('CanPush', block_name, goal_pos2d)]
goal = [('InRegion', block_name, goal_pos2d)]
skeleton = [
('move-arm', [arm, X, X, X]),
('push', [arm, block_name, X, X, X, X, X]),
('move-arm', [arm, X, X, X]),
]
constraints = PlanConstraints(skeletons=[skeleton], exact=True)
task = Task(init=init,
goal=goal,
arms=[arm],
required=[block_name],
reset_arms=True,
use_scales=False,
constraints=constraints)
initial_pose = ros_world.perception.get_pose(block_name)
feature = get_push_feature(ros_world, block_name, arm, initial_pose,
goal_pos2d)
for goal in task.goal:
if goal[0] == 'InRegion':
_, block_name, goal_pos2d = goal
draw_push_goal(ros_world, block_name, goal_pos2d)
return task, feature
def test_block(visualize):
#arms = [LEFT_ARM]
arms = ARMS
block_name = create_name('greenblock', 1)
tray_name = create_name('tray', 1)
world, table_body = create_world([tray_name, block_name], visualize=visualize)
with ClientSaver(world.perception.client):
block_z = stable_z(world.perception.sim_bodies[block_name], table_body) + Z_EPSILON
tray_z = stable_z(world.perception.sim_bodies[tray_name], table_body) + Z_EPSILON
#attach_viewcone(world.perception.pr2, depth=1, head_name='high_def_frame')
#dump_body(world.perception.pr2)
#block_y = 0.0
block_y = -0.4
world.perception.set_pose(block_name, Point(0.6, block_y, block_z), unit_quat())
world.perception.set_pose(tray_name, Point(0.6, 0.4, tray_z), unit_quat())
update_world(world, table_body)
init = [
('Stackable', block_name, tray_name, TOP),
]
goal = [
('On', block_name, tray_name, TOP),
]
#goal = [
# ('Grasped', arms[0], block_name),
#]
task = Task(init=init, goal=goal, arms=arms, empty_arms=True)
return world, task
def collect_scoop(args, ros_world):
arm, _ = ACTIVE_ARMS['scoop']
cup_name, bowl_name = get_scoop_items(ros_world)
spoon_name = create_name(args.spoon,
1) # grey_spoon | orange_spoon | green_spoon
init_holding = get_spoon_init_holding(arm, spoon_name)
init = [
('Contains', bowl_name, SUGAR),
]
goal = [
('Contains', cup_name, SUGAR),
]
skeleton = [
('move-arm', [arm, X, X, X]),
('scoop', [arm, bowl_name, X, spoon_name, X, SUGAR, X, X, X]),
('move-arm', [arm, X, X, X]),
('pour', [arm, cup_name, X, spoon_name, X, SUGAR, X, X, X]),
('move-arm', [arm, X, X, X]),
]
constraints = PlanConstraints(skeletons=[skeleton], exact=True)
task = Task(init=init,
init_holding=init_holding,
goal=goal,
arms=[arm],
required=[cup_name, bowl_name],
reset_arms=True,
use_scales=True,
constraints=constraints)
add_holding(task, ros_world)
feature = get_scoop_feature(ros_world, bowl_name, spoon_name)
return task, feature
def test_stack_pour(visualize):
arms = [LEFT_ARM]
bowl_name = create_name('whitebowl', 1)
cup_name = create_name('bluecup', 1)
purple_name = create_name('purpleblock', 1)
items = [bowl_name, cup_name, purple_name]
world, table_body = create_world(items, visualize=visualize)
cup_x = 0.5
initial_poses = {
bowl_name: ([cup_x, -0.2, 0.0], unit_quat()),
cup_name: ([cup_x, 0.1, 0.0], unit_quat()),
purple_name: ([cup_x, 0.4, 0.0], unit_quat()),
}
with ClientSaver(world.perception.client):
for name, pose in initial_poses.items():
point, quat = pose
point[2] += stable_z(world.perception.sim_bodies[name], table_body) + Z_EPSILON
world.perception.set_pose(name, point, quat)
update_world(world, table_body)
init = [
('Contains', cup_name, COFFEE),
('Stackable', bowl_name, purple_name, TOP),
]
goal = [
('Contains', bowl_name, COFFEE),
('On', bowl_name, purple_name, TOP),
]
task = Task(init=init, goal=goal, arms=arms, graspable=['whitebowl'])
return world, task
def test_holding(visualize, num_blocks=2):
arms = [LEFT_ARM]
#item_type = 'greenblock'
#item_type = 'bowl'
item_type = 'purple_cup'
item_poses = [
([0.6, +0.4, 0.0], z_rotation(np.pi / 2)),
([0.6, -0.4, 0.0], unit_quat()),
]
items = [create_name(item_type, i) for i in range(min(len(item_poses), num_blocks))]
world, table_body = create_world(items, visualize=visualize)
with ClientSaver(world.perception.client):
for name, pose in zip(items, item_poses):
point, quat = pose
point[2] += stable_z(world.perception.sim_bodies[name], table_body) + Z_EPSILON
world.perception.set_pose(name, point, quat)
update_world(world, table_body)
init = [
('Graspable', item) for item in items
]
goal = [
#('Holding', items[0]),
('HoldingType', item_type),
]
task = Task(init=init, goal=goal, arms=arms)
return world, task
def test_push(visualize):
arms = [LEFT_ARM]
block_name = create_name('purpleblock', 1) # greenblock | purpleblock
world, table_body = create_world([block_name], visualize=visualize)
with ClientSaver(world.perception.client):
block_z = stable_z(world.perception.sim_bodies[block_name], table_body) + Z_EPSILON
#pos = (0.6, 0, block_z)
pos = (0.5, 0.2, block_z)
world.perception.set_pose(block_name, pos, quat_from_euler(Euler(yaw=-np.pi/6)))
update_world(world, table_body)
# TODO: push into reachable region
goal_pos2d = np.array(pos[:2]) + np.array([0.025, 0.1])
#goal_pos2d = np.array(pos[:2]) + np.array([0.025, -0.1])
#goal_pos2d = np.array(pos[:2]) + np.array([-0.1, -0.05])
#goal_pos2d = np.array(pos[:2]) + np.array([0.15, -0.05])
#goal_pos2d = np.array(pos[:2]) + np.array([0, 0.7]) # Intentionally not feasible
draw_point(np.append(goal_pos2d, [block_z]), size=0.05, color=(1, 0, 0))
init = [
('CanPush', block_name, goal_pos2d),
]
goal = [
('InRegion', block_name, goal_pos2d),
]
task = Task(init=init, goal=goal, arms=arms)
return world, task
def collect_pour(args, ros_world):
arm, _ = ACTIVE_ARMS['pour']
cup_name, bowl_name = get_pour_items(ros_world)
init = [
('Contains', cup_name, COFFEE),
]
goal = [
('Contains', bowl_name, COFFEE),
]
skeleton = [
('move-arm', [arm, X, X, X]),
('pick', [arm, cup_name, X, X, X, X, X]),
('move-arm', [arm, X, X, X]),
('pour', [arm, bowl_name, X, cup_name, X, COFFEE, X, X, X]),
('move-arm', [arm, X, X, X]),
('place', [arm, cup_name, X, X, X, X, X, X, X, X]),
('move-arm', [arm, X, X, X]),
]
constraints = PlanConstraints(skeletons=[skeleton], exact=True)
# TODO: remove required
# TODO: ensure the robot reaches its start configuration
task = Task(
init=init,
goal=goal,
arms=[arm],
required=[cup_name, bowl_name], # TODO: table?
reset_arms=True,
empty_arms=True,
use_scales=True,
constraints=constraints)
feature = get_pour_feature(ros_world, bowl_name, cup_name)
return task, feature
def test_push_pour(visualize):
arms = ARMS
cup_name = create_name('bluecup', 1)
bowl_name = create_name('bowl', 1)
items = [bowl_name, cup_name, bowl_name]
world, table_body = create_world(items, visualize=visualize)
set_point(table_body, np.array(TABLE_POSE[0]) + np.array([0, -0.1, 0]))
with ClientSaver(world.perception.client):
cup_z = stable_z(world.perception.sim_bodies[cup_name], table_body) + Z_EPSILON
bowl_z = stable_z(world.perception.sim_bodies[bowl_name], table_body) + Z_EPSILON
world.perception.set_pose(cup_name, Point(0.75, 0.4, cup_z), unit_quat())
world.perception.set_pose(bowl_name, Point(0.5, -0.6, bowl_z), unit_quat())
update_world(world, table_body)
# TODO: can prevent the right arm from being able to pick
init = [
('Contains', cup_name, COFFEE),
#('Graspable', bowl_name),
('CanPush', bowl_name, LEFT_ARM), # TODO: intersection of these regions
]
goal = [
('Contains', bowl_name, COFFEE),
('InRegion', bowl_name, LEFT_ARM),
]
task = Task(init=init, goal=goal, arms=arms, pushable=[bowl_name])
# Most of the time places the cup to exchange arms
return world, task
def test_pick():
arms = [LEFT_ARM]
#arms = ARMS
goal_block = create_name('greenblock', 1)
init = []
goal = [
('Grasped', arms[0], goal_block),
]
return Task(init=init, goal=goal, arms=arms,
reset_arms=True)
def test_transfer():
arms = [LEFT_ARM, RIGHT_ARM]
green_name = create_name('greenblock', 1)
init = [
('Stackable', green_name, STOVE_NAME, TOP),
]
goal = [
('On', green_name, STOVE_NAME, TOP),
]
return Task(init=init, goal=goal, arms=arms,
reset_arms=True, empty_arms=True)
def test_pour():
arms = [LEFT_ARM]
cup_name = create_name('bluecup', 1)
bowl_name = create_name('bowl', 1)
init = [
('Contains', cup_name, COFFEE),
]
goal = [
('Contains', bowl_name, COFFEE),
]
return Task(init=init, goal=goal, arms=arms,
reset_arms=True, empty_arms=True)
def test_shelves(visualize):
arms = [LEFT_ARM]
# TODO: smaller (2) shelves
# TODO: sample with respect to the link it will supported by
# shelves.off | shelves_points.off | tableShelves.off
# import os
# data_directory = '/Users/caelan/Programs/LIS/git/lis-data/meshes/'
# mesh = read_mesh_off(os.path.join(data_directory, 'tableShelves.off'))
# draw_mesh(mesh)
# wait_for_interrupt()
start_link = 'shelf2' # shelf1 | shelf2 | shelf3 | shelf4
end_link = 'shelf1'
shelf_name = 'two_shelves'
#shelf_name = 'three_shelves'
cup_name = create_name('bluecup', 1)
world, table_body = create_world([shelf_name, cup_name], visualize=visualize)
cup_x = 0.65
#shelf_x = 0.8
shelf_x = 0.75
initial_poses = {
shelf_name: ([shelf_x, 0.3, 0.0], quat_from_euler(Euler(yaw=-np.pi/2))),
#cup_name: ([cup_x, 0.0, 0.0], unit_quat()),
}
with ClientSaver(world.perception.client):
for name, pose in initial_poses.items():
point, quat = pose
point[2] += stable_z(world.perception.sim_bodies[name], table_body) + Z_EPSILON
world.perception.set_pose(name, point, quat)
shelf_body = world.perception.sim_bodies[shelf_name]
#print([str(get_link_name(shelf_body, link)) for link in get_links(shelf_body)])
#print([str(get_link_name(shelf_body, link)) for link in get_links(world.perception.sim_bodies[cup_name])])
#shelf_link = None
shelf_link = link_from_name(shelf_body, start_link)
cup_z = stable_z(world.perception.sim_bodies[cup_name], shelf_body, surface_link=shelf_link) + Z_EPSILON
world.perception.set_pose(cup_name, [cup_x, 0.1, cup_z], unit_quat())
update_world(world, table_body, camera_point=np.array([-0.5, -1, 1.5]))
init = [
('Stackable', cup_name, shelf_name, end_link),
]
goal = [
('On', cup_name, shelf_name, end_link),
#('On', cup_name, TABLE_NAME, TOP),
#('Holding', cup_name),
]
task = Task(init=init, goal=goal, arms=arms)
return world, task
def test_push():
arms = [LEFT_ARM]
#block_name = get_name('greenblock', 1)
block_name = create_name('purpleblock', 1)
goal_pos2d = np.array([0.6, 0.15])
init = [
('CanPush', block_name, goal_pos2d),
]
goal = [
('InRegion', block_name, goal_pos2d),
]
return Task(init=init, goal=goal, arms=arms,
reset_arms=True, empty_arms=True)
def test_stir(visualize):
# TODO: change the stirrer coordinate frame to be on its side
arms = [LEFT_ARM]
#spoon_name = 'spoon' # spoon.urdf is very messy and has a bad bounding box
#spoon_quat = multiply_quats(quat_from_euler(Euler(pitch=-np.pi/2 - np.pi/16)), quat_from_euler(Euler(yaw=-np.pi/8)))
#spoon_name, spoon_quat = 'stirrer', quat_from_euler(Euler(roll=-np.pi/2, yaw=np.pi/2))
spoon_name, spoon_quat = 'grey_spoon', quat_from_euler(Euler(yaw=-np.pi/2)) # grey_spoon | orange_spoon | green_spoon
# *_spoon points in +y
#bowl_name = 'bowl'
bowl_name = 'whitebowl'
items = [spoon_name, bowl_name]
world, table_body = create_world(items, visualize=visualize)
set_quat(world.get_body(spoon_name), spoon_quat) # get_reference_pose(spoon_name)
initial_positions = {
#spoon_name: [0.5, -0.2, 0],
spoon_name: [0.3, 0.5, 0],
bowl_name: [0.6, 0.1, 0],
}
with ClientSaver(world.perception.client):
for name, point in initial_positions.items():
body = world.perception.sim_bodies[name]
point[2] += stable_z(body, table_body) + Z_EPSILON
world.perception.set_pose(name, point, get_quat(body))
#draw_aabb(get_aabb(body))
#wait_for_interrupt()
#enable_gravity()
#for i in range(10):
# simulate_for_sim_duration(sim_duration=0.05, frequency=0.1)
# print(get_quat(world.perception.sim_bodies[spoon_name]))
# raw_input('Continue?')
update_world(world, table_body)
init_holding = hold_item(world, arms[0], spoon_name)
assert init_holding is not None
# TODO: add the concept of a recipe
init = [
('Contains', bowl_name, COFFEE),
('Contains', bowl_name, SUGAR),
]
goal = [
#('Holding', spoon_name),
('Mixed', bowl_name),
]
task = Task(init=init, init_holding=init_holding, goal=goal,
arms=arms, reset_arms=False)
return world, task
def test_holding():
#arms = [LEFT_ARM, RIGHT_ARM]
arms = [RIGHT_ARM]
item_type = 'greenblock'
#item_type = 'bowl'
item_name = create_name(item_type, 1)
init = [
('Graspable', item_name)
]
goal = [
#('Holding', item_name),
('HoldingType', item_type),
]
return Task(init=init, goal=goal, arms=arms)
def test_pour_two_cups():
#arms = ARMS
arms = [LEFT_ARM]
cup_name = create_name('bluecup', 2) # bluecup2 is on the left with a higher Y value
bowl_name = create_name('bluecup', 1)
init = [
('Contains', cup_name, COFFEE),
]
goal = [
('Contains', bowl_name, COFFEE),
]
return Task(init=init, goal=goal, arms=arms,
reset_arms=True, empty_arms=True)
def test_cook():
arms = ARMS
#arms = [LEFT_ARM]
broccoli_name = create_name('greenblock', 1)
init = [
('IsButton', BUTTON_NAME, STOVE_NAME),
('Stackable', broccoli_name, STOVE_NAME, TOP),
('Stackable', broccoli_name, PLACEMAT_NAME, TOP),
]
goal = [
('On', broccoli_name, PLACEMAT_NAME, TOP),
('Cooked', broccoli_name),
]
return Task(init=init, goal=goal, arms=arms,
use_kitchen=True, reset_arms=True, empty_arms=True)
def test_push_pour():
arms = ARMS
#arms = [LEFT_ARM]
cup_name = create_name('bluecup', 1)
bowl_type = 'bowl'
bowl_name = create_name(bowl_type, 1)
init = [
('Contains', cup_name, COFFEE),
('CanPush', bowl_name, LEFT_ARM),
]
goal = [
('Contains', bowl_name, COFFEE),
('InRegion', bowl_name, LEFT_ARM),
]
return Task(init=init, goal=goal, arms=arms, pushable=[bowl_type],
reset_arms=True, empty_arms=True)
def test_coffee(visualize):
arms = [LEFT_ARM, RIGHT_ARM]
spoon_name = create_name('orange_spoon', 1) # grey_spoon | orange_spoon | green_spoon
coffee_name = create_name('bluecup', 1)
sugar_name = create_name('bowl', 1) # bowl | tan_bowl
bowl_name = create_name('whitebowl', 1)
items = [spoon_name, coffee_name, sugar_name, bowl_name]
world, table_body = create_world(items, visualize=visualize)
initial_positions = {
coffee_name: [0.5, 0.3, 0],
sugar_name: [0.5, -0.3, 0],
bowl_name: [0.5, 0.0, 0],
}
with ClientSaver(world.perception.client):
for name, point in initial_positions.items():
body = world.perception.sim_bodies[name]
point[2] += stable_z(body, table_body) + Z_EPSILON
world.perception.set_pose(name, point, get_quat(body))
update_world(world, table_body)
init_holding = hold_item(world, RIGHT_ARM, spoon_name)
assert init_holding is not None
[grasp] = list(init_holding.values())
#print(grasp.grasp_pose)
#print(grasp.pre_direction)
#print(grasp.grasp_width)
init = [
('Contains', coffee_name, COFFEE),
('Contains', sugar_name, SUGAR),
]
goal = [
#('Contains', spoon_name, SUGAR),
#('Contains', bowl_name, COFFEE),
#('Contains', bowl_name, SUGAR),
('Mixed', bowl_name),
]
task = Task(init=init, init_holding=init_holding, goal=goal, arms=arms,
reset_arms=True, empty_arms=[LEFT_ARM])
return world, task
def test_coffee():
# TODO: ensure you use get_name when running on the pr2
spoon_name = create_name('orange_spoon', 1) # grey_spoon | orange_spoon | green_spoon
coffee_name = create_name('bluecup', 1)
sugar_name = create_name('bowl', 1)
bowl_name = create_name('whitebowl', 1)
init_holding = get_spoon_init_holding(RIGHT_ARM, spoon_name)
init = [
('Contains', coffee_name, COFFEE),
('Contains', sugar_name, SUGAR),
]
goal = [
#('Contains', bowl_name, COFFEE),
#('Contains', bowl_name, SUGAR),
('Mixed', bowl_name),
]
return Task(init=init, init_holding=init_holding, goal=goal, arms=ARMS, # arms=[RIGHT_ARM],
reset_arms=True, empty_arms=[LEFT_ARM])
def test_cup(visualize):
arms = [LEFT_ARM]
cup_name = create_name('greenblock', 1)
block_name = create_name('purpleblock', 1) # greenblock
tray_name = create_name('tray', 1)
world, table_body = create_world([tray_name, cup_name, block_name], visualize=visualize)
#cup_x = 0.6
cup_x = 0.8
block_x = cup_x - 0.15
initial_poses = {
cup_name: ([cup_x, 0.0, 0.0], unit_quat()),
block_name: ([block_x, 0.0, 0.0], unit_quat()), # z_rotation(np.pi/2)
tray_name: ([0.6, 0.4, 0.0], unit_quat()),
}
with ClientSaver(world.perception.client):
for name, pose in initial_poses.items():
point, quat = pose
point[2] += stable_z(world.perception.sim_bodies[name], table_body) + Z_EPSILON
world.perception.set_pose(name, point, quat)
update_world(world, table_body)
#with ClientSaver(world.perception.client):
# draw_pose(get_pose(world.perception.sim_bodies['bluecup']))
# draw_aabb(get_aabb(world.perception.sim_bodies['bluecup']))
# wait_for_interrupt()
init = [
#('Contains', cup_name, WATER),
('Stackable', cup_name, tray_name, TOP),
]
goal = [
#('Grasped', goal_arm, block_name),
#('Grasped', goal_arm, cup_name),
('On', cup_name, tray_name, TOP),
#('On', cup_name, TABLE_NAME, TOP),
#('Empty', goal_arm),
]
task = Task(init=init, goal=goal, arms=arms)
return world, task
def test_stacking(visualize):
# TODO: move & stack
arms = [LEFT_ARM]
#arms = ARMS
cup_name = create_name('bluecup', 1) # bluecup | spoon
green_name = create_name('greenblock', 1)
purple_name = create_name('purpleblock', 1)
items = [cup_name, green_name, purple_name]
world, table_body = create_world(items, visualize=visualize)
cup_x = 0.5
initial_poses = {
cup_name: ([cup_x, -0.2, 0.0], unit_quat()),
#cup_name: ([cup_x, -0.2, 0.3], unit_quat()),
green_name: ([cup_x, 0.1, 0.0], unit_quat()),
#green_name: ([cup_x, 0.1, 0.0], z_rotation(np.pi / 4)), # TODO: be careful about the orientation when stacking
purple_name: ([cup_x, 0.4, 0.0], unit_quat()),
}
with ClientSaver(world.perception.client):
for name, pose in initial_poses.items():
point, quat = pose
point[2] += stable_z(world.perception.sim_bodies[name], table_body) + Z_EPSILON
world.perception.set_pose(name, point, quat)
update_world(world, table_body)
init = [
#('Stackable', cup_name, purple_name, TOP),
('Stackable', cup_name, green_name, TOP),
('Stackable', green_name, purple_name, TOP),
]
goal = [
#('On', cup_name, purple_name, TOP),
('On', cup_name, green_name, TOP),
('On', green_name, purple_name, TOP),
]
task = Task(init=init, goal=goal, arms=arms)
return world, task
def test_stack_pour():
#arms = ARMS
arms = [LEFT_ARM]
purple_name = create_name('purpleblock', 1)
#purple_name = PLACEMAT_NAME
cup_name = create_name('bluecup', 1)
#bowl_type = 'bowl'
bowl_type = 'whitebowl'
bowl_name = create_name(bowl_type, 1)
# TODO: cook & pour
init = [
('Contains', cup_name, COFFEE),
('Stackable', bowl_name, purple_name, TOP),
]
goal = [
('Contains', bowl_name, COFFEE),
('On', bowl_name, purple_name, TOP),
]
return Task(init=init, goal=goal, arms=arms, graspable=[bowl_type],
reset_arms=True, empty_arms=True)
def test_stacking():
arms = [LEFT_ARM]
#arms = [RIGHT_ARM]
#arms = ARMS
cup_name = create_name('bluecup', 1)
#cup_name = get_name('greenblock', 2)
green_name = create_name('greenblock', 1)
#green_name = get_name('bowl', 1)
purple_name = create_name('purpleblock', 1)
init = [
('Graspable', cup_name),
('Graspable', green_name),
('Stackable', cup_name, green_name, TOP),
('Stackable', green_name, purple_name, TOP),
]
goal = [
('On', cup_name, green_name, TOP),
('On', green_name, purple_name, TOP),
]
return Task(init=init, goal=goal, arms=arms,
reset_arms=True, empty_arms=True)
def test_scoop(visualize):
# TODO: start with the spoon in a hand
arms = [LEFT_ARM]
spoon_name = 'grey_spoon' # green_spoon | grey_spoon | orange_spoon
spoon_quat = quat_from_euler(Euler(yaw=-np.pi/2))
# *_spoon points in +y
bowl1_name = 'whitebowl'
bowl2_name = 'bowl'
items = [spoon_name, bowl1_name, bowl2_name]
world, table_body = create_world(items, visualize=visualize)
set_quat(world.get_body(spoon_name), spoon_quat)
initial_positions = {
spoon_name: [0.3, 0.5, 0],
bowl1_name: [0.5, 0.1, 0],
bowl2_name: [0.6, 0.5, 0],
}
with ClientSaver(world.perception.client):
for name, point in initial_positions.items():
body = world.perception.sim_bodies[name]
point[2] += stable_z(body, table_body) + Z_EPSILON
world.perception.set_pose(name, point, get_quat(body))
update_world(world, table_body)
init = [
('Contains', bowl1_name, COFFEE),
#('Contains', spoon_name, WATER),
]
goal = [
#('Contains', spoon_name, WATER),
('Contains', bowl2_name, COFFEE),
]
task = Task(init=init, goal=goal, arms=arms, reset_arms=True)
# TODO: plan while not spilling the spoon
return world, task
def test_cook(visualize):
# TODO: can also disable collision bodies for stove & placemat
arms = [LEFT_ARM]
broccoli_name = 'greenblock'
stove_name = 'stove'
placemat_name = 'placemat'
button_name = 'button'
items = [stove_name, button_name, placemat_name, broccoli_name]
world, table_body = create_world(items, visualize=visualize)
update_world(world, table_body)
initial_poses = {
broccoli_name: ([-0.1, 0.0, 0.0], z_rotation(np.pi/2)),
stove_name: (STOVE_POSITION, unit_quat()),
placemat_name: (PLACEMAT_POSITION, unit_quat()),
button_name: (BUTTON_POSITION, unit_quat()),
}
with ClientSaver(world.perception.client):
for name, local_pose in initial_poses.items():
world_pose = multiply(TABLE_POSE, local_pose)
#z = stable_z(world.perception.sim_bodies[name], table_body) # Slightly above world_pose
world.perception.set_pose(name, *world_pose)
init = [
('IsButton', button_name, stove_name),
('Stackable', broccoli_name, stove_name, TOP),
('Stackable', broccoli_name, placemat_name, TOP),
]
goal = [
('Cooked', broccoli_name),
('On', broccoli_name, placemat_name, TOP),
]
task = Task(init=init, goal=goal, arms=arms)
return world, task
def test_clutter(visualize, num_blocks=5, num_beads=0):
arms = [LEFT_ARM]
cup_name = create_name('bluecup', 1)
bowl_name = create_name('bowl', 1) # bowl | cup_7
clutter = [create_name('greenblock', i) for i in range(num_blocks)]
world, table_body = create_world([cup_name, bowl_name] + clutter, visualize=visualize)
with ClientSaver(world.perception.client):
cup_z = stable_z(world.perception.sim_bodies[cup_name], table_body) + Z_EPSILON
bowl_z = stable_z(world.perception.sim_bodies[bowl_name], table_body) + Z_EPSILON
block_z = None
if 0 < num_blocks:
block_z = stable_z(world.perception.sim_bodies[clutter[0]], table_body)
# TODO(lagrassa): first pose collides with the bowl
xy_poses = [(0.6, -0.1), (0.5, -0.2), (0.6, 0.11), (0.45, 0.12), (0.5, 0.3), (0.7, 0.3)]
world.perception.set_pose(cup_name, Point(0.6, 0.2, cup_z), unit_quat())
world.perception.set_pose(bowl_name, Point(0.6, -0.1, bowl_z), unit_quat())
#if 0 < num_beads:
# world.perception.add_beads(cup_name, num_beads, bead_radius=0.007, bead_mass=0.005)
if block_z is not None:
clutter_poses = [np.append(xy, block_z) for xy in reversed(xy_poses)]
for obj, pose in zip(clutter, clutter_poses):
world.perception.set_pose(obj, pose, unit_quat())
update_world(world, table_body)
#init = [('Graspable', name) for name in [cup_name, bowl_name] + clutter]
init = [
('Contains', cup_name, COFFEE),
]
goal = [
('Contains', bowl_name, COFFEE),
]
task = Task(init=init, goal=goal, arms=arms)
return world, task
def collect_pour(world,
bowl_type=None,
cup_type=None,
randomize=True,
**kwargs):
arm = LEFT_ARM
bowl_type = random.choice(POUR_BOWLS) if bowl_type is None else bowl_type
cup_type = random.choice(POUR_CUPS) if cup_type is None else cup_type
# TODO: could directly randomize base_diameter and top_diameter
scale = int(randomize)
cup_name = create_name(cup_type, 1)
bowl_name = create_name(bowl_type, 1)
item_ranges = {
cup_name:
InitialRanges(
width_range=interval(extent=0.2 *
scale), # the cups are already fairly small
#height_range=(0.9, 1.1),
height_range=(1.0, 1.2), # Difficult to grasp when this shrinks
mass_range=interval(extent=0.2 * scale),
pose2d_range=([0.5, 0.3, -np.pi], [0.5, 0.3,
np.pi]), # x, y, theta
),
bowl_name:
InitialRanges(
width_range=interval(extent=0.4 * scale),
height_range=interval(extent=0.4 * scale),
mass_range=interval(extent=0.4 * scale),
pose2d_range=([0.5, 0.0, -np.pi], [0.5, 0.0,
np.pi]), # x, y, theta
),
}
if 'item_ranges' in kwargs:
for item in kwargs['item_ranges']:
item_ranges[item] = kwargs['item_ranges'][item]
#dump_ranges(POUR_CUPS, item_ranges[cup_name])
#dump_ranges(POUR_BOWLS, item_ranges[bowl_name])
#for name, limits in item_ranges.items():
# lower, upper = aabb_from_points(read_obj(get_body_obj(name)))
# extents = upper - lower
# print(name, 'width', (extents[0]*np.array(limits.width_range)).tolist())
# print(name, 'height', (extents[2]*np.array(limits.height_range)).tolist())
# TODO: check collisions/feasibility when sampling
cup_fraction = random.uniform(0.75, 1.0)
print('Cup fraction:', cup_fraction)
bowl_fraction = random.uniform(1.0, 1.0)
print('Bowl fraction:', bowl_fraction)
bead_radius = sample_norm(mu=0.006, sigma=0.001 * scale, lower=0.004)
print('Bead radius:', bead_radius)
num_beads = 100
with ClientSaver(world.client):
create_table_bodies(world, item_ranges, randomize=randomize)
bowl_body = world.get_body(bowl_name)
cup_body = world.get_body(cup_name)
update_world(world, bowl_body)
if body_pair_collision(cup_body, bowl_body):
# TODO: automatically try all pairs of bodies
return INFEASIBLE
if not has_grasp(world, cup_name):
return INFEASIBLE
# TODO: flatten and only store bowl vs cup
parameters_from_name = randomize_dynamics(world, randomize=randomize)
parameters_from_name['bead'] = sample_bead_parameters(
) # bead restitution is the most important
with LockRenderer():
all_beads = create_beads(num_beads,
bead_radius,
parameters=parameters_from_name['bead'])
bowl_beads = fill_with_beads(world,
bowl_name,
all_beads,
reset_contained=True,
height_fraction=bowl_fraction)
init_beads = fill_with_beads(world,
cup_name,
bowl_beads,
reset_contained=False,
height_fraction=cup_fraction)
#wait_for_user()
init_mass = sum(map(get_mass, init_beads))
print('Init beads: {} | Init mass: {:.3f}'.format(
len(init_beads), init_mass))
if len(init_beads) < MIN_BEADS:
return INFEASIBLE
world.initial_beads.update({bead: cup_body for bead in init_beads})
latent = {
'num_beads': len(init_beads),
'total_mass': init_mass,
'bead_radius': bead_radius,
DYNAMICS: parameters_from_name,
}
init = [('Contains', cup_name, COFFEE)]
goal = [('Contains', bowl_name, COFFEE)]
skeleton = [
('move-arm', [arm, X, X, X]),
('pick', [arm, cup_name, X, X, X, X, X]),
('move-arm', [arm, X, X, X]),
('pour', [arm, bowl_name, X, cup_name, X, COFFEE, X, X, X]),
]
constraints = PlanConstraints(skeletons=[skeleton], exact=True)
task = Task(init=init, goal=goal, arms=[arm],
constraints=constraints) #, init_holding=init_holding)
##################################################
feature = get_pour_feature(world, bowl_name, cup_name)
#feature['ranges'] = POUR_FEATURE_RANGES
initial_pose = world.get_pose(bowl_name)
def score_fn(plan):
assert plan is not None
final_pose = world.get_pose(bowl_name)
point_distance = get_distance(point_from_pose(initial_pose),
point_from_pose(final_pose)) #, norm=2)
quat_distance = quat_angle_between(quat_from_pose(initial_pose),
quat_from_pose(final_pose))
print('Translation: {:.5f} m | Rotation: {:.5f} rads'.format(
point_distance, quat_distance))
with ClientSaver(world.client):
# TODO: lift the bowl up (with particles around) to prevent scale detections
final_bowl_beads = get_contained_beads(bowl_body, init_beads)
fraction_bowl = safe_ratio(len(final_bowl_beads),
len(init_beads),
undefined=0)
mass_in_bowl = sum(map(get_mass, final_bowl_beads))
final_cup_beads = get_contained_beads(cup_body, init_beads)
fraction_cup = safe_ratio(len(final_cup_beads),
len(init_beads),
undefined=0)
mass_in_cup = sum(map(get_mass, final_cup_beads))
print('In Bowl: {} | In Cup: {}'.format(fraction_bowl, fraction_cup))
score = {
# Displacements
'bowl_translation': point_distance,
'bowl_rotation': quat_distance,
# Masses
'mass_in_bowl': mass_in_bowl,
'mass_in_cup': mass_in_cup,
# Counts
'bowl_beads': len(final_bowl_beads),
'cup_beads': len(final_cup_beads),
# Fractions
'fraction_in_bowl': fraction_bowl,
'fraction_in_cup': fraction_cup,
}
score.update(latent)
# TODO: store the cup path length to bias towards shorter paths
#_, args = find_unique(lambda a: a[0] == 'pour', plan)
#control = args[-1]
#feature = control['feature']
#parameter = control['parameter']
return score
return task, feature, score_fn
def collect_stir(world, num_beads=100):
arm = LEFT_ARM
# TODO: randomize geometries for stirrer
spoon_name = create_name(
'grey_spoon', 1) # green_spoon | grey_spoon | orange_spoon | stirrer
bowl_name = create_name('whitebowl', 1)
scale_name = create_name('onyx_scale', 1)
item_ranges = {
spoon_name:
InitialRanges(
width_range=(1., 1.),
height_range=(1., 1.),
mass_range=(1., 1.),
pose2d_range=([0.3, 0.5, -np.pi / 2], [0.3, 0.5, -np.pi / 2]),
surface=TABLE_NAME,
),
bowl_name:
InitialRanges(
width_range=(0.75, 1.25),
height_range=(0.75, 1.25),
mass_range=(1., 1.),
pose2d_range=([0.5, -0.05, -np.pi], [0.6, 0.05,
np.pi]), # x, y, theta
surface=scale_name,
),
}
# TODO: make a sandbox on the table to contain the beads
##################################################
#alpha = 0.75
alpha = 1
bead_colors = [
(1, 0, 0, alpha),
(0, 0, 1, alpha),
]
beads_fraction = random.uniform(0.75, 1.25)
print('Beads fraction:', beads_fraction)
bead_radius = sample_norm(mu=0.006, sigma=0.001, lower=0.004) # 0.007
print('Bead radius:', bead_radius)
# TODO: check collisions/feasibility when sampling
# TODO: grasps on the blue cup seem off for some reason...
with ClientSaver(world.client):
#dump_body(world.robot)
world.perception.add_surface('sandbox', TABLE_POSE)
world.perception.add_surface(scale_name, TABLE_POSE)
create_table_bodies(world, item_ranges)
#gripper = create_gripper(world.robot, LEFT_ARM)
#set_point(gripper, (1, 0, 1))
#wait_for_user()
bowl_body = world.get_body(bowl_name)
update_world(world, target_body=bowl_body)
init_holding = hold_item(world, arm, spoon_name)
if init_holding is None:
return INFEASIBLE
parameters_from_name = randomize_dynamics(
world) # TODO: parameters_from_name['bead']
_, (d, h) = approximate_as_cylinder(bowl_body)
bowl_area = np.pi * (d / 2.)**2
print('Bowl area:', bowl_area)
bead_area = np.pi * bead_radius**2
print('Bead area:', bead_area)
num_beads = int(np.ceil(beads_fraction * bowl_area / bead_area))
print('Num beads:', num_beads)
num_per_color = int(num_beads / len(bead_colors))
# TODO: randomize bead physics
beads_per_color = [
fill_with_beads(
world, bowl_name,
create_beads(num_beads,
bead_radius,
uniform_color=color,
parameters={})) for color in bead_colors
]
world.initial_beads.update(
{bead: bowl_body
for beads in beads_per_color for bead in beads})
if any(len(beads) != num_per_color for beads in beads_per_color):
return INFEASIBLE
#wait_for_user()
init = [('Contains', bowl_name, COFFEE)]
goal = [('Mixed', bowl_name)]
skeleton = [
('move-arm', [arm, X, X, X]),
('stir', [arm, bowl_name, X, spoon_name, X, X, X, X]),
#('move-arm', [arm, X, X, X]),
]
constraints = PlanConstraints(skeletons=[skeleton], exact=True)
task = Task(init=init,
goal=goal,
arms=[arm],
init_holding=init_holding,
reset_arms=False,
constraints=constraints) # Reset arm to clear the scene
# TODO: constrain the plan skeleton within the task
feature = get_stir_feature(world, bowl_name, spoon_name)
##################################################
# table_body = world.get_body(TABLE_NAME)
# dump_body(table_body)
# joint = 0
# p.enableJointForceTorqueSensor(table_body, joint, enableSensor=1, physicsClientId=world.client)
# stabilize(world)
# reaction_force = get_joint_reaction_force(table_body, joint)
# print(np.array(reaction_force[:3])/ GRAVITY)
perception = world.perception
initial_pose = perception.get_pose(bowl_name)
bowl_body = perception.get_body(bowl_name)
scale_body = perception.get_body(scale_name)
with ClientSaver(world.client):
initial_distance = compute_dispersion(bowl_body, beads_per_color)
initial_mass = read_mass(scale_body)
print(initial_mass)
def score_fn(plan):
assert plan is not None
with ClientSaver(world.client):
rgb_image = take_image(world, bowl_body, beads_per_color)
values = score_image(rgb_image, bead_colors, beads_per_color)
final_pose = perception.get_pose(bowl_name)
point_distance = get_distance(point_from_pose(initial_pose),
point_from_pose(final_pose)) #, norm=2)
quat_distance = quat_angle_between(quat_from_pose(initial_pose),
quat_from_pose(final_pose))
print('Translation: {:.5f} m | Rotation: {:.5f} rads'.format(
point_distance, quat_distance))
with ClientSaver(world.client):
all_beads = list(flatten(beads_per_color))
bowl_beads = get_contained_beads(bowl_body, all_beads)
fraction_bowl = float(
len(bowl_beads)) / len(all_beads) if all_beads else 0
print('In Bowl: {}'.format(fraction_bowl))
with ClientSaver(world.client):
final_dispersion = compute_dispersion(bowl_body, beads_per_color)
print('Initial Dispersion: {:.3f} | Final Dispersion {:.3f}'.format(
initial_distance, final_dispersion))
score = {
'bowl_translation': point_distance,
'bowl_rotation': quat_distance,
'fraction_in_bowl': fraction_bowl,
'initial_dispersion': initial_distance,
'final_dispersion': final_dispersion,
'num_beads': len(all_beads), # Beads per color
DYNAMICS: parameters_from_name,
}
# TODO: include time required for stirring
# TODO: change in dispersion
#wait_for_user()
#_, args = find_unique(lambda a: a[0] == 'stir', plan)
#control = args[-1]
return score
return task, feature, score_fn
