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():
# TODO: store internal world info here to be reloaded
with HideOutput():
robot = load_model(DRAKE_IIWA_URDF)
#add_data_path()
#robot = load_pybullet(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)))
celery = load_model(BLOCK_URDF, fixed_base=False)
radish = load_model(SMALL_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)
body_names = {
sink: 'sink',
stove: 'stove',
celery: 'celery',
radish: 'radish',
}
movable_bodies = [celery, radish]
set_pose(celery, Pose(Point(y=0.5, z=stable_z(celery, floor))))
set_pose(radish, Pose(Point(y=-0.5, z=stable_z(radish, floor))))
set_default_camera()
return robot, body_names, movable_bodies
def load_world():
# TODO: store internal world info here to be reloaded
set_default_camera()
draw_global_system()
with HideOutput():
#add_data_path()
robot = load_model(DRAKE_IIWA_URDF, fixed_base=True) # DRAKE_IIWA_URDF | 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)))
celery = load_model(BLOCK_URDF, fixed_base=False)
radish = load_model(SMALL_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)
draw_pose(Pose(), parent=robot, parent_link=get_tool_link(robot)) # TODO: not working
# dump_body(robot)
# wait_for_user()
body_names = {
sink: 'sink',
stove: 'stove',
celery: 'celery',
radish: 'radish',
}
movable_bodies = [celery, radish]
set_pose(celery, Pose(Point(y=0.5, z=stable_z(celery, floor))))
set_pose(radish, Pose(Point(y=-0.5, z=stable_z(radish, floor))))
return robot, body_names, movable_bodies
def get_grasp_pose(translation, direction, angle, reverse, offset=1e-3):
#direction = Pose(euler=Euler(roll=np.pi / 2, pitch=direction))
return multiply(Pose(point=Point(z=offset)),
Pose(euler=Euler(yaw=angle)),
direction,
Pose(point=Point(z=translation)),
Pose(euler=Euler(roll=(1-reverse) * np.pi)))
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 test_grasps(robot, node_points, elements):
element = elements[-1]
[element_body] = create_elements(node_points, [element])
phi = 0
link = link_from_name(robot, TOOL_NAME)
link_pose = get_link_pose(robot, link)
draw_pose(link_pose) #, parent=robot, parent_link=link)
wait_for_interrupt()
n1, n2 = element
p1 = node_points[n1]
p2 = node_points[n2]
length = np.linalg.norm(p2 - p1)
# Bottom of cylinder is p1, top is p2
print(element, p1, p2)
for phi in np.linspace(0, np.pi, 10, endpoint=True):
theta = np.pi / 4
for t in np.linspace(-length / 2, length / 2, 10):
translation = Pose(
point=Point(z=-t)
) # Want to be moving backwards because +z is out end effector
direction = Pose(euler=Euler(roll=np.pi / 2 + theta, pitch=phi))
angle = Pose(euler=Euler(yaw=1.2))
grasp_pose = multiply(angle, direction, translation)
element_pose = multiply(link_pose, grasp_pose)
set_pose(element_body, element_pose)
wait_for_interrupt()
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 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 gen(rover, objective):
base_joints = get_base_joints(rover)
target_point = get_point(objective)
base_generator = visible_base_generator(rover, target_point,
base_range)
lower_limits, upper_limits = get_custom_limits(rover, base_joints,
custom_limits)
attempts = 0
while True:
if max_attempts <= attempts:
attempts = 0
yield None
attempts += 1
base_conf = next(base_generator)
if not all_between(lower_limits, base_conf, upper_limits):
continue
bq = Conf(rover, base_joints, base_conf)
bq.assign()
if any(pairwise_collision(rover, b) for b in obstacles):
continue
link_pose = get_link_pose(rover,
link_from_name(rover, KINECT_FRAME))
if use_cone:
mesh, _ = get_detection_cone(rover,
objective,
camera_link=KINECT_FRAME,
depth=max_range)
if mesh is None:
continue
cone = create_mesh(mesh, color=(0, 1, 0, 0.5))
local_pose = Pose()
else:
distance = get_distance(point_from_pose(link_pose),
target_point)
if max_range < distance:
continue
cone = create_cylinder(radius=0.01,
height=distance,
color=(0, 0, 1, 0.5))
local_pose = Pose(Point(z=distance / 2.))
set_pose(cone, multiply(link_pose, local_pose))
# TODO: colors corresponding to scanned object
if any(
pairwise_collision(cone, b) for b in obstacles
if b != objective and not is_placement(objective, b)):
# TODO: ensure that this works for the rover
remove_body(cone)
continue
if not reachable_test(rover, bq):
continue
print('Visibility attempts:', attempts)
y = Ray(cone, rover, objective)
yield Output(bq, y)
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 load_world():
# 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)
body_names = {
sink: 'sink',
stove: 'stove',
block: 'celery',
}
movable_bodies = [block]
set_pose(block, Pose(Point(y=0.5, z=stable_z(block, floor))))
set_default_camera()
return robot, body_names, movable_bodies
def gen(robot, body):
link = link_from_name(robot, BASE_LINK)
with BodySaver(robot):
set_base_conf(robot, np.zeros(3))
robot_pose = get_link_pose(robot, link)
robot_aabb = get_turtle_aabb(robot)
#draw_aabb(robot_aabb)
lower, upper = robot_aabb
center, (diameter, height) = approximate_as_cylinder(body)
_, _, z = get_point(body) # Assuming already placed stably
position = Point(x=upper[0] + diameter / 2., z=z)
for [scale] in halton_generator(d=1):
yaw = scale * 2 * np.pi - np.pi
quat = quat_from_euler(Euler(yaw=yaw))
body_pose = (position, quat)
robot_from_body = multiply(invert(robot_pose), body_pose)
grasp = Pose(body, robot_from_body)
yield (grasp, )
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 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_world():
# TODO: store internal world info here to be reloaded
with HideOutput():
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)
block1 = load_model(BLOCK_URDF, fixed_base=False)
block2 = load_model(BLOCK_URDF, fixed_base=False)
block3 = load_model(BLOCK_URDF, fixed_base=False)
block4 = load_model(BLOCK_URDF, fixed_base=False)
block5 = load_model(BLOCK_URDF, fixed_base=False)
block6 = load_model(BLOCK_URDF, fixed_base=False)
block7 = load_model(BLOCK_URDF, fixed_base=False)
block8 = load_model(BLOCK_URDF, fixed_base=False)
cup = load_model(
'models/cup.urdf', #'models/dinnerware/cup/cup_small.urdf'
fixed_base=False)
body_names = {
sink: 'sink',
stove: 'stove',
block: 'celery',
cup: 'cup',
}
movable_bodies = [
block, cup, block1, block2, block3, block4, block5, block6, block7,
block8
]
set_pose(block, Pose(Point(x=0.1, y=0.5, z=stable_z(block, floor))))
set_pose(block1, Pose(Point(x=-0.1, y=0.5, z=stable_z(block1, floor))))
set_pose(block2, Pose(Point(y=0.35, z=stable_z(block2, floor))))
set_pose(block3, Pose(Point(y=0.65, z=stable_z(block3, floor))))
set_pose(block4, Pose(Point(x=0.1, y=0.65, z=stable_z(block4, floor))))
set_pose(block5, Pose(Point(x=-0.1, y=0.65, z=stable_z(block5, floor))))
set_pose(block6, Pose(Point(x=0.1, y=0.35, z=stable_z(block6, floor))))
set_pose(block7, Pose(Point(x=-0.1, y=0.35, z=stable_z(block7, floor))))
set_pose(block8, Pose(Point(x=0, y=0.5, z=0.45)))
set_pose(cup, Pose(Point(y=0.5, z=stable_z(cup, floor))))
set_default_camera()
return robot, body_names, movable_bodies
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)