Fix agent-mission combination (#1868)

* Add permutation utility. * Update test. * Fix agent-mission combinations. * Fix tests. * Fix toggle of headless. * Clarify method use * Apply suggestions from code review Co-authored-by: Saul Field <[email protected]> Co-authored-by: adai <[email protected]> * Add warning for mission count mismatch. --------- Co-authored-by: Saul Field <[email protected]> Co-authored-by: adai <[email protected]>
huawei-noah · Feb 21, 2023 · dabd441 · dabd441
1 parent 1cdcf99
commit dabd441
Show file tree

Hide file tree

Showing 6 changed files with 122 additions and 71 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -10,9 +10,12 @@ Copy and pasting the git commit messages is __NOT__ enough.
 
 ## [Unreleased]
 ### Added
+- Added a math utility for generating combination groups out of two sequences with unique index use per group. This is intended for use to generate the combinations needed to give a unique agent-mission set per reset.
+- Added basic tests for `hiway-v1` resetting and unformatted observations and actions.
 ### Changed
 ### Deprecated
 ### Fixed
+- Ensured that `hiwayenv.reset` provides unique agent-mission sets per reset.
 ### Removed
 ### Security
 

diff --git a/envision/tests/test_data_formatter.py b/envision/tests/test_data_formatter.py
@@ -289,60 +289,15 @@ def test_complex_data(complex_data):
 def sim_data():
     return [
         0.1,
-        None,  # scene_id
-        "straight",  # scene_name
+        None,
+        "straight",
         [
-            [  # Traffic actor
-                0,  # id
-                1,  # lane id
-                -1.84,  # x
-                -3.2,  # y
-                0.0,  # z
-                -1.57,  # heading
-                4.62,  # speed
-                None,  # events
-                [],  # wapypoint paths
-                [],  # driven path
-                [],  # lidar
-                [],  # geometry
-                0,  # actor type
-                4,  # vehicle type
-            ],
-            [
-                2,
-                1,
-                -1.84,
-                0.0,
-                0.0,
-                -1.57,
-                5.0,
-                None,
-                [],
-                [],
-                [],
-                [],
-                0,
-                4,
-            ],
-            [
-                3,
-                1,
-                -1.84,
-                3.2,
-                0.0,
-                -1.57,
-                4.91,
-                None,
-                [],
-                [],
-                [],
-                [],
-                0,
-                4,
-            ],
+            [0, 1, -1.84, -3.2, 0.0, -1.57, 5.05, None, [], [], [], [], 0, 4],
+            [2, 1, -1.84, 0.0, 0.0, -1.57, 5.05, None, [], [], [], [], 0, 4],
+            [3, 1, -1.84, 3.2, 0.0, -1.57, 4.88, None, [], [], [], [], 0, 4],
         ],
         [],
-        [[90.0, -10.0, 90.0, 10.0, 110.0, 10.0, 110.0, -10.0, 90.0, -10.0]],  # bubbles
+        [[90.0, -10.0, 90.0, 10.0, 110.0, 10.0, 110.0, -10.0, 90.0, -10.0]],
         [],
         [4],
         {
@@ -448,6 +403,7 @@ def side_effect(state: State):
             es.add_any(state)
 
             data = es.resolve()
+
             assert data == sim_data
             assert data == unpack(data)
 

diff --git a/smarts/core/scenario.py b/smarts/core/scenario.py
@@ -60,7 +60,11 @@
 from smarts.core.traffic_history import TrafficHistory
 from smarts.core.utils.file import make_dir_in_smarts_log_dir, path2hash
 from smarts.core.utils.id import SocialAgentId
-from smarts.core.utils.math import radians_to_vec, vec_to_radians
+from smarts.core.utils.math import (
+    combination_pairs_with_unique_indices,
+    radians_to_vec,
+    vec_to_radians,
+)
 from smarts.sstudio import types as sstudio_types
 from smarts.sstudio.types import MapSpec
 from smarts.sstudio.types import Via as SSVia
@@ -213,7 +217,7 @@ def variations_for_all_scenario_roots(
             agent_missions = Scenario.discover_agent_missions(
                 scenario_root, agents_to_be_briefed
             )
-            agent_missions = [dict(zip(agents_to_be_briefed, agent_missions))]
+
             social_agent_infos = Scenario._discover_social_agents_info(scenario_root)
             social_agents = [
                 {
@@ -229,20 +233,27 @@ def variations_for_all_scenario_roots(
             # `or [None]` so that product(...) will not return an empty result
             # but insted a [(..., `None`), ...].
             agent_missions = agent_missions or [None]
+            if len(agents_to_be_briefed) == len(agent_missions):
+                warnings.warn(
+                    f"Scenario `{scenario_root}` has {len(agent_missions)} missions and"
+                    f" but there are {len(agents_to_be_briefed)} agents to assign"
+                    " missions to. The missions will be padded with random missions."
+                )
+            mission_agent_groups = combination_pairs_with_unique_indices(
+                agents_to_be_briefed, agent_missions
+            )
             social_agents = social_agents or [None]
             traffic_histories = Scenario.discover_traffic_histories(scenario_root) or [
                 None
             ]
             traffic = Scenario.discover_traffic(scenario_root) or [[]]
 
             roll_traffic = 0
-            roll_agent_missions = 0
             roll_social_agents = 0
             roll_traffic_histories = 0
 
             if shuffle_scenarios:
                 roll_traffic = random.randint(0, len(traffic))
-                roll_agent_missions = random.randint(0, len(agent_missions))
                 roll_social_agents = random.randint(0, len(social_agents))
                 roll_traffic_histories = 0  # random.randint(0, len(traffic_histories))
 
@@ -253,7 +264,7 @@ def variations_for_all_scenario_roots(
                 concrete_traffic_history,
             ) in product(
                 np.roll(traffic, roll_traffic, 0),
-                np.roll(agent_missions, roll_agent_missions, 0),
+                mission_agent_groups,
                 np.roll(social_agents, roll_social_agents, 0),
                 np.roll(traffic_histories, roll_traffic_histories, 0),
             ):
@@ -276,7 +287,7 @@ def variations_for_all_scenario_roots(
                     scenario_root,
                     traffic_specs=concrete_traffic,
                     missions={
-                        **(concrete_agent_missions or {}),
+                        **{a_id: mission for a_id, mission in concrete_agent_missions},
                         **concrete_social_agent_missions,
                     },
                     social_agents=concrete_social_agents,
@@ -321,18 +332,6 @@ def discover_agent_missions(scenario_root, agents_to_be_briefed):
         if not missions:
             missions = [None for _ in range(len(agents_to_be_briefed))]
 
-        if len(agents_to_be_briefed) == 1:
-            # single-agent, so we cycle through all missions individually.
-            return missions
-        elif len(agents_to_be_briefed) > 1:
-            # multi-agent, so we assume missions "drive" the agents (i.e. one
-            # mission per agent) and we will not be cycling through missions.
-            assert not missions or len(missions) == len(agents_to_be_briefed), (
-                "You must either provide an equal number of missions ({}) to "
-                "agents ({}) or provide no missions at all so they can be "
-                "randomly generated.".format(len(missions), len(agents_to_be_briefed))
-            )
-
         return missions
 
     @staticmethod

diff --git a/smarts/core/tests/test_observations.py b/smarts/core/tests/test_observations.py
@@ -101,7 +101,7 @@ def agent_spec(agent_interface):
 def env(agent_spec):
     env = gym.make(
         "smarts.env:hiway-v0",
-        scenarios=["scenarios/sumo/loop"],
+        scenarios=["scenarios/sumo/figure_eight"],
         agent_specs={AGENT_ID: agent_spec},
         headless=True,
         visdom=False,

diff --git a/smarts/core/utils/math.py b/smarts/core/utils/math.py
@@ -19,6 +19,7 @@
 # THE SOFTWARE.
 import math
 from dataclasses import dataclass
+from itertools import chain, permutations, product, repeat
 from math import factorial
 from typing import Callable, List, Sequence, Tuple, Union
 
@@ -584,3 +585,67 @@ def running_mean(prev_mean: float, prev_step: int, new_val: float) -> Tuple[floa
     new_step = prev_step + 1
     new_mean = prev_mean + (new_val - prev_mean) / new_step
     return new_mean, new_step
+
+
+def _unique_element_combination(first, second):
+    """Generates a combination of set of values result groupings only contain values from unique
+    indices. Only works if len(first_group) <= len(second_group).
+    _unique_element_combination('ab', '123') -> (a1 b2) (a1 b3) (a2 b1) (a2 b3) (a3 b1) (a3 b2)
+    _unique_element_combination('abc', '1') -> []
+
+    Args:
+        first (Sequence): A sequence of values.
+        second (Sequence): Another sequence of values.
+
+    Yields:
+        Tuple[Tuple[Any, Any], ...]:
+            A set of index to index combinations. [] if len(first) > len(second)
+    """
+    result = [[a] for a in first]
+    sl = len(second)
+    rl = len(first)
+    perms = list(permutations(range(sl), r=rl))
+    for i, p in enumerate(perms):
+        yield tuple(
+            tuple(result[(i * rl + j) % rl] + [second[idx]]) for j, idx in enumerate(p)
+        )
+
+
+def combination_pairs_with_unique_indices(
+    first_group, second_group, second_group_default=None
+):
+    """Generates sets of combinations that use up all of the first group and at least as many of
+    the second group. If len(first_group) > len(second_group) the second group is padded. Groups
+    are combined using only unique indices per result. The value at an index in one group is
+    matched to the value at an index in from the second group. Duplicate results only appear when
+    element values repeat one of the base groups.
+
+    ordered_combinations('ab', '123') -> (a1 b2) (a1 b3) (a2 b1) (a2 b3) (a3 b1) (a3 b2)
+    ordered_combinations('ab', '1', default="k") -> (a1 bk) (ak b1)
+
+    Args:
+        first_group (Sequence): A sequence of values.
+        second_group (Sequence): Another sequence of values which may be padded.
+        default (Any, optional): The default used to pad the second group. Defaults to None.
+
+    Returns:
+        Generator[Tuple[Tuple[Any, Any], ...], None, None]:
+            Unique index to index pairings using up all elements of the first group and as many of
+            the second group.
+    """
+    len_first = len(first_group)
+    len_second = len(second_group)
+    if len_second == 0:
+        return product(first_group, [second_group_default])
+    if len_first > len_second:
+        return _unique_element_combination(
+            first_group,
+            list(
+                chain(
+                    second_group, repeat(second_group_default, len_first - len_second)
+                )
+            ),
+        )
+    if len_first <= len_second:
+        return _unique_element_combination(first_group, second_group)
+    return []
diff --git a/smarts/core/utils/tests/test_math.py b/smarts/core/utils/tests/test_math.py
@@ -21,7 +21,11 @@
 # THE SOFTWARE.
 import numpy as np
 
-from smarts.core.utils.math import position_to_ego_frame, world_position_from_ego_frame
+from smarts.core.utils.math import (
+    combination_pairs_with_unique_indices,
+    position_to_ego_frame,
+    world_position_from_ego_frame,
+)
 
 
 def test_egocentric_conversion():
@@ -36,3 +40,27 @@ def test_egocentric_conversion():
     p_end = world_position_from_ego_frame(pec, pe, he)
 
     assert np.allclose(p_end, p_start)
+
+
+def test_combination_pairs_with_unique_indices():
+
+    assert not tuple(combination_pairs_with_unique_indices("", ""))
+    assert tuple(combination_pairs_with_unique_indices("a", "")) == (("a", None),)
+    assert tuple(
+        combination_pairs_with_unique_indices("abc", "12", second_group_default=4)
+    ) == (
+        (("a", "1"), ("b", "2"), ("c", 4)),
+        (("a", "1"), ("b", 4), ("c", "2")),
+        (("a", "2"), ("b", "1"), ("c", 4)),
+        (("a", "2"), ("b", 4), ("c", "1")),
+        (("a", 4), ("b", "1"), ("c", "2")),
+        (("a", 4), ("b", "2"), ("c", "1")),
+    )
+    assert tuple(combination_pairs_with_unique_indices("ab", "123")) == (
+        (("a", "1"), ("b", "2")),
+        (("a", "1"), ("b", "3")),
+        (("a", "2"), ("b", "1")),
+        (("a", "2"), ("b", "3")),
+        (("a", "3"), ("b", "1")),
+        (("a", "3"), ("b", "2")),
+    )