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)