From 3bfbc4d14d211c7e14b160017e608431b4f6975c Mon Sep 17 00:00:00 2001 From: Ansgar Kasselmann Date: Mon, 17 Mar 2025 14:15:43 +0100 Subject: [PATCH] Fix interpolation over wrapped array --- commonroad_reach/utility/configuration.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/commonroad_reach/utility/configuration.py b/commonroad_reach/utility/configuration.py index 69cf57a..27737d0 100644 --- a/commonroad_reach/utility/configuration.py +++ b/commonroad_reach/utility/configuration.py @@ -196,7 +196,7 @@ def compute_initial_state_cvln(config) -> Tuple: reference_path = config.planning.reference_path ref_orientation = compute_orientation_from_polyline(reference_path) ref_path_length = compute_pathlength_from_polyline(reference_path) - orientation_interpolated = np.interp(p_lon, ref_path_length, ref_orientation) + orientation_interpolated = np.interp(p_lon, ref_path_length, np.unwrap(ref_orientation)) v_lon = v * np.cos(orientation - orientation_interpolated) v_lat = v * np.sin(orientation - orientation_interpolated)