def get_smoothness(trace: List[Trace], agent: Agent) -> float:
if (not trace) or trace[-1].position != agent.position:
trace.append(Trace(agent.position))
angle = 0
prev_angle = None
for i in range(1, len(trace)):
if (not trace[i - 1].position == trace[i].position):
unit_vector1 = np.array(trace[i - 1].position -
trace[i].position) / np.linalg.norm(
np.array(trace[i - 1].position -
trace[i].position))
t = [0] * (len(agent.position) - 1)
t = tuple([1] + t)
dot_product = np.dot(unit_vector1, t)
if prev_angle is None:
prev_angle = np.arccos(dot_product)
continue
angle += abs(prev_angle - np.arccos(dot_product))
prev_angle = np.arccos(dot_product)
return (angle / len(trace))
def get_euclidean_distance_traveled(trace: List[Trace],
agent: Agent) -> float:
trace.append(Trace(agent.position))
dist = 0
for i in range(1, len(trace)):
dist += np.linalg.norm(
np.array(trace[i - 1].position) - np.array(trace[i].position))
return dist
def test_move_agent_out_of_bounds(self) -> None:
map1: SparseMap = DenseMap([
[DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID],
[DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.GOAL_ID],
]).convert_to_sparse_map()
map1.move_agent(Point(-1, 0))
self.assertEqual(Point(0, 1), map1.agent.position)
self.assertEqual([Trace(Point(0, 1))], map1.trace)
def test_move_agent_out_of_bounds(self) -> None:
map1: DenseMap = DenseMap([
[DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID],
[DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.GOAL_ID],
])
map1.move_agent(Point(-1, 0))
self.assertEqual(DenseMap.AGENT_ID, map1.at(Point(0, 1)))
self.assertEqual([Trace(Point(0, 1))], map1.trace)
def test_move_agent_normal(self) -> None:
map1: DenseMap = DenseMap([
[DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID],
[DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.GOAL_ID],
])
map1.move_agent(Point(1, 1))
self.assertEqual(Point(1, 1), map1.agent.position)
self.assertEqual(DenseMap.AGENT_ID, map1.at(Point(1, 1)))
self.assertTrue([Trace(Point(1, 1))], map1.trace)
def test_move_agent_normal(self) -> None:
map1: DenseMap = DenseMap(
[[[DenseMap.CLEAR_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID],
[DenseMap.AGENT_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID]],
[[DenseMap.CLEAR_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.CLEAR_ID, DenseMap.CLEAR_ID,
DenseMap.GOAL_ID]]]).convert_to_sparse_map()
map1.move_agent(Point(0, 0, 0))
self.assertEqual(Point(0, 0, 0), map1.agent.position)
self.assertTrue([Trace(Point(0, 0, 0))], map1.trace)
def next_position_index_label(mp: Map) -> List[torch.Tensor]:
move_indexes: List[int] = list(map(
lambda el: mp.get_move_index(
MapProcessing.__get_pos(el[1]) - MapProcessing.__get_pos(el[0])),
zip(mp.trace, mp.trace[1:] + [Trace(mp.goal.position)]))
)
res: List[torch.Tensor] = []
for move_idx in move_indexes:
res.append(torch.tensor(float(move_idx)))
return res
def move(self, entity: Entity, to: Point, no_trace: bool = False) -> bool:
"""
All maps should override this method
:param entity: The entity to move (currently only agent is supported)
:param to: The destination
:param no_trace: If we want to add the movement to the map history
"""
if not no_trace:
self.trace.append(Trace(self.agent.position))
return True
def test_move_agent_out_of_bounds(self) -> None:
map1: DenseMap = DenseMap([
[[DenseMap.CLEAR_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.AGENT_ID, DenseMap.CLEAR_ID,
DenseMap.EXTENDED_WALL_ID]],
[[DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.GOAL_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID]]
])
map1.move_agent(Point(-1, 0, 0))
self.assertEqual(Point(0, 1, 0), map1.agent.position)
self.assertEqual(DenseMap.AGENT_ID, map1.grid[0, 1, 0])
self.assertEqual([Trace(Point(0, 1, 0))], map1.trace)
def test_move_agent_normal(self) -> None:
map1: DenseMap = DenseMap([
[[DenseMap.CLEAR_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.AGENT_ID, DenseMap.CLEAR_ID,
DenseMap.EXTENDED_WALL_ID]],
[[DenseMap.WALL_ID, DenseMap.WALL_ID, DenseMap.WALL_ID],
[DenseMap.GOAL_ID, DenseMap.CLEAR_ID, DenseMap.CLEAR_ID]]
])
map1.move_agent(Point(0, 1, 1))
self.assertEqual(Point(0, 1, 1), map1.agent.position)
self.assertTrue(DenseMap.AGENT_ID, map1.grid[0, 1, 1])
self.assertTrue([Trace(Point(0, 1, 1))], map1.trace)
def test_ne_instance(self) -> None:
entity1: Trace = Trace(Point(2, 3))
entity2: Entity = Entity(Point(2, 3), 1)
self.assertNotEqual(entity1, entity2)
def test_ne_all(self) -> None:
entity1: Trace = Trace(Point(2, 3))
entity2: Trace = Trace(Point(1, 15))
self.assertNotEqual(entity1, entity2)
def test_eq(self) -> None:
entity1: Trace = Trace(Point(2, 3))
entity2: Trace = Trace(Point(2, 3))
self.assertEqual(entity1, entity2)
def test_str(self) -> None:
entity: Trace = Trace(Point(2, 3))
self.assertEqual("Trace: {position: Point(2, 3)}", str(entity))
def test_deep_copy(self) -> None:
entity1: Trace = Trace(Point(2, 3))
entity2: Trace = copy.deepcopy(entity1)
self.assertEqual(entity1, entity2)
def next_position_label(mp: Map) -> List[torch.Tensor]:
return list(map(
lambda el: mp.get_move_along_dir(
MapProcessing.__get_pos(el[1]) - MapProcessing.__get_pos(el[0])).to_tensor(),
zip(mp.trace, mp.trace[1:] + [Trace(mp.goal.position)]))
)