def create_elements(node_points, elements, radius=0.0005, color=(1, 0, 0, 1)): # TODO: just shrink the structure to prevent worrying about collisions at end-points #radius = 0.0001 #radius = 0.00005 #radius = 0.000001 radius = 1e-6 # TODO: seems to be a min radius shrink = 0.01 #shrink = 0. element_bodies = [] for (n1, n2) in elements: p1, p2 = node_points[n1], node_points[n2] height = max(np.linalg.norm(p2 - p1) - 2*shrink, 0) #if height == 0: # Cannot keep this here # continue center = (p1 + p2) / 2 # extents = (p2 - p1) / 2 body = create_cylinder(radius, height, color=color) set_point(body, center) element_bodies.append(body) delta = p2 - p1 x, y, z = delta phi = np.math.atan2(y, x) theta = np.math.acos(z / np.linalg.norm(delta)) set_quat(body, quat_from_euler(Euler(pitch=theta, yaw=phi))) # p1 is z=-height/2, p2 is z=+height/2 return element_bodies
def create_environment(base_extent, mound_height): # TODO: reuse this instead of making a new method floor = create_box(base_extent, base_extent, 0.001, color=TAN) # TODO: two rooms set_point(floor, Point(z=-0.001 / 2.)) #return floor, [] wall1 = create_box(base_extent + mound_height, mound_height, mound_height, color=GREY) set_point(wall1, Point(y=base_extent / 2., z=mound_height / 2.)) wall2 = create_box(base_extent + mound_height, mound_height, mound_height, color=GREY) set_point(wall2, Point(y=-base_extent / 2., z=mound_height / 2.)) wall3 = create_box(mound_height, base_extent + mound_height, mound_height, color=GREY) set_point(wall3, Point(x=base_extent / 2., z=mound_height / 2.)) wall4 = create_box(mound_height, base_extent + mound_height, mound_height, color=GREY) set_point(wall4, Point(x=-base_extent / 2., z=mound_height / 2.)) return floor, [wall1, wall2, wall3, wall4]
def load_world(): root_directory = os.path.dirname(os.path.abspath(__file__)) with HideOutput(): floor = load_model('models/short_floor.urdf') robot = load_pybullet(os.path.join(root_directory, KUKA_PATH), fixed_base=True) set_point(floor, Point(z=-0.01)) return floor, robot
def main(): 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 packed(arm='left', grasp_type='top', num=5): # TODO: packing problem where you have to place in one direction base_extent = 5.0 base_limits = (-base_extent / 2. * np.ones(2), base_extent / 2. * np.ones(2)) block_width = 0.07 block_height = 0.1 #block_height = 2*block_width block_area = block_width * block_width #plate_width = 2*math.sqrt(num*block_area) plate_width = 0.27 #plate_width = 0.28 #plate_width = 0.3 print('Width:', plate_width) plate_width = min(plate_width, 0.6) plate_height = 0.001 other_arm = get_other_arm(arm) initial_conf = get_carry_conf(arm, grasp_type) add_data_path() floor = load_pybullet("plane.urdf") pr2 = create_pr2() set_arm_conf(pr2, arm, initial_conf) open_arm(pr2, arm) set_arm_conf(pr2, other_arm, arm_conf(other_arm, REST_LEFT_ARM)) close_arm(pr2, other_arm) set_group_conf(pr2, 'base', [-1.0, 0, 0]) # Be careful to not set the pr2's pose table = create_table() plate = create_box(plate_width, plate_width, plate_height, color=GREEN) plate_z = stable_z(plate, table) set_point(plate, Point(z=plate_z)) surfaces = [table, plate] blocks = [ create_box(block_width, block_width, block_height, color=BLUE) for _ in range(num) ] initial_surfaces = {block: table for block in blocks} min_distances = {block: 0.05 for block in blocks} sample_placements(initial_surfaces, min_distances=min_distances) return Problem( robot=pr2, movable=blocks, arms=[arm], grasp_types=[grasp_type], surfaces=surfaces, #goal_holding=[(arm, block) for block in blocks]) goal_on=[(block, plate) for block in blocks], base_limits=base_limits)
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 problem_fn(n_robots=2, collisions=True): base_extent = 2.0 base_limits = (-base_extent / 2. * np.ones(2), base_extent / 2. * np.ones(2)) mound_height = 0.1 floor, walls = create_environment(base_extent, mound_height) width = base_extent / 2. - 4 * mound_height wall5 = create_box(mound_height, width, mound_height, color=GREY) set_point(wall5, Point(y=-(base_extent / 2 - width / 2.), z=mound_height / 2.)) wall6 = create_box(mound_height, width, mound_height, color=GREY) set_point(wall6, Point(y=+(base_extent / 2 - width / 2.), z=mound_height / 2.)) distance = 0.5 #initial_confs = [(-distance, -distance, 0), # (-distance, +distance, 0)] initial_confs = [(-distance, -distance, 0), (+distance, +distance, 0)] assert n_robots <= len(initial_confs) body_from_name = {} #robots = ['green'] robots = ['green', 'blue'] with LockRenderer(): for i, name in enumerate(robots): with HideOutput(): body = load_model( TURTLEBOT_URDF) # TURTLEBOT_URDF | ROOMBA_URDF body_from_name[name] = body robot_z = stable_z(body, floor) set_point(body, Point(z=robot_z)) set_base_conf(body, initial_confs[i]) set_body_color(body, COLOR_FROM_NAME[name]) goals = [(+distance, -distance, 0), (+distance, +distance, 0)] #goals = goals[::-1] goals = initial_confs[::-1] goal_confs = dict(zip(robots, goals)) return NAMOProblem(body_from_name, robots, base_limits, collisions=collisions, goal_confs=goal_confs)
def problem_fn(n_rovers=1, collisions=True): base_extent = 2.5 base_limits = (-base_extent / 2. * np.ones(2), base_extent / 2. * np.ones(2)) mound_height = 0.1 floor = create_box(base_extent, base_extent, 0.001, color=TAN) # TODO: two rooms set_point(floor, Point(z=-0.001 / 2.)) wall1 = create_box(base_extent + mound_height, mound_height, mound_height, color=GREY) set_point(wall1, Point(y=base_extent / 2., z=mound_height / 2.)) wall2 = create_box(base_extent + mound_height, mound_height, mound_height, color=GREY) set_point(wall2, Point(y=-base_extent / 2., z=mound_height / 2.)) wall3 = create_box(mound_height, base_extent + mound_height, mound_height, color=GREY) set_point(wall3, Point(x=base_extent / 2., z=mound_height / 2.)) wall4 = create_box(mound_height, base_extent + mound_height, mound_height, color=GREY) set_point(wall4, Point(x=-base_extent / 2., z=mound_height / 2.)) wall5 = create_box(mound_height, (base_extent + mound_height) / 2., mound_height, color=GREY) set_point(wall5, Point(y=base_extent / 4., z=mound_height / 2.)) rover_confs = [(+1, 0, np.pi), (-1, 0, 0)] assert n_rovers <= len(rover_confs) robots = [] for i in range(n_rovers): with HideOutput(): rover = load_model(TURTLEBOT_URDF) robot_z = stable_z(rover, floor) set_point(rover, Point(z=robot_z)) set_base_conf(rover, rover_confs[i]) robots.append(rover) goal_confs = {robots[0]: rover_confs[-1]} #goal_confs = {} # TODO: make the objects smaller cylinder_radius = 0.25 body1 = create_cylinder(cylinder_radius, mound_height, color=RED) set_point(body1, Point(y=-base_extent / 4., z=mound_height / 2.)) body2 = create_cylinder(cylinder_radius, mound_height, color=BLUE) set_point( body2, Point(x=base_extent / 4., y=3 * base_extent / 8., z=mound_height / 2.)) movable = [body1, body2] #goal_holding = {robots[0]: body1} goal_holding = {} return NAMOProblem(robots, base_limits, movable, collisions=collisions, goal_holding=goal_holding, goal_confs=goal_confs)
def problem1(n_rovers=1, n_objectives=1, n_rocks=2, n_soil=2, n_stores=1): base_extent = 5.0 base_limits = (-base_extent / 2. * np.ones(2), base_extent / 2. * np.ones(2)) floor = create_box(base_extent, base_extent, 0.001, color=TAN) # TODO: two rooms set_point(floor, Point(z=-0.001 / 2.)) add_data_path() lander = load_pybullet(HUSKY_URDF, scale=1) lander_z = stable_z(lander, floor) set_point(lander, Point(-2, -2, lander_z)) #wait_for_user() mount_width = 0.5 mound_height = mount_width mound1 = create_box(mount_width, mount_width, mound_height, color=GREY) set_point(mound1, [+2, 1.5, mound_height / 2.]) mound2 = create_box(mount_width, mount_width, mound_height, color=GREY) set_point(mound2, [-2, 1.5, mound_height / 2.]) initial_surfaces = {} rover_confs = [(+1, -1.75, np.pi), (-1, -1.75, 0)] assert n_rovers <= len(rover_confs) #body_names = map(get_name, env.GetBodies()) landers = [lander] stores = ['store{}'.format(i) for i in range(n_stores)] #affine_limits = aabb_extrema(aabb_union([aabb_from_body(body) for body in env.GetBodies()])).T rovers = [] for i in range(n_rovers): robot = create_pr2() dump_body(robot) # camera_rgb_optical_frame rovers.append(robot) set_group_conf(robot, 'base', rover_confs[i]) for arm in ARM_NAMES: set_arm_conf(robot, arm, arm_conf(arm, REST_LEFT_ARM)) close_arm(robot, arm) obj_width = 0.07 obj_height = 0.2 objectives = [] for _ in range(n_objectives): body = create_box(obj_width, obj_width, obj_height, color=BLUE) objectives.append(body) initial_surfaces[body] = mound1 for _ in range(n_objectives): body = create_box(obj_width, obj_width, obj_height, color=RED) initial_surfaces[body] = mound2 # TODO: it is moving to intermediate locations attempting to reach the rocks rocks = [] for _ in range(n_rocks): body = create_box(0.075, 0.075, 0.05, color=BLACK) rocks.append(body) initial_surfaces[body] = floor soils = [] for _ in range(n_soil): body = create_box(0.1, 0.1, 0.025, color=BROWN) soils.append(body) initial_surfaces[body] = floor sample_placements(initial_surfaces) #for name in rocks + soils: # env.GetKinBody(name).Enable(False) return RoversProblem(rovers, landers, objectives, rocks, soils, stores, base_limits)
def rovers1(n_rovers=2, n_objectives=4, n_rocks=3, n_soil=3, n_stores=1, n_obstacles=8): base_extent = 5.0 base_limits = (-base_extent / 2. * np.ones(2), base_extent / 2. * np.ones(2)) mount_width = 0.5 mound_height = 0.1 floor = create_box(base_extent, base_extent, 0.001, color=TAN) # TODO: two rooms set_point(floor, Point(z=-0.001 / 2.)) wall1 = create_box(base_extent + mound_height, mound_height, mound_height, color=GREY) set_point(wall1, Point(y=base_extent / 2., z=mound_height / 2.)) wall2 = create_box(base_extent + mound_height, mound_height, mound_height, color=GREY) set_point(wall2, Point(y=-base_extent / 2., z=mound_height / 2.)) wall3 = create_box(mound_height, base_extent + mound_height, mound_height, color=GREY) set_point(wall3, Point(x=base_extent / 2., z=mound_height / 2.)) wall4 = create_box(mound_height, base_extent + mound_height, mound_height, color=GREY) set_point(wall4, Point(x=-base_extent / 2., z=mound_height / 2.)) # TODO: can add obstacles along the wall wall = create_box(mound_height, base_extent, mound_height, color=GREY) # TODO: two rooms set_point(wall, Point(z=mound_height / 2.)) add_data_path() with HideOutput(): lander = load_pybullet(HUSKY_URDF, scale=1) lander_z = stable_z(lander, floor) set_point(lander, Point(-1.9, -2, lander_z)) mound1 = create_box(mount_width, mount_width, mound_height, color=GREY) set_point(mound1, [+2, 2, mound_height / 2.]) mound2 = create_box(mount_width, mount_width, mound_height, color=GREY) set_point(mound2, [-2, 2, mound_height / 2.]) mound3 = create_box(mount_width, mount_width, mound_height, color=GREY) set_point(mound3, [+0.5, 2, mound_height / 2.]) mound4 = create_box(mount_width, mount_width, mound_height, color=GREY) set_point(mound4, [-0.5, 2, mound_height / 2.]) mounds = [mound1, mound2, mound3, mound4] random.shuffle(mounds) body_types = [] initial_surfaces = OrderedDict() min_distances = {} for _ in range(n_obstacles): body = create_box(mound_height, mound_height, 4 * mound_height, color=GREY) initial_surfaces[body] = floor rover_confs = [(+1, -1.75, np.pi), (-1, -1.75, 0)] assert n_rovers <= len(rover_confs) landers = [lander] stores = ['store{}'.format(i) for i in range(n_stores)] rovers = [] for i in range(n_rovers): # camera_rgb_optical_frame with HideOutput(): rover = load_model(TURTLEBOT_URDF) robot_z = stable_z(rover, floor) set_point(rover, Point(z=robot_z)) #handles = draw_aabb(get_aabb(rover)) # Includes the origin #print(get_center_extent(rover)) #wait_for_user() set_base_conf(rover, rover_confs[i]) rovers.append(rover) #dump_body(rover) #draw_pose(get_link_pose(rover, link_from_name(rover, KINECT_FRAME))) obj_width = 0.07 obj_height = 0.2 objectives = [] for i in range(n_objectives): body = create_box(obj_width, obj_width, obj_height, color=BLUE) objectives.append(body) #initial_surfaces[body] = random.choice(mounds) initial_surfaces[body] = mounds[i] min_distances.update({r: 0.05 for r in objectives}) # TODO: it is moving to intermediate locations attempting to reach the rocks rocks = [] for _ in range(n_rocks): body = create_box(0.075, 0.075, 0.01, color=BLACK) rocks.append(body) body_types.append((body, 'stone')) for _ in range(n_soil): body = create_box(0.1, 0.1, 0.005, color=BROWN) rocks.append(body) body_types.append((body, 'soil')) soils = [] # Treating soil as rocks for simplicity initial_surfaces.update({r: floor for r in rocks}) min_distances.update({r: 0.2 for r in rocks}) sample_placements(initial_surfaces, min_distances=min_distances) return RoversProblem(rovers, landers, objectives, rocks, soils, stores, base_limits, body_types)
def blocked(arm='left', grasp_type='side', num=1): x_extent = 10.0 base_limits = (-x_extent / 2. * np.ones(2), x_extent / 2. * np.ones(2)) block_width = 0.07 #block_height = 0.1 block_height = 2 * block_width #block_height = 0.2 plate_height = 0.001 table_x = (x_extent - 1) / 2. other_arm = get_other_arm(arm) initial_conf = get_carry_conf(arm, grasp_type) add_data_path() floor = load_pybullet("plane.urdf") pr2 = create_pr2() set_arm_conf(pr2, arm, initial_conf) open_arm(pr2, arm) set_arm_conf(pr2, other_arm, arm_conf(other_arm, REST_LEFT_ARM)) close_arm(pr2, other_arm) set_group_conf( pr2, 'base', [x_extent / 4, 0, 0]) # Be careful to not set the pr2's pose table1 = create_table() set_point(table1, Point(x=+table_x, y=0)) table2 = create_table() set_point(table2, Point(x=-table_x, y=0)) #table3 = create_table() #set_point(table3, Point(x=0, y=0)) plate = create_box(0.6, 0.6, plate_height, color=GREEN) x, y, _ = get_point(table1) plate_z = stable_z(plate, table1) set_point(plate, Point(x=x, y=y - 0.3, z=plate_z)) #surfaces = [table1, table2, table3, plate] surfaces = [table1, table2, plate] green1 = create_box(block_width, block_width, block_height, color=BLUE) green1_z = stable_z(green1, table1) set_point(green1, Point(x=x, y=y + 0.3, z=green1_z)) # TODO: can consider a fixed wall here instead spacing = 0.15 #red_directions = [(-1, 0), (+1, 0), (0, -1), (0, +1)] red_directions = [(-1, 0)] #red_directions = [] red_bodies = [] for red_direction in red_directions: red = create_box(block_width, block_width, block_height, color=RED) red_bodies.append(red) x, y = get_point(green1)[:2] + spacing * np.array(red_direction) z = stable_z(red, table1) set_point(red, Point(x=x, y=y, z=z)) wall1 = create_box(0.01, 2 * spacing, block_height, color=GREY) wall2 = create_box(spacing, 0.01, block_height, color=GREY) wall3 = create_box(spacing, 0.01, block_height, color=GREY) z = stable_z(wall1, table1) x, y = get_point(green1)[:2] set_point(wall1, Point(x=x + spacing, y=y, z=z)) set_point(wall2, Point(x=x + spacing / 2, y=y + spacing, z=z)) set_point(wall3, Point(x=x + spacing / 2, y=y - spacing, z=z)) green_bodies = [ create_box(block_width, block_width, block_height, color=BLUE) for _ in range(num) ] body_types = [(b, 'green') for b in [green1] + green_bodies] # + [(table1, 'sink')] movable = [green1] + green_bodies + red_bodies initial_surfaces = {block: table2 for block in green_bodies} sample_placements(initial_surfaces) return Problem( robot=pr2, movable=movable, arms=[arm], grasp_types=[grasp_type], surfaces=surfaces, #sinks=[table1], #goal_holding=[(arm, '?green')], #goal_cleaned=['?green'], goal_on=[('?green', plate)], body_types=body_types, base_limits=base_limits, costs=True)
def load_pick_and_place(extrusion_name, scale=0.001, max_bricks=6): assert extrusion_name == 'choreo_brick_demo' root_directory = os.path.dirname(os.path.abspath(__file__)) bricks_directory = os.path.join(root_directory, PICKNPLACE_DIRECTORY, 'bricks') print('Name: {}'.format(extrusion_name)) with open( os.path.join(bricks_directory, PICKNPLACE_FILENAMES[extrusion_name]), 'r') as f: json_data = json.loads(f.read()) obj_directory = os.path.join(bricks_directory, 'meshes', 'collision') with HideOutput(): #world = load_pybullet(os.path.join(bricks_directory, 'urdf', 'brick_demo.urdf')) robot = load_pybullet(os.path.join( root_directory, 'framefab_kr6_r900_support/urdf/kr6_r900_2.urdf'), fixed_base=True) set_point(robot, (0.14, 0, 0)) pick_base_point = parse_point(json_data['pick_base_center_point']) draw_pose((pick_base_point, unit_quat())) place_base_point = parse_point(json_data['place_base_center_point']) draw_pose((place_base_point, unit_quat())) # workspace_geo_place_support_object_11 = pick_left_over_bricks_11 obstacle_from_name = {} for filename in json_data['pick_support_surface_file_names']: obstacle_from_name[strip_extension(filename)] = \ create_obj(os.path.join(obj_directory, filename), scale=scale, color=(0.5, 0.5, 0.5, 1)) for filename in json_data['place_support_surface_file_names']: obstacle_from_name[strip_extension(filename)] = \ create_obj(os.path.join(obj_directory, filename), scale=scale, color=(1., 0., 0., 1)) brick_from_index = {} for json_element in json_data['sequenced_elements']: index = json_element['order_id'] pick_body = create_obj(os.path.join( obj_directory, json_element['pick_element_geometry_file_name']), scale=scale, color=(0, 0, 1, 1)) add_body_name(pick_body, index) #place_body = create_obj(os.path.join(obj_directory, json_element['place_element_geometry_file_name']), # scale=scale, color=(0, 1, 0, 1)) #add_body_name(place_body, name) world_from_obj_pick = get_pose(pick_body) # [u'pick_grasp_plane', u'pick_grasp_retreat_plane', u'place_grasp_retreat_plane', # u'pick_grasp_approach_plane', u'place_grasp_approach_plane', u'place_grasp_plane'] ee_grasp_poses = [{ name: pose_from_tform(parse_transform(approach)) for name, approach in json_grasp.items() } for json_grasp in json_element['grasps']] # pick_grasp_plane is at the top of the object with z facing downwards grasp_index = 0 world_from_ee_pick = ee_grasp_poses[grasp_index]['pick_grasp_plane'] world_from_ee_place = ee_grasp_poses[grasp_index]['place_grasp_plane'] #draw_pose(world_from_ee_pick, length=0.04) #draw_pose(world_from_ee_place, length=0.04) ee_from_obj = multiply(invert(world_from_ee_pick), world_from_obj_pick) # Using pick frame world_from_obj_place = multiply(world_from_ee_place, ee_from_obj) #set_pose(pick_body, world_from_obj_place) grasps = [ Grasp( index, num, *[ multiply(invert(world_from_ee_pick[name]), world_from_obj_pick) for name in GRASP_NAMES ]) for num, world_from_ee_pick in enumerate(ee_grasp_poses) ] brick_from_index[index] = Brick( index=index, body=pick_body, initial_pose=WorldPose(index, world_from_obj_pick), goal_pose=WorldPose(index, world_from_obj_place), grasps=grasps, goal_supports=json_element.get('place_contact_ngh_ids', [])) # pick_contact_ngh_ids are movable element contact partial orders # pick_support_surface_file_names are fixed element contact partial orders return robot, brick_from_index, obstacle_from_name
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()