def one_planner(config, size):
print("size=" + str(size))
agent_pos, grid, idle_goals, jobs = config['params']
agent_pos = agent_pos[0:size]
jobs = jobs[0:size]
if 'milp' in config:
print("milp")
from planner.milp.milp import plan_milp
res_agent_job, res_paths = plan_milp(agent_pos, jobs, grid, config)
elif 'cobra' in config:
print("cobra")
from planner.cobra.funwithsnakes import plan_cobra
res_agent_job, res_paths = plan_cobra(agent_pos, jobs, grid, config)
elif 'greedy' in config:
print("greedy")
from planner.greedy.greedy import plan_greedy
res_agent_job, res_paths = plan_greedy(agent_pos, jobs, grid, config)
else:
res_agent_job, res_agent_idle, res_paths = plan(
agent_pos, jobs, [], idle_goals, grid, config
)
print(res_agent_job)
if is_cch():
fig = plt.figure()
ax1 = fig.add_subplot(121)
plot_inputs(ax1, agent_pos, [], jobs, grid)
ax2 = fig.add_subplot(122, projection='3d')
plot_results(ax2, [], res_paths, res_agent_job, agent_pos, grid, [], jobs)
plt.show()
return get_costs(res_paths, jobs, res_agent_job, True)
def one_planner(config, size):
print("size=" + str(size))
print("Testing with number_nearest=" + str(config['number_nearest']))
print("Testing with all_collisions=" + str(config['all_collisions']))
agent_pos, grid, idle_goals, jobs = config['params']
agent_pos = agent_pos[0:size]
jobs = jobs[0:size]
if 'milp' in config:
print("milp")
from planner.milp.milp import plan_milp
res_agent_job, res_paths = plan_milp(agent_pos, jobs, grid, config)
elif 'cobra' in config:
print("cobra")
from planner.cobra.funwithsnakes import plan_cobra
res_agent_job, res_paths = plan_cobra(agent_pos, jobs, grid, config)
elif 'greedy' in config:
print("greedy")
from planner.greedy.greedy import plan_greedy
res_agent_job, res_paths = plan_greedy(agent_pos, jobs, grid, config)
else:
res_agent_job, res_agent_idle, res_paths = plan(
agent_pos, jobs, [], idle_goals, grid, config, plot=False
)
print(res_agent_job)
return get_costs(res_paths, jobs, res_agent_job, True)
def test_cobra_simple(plot=False):
grid = np.zeros([5, 5, 30])
res_agent_job, res_paths = plan_cobra([(1, 1), (2, 2)],
[((3, 3), (1, 4), 0),
((4, 1), (0, 0), 0)], grid,
generate_config())
if plot:
plot_results([], res_paths, [], [], plt.figure(), grid, [], [])
assert res_agent_job, "No result"
assert res_paths, "No result"
def test_cobra_random(plot=False):
agent_pos, grid, idle_goals, jobs = get_data_random(seed=1,
map_res=8,
map_fill_perc=20,
agent_n=3,
job_n=3,
idle_goals_n=0)
res_agent_job, res_paths = plan_cobra(agent_pos, jobs, grid,
generate_config())
print(res_agent_job)
all_alloc = reduce(lambda a, b: a + b, res_agent_job, tuple())
jobs_is = list(range(len(jobs)))
for i_j in all_alloc:
jobs_is.remove(i_j)
assert not jobs_is, "Not all jobs allocated"
if plot:
fig = plt.figure()
ax = fig.add_subplot(121)
plot_inputs(ax, agent_pos, idle_goals, jobs, grid)
ax2 = fig.add_subplot(122, projection='3d')
plot_results(ax2, [], res_paths, res_agent_job, agent_pos, grid, [],
jobs)
plt.show()