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 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_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 construct_env():
"""Return the environment"""
floor = construct_default_floor_plan()
glb = np.array([0.8, 0.3, 0, 0])
gub = np.array([0.8, 0.7, 0, 0])
env = RangeGoalProblem(floor, glb, gub)
env.x0lb[0] = 0.1 # xmin
env.x0ub[0] = 0.9 # xmax
env.x0lb[1] = 0.1 # ymin
env.x0ub[1] = 0.9 # ymax
return env, floor
def construct_env(args):
"""Return the environment"""
floor = construct_default_floor_plan()
glb = np.array([0.1, 0.6, 0, 0])
gub = np.array([0.4, 0.9, 0, 0])
env = SensorRangeGoalProblem(floor, glb, gub)
if args.novio:
env.out_vio_step = 1
env.x0lb[0] = 0.1 # xmin
env.x0lb[1] = 0.6 # ymin
env.x0ub[0] = 0.4 # xmax
env.x0ub[1] = 0.9 # ymax
return env, floor
def main():
from floorPlan import construct_default_floor_plan
space = construct_default_floor_plan()
# create simulator
sim = LidarSimulator(space, 72, 2 * np.pi, 0.5)
center = np.array([0.2, 0.3])
angle = np.pi / 2
# dis = sim.observe_one(center, angle)
noise = 0
obs, pnts = sim.observe_span(center, angle, noise, return_point=True)
fig, ax = plt.subplots()
space.draw(ax)
ax.scatter(pnts[:, 0], pnts[:, 1], marker='*', color='r')
ax.scatter(center[0], center[1], marker='o', color='g')
fig.savefig('gallery/sensor.pdf')
plt.show()
def train_the_model(args):
"""Train a model for this particular problem."""
# construct the fix world thing
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.disable_collision()
# env.disable_bound_check()
env_name = str(env)
config = pu.get_train_gym_config(env_name=env_name,
seed=np.random.randint(10000))
pu.train_a_gym_model(env, config)
def train_the_model(args):
"""Train a model for this particular problem."""
# construct the fix world thing
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, cuda=True, seed=np.random.randint(10000))
pu.train_a_gym_model(env, config)
def construct_env(args):
"""Return the environment"""
floor = construct_default_floor_plan()
glb = np.array([0.1, 0.6, 0, 0])
gub = np.array([0.4, 0.9, 0, 0])
env = SensorRangeGoalProblem(floor, glb, gub)
if args.novio:
env.out_vio_step = 1
env.x0lb[0] = 0.1
env.x0lb[1] = 0.6
env.x0ub[0] = 0.4
env.x0ub[1] = 0.9
def update_fun(env, alpha):
env.glb[1] = 0.6 - 0.5 * alpha # tends to 0.1
env.gub[0] = 0.4 + 0.5 * alpha # tends to 0.9
env.x0lb[1] = env.glb[1]
env.x0ub[0] = env.gub[0]
return env
return env, floor, update_fun
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()
