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