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 move_look_trajectory(task, trajectory, max_tilt=np.pi / 6): # max_tilt=INF): # TODO: implement a minimum distance instead of max_tilt # TODO: pr2 movement restrictions #base_path = [pose.to_base_conf() for pose in trajectory.path] base_path = trajectory.path if not base_path: return trajectory obstacles = get_fixed_bodies(task) # TODO: movable objects robot = base_path[0].body target_path = get_target_path(trajectory) waypoints = [] index = 0 with BodySaver(robot): #current_conf = base_values_from_pose(get_pose(robot)) for i, conf in enumerate(base_path): # TODO: just do two loops? conf.assign() while index < len(target_path): if i < index: # Don't look at past or current conf target_point = target_path[index] head_conf = inverse_visibility( robot, target_point) # TODO: this is slightly slow #print(index, target_point, head_conf) if (head_conf is not None) and (head_conf[1] < max_tilt): break index += 1 else: head_conf = get_group_conf(robot, 'head') set_group_conf(robot, 'head', head_conf) #print(i, index, conf.values, head_conf) #, get_pose(robot)) waypoints.append(np.concatenate([conf.values, head_conf])) joints = tuple(base_path[0].joints) + tuple(get_group_joints( robot, 'head')) #joints = get_group_joints(robot, 'base') + get_group_joints(robot, 'head') #set_pose(robot, unit_pose()) #set_group_conf(robot, 'base', current_conf) path = plan_waypoints_joint_motion(robot, joints, waypoints, obstacles=obstacles, self_collisions=SELF_COLLISIONS) return create_trajectory(robot, joints, path)
def gen(o, p): if o in task.rooms: #bq = Pose(robot, unit_pose()) #bq = state.poses[robot] bq = Conf(robot, base_joints, np.zeros(len(base_joints))) #hq = Conf(robot, head_joints, np.zeros(len(head_joints))) #ht = create_trajectory(robot, head_joints, plan_pause_scan_path(robot, tilt=tilt)) waypoints = plan_scan_path(task.robot, tilt=tilt) set_group_conf(robot, 'head', waypoints[0]) path = plan_waypoints_joint_motion(robot, head_joints, waypoints[1:], obstacles=None, self_collisions=False) ht = create_trajectory(robot, head_joints, path) yield bq, ht.path[0], ht else: for bq, hq in vis_gen(o, p): ht = Trajectory([hq]) yield bq, ht.path[0], ht
def fn(o, p, bq): set_pose(o, p.value) # p.assign() bq.assign() if o in task.rooms: waypoints = plan_scan_path(task.robot, tilt=ROOM_SCAN_TILT) set_group_conf(robot, 'head', waypoints[0]) path = plan_waypoints_joint_motion(robot, head_joints, waypoints[1:], obstacles=None, self_collisions=False) if path is None: return None ht = create_trajectory(robot, head_joints, path) hq = ht.path[0] else: target_point = point_from_pose(p.value) head_conf = inverse_visibility(robot, target_point) if head_conf is None: # TODO: test if visible return None hq = Conf(robot, head_joints, head_conf) ht = Trajectory([hq]) return (hq, ht)
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 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)