Replace scipy (#2071)

* Remove TruncatedDistribution from sstudio

* Replace scipy cdist call with numpy equivalent

* Replace scipy.signal.place_poles with numpy implementation of Ackermann's formula

* Update changelog and remove unused code

* Fix docs

* Replace scipy KDTree in lanepoints

* Fixes and changelog updates

* Fix types

CHANGELOG.md

@@ -18,11 +18,14 @@ Copy and pasting the git commit messages is __NOT__ enough.
 - Updated `RLlibHiWayEnv` to use the `gymnasium` interface.
 - Renamed `rllib/rllib.py` to `rllib/pg_pbt_example.py`.
 - Loosened constraint of `gymnasium` from `==0.27.0` to `>=0.26.3`.
+- `LaneFollowingController` now uses a different pole placement method to compute lateral/heading gains. Numerical behaviour is unchanged. Performance is slightly faster.
 ### Deprecated
 ### Fixed
 - Missing neighborhood vehicle state `'lane_id'` is now added to the `hiway-v1` formatted observations.
 - Fixed a regression where `pybullet` build time messages returned.
 ### Removed
+- Removed `TruncatedDistribution` from scenario studio.
+- Removed `scipy` as a core package dependency.
 ### Security
 
 ## [1.2.0] # 2023-06-14

docs/spelling_wordlist.txt

@@ -4,6 +4,7 @@
 Δy
 Δheading
 ΔTime
+ẋ
 accelerometer
 Ackermann
 acyclic

setup.cfg

@@ -42,7 +42,6 @@ install_requires =
     # The following are planned for removal
     gym>=0.17.3,<=0.19.0
     cloudpickle>=1.3.0,<=2.1.0
-    scipy
 
 [options.packages.find]
 exclude =

smarts/core/argoverse_map.py

@@ -980,7 +980,7 @@ def waypoint_paths(
         # No route provided, so generate paths based on the closest lanepoints
         waypoint_paths = []
         closest_lps: List[LanePoint] = self._lanepoints.closest_lanepoints(
-            pose, within_radius=within_radius, maximum_count=15
+            pose, maximum_count=15
         )
         closest_lane: RoadMap.Lane = closest_lps[0].lane
 

smarts/core/controllers/lane_following_controller.py

@@ -20,7 +20,6 @@
 import math
 
 import numpy as np
-from scipy import signal
 
 from smarts.core.chassis import AckermannChassis
 from smarts.core.controllers.trajectory_tracking_controller import (
@@ -245,13 +244,6 @@ def perform_lane_following(
             vehicle_look_ahead_pt
         )
 
-        abs_heading_error = min(
-            abs((vehicle.heading % (2 * math.pi)) - reference_heading),
-            abs(
-                2 * math.pi - abs((vehicle.heading % (2 * math.pi)) - reference_heading)
-            ),
-        )
-
         curvature_radius = TrajectoryTrackingController.curvature_calculation(
             trajectory
         )
@@ -376,6 +368,31 @@ def find_current_lane(wp_paths, vehicle_position):
         ]
         return np.argmin(relative_distant_lane)
 
+    @staticmethod
+    def place_poles(A: np.ndarray, B: np.ndarray, poles: np.ndarray) -> np.ndarray:
+        """Given a linear system described by ẋ = Ax + Bu, compute the gain matrix K
+        such that the closed loop eigenvalues of A - BK are in the desired pole locations.
+
+        References:
+         - https://docs.scipy.org/doc/scipy/reference/generated/scipy.signal.place_poles.html
+         - https://en.wikipedia.org/wiki/Ackermann%27s_formula
+        """
+
+        # Controllability matrix
+        C = np.hstack(
+            [B] + [np.linalg.matrix_power(A, i) @ B for i in range(1, A.shape[0])]
+        )
+
+        # Desired characteristic polynomial of A - BK
+        poly = np.real(np.poly(poles))
+
+        # Solve using Ackermann's formula
+        n = np.size(poly)
+        p = poly[n - 1] * np.linalg.matrix_power(A, 0)
+        for i in np.arange(1, n):
+            p = p + poly[n - i - 1] * np.linalg.matrix_power(A, i)
+        return np.linalg.solve(C, p)[-1][:]
+
     @staticmethod
     def calculate_lateral_gains(sim, state, vehicle, desired_poles, target_speed):
         """Update the state lateral gains"""
@@ -420,18 +437,15 @@ def calculate_lateral_gains(sim, state, vehicle, desired_poles, target_speed):
                     [road_stiffness / (vehicle_mass * target_speed)],
                 ]
             )
-            fsf1 = signal.place_poles(
-                state_matrix, input_matrix, desired_poles, method="KNV0"
+            K = LaneFollowingController.place_poles(
+                state_matrix, input_matrix, desired_poles
             )
-            # 0.01 and 0.015 denote the max and min gains for heading controller
-            # This is done to ensure that the linearization error will not affect
-            # the stability of the controller.
-            state.heading_error_gain = np.clip(fsf1.gain_matrix[0][1], 0.02, 0.04)
-            # 3.4 and 4.1 denote the max and min gains for lateral error controller
-            # As for heading, this is done to ensure that the linearization error
-            # will not affect the stability and performance of the controller.
-            state.lateral_error_gain = np.clip(fsf1.gain_matrix[0][0], 3.4, 4.1)
 
+            # These constants are the min/max gains for the lateral error controller
+            # and the heading controller, respectively. This is done to ensure that
+            # the linearization error will not affect the stability of the controller.
+            state.lateral_error_gain = np.clip(K[0], 3.4, 4.1)
+            state.heading_error_gain = np.clip(K[1], 0.02, 0.04)
         else:
             # 0.01 and 0.36 are initial values for heading and lateral gains
             # This is only done to ensure that the vehicle starts to move for

smarts/core/coordinates.py

@@ -311,6 +311,7 @@ def __post_init__(self):
         assert len(self.position) <= 3
         if len(self.position) < 3:
             self.position = np.resize(self.position, 3)
+            self.position[-1] = 0
         assert len(self.orientation) == 4
         if not isinstance(self.orientation, np.ndarray):
             self.orientation = np.array(self.orientation, dtype=np.float64)

smarts/core/lanepoints.py

@@ -28,15 +28,10 @@
 from typing import List, NamedTuple, Sequence, Tuple
 
 import numpy as np
-from scipy.spatial import KDTree
 
 from smarts.core.coordinates import Heading, Point, Pose
 from smarts.core.road_map import RoadMap
-from smarts.core.utils.math import (
-    fast_quaternion_from_angle,
-    squared_dist,
-    vec_to_radians,
-)
+from smarts.core.utils.math import fast_quaternion_from_angle, vec_to_radians
 
 
 @dataclass(frozen=True)
@@ -76,7 +71,9 @@ def __init__(self, shape_lps: List[LinkedLanePoint], spacing: float):
         self._linked_lanepoints = LanePoints._interpolate_shape_lanepoints(
             shape_lps, spacing
         )
-        self._lanepoints_kd_tree = LanePoints._build_kd_tree(self._linked_lanepoints)
+        self._lp_points = np.array(
+            [l_lp.lp.pose.point.as_np_array[:2] for l_lp in self._linked_lanepoints]
+        )
 
         self._lanepoints_by_lane_id = defaultdict(list)
         self._lanepoints_by_edge_id = defaultdict(list)
@@ -85,13 +82,13 @@ def __init__(self, shape_lps: List[LinkedLanePoint], spacing: float):
             self._lanepoints_by_lane_id[linked_lp.lp.lane.lane_id].append(linked_lp)
             self._lanepoints_by_edge_id[lp_edge_id].append(linked_lp)
 
-        self._lanepoints_kd_tree_by_lane_id = {
-            lane_id: LanePoints._build_kd_tree(l_lps)
+        self._lp_points_by_lane_id = {
+            lane_id: np.array([l_lp.lp.pose.point.as_np_array[:2] for l_lp in l_lps])
             for lane_id, l_lps in self._lanepoints_by_lane_id.items()
         }
 
-        self._lanepoints_kd_tree_by_edge_id = {
-            edge_id: LanePoints._build_kd_tree(l_lps)
+        self._lp_points_by_edge_id = {
+            edge_id: np.array([l_lp.lp.pose.point.as_np_array[:2] for l_lp in l_lps])
             for edge_id, l_lps in self._lanepoints_by_edge_id.items()
         }
 
@@ -180,7 +177,7 @@ def _shape_lanepoints_along_lane(
                     lp=LanePoint(
                         lane=curr_lanepoint.lp.lane,
                         pose=Pose(
-                            position=lane_shape[-1],
+                            position=lane_shape[-1][:2],
                             orientation=curr_lanepoint.lp.pose.orientation,
                         ),
                         lane_width=lane_width,
@@ -610,13 +607,6 @@ def _shape_lanepoints_along_lane(
 
         return cls(shape_lps, spacing)
 
-    @staticmethod
-    def _build_kd_tree(linked_lps: Sequence[LinkedLanePoint]) -> KDTree:
-        return KDTree(
-            np.array([l_lp.lp.pose.point.as_np_array[:2] for l_lp in linked_lps]),
-            leafsize=50,
-        )
-
     @staticmethod
     def _interpolate_shape_lanepoints(
         shape_lanepoints: Sequence[LinkedLanePoint], spacing: float
@@ -760,151 +750,70 @@ def _process_interp_for_lane_lp(
         return curr_lanepoint
 
     @staticmethod
-    def _closest_linked_lp_in_kd_tree_batched(
-        points: Sequence[Point],
-        linked_lps,
-        tree: KDTree,
+    def _closest_linked_lp_to_point(
+        point: Point,
+        linked_lps: List[LinkedLanePoint],
+        points: np.ndarray,
         k: int = 1,
         filter_composites: bool = False,
-    ):
-        p2ds = np.array([p.as_np_array[:2] for p in points])
-        _, closest_indices = tree.query(p2ds, k=min(k, len(linked_lps)))
-        closest_indices = np.atleast_2d(closest_indices)
+    ) -> List[LinkedLanePoint]:
+        x = point.as_np_array[:2]
+        dists = np.sqrt(np.sum((points - x) ** 2, axis=1))
+        closest_indices = np.argsort(dists)[:k]
+
         if filter_composites:
             result = [
-                [
-                    linked_lps[idx]
-                    for idx in idxs
-                    if not linked_lps[idx].lp.lane.is_composite
-                ]
-                for idxs in closest_indices
+                linked_lps[idx]
+                for idx in closest_indices
+                if not linked_lps[idx].lp.lane.is_composite
             ]
             if result:
                 return result
-        # if filtering, only return lane-points in composite lanes if we didn't hit any in simple lanes...
-        return [[linked_lps[idx] for idx in idxs] for idxs in closest_indices]
 
-    @staticmethod
-    def _closest_linked_lp_in_kd_tree_with_pose_batched(
-        poses,
-        lanepoints,
-        tree,
-        within_radius: float,
-        k: int = 10,
-        filter_composites: bool = False,
-    ):
-        linked_lanepoints = LanePoints._closest_linked_lp_in_kd_tree_batched(
-            [pose.point for pose in poses],
-            lanepoints,
-            tree,
-            k=k,
-            filter_composites=filter_composites,
-        )
-
-        linked_lanepoints = [
-            sorted(
-                l_lps,
-                key=lambda _llp: squared_dist(
-                    poses[idx].as_position2d(), _llp.lp.pose.as_position2d()
-                ),
-            )
-            for idx, l_lps in enumerate(linked_lanepoints)
-        ]
-        # exclude those outside radius except closest
-        if within_radius is not None:
-            radius_sq = within_radius * within_radius
-            linked_lanepoints = [
-                [
-                    _llp
-                    for i, _llp in enumerate(_llps)
-                    if squared_dist(
-                        poses[idx].as_position2d(), _llp.lp.pose.as_position2d()
-                    )
-                    <= radius_sq
-                    or i == 0
-                ]
-                for idx, _llps in enumerate(linked_lanepoints)
-            ]
-        # Get the nearest point for the points where the radius check failed
-        unfound_lanepoints = [
-            (i, poses[i])
-            for i, group in enumerate(linked_lanepoints)
-            if len(group) == 0
-        ]
-        if len(unfound_lanepoints) > 0:
-            remaining_linked_lps = LanePoints._closest_linked_lp_in_kd_tree_batched(
-                [pose.point for _, pose in unfound_lanepoints],
-                lanepoints,
-                tree=tree,
-                k=k,
-                filter_composites=filter_composites,
-            )
-            # Replace the empty lanepoint locations
-            for (i, _), lps in [
-                g for g in zip(unfound_lanepoints, remaining_linked_lps)
-            ]:
-                linked_lanepoints[i] = [lps]
-
-        return [
-            sorted(
-                l_lps,
-                key=lambda _llp: squared_dist(
-                    poses[idx].as_position2d(), _llp.lp.pose.as_position2d()
-                )
-                + abs(poses[idx].heading.relative_to(_llp.lp.pose.heading)),
-            )
-            for idx, l_lps in enumerate(linked_lanepoints)
-        ]
+        # if filtering, only return lane-points in composite lanes if we didn't hit any in simple lanes...
+        return [linked_lps[idx] for idx in closest_indices]
 
     def closest_lanepoints(
         self,
         pose: Pose,
-        within_radius: float = 10,
         maximum_count: int = 10,
     ) -> List[LanePoint]:
         """Get the lane-points closest to the given pose.
         Args:
             pose:
                 The pose to look around for lane-points.
-            within_radius:
-                The radius which lane-points can be found from the given pose.
             maximum_count:
                 The maximum number of lane-points that should be found.
         """
-        lanepoints = self._linked_lanepoints
-        kd_tree = self._lanepoints_kd_tree
-        linked_lanepoints = LanePoints._closest_linked_lp_in_kd_tree_with_pose_batched(
-            [pose],
-            lanepoints,
-            kd_tree,
-            within_radius=within_radius,
+        linked_lanepoints = LanePoints._closest_linked_lp_to_point(
+            pose.point,
+            self._linked_lanepoints,
+            self._lp_points,
             k=maximum_count,
             filter_composites=True,
         )
-        return [l_lps.lp for l_lps in linked_lanepoints[0]]
-
-    def closest_lanepoint_on_lane_to_point(self, point, lane_id: str) -> LanePoint:
-        """Returns the closest lane-point on the given lane to the given world coordinate."""
-        return self.closest_linked_lanepoint_on_lane_to_point(point, lane_id).lp
+        return [llp.lp for llp in linked_lanepoints]
 
     def closest_linked_lanepoint_on_lane_to_point(
         self, point: Point, lane_id: str
     ) -> LinkedLanePoint:
         """Returns the closest linked lane-point on the given lane."""
-        lane_kd_tree = self._lanepoints_kd_tree_by_lane_id[lane_id]
-        return LanePoints._closest_linked_lp_in_kd_tree_batched(
-            [point], self._lanepoints_by_lane_id[lane_id], lane_kd_tree, k=1
-        )[0][0]
+        return LanePoints._closest_linked_lp_to_point(
+            point,
+            self._lanepoints_by_lane_id[lane_id],
+            self._lp_points_by_lane_id[lane_id],
+            k=1,
+        )[0]
 
     def closest_linked_lanepoint_on_road(
         self, point: Point, road_id: str
     ) -> LinkedLanePoint:
         """Returns the closest linked lane-point on the given road."""
-        return LanePoints._closest_linked_lp_in_kd_tree_batched(
-            [point],
+        return LanePoints._closest_linked_lp_to_point(
+            point,
             self._lanepoints_by_edge_id[road_id],
-            self._lanepoints_kd_tree_by_edge_id[road_id],
-        )[0][0]
+            self._lp_points_by_edge_id[road_id],
+        )[0]
 
     @lru_cache(maxsize=32)
     def paths_starting_at_lanepoint(

smarts/core/opendrive_road_network.py

@@ -1668,9 +1668,7 @@ def waypoint_paths(
             road_ids = [road.road_id for road in route.roads]
         if road_ids:
             return self._waypoint_paths_along_route(pose.point, lookahead, road_ids)
-        closest_lps = self._lanepoints.closest_lanepoints(
-            pose, within_radius=within_radius
-        )
+        closest_lps = self._lanepoints.closest_lanepoints(pose)
         closest_lane = closest_lps[0].lane
         waypoint_paths = []
         for lane in closest_lane.road.lanes:

smarts/core/sumo_road_network.py

@@ -996,9 +996,7 @@ def waypoint_paths(
 
         # No route provided, so generate paths based on the closest lanepoints
         waypoint_paths = []
-        closest_lps: List[LanePoint] = self._lanepoints.closest_lanepoints(
-            pose, within_radius=within_radius
-        )
+        closest_lps: List[LanePoint] = self._lanepoints.closest_lanepoints(pose)
 
         # First, see if we are in a junction and need to do something special
         junction_paths = self._resolve_in_junction(pose, closest_lps)