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 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_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_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 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_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 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_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 add_scales(task, ros_world):
scale_stackings = {}
holding = {grasp.obj_name for grasp in task.init_holding.values()}
with ClientSaver(ros_world.client):
perception = ros_world.perception
items = sorted(set(perception.get_items()) - holding,
key=lambda n: get_point(ros_world.get_body(n))[1],
reverse=False) # Right to left
for i, item in enumerate(items):
if not POUR and (get_type(item) != SCOOP_CUP):
continue
item_body = ros_world.get_body(item)
scale = create_name(SCALE_TYPE, i + 1)
with HideOutput():
scale_body = load_pybullet(get_body_urdf(get_type(scale)),
fixed_base=True)
ros_world.perception.sim_bodies[scale] = scale_body
ros_world.perception.sim_items[scale] = None
item_z = stable_z(item_body, scale_body)
scale_pose_item = Pose(
point=Point(z=-item_z)) # TODO: relies on origin in base
set_pose(scale_body, multiply(get_pose(item_body),
scale_pose_item))
roll, pitch, _ = get_euler(scale_body)
set_euler(scale_body, [roll, pitch, 0])
scale_stackings[scale] = item
#wait_for_user()
return scale_stackings
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_kitchen():
arms = [LEFT_ARM]
broccoli_name = create_name('greenblock', 1)
cup_name = create_name('bluecup', 1)
init = [
('IsButton', BUTTON_NAME, STOVE_NAME),
('Stackable', broccoli_name, STOVE_NAME, TOP),
('Stackable', broccoli_name, PLACEMAT_NAME, TOP),
('Stackable', cup_name, PLACEMAT_NAME, TOP),
('Contains', cup_name, COFFEE),
]
goal = [
('On', cup_name, PLACEMAT_NAME, TOP),
('On', broccoli_name, PLACEMAT_NAME, TOP),
('Cooked', broccoli_name),
('Contains', cup_name, COFFEE),
]
return Task(init=init, goal=goal, arms=arms,
use_kitchen=True, reset_arms=True, empty_arms=True)
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_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_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_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_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_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_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_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_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 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 add_walls(ros_world):
thickness = 0.01 # 0.005 | 0.01 | 0.02
height = 0.11 # 0.11 | 0.12
[table_name] = ros_world.perception.get_surfaces()
#table_body = ros_world.get_body(table_name)
table_info = ros_world.perception.info_from_name[table_name]
#draw_pose(table_info.pose, length=1)
with ros_world:
#aabb = aabb_from_points(apply_affine(table_info.pose, table_info.type.vertices))
aabb = aabb_from_points(table_info.type.vertices)
draw_aabb(aabb)
# pose = get_pose(table_body)
# pose = ros_world.get_pose(table_name)
# aabb = approximate_as_prism(table_body, pose)
x, y, z = get_aabb_center(aabb)
l, w, h = get_aabb_extent(aabb)
#right_wall = create_box(l, thickness, height)
#set_pose(right_wall, multiply(table_info.pose, (Pose(point=[x, y - (w + thickness) / 2., z + (h + height) / 2.]))))
#bottom_wall = create_box(thickness, w / 2., height)
#set_point(bottom_wall, [x - (l + thickness) / 2., y - w / 4., z + (h + height) / 2.])
top_wall = create_box(thickness, w, height)
set_pose(
top_wall,
multiply(table_info.pose, (Pose(
point=[x + (l + thickness) / 2., y, z + (h + height) / 2.]))))
walls = [top_wall]
#walls = [right_wall, top_wall] # , bottom_wall]
for i, body in enumerate(walls):
name = create_name('wall', i + 1)
ros_world.perception.sim_bodies[name] = body
ros_world.perception.sim_items[name] = None
#wait_for_user()
return walls # Return names?
from perception_tools.common import create_name
from plan_tools.common import ARMS, LEFT_ARM, RIGHT_ARM, TABLE, TOP, COFFEE, SUGAR, COLORS
from plan_tools.planner import Task
from plan_tools.ros_world import ROSWorld
from plan_tools.samplers.grasp import hold_item
from pybullet_tools.pr2_utils import inverse_visibility, set_group_conf, arm_from_arm, \
WIDE_LEFT_ARM, rightarm_from_leftarm, open_arm
from pybullet_tools.pr2_problems import create_floor
from pybullet_tools.utils import ClientSaver, get_point, unit_quat, unit_pose, link_from_name, draw_point, \
HideOutput, stable_z, quat_from_euler, Euler, set_camera_pose, z_rotation, multiply, \
get_quat, create_mesh, set_point, set_quat, set_pose, wait_for_user, \
get_visual_data, read_obj, BASE_LINK, \
approximate_as_prism, Pose, draw_mesh, rectangular_mesh, apply_alpha, Point
TABLE_NAME = create_name(TABLE, 1)
# TODO: randomly sample from interval
TABLE_POSE = ([0.55, 0.0, 0.735], unit_quat())
Z_EPSILON = 1e-3
STOVE_POSITION = np.array([0.0, 0.4, 0.0])
PLACEMAT_POSITION = np.array([0.1, 0.0, 0.0])
BUTTON_POSITION = STOVE_POSITION - np.array([0.15, 0.0, 0.0])
TORSO_POSITION = 0.325 # TODO: maintain average distance betweeen pr2 and table
CAMERA_OPTICAL_FRAME = 'head_mount_kinect_rgb_optical_frame'
CAMERA_FRAME = 'head_mount_kinect_rgb_link'
#CAMERA_FRAME = 'high_def_frame'
#CAMERA_OPTICAL_FRAME = 'high_def_optical_frame'
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
return task, feature
##################################################
POUR = False
PROBLEMS = [
collect_pour,
collect_scoop,
collect_push,
]
# TODO: front USB ports are unresponsive
SCALE_FROM_BUS = {
ARIADNE_BACK_USB: create_name(SCALE_TYPE, 1), # Right scale
}
if POUR:
SCALE_FROM_BUS.update({
ARIADNE_FRONT_USB: create_name(SCALE_TYPE, 2), # Left scale
})
def read_raw_masses(stackings):
if not stackings:
return {}
weights = {
SCALE_FROM_BUS[bus]: weight
for bus, weight in read_scales().items()
}
print('Raw weights (oz):', str_from_object(weights))
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 eval_task_with_seed(domain,
seed,
learner,
sample_strategy=DIVERSELK,
task_lengthscale=None,
obstacle=True):
if task_lengthscale is not None:
learner.task_lengthscale = task_lengthscale
print('sample a new task')
current_wd, trial_wd = start_task()
### fill in additional object
item_ranges = {}
###
sim_world, collector, task, feature, evalfunc, saver = sample_task_with_seed(
domain.skill, seed=seed, visualize=False, item_ranges=item_ranges)
context = domain.context_from_feature(feature)
print('context=', *context)
# create coffee machine
if obstacle:
bowl_name = 'bowl'
cup_name = 'cup'
if domain.skill == 'scoop':
cup_name = 'spoon'
for key in sim_world.perception.sim_bodies:
if bowl_name in key:
bowl_name = key
if cup_name in key:
cup_name = key
cylinder_size = (.01, .03)
dim_bowl = get_aabb_extent(
p.getAABB(sim_world.perception.sim_bodies[bowl_name]))
dim_cup = get_aabb_extent(
p.getAABB(sim_world.perception.sim_bodies[cup_name]))
bowl_point = get_point(sim_world.perception.sim_bodies[bowl_name])
bowl_point = np.array(bowl_point)
cylinder_point = bowl_point.copy()
cylinder_offset = 1.2
if domain.skill == 'scoop':
cylinder_offset = 1.6
cylinder_point[2] += (dim_bowl[2] + dim_cup[2]) * (np.random.random_sample() * .4 + cylinder_offset) \
+ cylinder_size[1] / 2.
# TODO figure out the task space
cylinder_name = create_name('cylinder', 1)
obstacle = create_cylinder(*cylinder_size, color=(0, 0, 0, 1))
INFO_FROM_BODY[(CLIENT, obstacle)] = ModelInfo(cylinder_name,
cylinder_size, 1, 1)
sim_world.perception.sim_bodies[cylinder_name] = obstacle
sim_world.perception.sim_items[cylinder_name] = None
set_pose(obstacle, (cylinder_point, unit_quat()))
box_name = create_name('box', 1)
box1_size = (max(.17,
dim_bowl[0] / 2 + cylinder_size[0] * 2), 0.01, 0.01)
if domain.skill == 'scoop':
box1_size = (max(.23,
dim_bowl[0] / 2 + cylinder_size[0] * 2 + 0.05),
0.01, 0.01)
obstacle = create_box(*box1_size)
INFO_FROM_BODY[(CLIENT, obstacle)] = ModelInfo(box_name, box1_size, 1,
1)
sim_world.perception.sim_bodies[box_name] = obstacle
sim_world.perception.sim_items[box_name] = None
box1_point = cylinder_point.copy()
box1_point[2] += cylinder_size[1] / 2 + box1_size[2] / 2
box1_point[0] += box1_size[0] / 2 - cylinder_size[0] * 2
set_pose(obstacle, (box1_point, unit_quat()))
box_name = create_name('box', 2)
box2_size = (0.01, .01,
box1_point[2] - bowl_point[2] + box1_size[2] / 2)
obstacle = create_box(*box2_size)
INFO_FROM_BODY[(CLIENT, obstacle)] = ModelInfo(box_name, box2_size, 1,
1)
sim_world.perception.sim_bodies[box_name] = obstacle
sim_world.perception.sim_items[box_name] = None
box2_point = bowl_point.copy()
box2_point[2] += box2_size[2] / 2
box2_point[0] = box1_point[0] + box1_size[0] / 2
set_pose(obstacle, (box2_point, unit_quat()))
result = get_sample_strategy_result(sample_strategy, learner, context,
sim_world, collector, task, feature,
evalfunc, saver)
print('context=', *context)
complete_task(sim_world, current_wd, trial_wd)
return result
