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_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_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_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_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_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 main(display='control'): # control | execute | step connect(use_gui=True) disable_real_time() # KUKA_IIWA_URDF | DRAKE_IIWA_URDF robot = load_model(DRAKE_IIWA_URDF, fixed_base=True) # floor = load_model('models/short_floor.urdf') floor = p.loadURDF("plane.urdf") block = load_model( "models/drake/objects/block_for_pick_and_place_heavy.urdf", fixed_base=False) set_pose(block, Pose(Point(y=0.5, z=stable_z(block, floor)))) set_default_camera() dump_world() saved_world = WorldSaver() command = plan(robot, block, fixed=[floor], teleport=False) if (command is None) or (display is None): print('Unable to find a plan!') return saved_world.restore() update_state() user_input('{}?'.format(display)) if display == 'control': enable_gravity() command.control(real_time=False, dt=0) elif display == 'execute': command.refine(num_steps=10).execute(time_step=0.005) elif display == 'step': command.step() else: raise ValueError(display) print('Quit?') wait_for_user() disconnect()
def main(display='execute'): # control | execute | step connect(use_gui=True) disable_real_time() draw_global_system() with HideOutput(): robot = load_model(DRAKE_IIWA_URDF) # KUKA_IIWA_URDF | DRAKE_IIWA_URDF floor = load_model('models/short_floor.urdf') block = load_model(BLOCK_URDF, fixed_base=False) set_pose(block, Pose(Point(y=0.5, z=stable_z(block, floor)))) set_default_camera(distance=2) dump_world() saved_world = WorldSaver() command = plan(robot, block, fixed=[floor], teleport=False) if (command is None) or (display is None): print('Unable to find a plan!') return saved_world.restore() update_state() wait_if_gui('{}?'.format(display)) if display == 'control': enable_gravity() command.control(real_time=False, dt=0) elif display == 'execute': command.refine(num_steps=10).execute(time_step=0.005) elif display == 'step': command.step() else: raise ValueError(display) print('Quit?') wait_if_gui() disconnect()
def main(): # TODO: move to pybullet-planning for now parser = argparse.ArgumentParser() parser.add_argument('-viewer', action='store_true', help='enable the viewer while planning') args = parser.parse_args() print(args) connect(use_gui=True) with LockRenderer(): draw_pose(unit_pose(), length=1) floor = create_floor() with HideOutput(): robot = load_pybullet(get_model_path(ROOMBA_URDF), fixed_base=True, scale=2.0) for link in get_all_links(robot): set_color(robot, link=link, color=ORANGE) robot_z = stable_z(robot, floor) set_point(robot, Point(z=robot_z)) #set_base_conf(robot, rover_confs[i]) data_path = add_data_path() shelf, = load_model(os.path.join(data_path, KIVA_SHELF_SDF), fixed_base=True) # TODO: returns a tuple dump_body(shelf) #draw_aabb(get_aabb(shelf)) wait_for_user() disconnect()
def main(): parser = argparse.ArgumentParser() # Automatically includes help parser.add_argument('-viewer', action='store_true', help='enable viewer.') args = parser.parse_args() connect(use_gui=True) with LockRenderer(): draw_pose(unit_pose(), length=1, width=1) floor = create_floor() set_point(floor, Point(z=-EPSILON)) table1 = create_table(width=TABLE_WIDTH, length=TABLE_WIDTH/2, height=TABLE_WIDTH/2, top_color=TAN, cylinder=False) set_point(table1, Point(y=+0.5)) table2 = create_table(width=TABLE_WIDTH, length=TABLE_WIDTH/2, height=TABLE_WIDTH/2, top_color=TAN, cylinder=False) set_point(table2, Point(y=-0.5)) tables = [table1, table2] #set_euler(table1, Euler(yaw=np.pi/2)) with HideOutput(): # data_path = add_data_path() # robot_path = os.path.join(data_path, WSG_GRIPPER) robot_path = get_model_path(WSG_50_URDF) # WSG_50_URDF | PANDA_HAND_URDF robot = load_pybullet(robot_path, fixed_base=True) #dump_body(robot) block1 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=RED) block_z = stable_z(block1, table1) set_point(block1, Point(y=-0.5, z=block_z)) block2 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=GREEN) set_point(block2, Point(x=-0.25, y=-0.5, z=block_z)) block3 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=BLUE) set_point(block3, Point(x=-0.15, y=+0.5, z=block_z)) blocks = [block1, block2, block3] set_camera_pose(camera_point=Point(x=-1, z=block_z+1), target_point=Point(z=block_z)) block_pose = get_pose(block1) open_gripper(robot) tool_link = link_from_name(robot, 'tool_link') base_from_tool = get_relative_pose(robot, tool_link) #draw_pose(unit_pose(), parent=robot, parent_link=tool_link) grasps = get_side_grasps(block1, tool_pose=Pose(euler=Euler(yaw=np.pi/2)), top_offset=0.02, grasp_length=0.03, under=False)[1:2] for grasp in grasps: gripper_pose = multiply(block_pose, invert(grasp)) set_pose(robot, multiply(gripper_pose, invert(base_from_tool))) wait_for_user() wait_for_user('Finish?') disconnect()
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_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 problem1(n_obstacles=10, wall_side=0.1, obst_width=0.25, obst_height=0.5): floor_extent = 5.0 base_limits = (-floor_extent/2.*np.ones(2), +floor_extent/2.*np.ones(2)) floor_height = 0.001 floor = create_box(floor_extent, floor_extent, floor_height, color=TAN) set_point(floor, Point(z=-floor_height/2.)) wall1 = create_box(floor_extent + wall_side, wall_side, wall_side, color=GREY) set_point(wall1, Point(y=floor_extent/2., z=wall_side/2.)) wall2 = create_box(floor_extent + wall_side, wall_side, wall_side, color=GREY) set_point(wall2, Point(y=-floor_extent/2., z=wall_side/2.)) wall3 = create_box(wall_side, floor_extent + wall_side, wall_side, color=GREY) set_point(wall3, Point(x=floor_extent/2., z=wall_side/2.)) wall4 = create_box(wall_side, floor_extent + wall_side, wall_side, color=GREY) set_point(wall4, Point(x=-floor_extent/2., z=wall_side/2.)) walls = [wall1, wall2, wall3, wall4] initial_surfaces = OrderedDict() for _ in range(n_obstacles): body = create_box(obst_width, obst_width, obst_height, color=GREY) initial_surfaces[body] = floor obstacles = walls + list(initial_surfaces) initial_conf = np.array([+floor_extent/3, -floor_extent/3, 3*PI/4]) goal_conf = -initial_conf with HideOutput(): robot = load_model(TURTLEBOT_URDF) base_joints = joints_from_names(robot, BASE_JOINTS) # base_link = child_link_from_joint(base_joints[-1]) base_link = link_from_name(robot, BASE_LINK_NAME) set_all_color(robot, BLUE) dump_body(robot) set_point(robot, Point(z=stable_z(robot, floor))) draw_pose(Pose(), parent=robot, parent_link=base_link, length=0.5) set_joint_positions(robot, base_joints, initial_conf) sample_placements(initial_surfaces, obstacles=[robot] + walls, savers=[BodySaver(robot, joints=base_joints, positions=goal_conf)], min_distances=10e-2) return robot, base_limits, goal_conf, obstacles
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_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 main(display='execute'): # control | execute | step connect(use_gui=True) disable_real_time() with HideOutput(): root_directory = os.path.dirname(os.path.abspath(__file__)) robot = load_pybullet(os.path.join(root_directory, IRB6600_TRACK_URDF), fixed_base=True) floor = load_model('models/short_floor.urdf') block = load_model(BLOCK_URDF, fixed_base=False) floor_x = 2 set_pose(floor, Pose(Point(x=floor_x, z=0.5))) set_pose(block, Pose(Point(x=floor_x, y=0, z=stable_z(block, floor)))) # set_default_camera() dump_world() saved_world = WorldSaver() with LockRenderer(): command = plan(robot, block, fixed=[floor], teleport=False) if (command is None) or (display is None): print('Unable to find a plan!') print('Quit?') wait_for_interrupt() disconnect() return saved_world.restore() update_state() user_input('{}?'.format(display)) if display == 'control': enable_gravity() command.control(real_time=False, dt=0) elif display == 'execute': command.refine(num_steps=10).execute(time_step=0.002) elif display == 'step': command.step() else: raise ValueError(display) print('Quit?') wait_for_interrupt() disconnect()
def load_world(): #print(get_data_path()) #p.loadURDF("samurai.urdf", useFixedBase=True) # World #p.loadURDF("kuka_lwr/kuka.urdf", useFixedBase=True) #p.loadURDF("kuka_iiwa/model_free_base.urdf", useFixedBase=True) # TODO: store internal world info here to be reloaded robot = load_model(DRAKE_IIWA_URDF) # robot = load_model(KUKA_IIWA_URDF) floor = load_model('models/short_floor.urdf') sink = load_model(SINK_URDF, pose=Pose(Point(x=-0.5))) stove = load_model(STOVE_URDF, pose=Pose(Point(x=+0.5))) block = load_model(BLOCK_URDF, fixed_base=False) #cup = load_model('models/dinnerware/cup/cup_small.urdf', # Pose(Point(x=+0.5, y=+0.5, z=0.5)), fixed_base=False) set_pose(block, Pose(Point(y=0.5, z=stable_z(block, floor)))) # print(get_camera()) set_default_camera() return robot, block
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 create_table_bodies(world, item_ranges, randomize=True): perception = world.perception with HideOutput(): add_data_path() floor_body = load_pybullet("plane.urdf") set_pose(floor_body, get_pose(world.robot)) add_table(world) for name, limits in sorted(item_ranges.items()): perception.sim_items[name] = None if randomize: body = randomize_body(name, width_range=limits.width_range, height_range=limits.height_range, mass_range=limits.mass_range) else: body = load_body(name) perception.sim_bodies[name] = body # perception.add_item(name, unit_pose()) x, y, yaw = np.random.uniform(*limits.pose2d_range) surface_body = perception.get_body(limits.surface) z = stable_z(body, surface_body) + Z_EPSILON pose = Pose((x, y, z), Euler(yaw=yaw)) perception.set_pose(name, *pose)
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 __init__(self, robot_name=FRANKA_CARTER, use_gui=True, full_kitchen=False): self.task = None self.interface = None self.client = connect(use_gui=use_gui) set_real_time(False) #set_caching(False) # Seems to make things worse disable_gravity() add_data_path() set_camera_pose(camera_point=[2, -1.5, 1]) if DEBUG: draw_pose(Pose(), length=3) #self.kitchen_yaml = load_yaml(KITCHEN_YAML) with HideOutput(enable=True): self.kitchen = load_pybullet(KITCHEN_PATH, fixed_base=True, cylinder=True) self.floor = load_pybullet('plane.urdf', fixed_base=True) z = stable_z(self.kitchen, self.floor) - get_point(self.floor)[2] point = np.array(get_point(self.kitchen)) - np.array([0, 0, z]) set_point(self.floor, point) self.robot_name = robot_name if self.robot_name == FRANKA_CARTER: urdf_path, yaml_path = FRANKA_CARTER_PATH, None #urdf_path, yaml_path = FRANKA_CARTER_PATH, FRANKA_YAML #elif self.robot_name == EVE: # urdf_path, yaml_path = EVE_PATH, None else: raise ValueError(self.robot_name) self.robot_yaml = yaml_path if yaml_path is None else load_yaml( yaml_path) with HideOutput(enable=True): self.robot = load_pybullet(urdf_path) #dump_body(self.robot) #chassis_pose = get_link_pose(self.robot, link_from_name(self.robot, 'chassis_link')) #wheel_pose = get_link_pose(self.robot, link_from_name(self.robot, 'left_wheel_link')) #wait_for_user() set_point(self.robot, Point(z=stable_z(self.robot, self.floor))) self.set_initial_conf() self.gripper = create_gripper(self.robot) self.environment_bodies = {} if full_kitchen: self._initialize_environment() self._initialize_ik(urdf_path) self.initial_saver = WorldSaver() self.body_from_name = {} # self.path_from_name = {} self.names_from_type = {} self.custom_limits = {} self.base_limits_handles = [] self.cameras = {} self.disabled_collisions = set() if self.robot_name == FRANKA_CARTER: self.disabled_collisions.update( tuple(link_from_name(self.robot, link) for link in pair) for pair in DISABLED_FRANKA_COLLISIONS) self.carry_conf = FConf(self.robot, self.arm_joints, self.default_conf) #self.calibrate_conf = Conf(self.robot, self.arm_joints, load_calibrate_conf(side='left')) self.calibrate_conf = FConf( self.robot, self.arm_joints, self.default_conf) # Must differ from carry_conf self.special_confs = [self.carry_conf] #, self.calibrate_conf] self.open_gq = FConf(self.robot, self.gripper_joints, get_max_limits(self.robot, self.gripper_joints)) self.closed_gq = FConf(self.robot, self.gripper_joints, get_min_limits(self.robot, self.gripper_joints)) self.gripper_confs = [self.open_gq, self.closed_gq] self.open_kitchen_confs = { joint: FConf(self.kitchen, [joint], [self.open_conf(joint)]) for joint in self.kitchen_joints } self.closed_kitchen_confs = { joint: FConf(self.kitchen, [joint], [self.closed_conf(joint)]) for joint in self.kitchen_joints } self._update_custom_limits() self._update_initial()
def main(floor_width=2.0): # Creates a pybullet world and a visualizer for it connect(use_gui=True) identity_pose = (unit_point(), unit_quat()) origin_handles = draw_pose( identity_pose, length=1.0 ) # Draws the origin coordinate system (x:RED, y:GREEN, z:BLUE) # Bodies are described by an integer index floor = create_box(w=floor_width, l=floor_width, h=0.001, color=TAN) # Creates a tan box object for the floor set_point(floor, [0, 0, -0.001 / 2.]) # Sets the [x,y,z] translation of the floor obstacle = create_box(w=0.5, l=0.5, h=0.1, color=RED) # Creates a red box obstacle set_point( obstacle, [0.5, 0.5, 0.1 / 2.]) # Sets the [x,y,z] position of the obstacle print('Position:', get_point(obstacle)) set_euler(obstacle, [0, 0, np.pi / 4 ]) # Sets the [roll,pitch,yaw] orientation of the obstacle print('Orientation:', get_euler(obstacle)) with LockRenderer( ): # Temporarily prevents the renderer from updating for improved loading efficiency with HideOutput(): # Temporarily suppresses pybullet output robot = load_model(ROOMBA_URDF) # Loads a robot from a *.urdf file robot_z = stable_z( robot, floor ) # Returns the z offset required for robot to be placed on floor set_point(robot, [0, 0, robot_z]) # Sets the z position of the robot dump_body(robot) # Prints joint and link information about robot set_all_static() # Joints are also described by an integer index # The turtlebot has explicit joints representing x, y, theta x_joint = joint_from_name(robot, 'x') # Looks up the robot joint named 'x' y_joint = joint_from_name(robot, 'y') # Looks up the robot joint named 'y' theta_joint = joint_from_name( robot, 'theta') # Looks up the robot joint named 'theta' joints = [x_joint, y_joint, theta_joint] base_link = link_from_name( robot, 'base_link') # Looks up the robot link named 'base_link' world_from_obstacle = get_pose( obstacle ) # Returns the pose of the origin of obstacle wrt the world frame obstacle_aabb = get_subtree_aabb(obstacle) draw_aabb(obstacle_aabb) random.seed(0) # Sets the random number generator state handles = [] for i in range(10): for handle in handles: remove_debug(handle) print('\nIteration: {}'.format(i)) x = random.uniform(-floor_width / 2., floor_width / 2.) set_joint_position(robot, x_joint, x) # Sets the current value of the x joint y = random.uniform(-floor_width / 2., floor_width / 2.) set_joint_position(robot, y_joint, y) # Sets the current value of the y joint yaw = random.uniform(-np.pi, np.pi) set_joint_position(robot, theta_joint, yaw) # Sets the current value of the theta joint values = get_joint_positions( robot, joints) # Obtains the current values for the specified joints print('Joint values: [x={:.3f}, y={:.3f}, yaw={:.3f}]'.format(*values)) world_from_robot = get_link_pose( robot, base_link) # Returns the pose of base_link wrt the world frame position, quaternion = world_from_robot # Decomposing pose into position and orientation (quaternion) x, y, z = position # Decomposing position into x, y, z print('Base link position: [x={:.3f}, y={:.3f}, z={:.3f}]'.format( x, y, z)) euler = euler_from_quat( quaternion) # Converting from quaternion to euler angles roll, pitch, yaw = euler # Decomposing orientation into roll, pitch, yaw print('Base link orientation: [roll={:.3f}, pitch={:.3f}, yaw={:.3f}]'. format(roll, pitch, yaw)) handles.extend( draw_pose(world_from_robot, length=0.5) ) # # Draws the base coordinate system (x:RED, y:GREEN, z:BLUE) obstacle_from_robot = multiply( invert(world_from_obstacle), world_from_robot) # Relative transformation from robot to obstacle robot_aabb = get_subtree_aabb( robot, base_link) # Computes the robot's axis-aligned bounding box (AABB) lower, upper = robot_aabb # Decomposing the AABB into the lower and upper extrema center = (lower + upper) / 2. # Computing the center of the AABB extent = upper - lower # Computing the dimensions of the AABB handles.extend(draw_aabb(robot_aabb)) collision = pairwise_collision( robot, obstacle ) # Checks whether robot is currently colliding with obstacle print('Collision: {}'.format(collision)) wait_for_duration(1.0) # Like sleep() but also updates the viewer wait_for_user() # Like raw_input() but also updates the viewer # Destroys the pybullet world disconnect()
def main(): parser = argparse.ArgumentParser() # Automatically includes help parser.add_argument('-viewer', action='store_true', help='enable viewer.') args = parser.parse_args() connect(use_gui=True) #ycb_path = os.path.join(DRAKE_PATH, DRAKE_YCB) #ycb_path = os.path.join(YCB_PATH, YCB_TEMPLATE.format('003_cracker_box')) #print(ycb_path) #load_pybullet(ycb_path) with LockRenderer(): draw_pose(unit_pose(), length=1, width=1) floor = create_floor() set_point(floor, Point(z=-EPSILON)) table = create_table(width=TABLE_WIDTH, length=TABLE_WIDTH / 2, height=TABLE_WIDTH / 2, top_color=TAN, cylinder=False) #set_euler(table, Euler(yaw=np.pi/2)) with HideOutput(False): # data_path = add_data_path() # robot_path = os.path.join(data_path, WSG_GRIPPER) robot_path = get_model_path( WSG_50_URDF) # WSG_50_URDF | PANDA_HAND_URDF #robot_path = get_file_path(__file__, 'mit_arch_suction_gripper/mit_arch_suction_gripper.urdf') robot = load_pybullet(robot_path, fixed_base=True) #dump_body(robot) #robot = create_cylinder(radius=0.5*BLOCK_SIDE, height=4*BLOCK_SIDE) # vacuum gripper block1 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=RED) block_z = stable_z(block1, table) set_point(block1, Point(z=block_z)) block2 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=GREEN) set_point(block2, Point(x=+0.25, z=block_z)) block3 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=BLUE) set_point(block3, Point(x=-0.15, z=block_z)) blocks = [block1, block2, block3] add_line(Point(x=-TABLE_WIDTH / 2, z=block_z - BLOCK_SIDE / 2 + EPSILON), Point(x=+TABLE_WIDTH / 2, z=block_z - BLOCK_SIDE / 2 + EPSILON), color=RED) set_camera_pose(camera_point=Point(y=-1, z=block_z + 1), target_point=Point(z=block_z)) wait_for_user() block_pose = get_pose(block1) open_gripper(robot) tool_link = link_from_name(robot, 'tool_link') base_from_tool = get_relative_pose(robot, tool_link) #draw_pose(unit_pose(), parent=robot, parent_link=tool_link) y_grasp, x_grasp = get_top_grasps(block1, tool_pose=unit_pose(), grasp_length=0.03, under=False) grasp = y_grasp # fingers won't collide gripper_pose = multiply(block_pose, invert(grasp)) set_pose(robot, multiply(gripper_pose, invert(base_from_tool))) wait_for_user('Finish?') disconnect()
def problem1(n_obstacles=10, wall_side=0.1, obst_width=0.25, obst_height=0.5): floor_extent = 5.0 base_limits = (-floor_extent / 2. * np.ones(2), +floor_extent / 2. * np.ones(2)) floor_height = 0.001 floor = create_box(floor_extent, floor_extent, floor_height, color=TAN) set_point(floor, Point(z=-floor_height / 2.)) wall1 = create_box(floor_extent + wall_side, wall_side, wall_side, color=GREY) set_point(wall1, Point(y=floor_extent / 2., z=wall_side / 2.)) wall2 = create_box(floor_extent + wall_side, wall_side, wall_side, color=GREY) set_point(wall2, Point(y=-floor_extent / 2., z=wall_side / 2.)) wall3 = create_box(wall_side, floor_extent + wall_side, wall_side, color=GREY) set_point(wall3, Point(x=floor_extent / 2., z=wall_side / 2.)) wall4 = create_box(wall_side, floor_extent + wall_side, wall_side, color=GREY) set_point(wall4, Point(x=-floor_extent / 2., z=wall_side / 2.)) wall5 = create_box(obst_width, obst_width, obst_height, color=GREY) set_point(wall5, Point(z=obst_height / 2.)) walls = [wall1, wall2, wall3, wall4, wall5] initial_surfaces = OrderedDict() for _ in range(n_obstacles - 1): body = create_box(obst_width, obst_width, obst_height, color=GREY) initial_surfaces[body] = floor pillars = list(initial_surfaces) obstacles = walls + pillars initial_conf = np.array([+floor_extent / 3, -floor_extent / 3, 3 * PI / 4]) goal_conf = -initial_conf robot = load_pybullet(TURTLEBOT_URDF, fixed_base=True, merge=True, sat=False) base_joints = joints_from_names(robot, BASE_JOINTS) # base_link = child_link_from_joint(base_joints[-1]) base_link = link_from_name(robot, BASE_LINK_NAME) set_all_color(robot, BLUE) dump_body(robot) set_point(robot, Point(z=stable_z(robot, floor))) #set_point(robot, Point(z=base_aligned_z(robot))) draw_pose(Pose(), parent=robot, parent_link=base_link, length=0.5) set_joint_positions(robot, base_joints, initial_conf) sample_placements( initial_surfaces, obstacles=[robot] + walls, savers=[BodySaver(robot, joints=base_joints, positions=goal_conf)], min_distances=10e-2) # The first calls appear to be the slowest # times = [] # for body1, body2 in combinations(pillars, r=2): # start_time = time.time() # colliding = pairwise_collision(body1, body2) # runtime = elapsed_time(start_time) # print(colliding, runtime) # times.append(runtime) # print(times) return robot, base_limits, goal_conf, obstacles