From 487eeb143df6fb393b85489f3e288842f7476123 Mon Sep 17 00:00:00 2001 From: Saul Field Date: Wed, 28 Jun 2023 17:59:07 -0400 Subject: [PATCH] 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 | 3 + docs/spelling_wordlist.txt | 1 + setup.cfg | 1 - smarts/core/argoverse_map.py | 2 +- .../controllers/lane_following_controller.py | 50 ++++-- smarts/core/coordinates.py | 1 + smarts/core/lanepoints.py | 167 ++++-------------- smarts/core/opendrive_road_network.py | 4 +- smarts/core/sumo_road_network.py | 4 +- smarts/core/tests/test_map.py | 21 +-- smarts/core/vehicle_state.py | 5 +- smarts/core/waymo_map.py | 4 +- smarts/sstudio/types/distribution.py | 21 --- 13 files changed, 81 insertions(+), 203 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 49ec54ff1d..9e79868dca 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/docs/spelling_wordlist.txt b/docs/spelling_wordlist.txt index 291a9c6d4c..78851b61ba 100644 --- a/docs/spelling_wordlist.txt +++ b/docs/spelling_wordlist.txt @@ -4,6 +4,7 @@ Δy Δheading ΔTime +ẋ accelerometer Ackermann acyclic diff --git a/setup.cfg b/setup.cfg index 93e463ba8e..81f7b839c6 100644 --- a/setup.cfg +++ b/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 = diff --git a/smarts/core/argoverse_map.py b/smarts/core/argoverse_map.py index f444d16df9..d7ee8226f0 100644 --- a/smarts/core/argoverse_map.py +++ b/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 diff --git a/smarts/core/controllers/lane_following_controller.py b/smarts/core/controllers/lane_following_controller.py index f92d3c3791..550696000c 100644 --- a/smarts/core/controllers/lane_following_controller.py +++ b/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 diff --git a/smarts/core/coordinates.py b/smarts/core/coordinates.py index a18bf84aa8..8d904c147a 100644 --- a/smarts/core/coordinates.py +++ b/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) diff --git a/smarts/core/lanepoints.py b/smarts/core/lanepoints.py index 688acfcbd5..f1086064a8 100644 --- a/smarts/core/lanepoints.py +++ b/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( diff --git a/smarts/core/opendrive_road_network.py b/smarts/core/opendrive_road_network.py index ae275bda7b..258401351f 100644 --- a/smarts/core/opendrive_road_network.py +++ b/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: diff --git a/smarts/core/sumo_road_network.py b/smarts/core/sumo_road_network.py index 0d2a5cff8b..540ab5245f 100644 --- a/smarts/core/sumo_road_network.py +++ b/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) diff --git a/smarts/core/tests/test_map.py b/smarts/core/tests/test_map.py index 84c27396d2..01ba68c0dd 100644 --- a/smarts/core/tests/test_map.py +++ b/smarts/core/tests/test_map.py @@ -337,13 +337,6 @@ def test_opendrive_map_4lane(opendrive_scenario_4lane): # Lanepoints lanepoints = road_map._lanepoints - point = Point(148.0, -17.0, 0) - l1_lane_point = lanepoints.closest_lanepoint_on_lane_to_point(point, l1.lane_id) - assert ( - round(l1_lane_point.pose.position[0], 2), - round(l1_lane_point.pose.position[1], 2), - ) == (148.4, -17.0) - r5 = road_map.road_by_id("60_0_R") point = Point(148.00, -47.00) r5_linked_lane_point = lanepoints.closest_linked_lanepoint_on_road( @@ -669,13 +662,6 @@ def test_opendrive_map_merge(opendrive_scenario_merge): # Lanepoints lanepoints = road_map._lanepoints - point = Point(48.39, 0.4, 0) - l1_lane_point = lanepoints.closest_lanepoint_on_lane_to_point(point, "1_1_R_-1") - assert ( - round(l1_lane_point.pose.position[0], 2), - round(l1_lane_point.pose.position[1], 2), - ) == (48.5, -0.15) - point = Point(20.0, 1.3, 0) r0_linked_lane_point = lanepoints.closest_linked_lanepoint_on_road(point, "1_0_L") assert r0_linked_lane_point.lp.lane.lane_id == "1_0_L_1" @@ -798,6 +784,7 @@ def test_waymo_map(): # nearest lane for a point outside all lanes point = Point(2910.0, -2612.0, 0) l3 = road_map.nearest_lane(point) + assert l3 assert l3.lane_id == "156" assert not l3.contains_point(point) @@ -933,12 +920,6 @@ def test_waymo_map(): # Lanepoints lanepoints = road_map._lanepoints - point = Point(2715.0, -2763.5, 0) - l1_lane_point = lanepoints.closest_lanepoint_on_lane_to_point(point, l1.lane_id) - assert ( - round(l1_lane_point.pose.position[0], 2), - round(l1_lane_point.pose.position[1], 2), - ) == (2713.84, -2762.52) r1 = road_map.road_by_id("waymo_road-100") r1_linked_lane_point = lanepoints.closest_linked_lanepoint_on_road( diff --git a/smarts/core/vehicle_state.py b/smarts/core/vehicle_state.py index f1c34fc4ef..12f2154aac 100644 --- a/smarts/core/vehicle_state.py +++ b/smarts/core/vehicle_state.py @@ -23,7 +23,6 @@ from typing import NamedTuple, Optional, Tuple import numpy as np -from scipy.spatial.distance import cdist from shapely.affinity import rotate as shapely_rotate from shapely.geometry import Polygon from shapely.geometry import box as shapely_box @@ -184,9 +183,7 @@ def neighborhood_vehicles_around_vehicle( return [] # calculate euclidean distances - distances = cdist( - other_positions, [vehicle_state.pose.position], metric="euclidean" - ).reshape(-1) + distances = np.linalg.norm(other_positions - vehicle_state.pose.position, axis=1) indices = np.argwhere(distances <= radius).flatten() return [other_states[i] for i in indices] diff --git a/smarts/core/waymo_map.py b/smarts/core/waymo_map.py index 960c3dbe57..55fc6e132a 100644 --- a/smarts/core/waymo_map.py +++ b/smarts/core/waymo_map.py @@ -1853,9 +1853,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: diff --git a/smarts/sstudio/types/distribution.py b/smarts/sstudio/types/distribution.py index 7493967157..0b7e7d9f57 100644 --- a/smarts/sstudio/types/distribution.py +++ b/smarts/sstudio/types/distribution.py @@ -55,24 +55,3 @@ def __post_init__(self): def sample(self): """Get the next sample.""" return random.uniform(self.a, self.b) - - -@dataclass -class TruncatedDistribution: - """A truncated normal distribution, by default, location=0, scale=1""" - - a: float - b: float - loc: float = 0 - scale: float = 1 - - def __post_init__(self): - assert self.a != self.b - if self.b < self.a: - self.a, self.b = self.b, self.a - - def sample(self): - """Get the next sample""" - from scipy.stats import truncnorm - - return truncnorm.rvs(self.a, self.b, loc=self.loc, scale=self.scale)