def main():
env_c = fw_coll_env_c.FwCollisionEnv(dt=0.1,
max_sim_time=30,
done_dist=25,
safety_dist=25,
goal1=fw_coll_env_c.Point(200, 0, 0),
goal2=fw_coll_env_c.Point(-200, 0, 0),
time_warp=-1)
avail_actions = fw_coll_env_c.FwAvailActions([15], [-12, 0, 12], [0])
res_lims = np.array([[-200, -200, -np.pi, 0], [200, 200, np.pi, 0]])
goal1_lims = np.array([[200, 0, 0], [200, 0, 0]])
goal2_lims = np.array([[-200, 0, 0], [-200, 0, 0]])
env = fw_coll_env.env.FwCollisionGymEnv(env_c, res_lims, res_lims,
goal1_lims, goal2_lims,
avail_actions)
env.reset()
while True:
ac = np.array([0, 0, 0])
obs, rew, done, info = env.step(ac)
env.render()
if done:
env.reset()
def test_uhat() -> None:
avail_v = [1, 2, 3]
avail_w = [-1, 0, 1]
avail_dz = [-1, 0, 1]
avail = fw_coll_env_c.FwAvailActions(v=avail_v, w=avail_w, dz=avail_dz)
uhat = fw_coll_env_c.Uhat(goal=fw_coll_env_c.Point(x=200, y=0, z=0),
dt=0.01,
avail_actions=avail)
x0 = fw_coll_env_c.FwSingleState(p=fw_coll_env_c.Point(0, 0, 0), th=0)
# pointing to the left of goal, should turn right
x0.th = np.pi / 2
ac = uhat.calc(x0)
assert ac.w < 0
assert ac.dz == 0
# pointing to the right of goal, should turn right
x0.th = -np.pi / 2
ac = uhat.calc(x0)
assert ac.w > 0
assert ac.dz == 0
# pickling
pickle.loads(pickle.dumps(uhat))
def test_fw_dynamics_alt_rate() -> None:
ac = FwSingleAction(v=0, w=0, dz=1)
x = FwSingleState(p=fw_coll_env_c.Point(0, 0, 0), th=0)
fw_coll_env_c.fw_dynamics(dt=1, ac=ac, x=x)
check_state(x, 0, 0, 0, 1)
ac = FwSingleAction(v=0, w=0, dz=-1)
x = FwSingleState(p=fw_coll_env_c.Point(0, 0, 0), th=0)
fw_coll_env_c.fw_dynamics(dt=1, ac=ac, x=x)
check_state(x, 0, 0, 0, -1)
def test_fw_state() -> None:
x1 = FwSingleState(p=fw_coll_env_c.Point(0, 1, 2), th=0)
x2 = FwSingleState(p=fw_coll_env_c.Point(0, 1, 2), th=0)
x = FwState(x1=x1, x2=x2)
assert np.isclose(x.x1.p.dist(fw_coll_env_c.Point(0, 1, 2)), 0, atol=1e-9)
assert np.isclose(x.x2.p.dist(fw_coll_env_c.Point(0, 1, 2)), 0, atol=1e-9)
exp_np = np.array([0, 1, 0, 2, 0, 1, 0, 2])
actual_np = np.asarray(x)
assert np.allclose(exp_np, actual_np, atol=1e-9)
assert np.allclose(x, FwState.from_numpy(exp_np), atol=1e-9)
def test_fw_single_state() -> None:
x = FwSingleState(p=fw_coll_env_c.Point(1, 2, 3), th=-1)
assert np.isclose(x.p.dist(fw_coll_env_c.Point(0, 1, 2)),
np.sqrt(3),
atol=1e-9)
exp_np = np.array([1, 2, -1, 3])
actual_np = np.asarray(x)
assert np.allclose(exp_np, actual_np, atol=1e-9)
assert np.allclose(x, FwSingleState.from_numpy(exp_np), atol=1e-9)
assert x.th == -1
x_unpickle = pickle.loads(pickle.dumps(x))
assert x_unpickle == x
def test_rho() -> None:
x1 = FwSingleState(p=fw_coll_env_c.Point(0, 0, 0), th=0)
x2 = FwSingleState(p=fw_coll_env_c.Point(5, 0, 0), th=0)
x = FwState(x1, x2)
rho = fw_coll_env_c.Rho(safety_dist=5, max_val=25)
assert rho(x) == 0
x.x2.p.x = 300
assert rho(x) == 25
rho_unpickled = pickle.loads(pickle.dumps(rho))
assert rho.safety_dist == rho_unpickled.safety_dist
assert rho.max_val == rho_unpickled.max_val
def test_fw_dynamics_velocity() -> None:
ac = FwSingleAction(v=1, w=0, dz=0)
x = FwSingleState(p=fw_coll_env_c.Point(1, 0, 0), th=0)
fw_coll_env_c.fw_dynamics(dt=1, ac=ac, x=x)
check_state(x, 2, 0, 0, 0)
x = FwSingleState(p=fw_coll_env_c.Point(1, 0, 0), th=np.pi)
fw_coll_env_c.fw_dynamics(dt=1, ac=ac, x=x)
check_state(x, 0, 0, np.pi, 0)
x = FwSingleState(p=fw_coll_env_c.Point(1, 0, 0), th=np.pi / 2)
fw_coll_env_c.fw_dynamics(dt=1, ac=ac, x=x)
check_state(x, 1, 1, np.pi / 2, 0)
x = FwSingleState(p=fw_coll_env_c.Point(1, 0, 0), th=-np.pi / 2)
fw_coll_env_c.fw_dynamics(dt=1, ac=ac, x=x)
check_state(x, 1, -1, -np.pi / 2, 0)