def test_calc_h() -> None:
bf = make_barrier_func()[1]
x1 = FwSingleState(Point(-50, 0, 0), th=0)
x2 = FwSingleState(Point(50, 0, 0), th=np.pi)
x = FwState(x1, x2)
h = bf.calc_h(x)
assert h < x1.p.dist(x2.p)
def test_bf_override() -> None:
avail, bf = make_barrier_func()
x1 = FwSingleState(Point(21, -1, 0), np.pi)
x2 = FwSingleState(Point(-21, 1, 0), 0)
x = FwState(x1, x2)
h = bf.calc_h(x)
assert h < MAX_VAL
correct_single_ac = FwSingleAction(V, W, 0)
correct_ac = FwAction(correct_single_ac, correct_single_ac)
max_evasive_bf_constraint = \
calc_bf_constraint(bf.calc_dh(x, correct_ac), bf.calc_h(x))
assert max_evasive_bf_constraint >= 0
action_index = fw_coll_env_c.FwActionIndex(avail)
for ac1 in avail.get_all_actions():
for ac2 in avail.get_all_actions():
ac = FwAction(ac1, ac2)
bf_constraint = calc_bf_constraint(bf.calc_dh(x, ac), bf.calc_h(x))
assert max_evasive_bf_constraint >= bf_constraint
x_npy = np.asarray(x)[np.newaxis, :]
ac_idx = np.array([action_index.action_to_idx(ac)])
safe_u_idx = bf.choose_u(x_npy, ac_idx)
safe_u = action_index.idx_to_action(safe_u_idx)
assert calc_bf_constraint(bf.calc_dh(x, safe_u), bf.calc_h(x)) >= 0
def test_state_getter() -> None:
env = make_base_env(0)
x1 = FwSingleState(Point(50, 0, 0), 0)
x2 = FwSingleState(Point(-50, 0, 0), 0)
env.reset(x1, x2, 0.0)
assert env.t == 0
assert env.x1 == x1
assert env.x2 == x2
def test_collision() -> None:
env = make_base_env(safety_dist=5)
x1 = FwSingleState(Point(0, 1, 0), np.pi / 2)
x2 = FwSingleState(Point(0, -1, 0), -np.pi / 2)
env.reset(x1, x2, 0.0)
uhat1, uhat2 = make_uhat([0])[:2]
a1 = uhat1.calc(env.x1)
a2 = uhat2.calc(env.x2)
env.step(a1, a2)
assert env.stats.done_collision
assert env.stats.dist_to_veh == 2
def test_copy() -> None:
p = Point(0, 1, 2)
p2 = copy.deepcopy(p)
p2.x = 5
assert p2.x == 5 and p.x == 0
single_ac1 = FwSingleAction(1, 2, 3)
single_ac2 = copy.deepcopy(single_ac1)
single_ac2.v = 5
assert single_ac2.v == 5 and single_ac1.v == 1
ac = FwAction(single_ac1, single_ac2)
ac2 = copy.deepcopy(ac)
ac2.a2 = single_ac1
assert ac2.a2 == single_ac1 and ac.a2 == single_ac2
single_x1 = FwSingleState(p, 0)
single_x2 = copy.deepcopy(single_x1)
single_x2.p = p2
single_x2.th = 1
assert single_x2.p == p2 and single_x1.p == p and \
single_x2.th == 1 and single_x1.th == 0
x3 = copy.deepcopy(single_x1)
fw_dynamics(1, single_ac1, x3)
assert single_x1 != x3
x = FwState(single_x1, single_x2)
x2 = copy.deepcopy(x)
x2.x2 = single_x1
assert x2.x2 == single_x1 and x.x2 == single_x2
def test_max_time() -> None:
env = make_base_env(safety_dist=-1)
x1 = FwSingleState(Point(0, 50, 0), np.pi / 2)
x2 = FwSingleState(Point(0, -50, 0), -np.pi / 2)
env.reset(x1, x2, 0.0)
uhat1, uhat2 = make_uhat([0])[:2]
while True:
a1 = uhat1.calc(env.x1)
a2 = uhat2.calc(env.x2)
done = env.step(a1, a2)
if done:
assert env.stats.done_time
assert not env.stats.done_goal
assert not env.stats.done_collision
break
def test_bf_max_val() -> None:
avail, bf = make_barrier_func()
action_index = fw_coll_env_c.FwActionIndex(avail)
x1 = FwSingleState(Point(5000, 0, 0), 0)
x2 = FwSingleState(Point(-5000, 0, 0), 0)
x = FwState(x1, x2)
assert bf.calc_h(x) == MAX_VAL
for ac1 in avail.get_all_actions():
for ac2 in avail.get_all_actions():
ac = FwAction(ac1, ac2)
assert bf.calc_dh(x, ac) == 0
ac_idx = np.array([action_index.action_to_idx(ac)])
x_npy = np.asarray(x)[np.newaxis, :]
assert bf.choose_u(x_npy, ac_idx) == ac_idx
def test_reaches_goal() -> None:
env = make_base_env(safety_dist=-1)
x1 = FwSingleState(Point(0, 50, 0), np.pi / 2)
x2 = FwSingleState(Point(0, -50, 0), -np.pi / 2)
env.reset(x1, x2, 0.0)
uhat1, uhat2 = make_uhat([20])[:2]
while True:
a1 = uhat1.calc(env.x1)
a2 = uhat2.calc(env.x2)
done = env.step(a1, a2)
if done:
assert not env.stats.done_time
assert env.stats.done_goal
assert not env.stats.done_collision
assert env.stats.dist_to_goal1 < DONE_DIST or \
env.stats.dist_to_goal2 < DONE_DIST
assert env.stats.dist_to_veh > 100
break
def test_pickle() -> None:
env = make_base_env(safety_dist=-1)
env.x1.p = Point(1, 2, 3)
env.x1.th = 4
env.x2.p = env.x1.p
env.x2.th = env.x1.th
orig_x1 = env.x1
env_pickle = pickle.dumps(env)
env2 = pickle.loads(env_pickle)
assert orig_x1 == env2.x1
def test_point() -> None:
p1 = Point(1, 2, 3)
p2 = Point(x=0, y=1, z=2)
assert np.isclose(p1.dist(p2), np.sqrt(3), atol=1e-9)
assert p1.x == 1
assert p1.y == 2
assert p1.z == 3
assert p2.x == 0
assert p2.y == 1
assert p2.z == 2
exp_np = np.array([1, 2, 3])
actual_np = np.asarray(p1)
assert np.allclose(exp_np, actual_np, atol=1e-9)
assert np.allclose(p1, Point.from_numpy(exp_np), atol=1e-9)
p1_unpickle = pickle.loads(pickle.dumps(p1))
assert p1_unpickle == p1
def _new_state() -> FwSingleState:
point = Point(*np.random.uniform(low=(-200, -200, -0), high=(200, 200, 0)))
return FwSingleState(point, th=np.random.uniform(-np.pi, np.pi))
import pickle
from typing import Tuple
import numpy as np
import fw_coll_env_c
from fw_coll_env_c import FwAvailActions, BarrierGammaTurn, FwSingleState, \
FwState, Point, FwSingleAction, FwAction, FwCollisionEnv
DT = 0.1
V = 15
W = 12
MAX_VAL = 300
SAFETY_DIST = 5
GOAL1 = Point(200, 0, 0)
GOAL2 = Point(-200, 0, 0)
def make_barrier_func() -> Tuple[FwAvailActions, BarrierGammaTurn]:
avail_v = [15, 20, 25]
avail_w = [-W, 0, W]
avail_dz = [0]
avail = FwAvailActions(v=avail_v, w=avail_w, dz=avail_dz)
bf = BarrierGammaTurn(dt=DT,
max_val=MAX_VAL,
v=V,
w_deg_per_sec=W,
safety_dist=SAFETY_DIST,
avail_actions=avail)
return avail, bf