def test(traj1, traj2):
if not problem.collisions:
return True
if (robot1 is None) or (robot2 is None):
return True
if traj1 is traj2:
return False
if not aabb_overlap(get_turtle_traj_aabb(traj1),
get_turtle_traj_aabb(traj2)):
return True
# TODO: could also use radius to prune
# TODO: prune if start and end conf collide
for conf1 in randomize(traj1.iterate()):
if not aabb_overlap(get_turtle_traj_aabb(conf1),
get_turtle_traj_aabb(traj2)):
continue
set_base_conf(robot1, conf1.values)
for conf2 in randomize(traj2.iterate()):
if not aabb_overlap(get_turtle_traj_aabb(conf1),
get_turtle_traj_aabb(conf2)):
continue
set_base_conf(robot2, conf2.values)
if pairwise_collision(robot1, robot2):
return False
return True
def display_plan(problem, state, plan, time_step=0.01, sec_per_step=0.005):
duration = compute_duration(plan)
real_time = None if sec_per_step is None else (duration * sec_per_step / time_step)
print('Duration: {} | Step size: {} | Real time: {}'.format(duration, time_step, real_time))
colors = {robot: COLOR_FROM_NAME[robot] for robot in problem.robots}
for i, t in enumerate(inclusive_range(0, duration, time_step)):
print('Step={} | t={}'.format(i, t))
for action in plan:
name, args, start, duration = action
if not (action.start <= t <= get_end(action)):
continue
if name == 'move':
robot, conf1, traj, conf2 = args
traj = [conf1.values, conf2.values]
fraction = (t - action.start) / action.duration
conf = get_value_at_time(traj, fraction)
body = problem.get_body(robot)
color = COLOR_FROM_NAME[robot]
if colors[robot] != color:
set_color(body, color, link_from_name(body, TOP_LINK))
colors[robot] = color
set_base_conf(body, conf)
elif name == 'recharge':
robot, conf = args
body = problem.get_body(robot)
color = YELLOW
if colors[robot] != color:
set_color(body, color, link_from_name(body, TOP_LINK))
colors[robot] = color
else:
raise ValueError(name)
if sec_per_step is None:
wait_if_gui('Continue?')
else:
wait_for_duration(sec_per_step)
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)