Misc. backfill (#1205)

* remove uses of open_agent_actor in some scenarios; limit num_cpus used in some tests * gain updates to trajectory tracking controller * Fix early end due to being off_road * Ignore sumo map errors * revert case-related changes in sstudio/type * updates from review * made it clearer that the error gains are not coming from the model config file * cleanup based on review
huawei-noah · Jan 6, 2022 · cb7ae87 · cb7ae87
1 parent 4c9935e
commit cb7ae87
Show file tree

Hide file tree

Showing 12 changed files with 91 additions and 48 deletions.
diff --git a/Makefile b/Makefile
@@ -7,7 +7,7 @@ test: build-all-scenarios
 		--doctest-modules \
 		--forked \
 		--dist=loadscope \
-		-n `nproc --ignore 2` \
+		-n `expr \( \`nproc\` \/ 2 \& \`nproc\` \> 3 \) \| 2` \
 		--nb-exec-timeout 65536 \
 		./examples/tests ./smarts/env ./envision ./smarts/contrib ./smarts/core ./smarts/sstudio ./tests \
 		--ignore=./smarts/core/tests/test_smarts_memory_growth.py \
@@ -26,7 +26,7 @@ sanity-test: build-all-scenarios
 		--forked \
 		--dist=loadscope \
 		--junitxml="sanity_test_result.xml" \
-		-n `nproc --ignore 2` \
+		-n `expr \( \`nproc\` \/ 2 \& \`nproc\` \> 3 \) \| 2` \
 		./smarts/core/tests/test_python_version.py::test_python_version \
 		./smarts/core/tests/test_sumo_version.py::test_sumo_version \
 		./smarts/core/tests/test_dynamics_backend.py::test_set_pose \
@@ -51,7 +51,7 @@ test-memory-growth: build-all-scenarios
 	rm -f .coverage*
 
 .PHONY: test-long-determinism
-test-long-determinism: 
+test-long-determinism:
 	scl scenario build --clean scenarios/minicity
 	PYTHONHASHSEED=42 pytest -v \
 		--forked \
@@ -158,3 +158,4 @@ rm-pycache:
 rm-cov:
 	find . -type f -name ".coverage.*" -delete
 	find . -type f -name ".coverage*" -delete
+
diff --git a/examples/ray_multi_instance.py b/examples/ray_multi_instance.py
@@ -3,6 +3,7 @@
 
 import gym
 import numpy as np
+import psutil
 import torch
 
 # ray[rllib] is not the part of main dependency of the SMARTS package. It needs to be installed separately
@@ -175,8 +176,12 @@ def main(
     headless,
     num_episodes,
     seed,
+    num_cpus=None,
 ):
-    ray.init()
+    num_cpus = max(
+        2, num_cpus or psutil.cpu_count(logical=False) or (psutil.cpu_count() / 2)
+    )
+    ray.init(num_cpus=max(num_cpus, 1))
     ray.wait(
         [
             train.remote(

diff --git a/examples/tests/test_examples.py b/examples/tests/test_examples.py
@@ -1,6 +1,7 @@
 import tempfile
 from pathlib import Path
 
+import psutil
 import pytest
 
 from smarts.core.utils import import_utils
@@ -37,13 +38,15 @@ def test_ray_multi_instance_example():
     from examples import ray_multi_instance
 
     main = ray_multi_instance.main
+    num_cpus = max(2, min(10, (psutil.cpu_count(logical=False) - 1)))
     main(
         training_scenarios=["scenarios/loop"],
         evaluation_scenarios=["scenarios/loop"],
         sim_name=None,
         headless=True,
         num_episodes=1,
         seed=42,
+        num_cpus=num_cpus,
     )
 
 

diff --git a/scenarios/intersections/4lane_t/requirements.txt b/scenarios/intersections/4lane_t/requirements.txt
diff --git a/scenarios/intersections/4lane_t/scenario.py b/scenarios/intersections/4lane_t/scenario.py
@@ -79,9 +79,9 @@
     "all": (
         [
             t.SocialAgentActor(
-                name="open-agent", agent_locator="open_agent:open_agent-v0"
+                name="keep-lane-agent-v0",
+                agent_locator="zoo.policies:keep-lane-agent-v0",
             ),
-            t.SocialAgentActor(name="rl-agent", agent_locator="rl_agent:rl-agent-v1"),
         ],
         [
             t.Mission(

diff --git a/scenarios/loop/requirements.txt b/scenarios/loop/requirements.txt
diff --git a/scenarios/loop/scenario.py b/scenarios/loop/scenario.py
@@ -23,10 +23,6 @@
     ]
 )
 
-open_agent_actor = t.SocialAgentActor(
-    name="open-agent", agent_locator="open_agent:open_agent-v0"
-)
-
 laner_actor = t.SocialAgentActor(
     name="keep-lane-agent",
     agent_locator="zoo.policies:keep-lane-agent-v0",
@@ -36,13 +32,13 @@
     t.Scenario(
         traffic={"basic": traffic},
         social_agent_missions={
-            "all": ([laner_actor, open_agent_actor], [t.Mission(route=t.RandomRoute())])
+            "all": ([laner_actor], [t.Mission(route=t.RandomRoute())])
         },
         bubbles=[
             t.Bubble(
                 zone=t.PositionalZone(pos=(50, 0), size=(10, 15)),
                 margin=5,
-                actor=open_agent_actor,
+                actor=laner_actor,
                 follow_actor_id=t.Bubble.to_actor_id(laner_actor, mission_group="all"),
                 follow_offset=(-7, 10),
             ),

diff --git a/scenarios/minicity/requirements.txt b/scenarios/minicity/requirements.txt
diff --git a/scenarios/minicity/scenario.py b/scenarios/minicity/scenario.py
@@ -32,10 +32,6 @@
     ]
 )
 
-open_agent_actor = t.SocialAgentActor(
-    name="open-agent", agent_locator="open_agent:open_agent-v0"
-)
-
 laner_actor = t.SocialAgentActor(
     name="keep-lane-agent",
     agent_locator="zoo.policies:keep-lane-agent-v0",
@@ -45,7 +41,7 @@
     t.Bubble(
         zone=t.PositionalZone(pos=(50, 0), size=(10, 50)),
         margin=5,
-        actor=open_agent_actor,
+        actor=laner_actor,
         follow_actor_id=t.Bubble.to_actor_id(laner_actor, mission_group="all"),
         follow_offset=(-7, 10),
     )
@@ -62,15 +58,15 @@
     t.Bubble(
         zone=t.PositionalZone(pos=(1012.19, 1084.20), size=(30, 30)),
         margin=5,
-        actor=open_agent_actor,
+        actor=laner_actor,
     )
 ]
 
 gen_scenario(
     t.Scenario(
         traffic={"basic": traffic},
         social_agent_missions={
-            "all": ([laner_actor, open_agent_actor], [t.Mission(route=t.RandomRoute())])
+            "all": ([laner_actor], [t.Mission(route=t.RandomRoute())])
         },
         bubbles=[*travelling_bubbles, *static_bubbles],
     ),

diff --git a/smarts/core/controllers/trajectory_tracking_controller.py b/smarts/core/controllers/trajectory_tracking_controller.py
@@ -193,37 +193,70 @@ def perform_trajectory_tracking_PD(
         velocity_damping_gain = params["velocity_damping_gain"]
         windup_gain = params["windup_gain"]
 
-        curvature_radius = TrajectoryTrackingController.curvature_calculation(
-            trajectory
-        )
-        # The gains are varying according to the desired velocity along
+        # XXX: note that these values may be further adjusted below based on speed and curvature
+        # XXX: we might want to combine the computation of these into a single fn
+        lateral_gain = 0.61
+        heading_gain = 0.01
+        lateral_error_derivative_gain = 0.15
+        heading_error_derivative_gain = 0.5
+
+        # The gains can vary according to the desired velocity along
         # the trajectory. To achieve this, the desired speed is normalized
         # between 20 (km/hr) to 80 (km/hr) and the respective gains are
         # calculated using interpolation. 3, 0.03, 1.5, 0.2 are the
         # controller gains for lateral error, heading error and their
-        #  derivatives at the desired speed 20 (km/hr).
+        # derivatives at the desired speed 20 (km/hr).
         normalized_speed = np.clip(
             (METER_PER_SECOND_TO_KM_PER_HR * trajectory[3][0] - 20) / (80 - 20), 0, 1
         )
-        lateral_gain = lerp(3, final_lateral_gain, normalized_speed)
-        heading_gain = lerp(0.03, final_heading_gain, normalized_speed)
+        adjust_gains_for_normalized_speed = False  # XXX: not using model config here
+        if adjust_gains_for_normalized_speed:
+            lateral_gain = lerp(3, final_lateral_gain, normalized_speed)
+            lateral_error_derivative_gain = lerp(
+                0.2, final_lateral_error_derivative_gain, normalized_speed
+            )
+            heading_gain = lerp(0.03, final_heading_gain, normalized_speed)
+            heading_error_derivative_gain = lerp(
+                1.5, final_heading_error_derivative_gain, normalized_speed
+            )
+            steering_filter_constant = lerp(
+                12, final_steering_filter_constant, normalized_speed
+            )
+        elif vehicle.speed > 70 / METER_PER_SECOND_TO_KM_PER_HR:
+            lateral_gain = 1.51
+            heading_error_derivative_gain = 0.1
+
+        # XXX: This should be handled like the other gains above...
         steering_filter_constant = lerp(
-            2, final_steering_filter_constant, normalized_speed
-        )
-        heading_error_derivative_gain = lerp(
-            1.5, final_heading_error_derivative_gain, normalized_speed
-        )
-        lateral_error_derivative_gain = lerp(
-            0.2, final_lateral_error_derivative_gain, normalized_speed
+            12, final_steering_filter_constant, normalized_speed
         )
 
+        if abs(min_angles_difference_signed(trajectory[2][-1], trajectory[2][0])) > 2:
+            throttle_filter_constant = 2.5
+
+        if (
+            abs(
+                TrajectoryTrackingController.curvature_calculation(
+                    trajectory, 0, num_points=3
+                )
+            )
+            < 150
+        ):
+            heading_gain = 0.05
+            lateral_error_derivative_gain = 0.015
+            heading_error_derivative_gain = 0.05
+
         (
             heading_error,
             lateral_error,
         ) = TrajectoryTrackingController.calulate_heading_lateral_error(
             vehicle, trajectory, initial_look_ahead_distant, speed_reduction_activation
         )
 
+        curvature_radius = TrajectoryTrackingController.curvature_calculation(
+            trajectory
+        )
+
         # Derivative terms of the controller (use with caution for large time steps>=0.1).
         # Increasing the values will increase the convergence time and reduces the oscillation.
         z_yaw = vehicle.chassis.velocity_vectors[1][2]
@@ -326,7 +359,7 @@ def calculate_raw_throttle_feedback(
         velocity_error = vehicle.speed - desired_speed
         velocity_error_damping_term = (velocity_error - state.velocity_error) / dt_sec
         raw_throttle = METER_PER_SECOND_TO_KM_PER_HR * (
-            -1 * velocity_gain * velocity_error
+            -0.5 * velocity_gain * velocity_error
             - velocity_integral_gain
             * (integral_velocity_error + windup_gain * state.integral_windup_error)
             - velocity_damping_gain * velocity_error_damping_term

diff --git a/smarts/core/models/controller_parameters.yaml b/smarts/core/models/controller_parameters.yaml
@@ -3,7 +3,7 @@ sedan:
             final_heading_gain: .15
             final_lateral_gain: 4.65
             final_steering_filter_constant: 23.5
-            throttle_filter_constant: 2.5
+            throttle_filter_constant: 22.5
             velocity_gain: 5.1
             velocity_integral_gain: 0
             traction_gain: 6
@@ -20,7 +20,7 @@ sedan:
             max_turn_radius : 2.2
             wheel_radius : 0.31265
             max_torque : 1600
-            max_btorque : 1100
+            max_btorque : 1400
             max_steering : 12.56
             steering_gear_ratio : 17.4
 

diff --git a/smarts/sstudio/types.py b/smarts/sstudio/types.py
@@ -22,6 +22,7 @@
 import logging
 import pickle
 import random
+from enum import IntEnum
 from ctypes import c_int64
 from dataclasses import dataclass, field
 from sys import maxsize
@@ -45,6 +46,11 @@
 from smarts.core.utils.math import rotate_around_point
 
 
+class _SUMO_PARAMS_MODE(IntEnum):
+    TITLE_CASE = 0
+    KEEP_SNAKE_CASE = 1
+
+
 def _pickle_hash(obj) -> int:
     pickle_bytes = pickle.dumps(obj, protocol=4)
     hasher = hashlib.md5()
@@ -59,16 +65,28 @@ class _SumoParams(collections_abc.Mapping):
     between PEP8-compatible naming and Sumo's.
     """
 
-    def __init__(self, prefix, whitelist=[], **kwargs):
+    def __init__(
+        self, prefix, whitelist=[], mode=_SUMO_PARAMS_MODE.TITLE_CASE, **kwargs
+    ):
         def snake_to_title(word):
             return "".join(x.capitalize() or "_" for x in word.split("_"))
 
+        def keep_snake_case(word: str):
+            w = word[0].upper() + word[1:]
+            return "".join(x or "_" for x in w.split("_"))
+
+        func: function = snake_to_title
+        if mode == _SUMO_PARAMS_MODE.TITLE_CASE:
+            pass
+        elif mode == _SUMO_PARAMS_MODE.KEEP_SNAKE_CASE:
+            func = keep_snake_case
+
         # XXX: On rare occasions sumo doesn't respect their own conventions
         #      (e.x. junction model's impatience).
         self._params = {key: kwargs.pop(key) for key in whitelist if key in kwargs}
 
         for key, value in kwargs.items():
-            self._params[f"{prefix}{snake_to_title(key)}"] = value
+            self._params[f"{prefix}{func(key)}"] = value
 
     def __iter__(self):
         return iter(self._params)
@@ -90,7 +108,7 @@ class LaneChangingModel(_SumoParams):
     """Models how the actor acts with respect to lane changes."""
 
     def __init__(self, **kwargs):
-        super().__init__("lc", **kwargs)
+        super().__init__("lc", mode=_SUMO_PARAMS_MODE.KEEP_SNAKE_CASE, **kwargs)
 
 
 class JunctionModel(_SumoParams):