Skip to content

Interfaces

commonroad_control.vehicle_dynamics.vehicle_model_interface

VehicleModelInterface

Bases: ABC

Interface for vehicle dynamics models: among others, the classes implementing this interface provide the dynamics function, both in continous- and discrete-time, or methods for computing the longitudinal and lateral accelerations.

Source code in commonroad_control/vehicle_dynamics/vehicle_model_interface.py 
 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
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
class VehicleModelInterface(ABC):
    """
    Interface for vehicle dynamics models: among others, the classes implementing this interface provide the dynamics function, both in continous- and discrete-time, or methods for computing the longitudinal and lateral accelerations.
    """

    @classmethod
    @abstractmethod
    def factory_method(cls, params: VehicleParameters, delta_t: float) -> Any:
        """
        Factory method to generate class
        :param params: CommonRoad-Control vehicle parameters
        :param delta_t: sampling time
        :return: instance
        """
        pass

    def __init__(self, params: VehicleParameters, nx: int, nu: int, nw: int, delta_t: float):
        """
        Initialize abstract baseclass.
        :param params: CommonRoad-Control vehicle parameters
        :param nx: dimension of the state space
        :param nu: dimension of the input space
        :param nw: dimension of the disturbance space
        :param delta_t: sampling time
        """
        self._nx: int = nx
        self._nu: int = nu
        self._nw: int = nw
        self._delta_t: float = delta_t

        # input bounds
        self._u_lb, self._u_ub = self._set_input_bounds(params)

        # discretize (nominal) vehicle model
        self._dynamics_dt, self._jac_dynamics_dt_x, self._jac_dynamics_dt_u = self._discretize_nominal()

        # differentiate acceleration constraint functions
        (
            self._a_norm,
            self._jac_a_norm_long_x,
            self._jac_a_norm_long_u,
            self._jac_a_norm_lat_x,
            self._jac_a_norm_lat_u,
        ) = self._differentiate_acceleration_constraints()

    def simulate_dt_nom(
        self,
        x: Union[StateInterface, np.ndarray[tuple[float], np.dtype[np.float64]]],
        u: Union[InputInterface, np.ndarray[tuple[float], np.dtype[np.float64]]],
    ) -> np.ndarray:
        """
        One-step simulation of the time-discretized nominal vehicle dynamics.
        :param x: initial state - StateInterface/ array of dimension (self._nx,)
        :param u: control input - InputInterface/ array of dimension (self._nu,)
        :return: nominal state at next time step
        """

        # convert state and input to arrays
        if isinstance(x, StateInterface):
            x_np = x.convert_to_array()
        else:
            x_np = x

        if isinstance(u, InputInterface):
            u_np = u.convert_to_array()
        else:
            u_np = u

        # evaluate discretized dynamics at (x,u)
        x_next = self._dynamics_dt(x_np, u_np).full()

        return x_next.squeeze()

    def dynamics_ct(
        self,
        x: Union[StateInterface, np.ndarray[tuple[float], np.dtype[np.float64]]],
        u: Union[InputInterface, np.ndarray[tuple[float], np.dtype[np.float64]]],
        w: Union[DisturbanceInterface, np.ndarray[tuple[float], np.dtype[np.float64]]],
    ) -> np.ndarray:
        """
        Interface to the continuous-time dynamics function of the vehicle model.
        :param x: state - StateInterface/ array of dimension (self._nx,)
        :param u: control input - InputInterface/ array of dimension (self._nu,)
        :param w: disturbance - DisturbanceInterface/ array of dimension (self._nw,)
        :return: dynamics function evaluated at (x, u, w) - array of dimension (self._nx,)
        """

        # convert state, input, and disturbance to arrays
        if isinstance(x, StateInterface):
            x_np = x.convert_to_array()
        else:
            x_np = x

        if isinstance(u, InputInterface):
            u_np = u.convert_to_array()
        else:
            u_np = u

        if isinstance(w, DisturbanceInterface):
            w_np = w.convert_to_array()
        else:
            w_np = w

        x_next = self._dynamics_cas(x_np, u_np, w_np)
        x_next = np.reshape(x_next, (1, self._nx), order="F").squeeze()

        return x_next

    @abstractmethod
    def _dynamics_cas(
        self,
        x: Union[cas.SX.sym, np.ndarray[tuple[float], np.dtype[np.float64]]],
        u: Union[cas.SX.sym, np.ndarray[tuple[float], np.dtype[np.float64]]],
        w: Union[cas.SX.sym, np.ndarray[tuple[float], np.dtype[np.float64]]],
    ) -> Union[cas.SX.sym, np.ndarray]:
        """
        Continuous-time dynamics function of the vehicle model for simulation and symbolic operations using CasADi.
        :param x: state - CasADi symbolic/ array of dimension (self._nx,)
        :param u: control input - CasADi symbolic/ array of dimension (self._nu,)
        :param w: disturbance - CasADi symbolic/ array of dimension (self._nw,)
        :return: dynamics function evaluated at (x,u,w) - CasADi symbolic/ array of dimension (self._nx,)
        """
        pass

    def linearize_dt_nom_at(
        self, x: Union[StateInterface, np.array], u: Union[InputInterface, np.array]
    ) -> Tuple[np.array, np.array, np.array]:
        """
        Linearization of the time-discretized nominal vehicle dynamics at a given state-input-pair, e.g., for solving a
        convex(ified) optimal control problem.
        :param x: state for linearization - StateInterface/ array of dimension (self._nx,)
        :param u: input for linearization - InputInterface/ array of dimension (self._nu,)
        :return: nominal dynamics at (x,u) and Jacobians at (x,u) w.r.t. x and u
        """

        # convert state and input to arrays
        if isinstance(x, StateInterface):
            x_np = x.convert_to_array()
        else:
            x_np = x

        if isinstance(u, InputInterface):
            u_np = u.convert_to_array()
        else:
            u_np = u

        # evaluate discretized dynamics at (x,u)
        x_next = self._dynamics_dt(x_np, u_np).full()

        # evaluate linearized dynamics
        jac_x = self._jac_dynamics_dt_x(x_np, u_np).full()
        jac_u = self._jac_dynamics_dt_u(x_np, u_np).full()

        return x_next, jac_x, jac_u

    def _discretize_nominal(self) -> Tuple[cas.Function, cas.Function, cas.Function]:
        """
        Time-discretization of the nominal dynamics model assuming a constant control input throughout the time interval [0, delta_t].
        :return: time-discretized dynamical system (CasADi function) and its Jacobians (CasADi function)
        """

        xk = cas.SX.sym("xk", self._nx, 1)
        uk = cas.SX.sym("uk", self._nu, 1)
        # nominal disturbance
        wk = np.zeros((self._nw,))

        # discretize dynamics
        x_next = cas.Function(
            "dynamics_dt",
            [xk, uk],
            [rk4_integrator(xk, uk, wk, self._dynamics_cas, self._delta_t)],
        )

        # compute Jacobian of discretized dynamics
        jac_x = cas.Function("jac_dynamics_dt", [xk, uk], [cas.jacobian(x_next(xk, uk), xk)])
        jac_u = cas.Function("jac_dynamics_dt", [xk, uk], [cas.jacobian(x_next(xk, uk), uk)])
        return x_next, jac_x, jac_u

    @abstractmethod
    def compute_normalized_acceleration(
        self,
        x: Union[StateInterface, cas.SX.sym, np.array],
        u: Union[InputInterface, cas.SX.sym, np.array],
    ) -> Tuple[Union[float, cas.SX.sym], Union[float, cas.SX.sym]]:
        """
        Computes the normalized longitudinal and lateral acceleration (w.r.t. the maximum acceleration).
        :param x: state - StateInterface/ CasADi symbolic/ array of dimension (self._nx,)
        :param u: control input - InputInterface/ CasADi symbolic/ array of dimension (self._nu,)
        :return: normalized longitudinal and lateral acceleration - float/ CasADi symbolic
        """
        pass

    def linearize_acceleration_constraints_at(
        self, x: Union[StateInterface, np.array], u: Union[InputInterface, np.array]
    ) -> Tuple[np.array, np.array, np.array, np.array, np.array, np.array]:
        """
        Linearization of the (normalized) acceleration constraint functions at a given state-input-pair, e.g., for solving
        a convex(ified) optimal control problem.
        :param x: state for linearization - array of dimension (self._nx,)
        :param u: input for linearization - array of dimension (self._nu,)
        :return: (normalized) acceleration constraint functions and respective Jacobians w.r.t. x and u
        """

        # convert state and input to arrays
        if isinstance(x, StateInterface):
            x_np = x.convert_to_array()
        else:
            x_np = x

        if isinstance(u, InputInterface):
            u_np = u.convert_to_array()
        else:
            u_np = u

        # evaluate acceleration constraint function at (x,u)
        a_long, a_lat = self._a_norm(x_np, u_np)
        a_long = a_long.full()
        a_lat = a_lat.full()

        # evaluate linearized constraint functions
        jac_a_long_x = self._jac_a_norm_long_x(x_np, u_np).full()
        jac_a_long_u = self._jac_a_norm_long_u(x_np, u_np).full()
        jac_a_lat_x = self._jac_a_norm_lat_x(x_np, u_np).full()
        jac_a_lat_u = self._jac_a_norm_lat_u(x_np, u_np).full()

        return a_long, a_lat, jac_a_long_x, jac_a_long_u, jac_a_lat_x, jac_a_lat_u

    def _differentiate_acceleration_constraints(
        self,
    ) -> Tuple[cas.Function, cas.Function, cas.Function, cas.Function, cas.Function]:
        """
        Differentiation of the (normalized) acceleration constraint functions.
        :return: acceleration constraint functions (longitudinal and lateral, CasADi functions) and respective Jacobians (CasADi functions)
        """
        xk = cas.SX.sym("xk", self._nx, 1)
        uk = cas.SX.sym("uk", self._nu, 1)

        # casadi function to normalized acceleration
        a_norm = cas.Function(
            "a_norm",
            [xk, uk],
            [
                self.compute_normalized_acceleration(xk, uk)[0],
                self.compute_normalized_acceleration(xk, uk)[1],
            ],
            ["xk", "uk"],
            ["a_long_norm", "a_lat_norm"],
        )

        # compute Jacobian of normalized longitudinal acceleration
        jac_a_long_x = cas.Function("jac_a_long_x", [xk, uk], [cas.jacobian(a_norm(xk, uk)[0], xk)])
        jac_a_long_u = cas.Function("jac_a_long_u", [xk, uk], [cas.jacobian(a_norm(xk, uk)[0], uk)])

        # compute Jacobian of normalized lateral acceleration
        jac_a_lat_x = cas.Function("jac_a_lat_x", [xk, uk], [cas.jacobian(a_norm(xk, uk)[1], xk)])
        jac_a_lat_u = cas.Function("jac_a_lat_u", [xk, uk], [cas.jacobian(a_norm(xk, uk)[1], uk)])

        return a_norm, jac_a_long_x, jac_a_long_u, jac_a_lat_x, jac_a_lat_u

    @abstractmethod
    def _set_input_bounds(self, params: VehicleParameters):
        """
        Extract input bounds from vehicle parameters and returns them as instances of the Input class.
        :param params: CommonRoad-Control vehicle parameters
        :return: lower and upper bounds - InputInterface
        """
        pass

    def input_bounds(self) -> Tuple[InputInterface, InputInterface]:
        """
        Returns the lower and upper bound on the control inputs.
        :return: lower bound and upper bound - InputInterface
        """
        return self._u_lb, self._u_ub

    @property
    def state_dimension(self):
        """
        :return: dimension of the state space
        """
        return self._nx

    @property
    def input_dimension(self):
        """
        :return: dimension of the input space
        """
        return self._nu

    @property
    def disturbance_dimension(self):
        """
        :return: dimension of the disturbance space
        """
        return self._nw
disturbance_dimension property

Returns:

Type Description

dimension of the disturbance space
input_dimension property

Returns:

Type Description

dimension of the input space
state_dimension property

Returns:

Type Description

dimension of the state space
__init__(params, nx, nu, nw, delta_t)

Initialize abstract baseclass.

Parameters:

Name Type Description Default
params VehicleParameters

CommonRoad-Control vehicle parameters

 required
nx int

dimension of the state space

 required
nu int

dimension of the input space

 required
nw int

dimension of the disturbance space

 required
delta_t float

sampling time

 required
Source code in commonroad_control/vehicle_dynamics/vehicle_model_interface.py 
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
def __init__(self, params: VehicleParameters, nx: int, nu: int, nw: int, delta_t: float):
    """
    Initialize abstract baseclass.
    :param params: CommonRoad-Control vehicle parameters
    :param nx: dimension of the state space
    :param nu: dimension of the input space
    :param nw: dimension of the disturbance space
    :param delta_t: sampling time
    """
    self._nx: int = nx
    self._nu: int = nu
    self._nw: int = nw
    self._delta_t: float = delta_t

    # input bounds
    self._u_lb, self._u_ub = self._set_input_bounds(params)

    # discretize (nominal) vehicle model
    self._dynamics_dt, self._jac_dynamics_dt_x, self._jac_dynamics_dt_u = self._discretize_nominal()

    # differentiate acceleration constraint functions
    (
        self._a_norm,
        self._jac_a_norm_long_x,
        self._jac_a_norm_long_u,
        self._jac_a_norm_lat_x,
        self._jac_a_norm_lat_u,
    ) = self._differentiate_acceleration_constraints()
_differentiate_acceleration_constraints()

Differentiation of the (normalized) acceleration constraint functions.

Returns:

Type Description
Tuple[Function, Function, Function, Function, Function]

acceleration constraint functions (longitudinal and lateral, CasADi functions) and respective Jacobians (CasADi functions)
Source code in commonroad_control/vehicle_dynamics/vehicle_model_interface.py 
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
def _differentiate_acceleration_constraints(
    self,
) -> Tuple[cas.Function, cas.Function, cas.Function, cas.Function, cas.Function]:
    """
    Differentiation of the (normalized) acceleration constraint functions.
    :return: acceleration constraint functions (longitudinal and lateral, CasADi functions) and respective Jacobians (CasADi functions)
    """
    xk = cas.SX.sym("xk", self._nx, 1)
    uk = cas.SX.sym("uk", self._nu, 1)

    # casadi function to normalized acceleration
    a_norm = cas.Function(
        "a_norm",
        [xk, uk],
        [
            self.compute_normalized_acceleration(xk, uk)[0],
            self.compute_normalized_acceleration(xk, uk)[1],
        ],
        ["xk", "uk"],
        ["a_long_norm", "a_lat_norm"],
    )

    # compute Jacobian of normalized longitudinal acceleration
    jac_a_long_x = cas.Function("jac_a_long_x", [xk, uk], [cas.jacobian(a_norm(xk, uk)[0], xk)])
    jac_a_long_u = cas.Function("jac_a_long_u", [xk, uk], [cas.jacobian(a_norm(xk, uk)[0], uk)])

    # compute Jacobian of normalized lateral acceleration
    jac_a_lat_x = cas.Function("jac_a_lat_x", [xk, uk], [cas.jacobian(a_norm(xk, uk)[1], xk)])
    jac_a_lat_u = cas.Function("jac_a_lat_u", [xk, uk], [cas.jacobian(a_norm(xk, uk)[1], uk)])

    return a_norm, jac_a_long_x, jac_a_long_u, jac_a_lat_x, jac_a_lat_u
_discretize_nominal()

Time-discretization of the nominal dynamics model assuming a constant control input throughout the time interval [0, delta_t].

Returns:

Type Description
Tuple[Function, Function, Function]

time-discretized dynamical system (CasADi function) and its Jacobians (CasADi function)
Source code in commonroad_control/vehicle_dynamics/vehicle_model_interface.py 
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
def _discretize_nominal(self) -> Tuple[cas.Function, cas.Function, cas.Function]:
    """
    Time-discretization of the nominal dynamics model assuming a constant control input throughout the time interval [0, delta_t].
    :return: time-discretized dynamical system (CasADi function) and its Jacobians (CasADi function)
    """

    xk = cas.SX.sym("xk", self._nx, 1)
    uk = cas.SX.sym("uk", self._nu, 1)
    # nominal disturbance
    wk = np.zeros((self._nw,))

    # discretize dynamics
    x_next = cas.Function(
        "dynamics_dt",
        [xk, uk],
        [rk4_integrator(xk, uk, wk, self._dynamics_cas, self._delta_t)],
    )

    # compute Jacobian of discretized dynamics
    jac_x = cas.Function("jac_dynamics_dt", [xk, uk], [cas.jacobian(x_next(xk, uk), xk)])
    jac_u = cas.Function("jac_dynamics_dt", [xk, uk], [cas.jacobian(x_next(xk, uk), uk)])
    return x_next, jac_x, jac_u
_dynamics_cas(x, u, w) abstractmethod

Continuous-time dynamics function of the vehicle model for simulation and symbolic operations using CasADi.

Parameters:

Name Type Description Default
x Union[sym, ndarray[tuple[float], dtype[float64]]]

state - CasADi symbolic/ array of dimension (self._nx,)

 required
u Union[sym, ndarray[tuple[float], dtype[float64]]]

control input - CasADi symbolic/ array of dimension (self._nu,)

 required
w Union[sym, ndarray[tuple[float], dtype[float64]]]

disturbance - CasADi symbolic/ array of dimension (self._nw,)

 required

Returns:

Type Description
Union[sym, ndarray]

dynamics function evaluated at (x,u,w) - CasADi symbolic/ array of dimension (self._nx,)
Source code in commonroad_control/vehicle_dynamics/vehicle_model_interface.py 
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
@abstractmethod
def _dynamics_cas(
    self,
    x: Union[cas.SX.sym, np.ndarray[tuple[float], np.dtype[np.float64]]],
    u: Union[cas.SX.sym, np.ndarray[tuple[float], np.dtype[np.float64]]],
    w: Union[cas.SX.sym, np.ndarray[tuple[float], np.dtype[np.float64]]],
) -> Union[cas.SX.sym, np.ndarray]:
    """
    Continuous-time dynamics function of the vehicle model for simulation and symbolic operations using CasADi.
    :param x: state - CasADi symbolic/ array of dimension (self._nx,)
    :param u: control input - CasADi symbolic/ array of dimension (self._nu,)
    :param w: disturbance - CasADi symbolic/ array of dimension (self._nw,)
    :return: dynamics function evaluated at (x,u,w) - CasADi symbolic/ array of dimension (self._nx,)
    """
    pass
_set_input_bounds(params) abstractmethod

Extract input bounds from vehicle parameters and returns them as instances of the Input class.

Parameters:

Name Type Description Default
params VehicleParameters

CommonRoad-Control vehicle parameters

 required

Returns:

Type Description

lower and upper bounds - InputInterface
Source code in commonroad_control/vehicle_dynamics/vehicle_model_interface.py 
275
276
277
278
279
280
281
282
@abstractmethod
def _set_input_bounds(self, params: VehicleParameters):
    """
    Extract input bounds from vehicle parameters and returns them as instances of the Input class.
    :param params: CommonRoad-Control vehicle parameters
    :return: lower and upper bounds - InputInterface
    """
    pass
compute_normalized_acceleration(x, u) abstractmethod

Computes the normalized longitudinal and lateral acceleration (w.r.t. the maximum acceleration).

Parameters:

Name Type Description Default
x Union[StateInterface, sym, array]

state - StateInterface/ CasADi symbolic/ array of dimension (self._nx,)

 required
u Union[InputInterface, sym, array]

control input - InputInterface/ CasADi symbolic/ array of dimension (self._nu,)

 required

Returns:

Type Description
Tuple[Union[float, sym], Union[float, sym]]

normalized longitudinal and lateral acceleration - float/ CasADi symbolic
Source code in commonroad_control/vehicle_dynamics/vehicle_model_interface.py 
194
195
196
197
198
199
200
201
202
203
204
205
206
@abstractmethod
def compute_normalized_acceleration(
    self,
    x: Union[StateInterface, cas.SX.sym, np.array],
    u: Union[InputInterface, cas.SX.sym, np.array],
) -> Tuple[Union[float, cas.SX.sym], Union[float, cas.SX.sym]]:
    """
    Computes the normalized longitudinal and lateral acceleration (w.r.t. the maximum acceleration).
    :param x: state - StateInterface/ CasADi symbolic/ array of dimension (self._nx,)
    :param u: control input - InputInterface/ CasADi symbolic/ array of dimension (self._nu,)
    :return: normalized longitudinal and lateral acceleration - float/ CasADi symbolic
    """
    pass
dynamics_ct(x, u, w)

Interface to the continuous-time dynamics function of the vehicle model.

Parameters:

Name Type Description Default
x Union[StateInterface, ndarray[tuple[float], dtype[float64]]]

state - StateInterface/ array of dimension (self._nx,)

 required
u Union[InputInterface, ndarray[tuple[float], dtype[float64]]]

control input - InputInterface/ array of dimension (self._nu,)

 required
w Union[DisturbanceInterface, ndarray[tuple[float], dtype[float64]]]

disturbance - DisturbanceInterface/ array of dimension (self._nw,)

 required

Returns:

Type Description
ndarray

dynamics function evaluated at (x, u, w) - array of dimension (self._nx,)
Source code in commonroad_control/vehicle_dynamics/vehicle_model_interface.py 
 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
def dynamics_ct(
    self,
    x: Union[StateInterface, np.ndarray[tuple[float], np.dtype[np.float64]]],
    u: Union[InputInterface, np.ndarray[tuple[float], np.dtype[np.float64]]],
    w: Union[DisturbanceInterface, np.ndarray[tuple[float], np.dtype[np.float64]]],
) -> np.ndarray:
    """
    Interface to the continuous-time dynamics function of the vehicle model.
    :param x: state - StateInterface/ array of dimension (self._nx,)
    :param u: control input - InputInterface/ array of dimension (self._nu,)
    :param w: disturbance - DisturbanceInterface/ array of dimension (self._nw,)
    :return: dynamics function evaluated at (x, u, w) - array of dimension (self._nx,)
    """

    # convert state, input, and disturbance to arrays
    if isinstance(x, StateInterface):
        x_np = x.convert_to_array()
    else:
        x_np = x

    if isinstance(u, InputInterface):
        u_np = u.convert_to_array()
    else:
        u_np = u

    if isinstance(w, DisturbanceInterface):
        w_np = w.convert_to_array()
    else:
        w_np = w

    x_next = self._dynamics_cas(x_np, u_np, w_np)
    x_next = np.reshape(x_next, (1, self._nx), order="F").squeeze()

    return x_next
factory_method(params, delta_t) abstractmethod classmethod

Factory method to generate class

Parameters:

Name Type Description Default
params VehicleParameters

CommonRoad-Control vehicle parameters

 required
delta_t float

sampling time

 required

Returns:

Type Description
Any

instance
Source code in commonroad_control/vehicle_dynamics/vehicle_model_interface.py 
21
22
23
24
25
26
27
28
29
30
@classmethod
@abstractmethod
def factory_method(cls, params: VehicleParameters, delta_t: float) -> Any:
    """
    Factory method to generate class
    :param params: CommonRoad-Control vehicle parameters
    :param delta_t: sampling time
    :return: instance
    """
    pass
input_bounds()

Returns the lower and upper bound on the control inputs.

Returns:

Type Description
Tuple[InputInterface, InputInterface]

lower bound and upper bound - InputInterface
Source code in commonroad_control/vehicle_dynamics/vehicle_model_interface.py 
284
285
286
287
288
289
def input_bounds(self) -> Tuple[InputInterface, InputInterface]:
    """
    Returns the lower and upper bound on the control inputs.
    :return: lower bound and upper bound - InputInterface
    """
    return self._u_lb, self._u_ub
linearize_acceleration_constraints_at(x, u)

Linearization of the (normalized) acceleration constraint functions at a given state-input-pair, e.g., for solving a convex(ified) optimal control problem.

Parameters:

Name Type Description Default
x Union[StateInterface, array]

state for linearization - array of dimension (self._nx,)

 required
u Union[InputInterface, array]

input for linearization - array of dimension (self._nu,)

 required

Returns:

Type Description
Tuple[array, array, array, array, array, array]

(normalized) acceleration constraint functions and respective Jacobians w.r.t. x and u
Source code in commonroad_control/vehicle_dynamics/vehicle_model_interface.py 
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
def linearize_acceleration_constraints_at(
    self, x: Union[StateInterface, np.array], u: Union[InputInterface, np.array]
) -> Tuple[np.array, np.array, np.array, np.array, np.array, np.array]:
    """
    Linearization of the (normalized) acceleration constraint functions at a given state-input-pair, e.g., for solving
    a convex(ified) optimal control problem.
    :param x: state for linearization - array of dimension (self._nx,)
    :param u: input for linearization - array of dimension (self._nu,)
    :return: (normalized) acceleration constraint functions and respective Jacobians w.r.t. x and u
    """

    # convert state and input to arrays
    if isinstance(x, StateInterface):
        x_np = x.convert_to_array()
    else:
        x_np = x

    if isinstance(u, InputInterface):
        u_np = u.convert_to_array()
    else:
        u_np = u

    # evaluate acceleration constraint function at (x,u)
    a_long, a_lat = self._a_norm(x_np, u_np)
    a_long = a_long.full()
    a_lat = a_lat.full()

    # evaluate linearized constraint functions
    jac_a_long_x = self._jac_a_norm_long_x(x_np, u_np).full()
    jac_a_long_u = self._jac_a_norm_long_u(x_np, u_np).full()
    jac_a_lat_x = self._jac_a_norm_lat_x(x_np, u_np).full()
    jac_a_lat_u = self._jac_a_norm_lat_u(x_np, u_np).full()

    return a_long, a_lat, jac_a_long_x, jac_a_long_u, jac_a_lat_x, jac_a_lat_u
linearize_dt_nom_at(x, u)

Linearization of the time-discretized nominal vehicle dynamics at a given state-input-pair, e.g., for solving a convex(ified) optimal control problem.

Parameters:

Name Type Description Default
x Union[StateInterface, array]

state for linearization - StateInterface/ array of dimension (self._nx,)

 required
u Union[InputInterface, array]

input for linearization - InputInterface/ array of dimension (self._nu,)

 required

Returns:

Type Description
Tuple[array, array, array]

nominal dynamics at (x,u) and Jacobians at (x,u) w.r.t. x and u
Source code in commonroad_control/vehicle_dynamics/vehicle_model_interface.py 
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
def linearize_dt_nom_at(
    self, x: Union[StateInterface, np.array], u: Union[InputInterface, np.array]
) -> Tuple[np.array, np.array, np.array]:
    """
    Linearization of the time-discretized nominal vehicle dynamics at a given state-input-pair, e.g., for solving a
    convex(ified) optimal control problem.
    :param x: state for linearization - StateInterface/ array of dimension (self._nx,)
    :param u: input for linearization - InputInterface/ array of dimension (self._nu,)
    :return: nominal dynamics at (x,u) and Jacobians at (x,u) w.r.t. x and u
    """

    # convert state and input to arrays
    if isinstance(x, StateInterface):
        x_np = x.convert_to_array()
    else:
        x_np = x

    if isinstance(u, InputInterface):
        u_np = u.convert_to_array()
    else:
        u_np = u

    # evaluate discretized dynamics at (x,u)
    x_next = self._dynamics_dt(x_np, u_np).full()

    # evaluate linearized dynamics
    jac_x = self._jac_dynamics_dt_x(x_np, u_np).full()
    jac_u = self._jac_dynamics_dt_u(x_np, u_np).full()

    return x_next, jac_x, jac_u
simulate_dt_nom(x, u)

One-step simulation of the time-discretized nominal vehicle dynamics.

Parameters:

Name Type Description Default
x Union[StateInterface, ndarray[tuple[float], dtype[float64]]]

initial state - StateInterface/ array of dimension (self._nx,)

 required
u Union[InputInterface, ndarray[tuple[float], dtype[float64]]]

control input - InputInterface/ array of dimension (self._nu,)

 required

Returns:

Type Description
ndarray

nominal state at next time step
Source code in commonroad_control/vehicle_dynamics/vehicle_model_interface.py 
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
def simulate_dt_nom(
    self,
    x: Union[StateInterface, np.ndarray[tuple[float], np.dtype[np.float64]]],
    u: Union[InputInterface, np.ndarray[tuple[float], np.dtype[np.float64]]],
) -> np.ndarray:
    """
    One-step simulation of the time-discretized nominal vehicle dynamics.
    :param x: initial state - StateInterface/ array of dimension (self._nx,)
    :param u: control input - InputInterface/ array of dimension (self._nu,)
    :return: nominal state at next time step
    """

    # convert state and input to arrays
    if isinstance(x, StateInterface):
        x_np = x.convert_to_array()
    else:
        x_np = x

    if isinstance(u, InputInterface):
        u_np = u.convert_to_array()
    else:
        u_np = u

    # evaluate discretized dynamics at (x,u)
    x_next = self._dynamics_dt(x_np, u_np).full()

    return x_next.squeeze()

commonroad_control.vehicle_dynamics.state_interface

StateInterface dataclass

Bases: ABC

Interface for dataclass objects storing states of the vehicle models.

Source code in commonroad_control/vehicle_dynamics/state_interface.py 
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
@dataclass
class StateInterface(ABC):
    """
    Interface for dataclass objects storing states of the vehicle models.
    """

    @property
    def dim(self) -> int:
        """
        :return: state dimension
        """
        return StateInterfaceIndex.dim

    @abstractmethod
    def convert_to_array(self) -> np.ndarray:
        """
        Converts instance of class to numpy array.
        :return: np.ndarray of dimension (self.dim,1)
        """
        pass
dim property

Returns:

Type Description
int

state dimension
convert_to_array() abstractmethod

Converts instance of class to numpy array.

Returns:

Type Description
ndarray

np.ndarray of dimension (self.dim,1)
Source code in commonroad_control/vehicle_dynamics/state_interface.py 
29
30
31
32
33
34
35
@abstractmethod
def convert_to_array(self) -> np.ndarray:
    """
    Converts instance of class to numpy array.
    :return: np.ndarray of dimension (self.dim,1)
    """
    pass

StateInterfaceIndex dataclass

Bases: ABC

Interface for indices of the state variables.

Source code in commonroad_control/vehicle_dynamics/state_interface.py 
 7
 8
 9
10
11
12
13
@dataclass(frozen=True)
class StateInterfaceIndex(ABC):
    """
    Interface for indices of the state variables.
    """

    dim: int

commonroad_control.vehicle_dynamics.input_interface

InputInterface dataclass

Bases: ABC

Interface for dataclass objects storing control inputs of the vehicle models.

Source code in commonroad_control/vehicle_dynamics/input_interface.py 
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
@dataclass
class InputInterface(ABC):
    """
    Interface for dataclass objects storing control inputs of the vehicle models.
    """

    @property
    def dim(self) -> int:
        """
        :return: control input dimension
        """
        return InputInterfaceIndex.dim

    @abstractmethod
    def convert_to_array(self) -> np.ndarray:
        """
        Converts instance of class to numpy array.
        :return: (dim, 1) np.ndarray
        """
        pass
dim property

Returns:

Type Description
int

control input dimension
convert_to_array() abstractmethod

Converts instance of class to numpy array.

Returns:

Type Description
ndarray

(dim, 1) np.ndarray
Source code in commonroad_control/vehicle_dynamics/input_interface.py 
29
30
31
32
33
34
35
@abstractmethod
def convert_to_array(self) -> np.ndarray:
    """
    Converts instance of class to numpy array.
    :return: (dim, 1) np.ndarray
    """
    pass

InputInterfaceIndex dataclass

Bases: ABC

Interface for the indices of the control inputs.

Source code in commonroad_control/vehicle_dynamics/input_interface.py 
 7
 8
 9
10
11
12
13
@dataclass(frozen=True)
class InputInterfaceIndex(ABC):
    """
    Interface for the indices of the control inputs.
    """

    dim: int

commonroad_control.vehicle_dynamics.trajectory_interface

TrajectoryInterface dataclass

Bases: ABC

Interface for State/Input/Disturbance trajectories of a given vehicle model. Trajectory points are stored as a dict of points and assumed to be sampled at constant rate of 1/delta_t.

Source code in commonroad_control/vehicle_dynamics/trajectory_interface.py 
 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
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
@dataclass
class TrajectoryInterface(ABC):
    """
    Interface for State/Input/Disturbance trajectories of a given vehicle model.
    Trajectory points are stored as a dict of points and assumed to be sampled at constant rate of 1/delta_t.
    """

    points: Dict[int, Any]  # dict of points
    delta_t: float  # sampling time
    mode: TrajectoryMode  # state/input/disturbance trajectory
    t_0: float = 0.0  # initial time
    steps: List[int] = None  # time steps of the trajectory points
    t_final: Optional[float] = None  # final time
    initial_point: Union[StateInterface, InputInterface] = None
    final_point: Union[StateInterface, InputInterface] = None
    dim: Optional[int] = None  # dimension of points

    def __post_init__(self):
        self.sanity_check()
        self.dim = self.points[0].dim
        self.initial_point = self.points[min(self.points.keys())]
        self.final_point = self.points[max(self.points.keys())]
        self.steps = sorted(self.points.keys())
        self.t_final = self.t_0 + max(self.points.keys()) * self.delta_t

    def sanity_check(self) -> None:
        """
        Sanity check
        """
        if len(self.points.keys()) == 0:
            logger.error("Dict of points must contain more than 0 values.")
            raise ValueError("Dict of points must contain more than 0 values")
        if None in self.points.values():
            logger.error("Points must not contain None")
            raise ValueError("Points must not contain None")
        if 0 != min(self.points.keys()):
            logger.error("Time step of initial points must be 0.")
            raise ValueError("Time step of initial point must be 0.")
        initial_point = self.points[0]
        for point in self.points.values():
            if type(point) is not type(initial_point):
                logger.error("Type of trajectory points is not unique.")
                raise TypeError("Type of trajectory points is not unique.")

    def convert_to_numpy_array(
        self,
        time: List[float],
        linear_interpolate: bool = False,
        sidt_factory: Optional["StateInputDisturbanceTrajectoryFactoryInterface"] = None,
    ) -> np.ndarray:
        """
        Extracts the trajectory points at given points in time and stores the point variables in an array.
        By default, the to-be-extracted points are approximated by the point at the closest discrete point in time of the trajectory.
        If a more accurate result is desired, the to-be-extracted points can be approximated using linear interpolation between trajectory points at adjacent time steps (see input argument: linear_interpolate).
        :param time: desired points in time for sampling the trajectory - list of floats
        :param linear_interpolate: if true, compute to-be-extracted points using linear interpolation; otherwise, approximate by the closest trajectory point
        :param sidt_factory: factory for generating points, required if linear_interpolate=True - StateInputDisturbanceTrajectoryFactoryInterface
        :return: (approximation of) desired trajectory points - array of dimension (dim, len(time))
        """

        traj_np = []
        for ii in range(len(time)):
            if linear_interpolate:
                y_ii = self.get_point_at_time(time=time[ii], factory=sidt_factory)
            else:
                y_ii = self.get_point_at_time_step(time_step=round((time[ii] - self.t_0) / self.delta_t))
            traj_np.append(np.reshape(y_ii.convert_to_array(), (y_ii.dim, 1), order="F"))

        return np.hstack(traj_np)

    def get_point_at_time_step(self, time_step: int) -> Union[StateInterface, InputInterface, None]:
        """
        Returns the trajectory point at a given (discrete) time step or None if not existing.
        :param time_step: time step - int
        :return: StateInterface/InputInterface/DisturbanceInterface at step or None if not existing
        """

        return self.points[time_step] if time_step in self.points.keys() else None

    def get_point_before_and_after_time(self, time: float) -> Tuple[Any, Any, int, int]:
        """
        Finds trajectory points at discrete time steps before and after a given point in time.
        :param time: point in time - float
        :return: point before time, point after time, time step before, time step after
        """
        if lt_tol(time, self.t_0):
            logger.error(f"Time {time} is before trajectory initial time {self.t_0}")
            raise ValueError(f"Time {time} is before trajectory initial time {self.t_0}")
        if gt_tol(time, self.t_final):
            logger.error(f"Time {time} is after trajectory final time {self.t_final}")
            raise ValueError(f"Time {time} is after trajectory final time {self.t_final}")
        idx_lower: int = min(math.floor((time - self.t_0) / self.delta_t), max(self.points.keys()))
        idx_upper: int = min(math.ceil((time - self.t_0) / self.delta_t), max(self.points.keys()))

        return self.points[idx_lower], self.points[idx_upper], idx_lower, idx_upper

    def append_point(self, next_point: Union[StateInterface, InputInterface, DisturbanceInterface]) -> None:
        """
        Appends a point to the trajectory.
        :param next_point: point to be appended
        """
        if type(next_point) is type(self.final_point):
            self.points[self.steps[-1] + 1] = next_point
            self.__post_init__()
        else:
            logger.error(f"Expected point of type {type(self.final_point)}, " f"got {type(next_point)}instead")
            raise TypeError(f"Expected point of type {type(self.final_point)}, " f"got {type(next_point)}instead")

    def check_goal_reached(self, planning_problem: PlanningProblem, lanelet_network: LaneletNetwork) -> bool:
        """
        Returns true if one of the states is within the goal state.
        Always returns False for input trajectories.
        :param planning_problem: CommonRoad planning problem
        :return: True, if at least one state is within goal region of planning problem
        """
        is_reached_goal: bool = False
        if self.mode == TrajectoryMode.State:
            for step, state in self.points.items():
                custom_state = CustomState(
                    position=np.asarray([state.position_x, state.position_y]),
                    orientation=state.heading,
                    velocity=state.velocity,
                    time_step=step,
                )
                is_reached_goal: bool = planning_problem.goal.is_reached(custom_state)

                if not is_reached_goal:
                    is_reached_goal = check_position_in_goal_region(
                        goal_region=planning_problem.goal,
                        lanelet_network=lanelet_network,
                        position_x=state.position_x,
                        position_y=state.position_y,
                    )

                if is_reached_goal:
                    break
        return is_reached_goal

    def get_point_at_time(
        self, time: float, factory: "StateInputDisturbanceTrajectoryFactoryInterface"
    ) -> Union[StateInterface, InputInterface, DisturbanceInterface]:
        """
        Computes a point at a given time by linearly interpolating between the trajectory points at the adjacent
        (discrete) time steps.
        :param time: time at which to interpolate
        :param factory: sidt_factory for instantiating the interpolated point (dataclass object)
        :return: StateInterface/InputInterface/DisturbanceInterface
        """

        lower_point, upper_point, lower_idx, upper_idx = self.get_point_before_and_after_time(time=time)
        if lower_idx == upper_idx:
            new_point = lower_point
        else:
            alpha = (upper_idx * self.delta_t - time) / self.delta_t
            new_point_array: np.ndarray = (
                1 - alpha
            ) * upper_point.convert_to_array() + alpha * lower_point.convert_to_array()
            if self.mode is TrajectoryMode.State:
                new_point: StateInterface = factory.state_from_numpy_array(new_point_array)
            elif self.mode is TrajectoryMode.Input:
                new_point: InputInterface = factory.input_from_numpy_array(new_point_array)
            elif self.mode is TrajectoryMode.Disturbance:
                new_point: DisturbanceInterface = factory.disturbance_from_numpy_array(new_point_array)
            else:
                logger.error(f"Instantiation of new point not implemented for trajectory mode {self.mode}")
                raise TypeError(f"Instantiation of new point not implemented for trajectory mode {self.mode}")

        return new_point
append_point(next_point)

Appends a point to the trajectory.

Parameters:

Name Type Description Default
next_point Union[StateInterface, InputInterface, DisturbanceInterface]

point to be appended

 required
Source code in commonroad_control/vehicle_dynamics/trajectory_interface.py 
126
127
128
129
130
131
132
133
134
135
136
def append_point(self, next_point: Union[StateInterface, InputInterface, DisturbanceInterface]) -> None:
    """
    Appends a point to the trajectory.
    :param next_point: point to be appended
    """
    if type(next_point) is type(self.final_point):
        self.points[self.steps[-1] + 1] = next_point
        self.__post_init__()
    else:
        logger.error(f"Expected point of type {type(self.final_point)}, " f"got {type(next_point)}instead")
        raise TypeError(f"Expected point of type {type(self.final_point)}, " f"got {type(next_point)}instead")
check_goal_reached(planning_problem, lanelet_network)

Returns true if one of the states is within the goal state. Always returns False for input trajectories.

Parameters:

Name Type Description Default
planning_problem PlanningProblem

CommonRoad planning problem

 required

Returns:

Type Description
bool

True, if at least one state is within goal region of planning problem
Source code in commonroad_control/vehicle_dynamics/trajectory_interface.py 
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
def check_goal_reached(self, planning_problem: PlanningProblem, lanelet_network: LaneletNetwork) -> bool:
    """
    Returns true if one of the states is within the goal state.
    Always returns False for input trajectories.
    :param planning_problem: CommonRoad planning problem
    :return: True, if at least one state is within goal region of planning problem
    """
    is_reached_goal: bool = False
    if self.mode == TrajectoryMode.State:
        for step, state in self.points.items():
            custom_state = CustomState(
                position=np.asarray([state.position_x, state.position_y]),
                orientation=state.heading,
                velocity=state.velocity,
                time_step=step,
            )
            is_reached_goal: bool = planning_problem.goal.is_reached(custom_state)

            if not is_reached_goal:
                is_reached_goal = check_position_in_goal_region(
                    goal_region=planning_problem.goal,
                    lanelet_network=lanelet_network,
                    position_x=state.position_x,
                    position_y=state.position_y,
                )

            if is_reached_goal:
                break
    return is_reached_goal
convert_to_numpy_array(time, linear_interpolate=False, sidt_factory=None)

Extracts the trajectory points at given points in time and stores the point variables in an array. By default, the to-be-extracted points are approximated by the point at the closest discrete point in time of the trajectory. If a more accurate result is desired, the to-be-extracted points can be approximated using linear interpolation between trajectory points at adjacent time steps (see input argument: linear_interpolate).

Parameters:

Name Type Description Default
time List[float]

desired points in time for sampling the trajectory - list of floats

 required
linear_interpolate bool

if true, compute to-be-extracted points using linear interpolation; otherwise, approximate by the closest trajectory point

 False
sidt_factory Optional['StateInputDisturbanceTrajectoryFactoryInterface']

factory for generating points, required if linear_interpolate=True - StateInputDisturbanceTrajectoryFactoryInterface

 None

Returns:

Type Description
ndarray

(approximation of) desired trajectory points - array of dimension (dim, len(time))
Source code in commonroad_control/vehicle_dynamics/trajectory_interface.py 
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
def convert_to_numpy_array(
    self,
    time: List[float],
    linear_interpolate: bool = False,
    sidt_factory: Optional["StateInputDisturbanceTrajectoryFactoryInterface"] = None,
) -> np.ndarray:
    """
    Extracts the trajectory points at given points in time and stores the point variables in an array.
    By default, the to-be-extracted points are approximated by the point at the closest discrete point in time of the trajectory.
    If a more accurate result is desired, the to-be-extracted points can be approximated using linear interpolation between trajectory points at adjacent time steps (see input argument: linear_interpolate).
    :param time: desired points in time for sampling the trajectory - list of floats
    :param linear_interpolate: if true, compute to-be-extracted points using linear interpolation; otherwise, approximate by the closest trajectory point
    :param sidt_factory: factory for generating points, required if linear_interpolate=True - StateInputDisturbanceTrajectoryFactoryInterface
    :return: (approximation of) desired trajectory points - array of dimension (dim, len(time))
    """

    traj_np = []
    for ii in range(len(time)):
        if linear_interpolate:
            y_ii = self.get_point_at_time(time=time[ii], factory=sidt_factory)
        else:
            y_ii = self.get_point_at_time_step(time_step=round((time[ii] - self.t_0) / self.delta_t))
        traj_np.append(np.reshape(y_ii.convert_to_array(), (y_ii.dim, 1), order="F"))

    return np.hstack(traj_np)
get_point_at_time(time, factory)

Computes a point at a given time by linearly interpolating between the trajectory points at the adjacent (discrete) time steps.

Parameters:

Name Type Description Default
time float

time at which to interpolate

 required
factory 'StateInputDisturbanceTrajectoryFactoryInterface'

sidt_factory for instantiating the interpolated point (dataclass object)

 required

Returns:

Type Description
Union[StateInterface, InputInterface, DisturbanceInterface]

StateInterface/InputInterface/DisturbanceInterface
Source code in commonroad_control/vehicle_dynamics/trajectory_interface.py 
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
def get_point_at_time(
    self, time: float, factory: "StateInputDisturbanceTrajectoryFactoryInterface"
) -> Union[StateInterface, InputInterface, DisturbanceInterface]:
    """
    Computes a point at a given time by linearly interpolating between the trajectory points at the adjacent
    (discrete) time steps.
    :param time: time at which to interpolate
    :param factory: sidt_factory for instantiating the interpolated point (dataclass object)
    :return: StateInterface/InputInterface/DisturbanceInterface
    """

    lower_point, upper_point, lower_idx, upper_idx = self.get_point_before_and_after_time(time=time)
    if lower_idx == upper_idx:
        new_point = lower_point
    else:
        alpha = (upper_idx * self.delta_t - time) / self.delta_t
        new_point_array: np.ndarray = (
            1 - alpha
        ) * upper_point.convert_to_array() + alpha * lower_point.convert_to_array()
        if self.mode is TrajectoryMode.State:
            new_point: StateInterface = factory.state_from_numpy_array(new_point_array)
        elif self.mode is TrajectoryMode.Input:
            new_point: InputInterface = factory.input_from_numpy_array(new_point_array)
        elif self.mode is TrajectoryMode.Disturbance:
            new_point: DisturbanceInterface = factory.disturbance_from_numpy_array(new_point_array)
        else:
            logger.error(f"Instantiation of new point not implemented for trajectory mode {self.mode}")
            raise TypeError(f"Instantiation of new point not implemented for trajectory mode {self.mode}")

    return new_point
get_point_at_time_step(time_step)

Returns the trajectory point at a given (discrete) time step or None if not existing.

Parameters:

Name Type Description Default
time_step int

time step - int

 required

Returns:

Type Description
Union[StateInterface, InputInterface, None]

StateInterface/InputInterface/DisturbanceInterface at step or None if not existing
Source code in commonroad_control/vehicle_dynamics/trajectory_interface.py 
100
101
102
103
104
105
106
107
def get_point_at_time_step(self, time_step: int) -> Union[StateInterface, InputInterface, None]:
    """
    Returns the trajectory point at a given (discrete) time step or None if not existing.
    :param time_step: time step - int
    :return: StateInterface/InputInterface/DisturbanceInterface at step or None if not existing
    """

    return self.points[time_step] if time_step in self.points.keys() else None
get_point_before_and_after_time(time)

Finds trajectory points at discrete time steps before and after a given point in time.

Parameters:

Name Type Description Default
time float

point in time - float

 required

Returns:

Type Description
Tuple[Any, Any, int, int]

point before time, point after time, time step before, time step after
Source code in commonroad_control/vehicle_dynamics/trajectory_interface.py 
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
def get_point_before_and_after_time(self, time: float) -> Tuple[Any, Any, int, int]:
    """
    Finds trajectory points at discrete time steps before and after a given point in time.
    :param time: point in time - float
    :return: point before time, point after time, time step before, time step after
    """
    if lt_tol(time, self.t_0):
        logger.error(f"Time {time} is before trajectory initial time {self.t_0}")
        raise ValueError(f"Time {time} is before trajectory initial time {self.t_0}")
    if gt_tol(time, self.t_final):
        logger.error(f"Time {time} is after trajectory final time {self.t_final}")
        raise ValueError(f"Time {time} is after trajectory final time {self.t_final}")
    idx_lower: int = min(math.floor((time - self.t_0) / self.delta_t), max(self.points.keys()))
    idx_upper: int = min(math.ceil((time - self.t_0) / self.delta_t), max(self.points.keys()))

    return self.points[idx_lower], self.points[idx_upper], idx_lower, idx_upper
sanity_check()

Sanity check

Source code in commonroad_control/vehicle_dynamics/trajectory_interface.py 
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
def sanity_check(self) -> None:
    """
    Sanity check
    """
    if len(self.points.keys()) == 0:
        logger.error("Dict of points must contain more than 0 values.")
        raise ValueError("Dict of points must contain more than 0 values")
    if None in self.points.values():
        logger.error("Points must not contain None")
        raise ValueError("Points must not contain None")
    if 0 != min(self.points.keys()):
        logger.error("Time step of initial points must be 0.")
        raise ValueError("Time step of initial point must be 0.")
    initial_point = self.points[0]
    for point in self.points.values():
        if type(point) is not type(initial_point):
            logger.error("Type of trajectory points is not unique.")
            raise TypeError("Type of trajectory points is not unique.")

commonroad_control.vehicle_dynamics.disturbance_interface

DisturbanceInterface dataclass

Bases: UncertaintyInterface

Interface for dataclass objects storing disturbances of the vehicle models.

Source code in commonroad_control/vehicle_dynamics/disturbance_interface.py 
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
@dataclass
class DisturbanceInterface(UncertaintyInterface):
    """
    Interface for dataclass objects storing disturbances of the vehicle models.
    """

    @property
    def dim(self) -> int:
        """
        :return: disturbance dimension
        """
        return DisturbanceInterfaceIndex.dim

    @abstractmethod
    def convert_to_array(self) -> np.ndarray:
        """
        Converts instance of class to numpy array.
        :return: np.ndarray of dimension (self.dim,)
        """
        pass
dim property

Returns:

Type Description
int

disturbance dimension
convert_to_array() abstractmethod

Converts instance of class to numpy array.

Returns:

Type Description
ndarray

np.ndarray of dimension (self.dim,)
Source code in commonroad_control/vehicle_dynamics/disturbance_interface.py 
34
35
36
37
38
39
40
@abstractmethod
def convert_to_array(self) -> np.ndarray:
    """
    Converts instance of class to numpy array.
    :return: np.ndarray of dimension (self.dim,)
    """
    pass

DisturbanceInterfaceIndex dataclass

Bases: UncertaintyInterfaceIndex

Indices of the disturbance variables.

Source code in commonroad_control/vehicle_dynamics/disturbance_interface.py 
12
13
14
15
16
17
18
@dataclass(frozen=True)
class DisturbanceInterfaceIndex(UncertaintyInterfaceIndex):
    """
    Indices of the disturbance variables.
    """

    dim: int

commonroad_control.vehicle_dynamics.full_state_noise_interface

FullStateNoiseInterface dataclass

Bases: UncertaintyInterface

Abstract base class for full state noise - required for the full state feedback sensor model.

Source code in commonroad_control/vehicle_dynamics/full_state_noise_interface.py 
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
@dataclass
class FullStateNoiseInterface(UncertaintyInterface):
    """
    Abstract base class for full state noise - required for the full state feedback sensor model.
    """

    @property
    def dim(self) -> int:
        """
        :return: noise dimension
        """
        return FullStateNoiseInterfaceIndex.dim

    @abstractmethod
    def convert_to_array(self) -> np.ndarray:
        """
        Converts instance of class to numpy array.
        :return: np.ndarray of dimension (self.dim,)
        """
        pass
dim property

Returns:

Type Description
int

noise dimension
convert_to_array() abstractmethod

Converts instance of class to numpy array.

Returns:

Type Description
ndarray

np.ndarray of dimension (self.dim,)
Source code in commonroad_control/vehicle_dynamics/full_state_noise_interface.py 
34
35
36
37
38
39
40
@abstractmethod
def convert_to_array(self) -> np.ndarray:
    """
    Converts instance of class to numpy array.
    :return: np.ndarray of dimension (self.dim,)
    """
    pass

FullStateNoiseInterfaceIndex dataclass

Bases: UncertaintyInterfaceIndex

Indices of the noise variables.

Source code in commonroad_control/vehicle_dynamics/full_state_noise_interface.py 
12
13
14
15
16
17
18
@dataclass(frozen=True)
class FullStateNoiseInterfaceIndex(UncertaintyInterfaceIndex):
    """
    Indices of the noise variables.
    """

    dim: int

commonroad_control.vehicle_dynamics.sidt_factory_interface

StateInputDisturbanceTrajectoryFactoryInterface

Bases: ABC

Factory for creating State, Input, Disturbance, or Trajectory instances from the corresponding input arguments (fields of the corresponding dataclasses) or numpy arrays.

Source code in commonroad_control/vehicle_dynamics/sidt_factory_interface.py 
 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
class StateInputDisturbanceTrajectoryFactoryInterface(ABC):
    """
    Factory for creating State, Input, Disturbance, or Trajectory instances from the corresponding input arguments (fields of the corresponding dataclasses) or numpy arrays.
    """

    state_dimension: int
    input_dimension: int
    disturbance_dimension: int

    @classmethod
    @abstractmethod
    def state_from_args(cls, *args) -> StateInterface:
        """
        Create State from args
        """
        pass

    @classmethod
    @abstractmethod
    def input_from_args(cls, *args) -> InputInterface:
        """
        Create Input args
        """
        pass

    @classmethod
    @abstractmethod
    def disturbance_from_args(cls, *args) -> DisturbanceInterface:
        """
        Crate Disturbance args
        """
        pass

    @classmethod
    @abstractmethod
    def state_from_numpy_array(
        cls,
        x_np: np.ndarray,
    ) -> StateInterface:
        """
        Create State from numpy array.
        """
        pass

    @classmethod
    @abstractmethod
    def input_from_numpy_array(cls, u_np: np.ndarray) -> InputInterface:
        """
        Create Input from numpy array.
        """
        pass

    @classmethod
    @abstractmethod
    def disturbance_from_numpy_array(cls, w_np: np.ndarray) -> Union[Any]:
        """
        Create Disturbance from numpy array.
        """
        pass

    @classmethod
    @abstractmethod
    def trajectory_from_points(
        cls,
        trajectory_dict: Union[
            Dict[int, StateInterface],
            Dict[int, InputInterface],
            Dict[int, DisturbanceInterface],
        ],
        mode: TrajectoryMode,
        t_0: float,
        delta_t: float,
    ) -> TrajectoryInterface:
        """
        Create State, Input, or Disturbance Trajectory from list of points.
        """
        pass

    @classmethod
    @abstractmethod
    def trajectory_from_numpy_array(
        cls,
        traj_np: np.ndarray,
        mode: TrajectoryMode,
        time_steps: List[int],
        t_0: float,
        delta_t: float,
    ) -> TrajectoryInterface:
        """
        Create State, Input, or Disturbance Trajectory from numpy array.
        """
        pass
disturbance_from_args(*args) abstractmethod classmethod

Crate Disturbance args

Source code in commonroad_control/vehicle_dynamics/sidt_factory_interface.py 
40
41
42
43
44
45
46
@classmethod
@abstractmethod
def disturbance_from_args(cls, *args) -> DisturbanceInterface:
    """
    Crate Disturbance args
    """
    pass
disturbance_from_numpy_array(w_np) abstractmethod classmethod

Create Disturbance from numpy array.

Source code in commonroad_control/vehicle_dynamics/sidt_factory_interface.py 
67
68
69
70
71
72
73
@classmethod
@abstractmethod
def disturbance_from_numpy_array(cls, w_np: np.ndarray) -> Union[Any]:
    """
    Create Disturbance from numpy array.
    """
    pass
input_from_args(*args) abstractmethod classmethod

Create Input args

Source code in commonroad_control/vehicle_dynamics/sidt_factory_interface.py 
32
33
34
35
36
37
38
@classmethod
@abstractmethod
def input_from_args(cls, *args) -> InputInterface:
    """
    Create Input args
    """
    pass
input_from_numpy_array(u_np) abstractmethod classmethod

Create Input from numpy array.

Source code in commonroad_control/vehicle_dynamics/sidt_factory_interface.py 
59
60
61
62
63
64
65
@classmethod
@abstractmethod
def input_from_numpy_array(cls, u_np: np.ndarray) -> InputInterface:
    """
    Create Input from numpy array.
    """
    pass
state_from_args(*args) abstractmethod classmethod

Create State from args

Source code in commonroad_control/vehicle_dynamics/sidt_factory_interface.py 
24
25
26
27
28
29
30
@classmethod
@abstractmethod
def state_from_args(cls, *args) -> StateInterface:
    """
    Create State from args
    """
    pass
state_from_numpy_array(x_np) abstractmethod classmethod

Create State from numpy array.

Source code in commonroad_control/vehicle_dynamics/sidt_factory_interface.py 
48
49
50
51
52
53
54
55
56
57
@classmethod
@abstractmethod
def state_from_numpy_array(
    cls,
    x_np: np.ndarray,
) -> StateInterface:
    """
    Create State from numpy array.
    """
    pass
trajectory_from_numpy_array(traj_np, mode, time_steps, t_0, delta_t) abstractmethod classmethod

Create State, Input, or Disturbance Trajectory from numpy array.

Source code in commonroad_control/vehicle_dynamics/sidt_factory_interface.py 
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
@classmethod
@abstractmethod
def trajectory_from_numpy_array(
    cls,
    traj_np: np.ndarray,
    mode: TrajectoryMode,
    time_steps: List[int],
    t_0: float,
    delta_t: float,
) -> TrajectoryInterface:
    """
    Create State, Input, or Disturbance Trajectory from numpy array.
    """
    pass
trajectory_from_points(trajectory_dict, mode, t_0, delta_t) abstractmethod classmethod

Create State, Input, or Disturbance Trajectory from list of points.

Source code in commonroad_control/vehicle_dynamics/sidt_factory_interface.py 
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
@classmethod
@abstractmethod
def trajectory_from_points(
    cls,
    trajectory_dict: Union[
        Dict[int, StateInterface],
        Dict[int, InputInterface],
        Dict[int, DisturbanceInterface],
    ],
    mode: TrajectoryMode,
    t_0: float,
    delta_t: float,
) -> TrajectoryInterface:
    """
    Create State, Input, or Disturbance Trajectory from list of points.
    """
    pass