def show_the_model(args):
"""Show the problem"""
floor = construct_default_floor_plan()
goal = np.array([0.8, 0.8])
env = SensorSingleGoalProblem(floor, goal)
if args.novio:
env.out_vio_step = 1
if args.harder:
env.x0lb[0] = 0.1
env.x0ub[0] = 0.9
env.x0lb[1] = 0.1
env.x0ub[1] = 0.9
env_name = str(env)
print('Environment is %s' % env_name)
config = pu.get_train_gym_config(env_name=env_name)
sim_n = args.num
v_x0, v_xf, v_traj = pu.policy_rollout(env, config, sim_n, False, True)
plt.switch_backend('TkAgg')
fig, axes = pl.subplots(sim_n)
for i in range(sim_n):
ax = axes[i]
sim_rst = v_traj[i]
x, u, dt = sim_rst['state'], sim_rst['action'], sim_rst['dt']
floor.draw(ax)
circle1 = plt.Circle((goal[0], goal[1]), 0.05, color='g')
ax.add_artist(circle1)
ax.plot(*x[:, :2].T)
plt.savefig('gallery/%s-%d-cases.pdf' % (env_name, sim_n))
plt.show()
def perform_mass_test(args):
"""Do a massive simulation on this problem."""
env, floor = construct_env()
env_name = str(env)
env.out_vio_step = 1
print('Environment is %s' % env_name)
config = pu.get_train_gym_config(env_name=env_name)
sim_n = args.num
assert sim_n == 1000
v_x0, v_xf, v_flag, v_goal = pu.policy_rollout(env, config, sim_n, show=False, return_success=True, return_goal=True)
n_succeed = np.sum(v_flag == 1)
n_collision = np.sum(v_flag == -1)
print('succeed in %d / %d' % (n_succeed, sim_n))
print('collision in %d / %d' % (n_collision, sim_n))
print('0.1: ', np.sum(np.linalg.norm(v_xf - v_goal, axis=1) < 0.1))
with open('succeed_log.txt', 'at') as f:
f.write('Env:%s' % env_name)
f.write('Succeed: %d' % n_succeed)
f.write('Collision: %d' % n_collision)
return
floor.draw(ax)
ax.scatter(*v_x0[mask0, :2].T, label='Succeed')
ax.scatter(*v_x0[~mask0, :2].T, label='Failure')
ax.legend()
fig.savefig('gallery/%s-massive-sim-x0.pdf' % env_name)
plt.show()
def show_the_model(args):
"""Show the problem"""
floor = construct_default_floor_plan()
goal = np.array([0.8, 0.8])
env = SingleGoalProblem(floor, goal)
env.x0lb[:2] = [0.1, 0.1]
env.x0ub[:2] = [0.9, 0.9]
if args.novio:
env.out_vio_step = 1
env_name = str(env)
config = pu.get_train_gym_config(env_name=env_name)
sim_n = args.num
v_x0, v_xf, v_traj = pu.policy_rollout(env, config, sim_n, False, True)
# fig, ax = pld.get3dAxis()
matplotlib.use('TkAgg')
plt.switch_backend('TkAgg')
fig, axes = pl.subplots(sim_n)
for i in range(sim_n):
sim_rst = v_traj[i]
x, u, dt = sim_rst['state'], sim_rst['action'], sim_rst['dt']
ax = axes[i]
floor.draw(ax)
circle1 = plt.Circle((0.8, 0.8), 0.05, color='g')
ax.add_artist(circle1)
ax.plot(*x[:, :2].T)
fig.tight_layout()
plt.savefig('gallery/%s-%d-cases.pdf' % (env_name, sim_n))
plt.show()
def perform_massive_test(args):
"""Perform some simulation and see the results."""
floor = construct_default_floor_plan()
goal = np.array([0.8, 0.8])
env = SingleGoalProblem(floor, goal)
env.x0lb[:2] = [0.1, 0.1]
env.x0ub[:2] = [0.9, 0.9]
if args.novio:
env.out_vio_step = 1
env_name = str(env)
config = pu.get_train_gym_config(env_name=env_name)
sim_n = args.num
assert sim_n == 1000
v_x0, v_xf, v_flag = pu.policy_rollout(env,
config,
sim_n,
show=False,
return_success=True)
mask0 = v_flag == 1
collision = np.sum(v_flag == -1)
print('succeed in %d / %d' % (np.sum(mask0), sim_n))
print('collision in %d / %d' % (collision, sim_n))
with open('succeed_log.txt', 'wt') as f:
f.write('Env:%s' % env_name)
f.write('Succeed: %d' % np.sum(mask0))
f.write('Collision: %d' % collision)
def show_the_model(args):
"""Show the model"""
env, floor, update_fun = construct_env(args)
env_name = 'update_%s' % str(env)
config = pu.get_train_gym_config(env_name=env_name, seed=np.random.randint(100), num_frames=2e6)
sim_n = args.num
v_x0, v_xf, v_traj = pu.policy_rollout(env, config, sim_n, show=False, return_traj=True)
plt.switch_backend('TkAgg')
fig, axes = pl.subplots(sim_n)
for i in range(sim_n):
ax = axes[i]
sim_rst = v_traj[i]
x, u, dt = sim_rst['state'], sim_rst['action'], sim_rst['dt']
floor.draw(ax)
ax.plot(*x[:, :2].T)
plt.savefig('gallery/%s-%d-cases.pdf' % (env_name, sim_n))
plt.show()
def perform_mass_test(args):
"""Do a massive simulation on this problem."""
env, floor = construct_env(args)
env_name = str(env)
env.out_vio_step = 1
print('Environment is %s' % env_name)
config = pu.get_train_gym_config(env_name=env_name)
sim_n = args.num
v_x0, v_xf, v_flag, v_goal = pu.policy_rollout(env, config, sim_n, show=False, return_success=True, return_goal=True)
succeed = np.sum(v_flag == 1)
collision = np.sum(v_flag == -1)
print('succeed in %d / %d' % (succeed, sim_n))
print('collision in %d / %d' % (collision, sim_n))
print('0.1:', np.sum(np.linalg.norm(v_xf - v_goal, axis=1) < 0.1))
with open('succeed_log.txt', 'at') as f:
f.write('Env:%s' % env_name)
f.write('Succeed: %d' % succeed)
f.write('Collision: %d' % collision)
return
def show_the_model(args):
"""Show the problem"""
env, floor = construct_env()
env_name = str(env)
print('Environment is %s' % env_name)
config = pu.get_train_gym_config(env_name=env_name)
sim_n = args.num
v_x0, v_xf, v_traj, v_goal = pu.policy_rollout(env, config, sim_n, show=False, return_traj=True, return_goal=True)
plt.switch_backend('TkAgg')
fig, axes = pl.subplots(sim_n)
for i in range(sim_n):
ax = axes[i]
sim_rst = v_traj[i]
goal = v_goal[i]
x, u, dt = sim_rst['state'], sim_rst['action'], sim_rst['dt']
floor.draw(ax)
circle1 = plt.Circle((goal[0], goal[1]), 0.05, color='g')
ax.add_artist(circle1)
ax.plot(*x[:, :2].T)
plt.savefig('gallery/%s-%d-cases.pdf' % (env_name, sim_n))
plt.show()
def perform_mass_test(args):
"""Do a massive simulation on this problem."""
floor = construct_default_floor_plan()
goal = np.array([0.8, 0.8])
env = SensorSingleGoalProblem(floor, goal)
if args.novio:
env.out_vio_step = 1
env.x0lb[0] = 0.1
env.x0ub[0] = 0.9
env.x0lb[1] = 0.1
env.x0ub[1] = 0.9
env_name = str(env)
env.out_vio_step = 1 # do this anyway
print('Environment is %s' % env_name)
config = pu.get_train_gym_config(env_name=env_name)
sim_n = args.num
v_x0, v_xf, v_flag = pu.policy_rollout(env, config, sim_n, show=False, return_success=True)
succeed = np.sum(v_flag == 1)
collision = np.sum(v_flag == -1)
print('succeed in %d / %d' % (succeed, sim_n))
print('collision in %d / %d' % (collision, sim_n))
print('0.1:', np.sum(np.linalg.norm(v_xf - np.array([0.8, 0.8, 0, 0]), axis=1) < 0.1))
with open('succeed_log.txt', 'at') as f:
f.write('Env:%s' % env_name)
f.write('Succeed: %d' % succeed)
f.write('Collision: %d' % collision)
return
fig, ax = pl.subplots()
plt.switch_backend('TkAgg')
floor.draw(ax)
ax.scatter(*v_x0[mask0, :2].T, label='Succeed')
ax.scatter(*v_x0[~mask0, :2].T, label='Failure')
ax.legend()
fig.savefig('gallery/%s-massive-sim-x0.pdf' % env_name)
plt.show()
def show_the_model(args):
"""Show the problem"""
matplotlib.use('TkAgg')
plt.switch_backend('TkAgg')
env, env_name = model_build(fix=args.fix,
small=args.small,
medium=args.medium,
medhard=args.medhard)
config = pu.get_train_gym_config(env_name=env_name,
seed=np.random.randint(10000))
sim_n = 200
if args.small:
sim_n = 200
if args.medium:
sim_n = 500
valid_x0 = None # use this to load validation set
if args.valid:
valid_x0 = np.load(VALIDATION_SET)['x0']
sim_n = valid_x0.shape[0]
v_x0, v_xf, v_traj = pu.policy_rollout(env,
config,
sim_n,
show=args.render,
return_traj=True,
valid_x0=valid_x0)
v_xf_norm = np.linalg.norm(v_xf, axis=1)
level = [0.1, 0.5, 1, 2]
for level_ in level:
print('level %f count %d' % (level_, np.sum(v_xf_norm < level_)))
# find closest state
v_min_state_norm = np.zeros(sim_n)
for i in range(sim_n):
v_min_state_norm[i] = np.amin(
np.linalg.norm(v_traj[i]['state'], axis=1))
datafnm = 'data/%s_all_rst.npz' % env_name
figfnm = 'gallery/%s_rollout_xf.pdf' % env_name
figfnm_min = 'gallery/%s_rollout_min_x.pdf' % env_name
if args.small:
datafnm = datafnm.replace('.npz', '_small.npz')
figfnm = figfnm.replace('.pdf', '_small.pdf')
figfnm_min = figfnm_min.replace('.pdf', '_small.pdf')
if args.medium:
datafnm = datafnm.replace('.npz', '_medium.npz')
figfnm = figfnm.replace('.pdf', '_medium.pdf')
figfnm_min = figfnm_min.replace('.pdf', '_medium.pdf')
if args.medhard:
datafnm = datafnm.replace('.npz', '_medhard.npz')
figfnm = figfnm.replace('.pdf', '_medhard.pdf')
figfnm_min = figfnm_min.replace('.pdf', '_medhard.pdf')
if args.valid:
datafnm = datafnm.replace('.npz', '_valid.npz')
figfnm = figfnm.replace('.pdf', '_valid.pdf')
figfnm_min = figfnm_min.replace('.pdf', '_valid.pdf')
for level_ in level:
print('min level %f count %d' %
(level_, np.sum(v_min_state_norm < level_)))
np.savez(datafnm, x0=v_x0, xf=v_xf, traj=v_traj)
fig, ax = plt.subplots()
ax.hist(v_xf_norm, bins='auto')
ax.set_xlabel(r'$\|x_f\|$')
ax.set_ylabel('Count')
fig.savefig(figfnm)
fig, ax = plt.subplots()
ax.hist(v_min_state_norm, bins='auto')
ax.set_xlabel(r'$\|x\|_{\min}$')
ax.set_ylabel('Count')
fig.savefig(figfnm_min)
plt.show()