Skip to content

Spline Profile

SplineProfile

Spline profile saving several values such as velocity, acceleration and jerk for interpolation at arc length. Note that this spline profile uses a downsampled reference path that only goes from initial state to goal state.

Source code in commonroad_velocity_planner/spline_profile.py 
  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
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
class SplineProfile:
    """
    Spline profile saving several values such as velocity, acceleration and jerk for interpolation at arc length.
    Note that this spline profile uses a downsampled reference path that only goes from initial state to goal state.
    """

    def __init__(
        self,
        path_length_per_point: np.ndarray,
        velocity_profile: np.ndarray,
        acceleration_profile: np.ndarray = None,
        goal_velocity: float = 0.01,
    ):
        """
        Spline profile saving several values such as velocity, acceleration and jerk for interpolation at arc length.
        :param path_length_per_point: arc length at point
        :param velocity_profile: velocity profile as np.ndarray
        :param acceleration_profile: acceleration profile as np.ndarray
        :param goal_velocity: default goal velocity.
        """
        self._path_length_per_point = path_length_per_point
        self._velocity_profile = velocity_profile
        self._velocity_spline = CubicSpline(
            self._path_length_per_point, self._velocity_profile
        )

        self._goal_velocity: float = goal_velocity

        interpoint_distance = path_length_per_point[1] - path_length_per_point[0]
        if acceleration_profile is None:
            acceleration_profile = np.zeros_like(velocity_profile)
            acceleration_profile[1:] = (
                # t = delta_v / s & a = delta_v / t --> delta_v2 / s = a
                np.power(np.diff(velocity_profile), 2)
                / interpoint_distance
            )
            acceleration_profile[0] = acceleration_profile[1]
        self._acceleration_profile = acceleration_profile

        self._acceleration_spline = CubicSpline(
            self._path_length_per_point, self._acceleration_profile
        )

        self._jerk_profile = (
            np.diff(self._acceleration_profile) / interpoint_distance
        ) * self._velocity_profile[:-1]

    @property
    def path_length_per_point(self) -> np.ndarray:
        """
        :return: (n,) interpoint arc length
        """
        return self._path_length_per_point

    @property
    def velocity_profile(self) -> np.ndarray:
        """
        :return: (n,) velocity profile per foint
        """
        return self._velocity_profile

    @property
    def velocity_spline(self) -> CubicSpline:
        """
        :return: velocity polyspline for interpolating between points
        """
        return self._velocity_spline

    @property
    def goal_velocity(self) -> float:
        """
        :return: goal velocity
        """
        return self._goal_velocity

    @property
    def acceleration_profile(self) -> np.ndarray:
        """
        :return: (n,) acceleration profile per point
        """
        return self._acceleration_profile

    @property
    def acceleration_spline(self) -> CubicSpline:
        """
        :return: (n,) acceleration spline
        """
        return self._acceleration_spline

    @property
    def jerk_profile(self) -> np.ndarray:
        """
        :return: (n,) jerk profile per point
        """
        return self._jerk_profile

    def interpolate_velocity_at_arc_lenth(
        self,
        s: [np.ndarray],
        clip_min: float = 0.0,
        goal_velocity: float = None,
    ) -> [np.ndarray]:
        """
        Interpoalte velocity at arc length
        :param s: arc
        :param clip_min: clip at minimum value
        :param goal_velocity: after the goal, this velocity is taken
        :return: interpolation velocity as np.ndarry
        """
        goal_vel: float = (
            goal_velocity if goal_velocity is not None else self._goal_velocity
        )

        # cubic splines have problems when exceeding last point, therefore just set it as last during interpolation
        interpolation = self._velocity_spline(s[s <= self._path_length_per_point[-1]])
        interpolation = np.concatenate(
            (
                interpolation,
                np.ones_like(s[s > self._path_length_per_point[-1]]) * goal_vel,
            )
        )

        # clip all values to lower bound
        interpolation = interpolation.clip(min=clip_min)

        return interpolation

    def interpolate_acceleration_at_arc_lenth(
        self, s: [np.ndarray], goal_acceleration: float = 0
    ) -> [np.ndarray]:
        """
        Interpoalte acceleration at arc length
        :param s: arc
        :param goal_acceleration: after the goal, this acceleration is taken
        :return: interpolation acceleration as np.ndarry
        """
        # cubic splines have problems when exceeding last point, therefore just set it as last during interpolation
        interpolation = self._acceleration_spline(
            s[s <= self._path_length_per_point[-1]]
        )
        interpolation = np.concatenate(
            (
                interpolation,
                np.ones_like(s[s > self._path_length_per_point[-1]])
                * goal_acceleration,
            )
        )

        return interpolation

acceleration_profile: np.ndarray property

Returns:

Type Description
ndarray

(n,) acceleration profile per point

acceleration_spline: CubicSpline property

Returns:

Type Description
CubicSpline

(n,) acceleration spline

goal_velocity: float property

Returns:

Type Description
float

goal velocity

jerk_profile: np.ndarray property

Returns:

Type Description
ndarray

(n,) jerk profile per point

path_length_per_point: np.ndarray property

Returns:

Type Description
ndarray

(n,) interpoint arc length

velocity_profile: np.ndarray property

Returns:

Type Description
ndarray

(n,) velocity profile per foint

velocity_spline: CubicSpline property

Returns:

Type Description
CubicSpline

velocity polyspline for interpolating between points

__init__(path_length_per_point, velocity_profile, acceleration_profile=None, goal_velocity=0.01)

Spline profile saving several values such as velocity, acceleration and jerk for interpolation at arc length.

Parameters:

Name Type Description Default
path_length_per_point ndarray

arc length at point

 required
velocity_profile ndarray

velocity profile as np.ndarray

 required
acceleration_profile ndarray

acceleration profile as np.ndarray

 None
goal_velocity float

default goal velocity.

 0.01
Source code in commonroad_velocity_planner/spline_profile.py 
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
def __init__(
    self,
    path_length_per_point: np.ndarray,
    velocity_profile: np.ndarray,
    acceleration_profile: np.ndarray = None,
    goal_velocity: float = 0.01,
):
    """
    Spline profile saving several values such as velocity, acceleration and jerk for interpolation at arc length.
    :param path_length_per_point: arc length at point
    :param velocity_profile: velocity profile as np.ndarray
    :param acceleration_profile: acceleration profile as np.ndarray
    :param goal_velocity: default goal velocity.
    """
    self._path_length_per_point = path_length_per_point
    self._velocity_profile = velocity_profile
    self._velocity_spline = CubicSpline(
        self._path_length_per_point, self._velocity_profile
    )

    self._goal_velocity: float = goal_velocity

    interpoint_distance = path_length_per_point[1] - path_length_per_point[0]
    if acceleration_profile is None:
        acceleration_profile = np.zeros_like(velocity_profile)
        acceleration_profile[1:] = (
            # t = delta_v / s & a = delta_v / t --> delta_v2 / s = a
            np.power(np.diff(velocity_profile), 2)
            / interpoint_distance
        )
        acceleration_profile[0] = acceleration_profile[1]
    self._acceleration_profile = acceleration_profile

    self._acceleration_spline = CubicSpline(
        self._path_length_per_point, self._acceleration_profile
    )

    self._jerk_profile = (
        np.diff(self._acceleration_profile) / interpoint_distance
    ) * self._velocity_profile[:-1]

interpolate_acceleration_at_arc_lenth(s, goal_acceleration=0)

Interpoalte acceleration at arc length

Parameters:

Name Type Description Default
s [ndarray]

arc

 required
goal_acceleration float

after the goal, this acceleration is taken

 0

Returns:

Type Description
[ndarray]

interpolation acceleration as np.ndarry
Source code in commonroad_velocity_planner/spline_profile.py 
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
def interpolate_acceleration_at_arc_lenth(
    self, s: [np.ndarray], goal_acceleration: float = 0
) -> [np.ndarray]:
    """
    Interpoalte acceleration at arc length
    :param s: arc
    :param goal_acceleration: after the goal, this acceleration is taken
    :return: interpolation acceleration as np.ndarry
    """
    # cubic splines have problems when exceeding last point, therefore just set it as last during interpolation
    interpolation = self._acceleration_spline(
        s[s <= self._path_length_per_point[-1]]
    )
    interpolation = np.concatenate(
        (
            interpolation,
            np.ones_like(s[s > self._path_length_per_point[-1]])
            * goal_acceleration,
        )
    )

    return interpolation

interpolate_velocity_at_arc_lenth(s, clip_min=0.0, goal_velocity=None)

Interpoalte velocity at arc length

Parameters:

Name Type Description Default
s [ndarray]

arc

 required
clip_min float

clip at minimum value

 0.0
goal_velocity float

after the goal, this velocity is taken

 None

Returns:

Type Description
[ndarray]

interpolation velocity as np.ndarry
Source code in commonroad_velocity_planner/spline_profile.py 
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
def interpolate_velocity_at_arc_lenth(
    self,
    s: [np.ndarray],
    clip_min: float = 0.0,
    goal_velocity: float = None,
) -> [np.ndarray]:
    """
    Interpoalte velocity at arc length
    :param s: arc
    :param clip_min: clip at minimum value
    :param goal_velocity: after the goal, this velocity is taken
    :return: interpolation velocity as np.ndarry
    """
    goal_vel: float = (
        goal_velocity if goal_velocity is not None else self._goal_velocity
    )

    # cubic splines have problems when exceeding last point, therefore just set it as last during interpolation
    interpolation = self._velocity_spline(s[s <= self._path_length_per_point[-1]])
    interpolation = np.concatenate(
        (
            interpolation,
            np.ones_like(s[s > self._path_length_per_point[-1]]) * goal_vel,
        )
    )

    # clip all values to lower bound
    interpolation = interpolation.clip(min=clip_min)

    return interpolation