-
Notifications
You must be signed in to change notification settings - Fork 9
/
pdm_geometry_utils.py
101 lines (85 loc) · 3.33 KB
/
pdm_geometry_utils.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
from typing import List
import numpy as np
import numpy.typing as npt
from nuplan.common.actor_state.state_representation import StateSE2
from navsim.planning.simulation.planner.pdm_planner.utils.pdm_enums import (
SE2Index,
)
def normalize_angle(angle):
"""
Map a angle in range [-π, π]
:param angle: any angle as float
:return: normalized angle
"""
return np.arctan2(np.sin(angle), np.cos(angle))
def parallel_discrete_path(
discrete_path: List[StateSE2], offset=float
) -> List[StateSE2]:
"""
Creates a parallel discrete path for a given offset.
:param discrete_path: baseline path (x,y,θ)
:param offset: parall loffset
:return: parallel discrete path
"""
parallel_discrete_path = []
for state in discrete_path:
theta = state.heading + np.pi / 2
x_new = state.x + np.cos(theta) * offset
y_new = state.y + np.sin(theta) * offset
parallel_discrete_path.append(StateSE2(x_new, y_new, state.heading))
return parallel_discrete_path
def translate_lon_and_lat(
centers: npt.NDArray[np.float64],
headings: npt.NDArray[np.float64],
lon: float,
lat: float,
) -> npt.NDArray[np.float64]:
"""
Translate the position component of an centers point array
:param centers: array to be translated
:param headings: array with heading angles
:param lon: [m] distance by which a point should be translated in longitudinal direction
:param lat: [m] distance by which a point should be translated in lateral direction
:return array of translated coordinates
"""
half_pi = np.pi / 2.0
translation: npt.NDArray[np.float64] = np.stack(
[
(lat * np.cos(headings + half_pi)) + (lon * np.cos(headings)),
(lat * np.sin(headings + half_pi)) + (lon * np.sin(headings)),
],
axis=-1,
)
return centers + translation
def calculate_progress(path: List[StateSE2]) -> List[float]:
"""
Calculate the cumulative progress of a given path.
:param path: a path consisting of StateSE2 as waypoints
:return: a cumulative list of progress
"""
x_position = [point.x for point in path]
y_position = [point.y for point in path]
x_diff = np.diff(x_position)
y_diff = np.diff(y_position)
points_diff: npt.NDArray[np.float64] = np.concatenate(
([x_diff], [y_diff]), axis=0, dtype=np.float64
)
progress_diff = np.append(0.0, np.linalg.norm(points_diff, axis=0))
return np.cumsum(progress_diff, dtype=np.float64) # type: ignore
def convert_absolute_to_relative_se2_array(
origin: StateSE2, state_se2_array: npt.NDArray[np.float64]
) -> npt.NDArray[np.float64]:
"""
Converts an StateSE2 array from global to relative coordinates.
:param origin: origin pose of relative coords system
:param state_se2_array: array of SE2 states with (x,y,θ) in last dim
:return: SE2 coords array in relative coordinates
"""
assert len(SE2Index) == state_se2_array.shape[-1]
theta = -origin.heading
origin_array = np.array([[origin.x, origin.y, origin.heading]], dtype=np.float64)
R = np.array([[np.cos(theta), -np.sin(theta)], [np.sin(theta), np.cos(theta)]])
points_rel = state_se2_array - origin_array
points_rel[..., :2] = points_rel[..., :2] @ R.T
points_rel[:, 2] = normalize_angle(points_rel[:, 2])
return points_rel