Skip to content

Double Integrator

commonroad_control.vehicle_dynamics.double_integrator.double_integrator

DoubleIntegrator

Bases: VehicleModelInterface

Double integrator model.

Source code in commonroad_control/vehicle_dynamics/double_integrator/double_integrator.py 
 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
class DoubleIntegrator(VehicleModelInterface):
    """
    Double integrator model.
    """

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

    def __init__(self, params: VehicleParameters, delta_t: float):

        self._sys_mat, self._input_mat = self._system_matrices()

        # set vehicle parameters
        self._a_long_max = params.a_long_max
        self._a_lat_max = params.a_lat_max

        # init base class
        super().__init__(
            params=params,
            nx=DIStateIndices.dim,
            nu=DIInputIndices.dim,
            nw=DIDisturbanceIndices.dim,
            delta_t=delta_t,
        )

    @staticmethod
    def _system_matrices() -> Tuple[np.array, np.array]:
        """
        :return: system and input matrix - arrays of dimension (self._nx, self._nx) and (self._nx, self._nu)
        """

        # system matrix
        sys_mat = np.zeros((DIStateIndices.dim, DIStateIndices.dim))
        sys_mat[DIStateIndices.position_long, DIStateIndices.velocity_long] = 1.0
        sys_mat[DIStateIndices.position_lat, DIStateIndices.velocity_lat] = 1.0

        # input matrix
        input_mat = np.zeros((DIStateIndices.dim, DIInputIndices.dim))
        input_mat[DIStateIndices.velocity_long, DIInputIndices.acceleration_long] = 1.0
        input_mat[DIStateIndices.velocity_lat, DIInputIndices.acceleration_lat] = 1.0

        return sys_mat, input_mat

    def _set_input_bounds(self, params: VehicleParameters) -> Tuple[DIInput, DIInput]:
        """
        Extract input bounds from vehicle parameters and returns them as instances of the Input class.
        :param params: vehicle parameters
        :return: lower and upper bound on the inputs - DIInputs
        """

        # lower bound
        u_lb = DIInput(acceleration_long=-params.a_long_max, acceleration_lat=-params.a_lat_max)
        # upper bound
        u_ub = DIInput(acceleration_long=params.a_long_max, acceleration_lat=params.a_lat_max)

        return u_lb, u_ub

    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]]],
    ) -> cas.SX.sym:
        """
        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,)
        """

        return self._sys_mat @ x + self._input_mat @ u + w

    @staticmethod
    def state_bounds(self) -> Tuple[np.array, np.array, np.array, np.array]:
        """

        :return:
        """
        # lower bound lb <= mat_lb*x
        mat_lb = np.zeros((2, DIStateIndices.dim), dtype=float)
        mat_lb[0, DIStateIndices.velocity_long] = 1.0
        mat_lb[1, DIStateIndices.velocity_lat] = 1.0

        lb = np.zeros((DIStateIndices.dim, 1), dtype=float)
        lb[DIStateIndices.velocity_long] = 0.0
        lb[DIStateIndices.velocity_lat] = -2.0
        lb = mat_lb @ lb

        # uppber bound mat_ub*x <= ub
        mat_ub = mat_lb

        ub = np.zeros((DIStateIndices.dim, 1), dtype=float)
        ub[DIStateIndices.velocity_long] = 10.0
        ub[DIStateIndices.velocity_lat] = 2.0
        ub = mat_ub @ ub

        return mat_lb, lb, mat_ub, ub

    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)
        """

        # compute matrices of discrete-time LTI system
        lit_ct = scsi.lti(
            self._sys_mat,
            self._input_mat,
            np.eye(self._nx),
            np.zeros((self._nx, self._nu)),
        )
        lit_dt = lit_ct.to_discrete(dt=self._delta_t, method="zoh")
        sys_mat_dt = lit_dt.A
        input_mat_dt = lit_dt.B

        # discrete-time dynamics
        xk = cas.SX.sym("xk", self._nx, 1)
        uk = cas.SX.sym("uk", self._nu, 1)
        x_next = cas.Function("dynamics_dt", [xk, uk], [sys_mat_dt @ xk + input_mat_dt @ uk])

        # Jacobians of the discrete-time dynamics
        jac_x = cas.Function("jac_dynamics_dt_x", [xk, uk], [sys_mat_dt])
        jac_u = cas.Function("jac_dynamics_dt_u", [xk, uk], [input_mat_dt])

        return x_next, jac_x, jac_u

    def compute_normalized_acceleration(
        self,
        x: Union[DIState, cas.SX.sym, np.array],
        u: Union[DIInput, 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 - State/ CasADi symbolic/ array of dimension (self._nx,)
        :param u: control input - Input/ CasADi symbolic/ array of dimension (self._nu,)
        :return: normalized longitudinal and lateral acceleration - float/ CasADi symbolic
        """

        # extract control input
        if isinstance(u, DIInput):
            u = u.convert_to_array()
        a_long = u[DIInputIndices.acceleration_long]
        a_lat = u[DIInputIndices.acceleration_lat]

        # normalized acceleration
        a_long_norm = a_long / self._a_long_max
        a_lat_norm = a_lat / self._a_lat_max

        return a_long_norm, a_lat_norm
_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/double_integrator/double_integrator.py 
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
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)
    """

    # compute matrices of discrete-time LTI system
    lit_ct = scsi.lti(
        self._sys_mat,
        self._input_mat,
        np.eye(self._nx),
        np.zeros((self._nx, self._nu)),
    )
    lit_dt = lit_ct.to_discrete(dt=self._delta_t, method="zoh")
    sys_mat_dt = lit_dt.A
    input_mat_dt = lit_dt.B

    # discrete-time dynamics
    xk = cas.SX.sym("xk", self._nx, 1)
    uk = cas.SX.sym("uk", self._nu, 1)
    x_next = cas.Function("dynamics_dt", [xk, uk], [sys_mat_dt @ xk + input_mat_dt @ uk])

    # Jacobians of the discrete-time dynamics
    jac_x = cas.Function("jac_dynamics_dt_x", [xk, uk], [sys_mat_dt])
    jac_u = cas.Function("jac_dynamics_dt_u", [xk, uk], [input_mat_dt])

    return x_next, jac_x, jac_u
_dynamics_cas(x, u, w)

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
sym

dynamics function evaluated at (x,u,w) - CasADi symbolic/ array of dimension (self._nx,)
Source code in commonroad_control/vehicle_dynamics/double_integrator/double_integrator.py 
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
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]]],
) -> cas.SX.sym:
    """
    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,)
    """

    return self._sys_mat @ x + self._input_mat @ u + w
_set_input_bounds(params)

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

Parameters:

Name Type Description Default
params VehicleParameters

vehicle parameters

 required

Returns:

Type Description
Tuple[DIInput, DIInput]

lower and upper bound on the inputs - DIInputs
Source code in commonroad_control/vehicle_dynamics/double_integrator/double_integrator.py 
74
75
76
77
78
79
80
81
82
83
84
85
86
def _set_input_bounds(self, params: VehicleParameters) -> Tuple[DIInput, DIInput]:
    """
    Extract input bounds from vehicle parameters and returns them as instances of the Input class.
    :param params: vehicle parameters
    :return: lower and upper bound on the inputs - DIInputs
    """

    # lower bound
    u_lb = DIInput(acceleration_long=-params.a_long_max, acceleration_lat=-params.a_lat_max)
    # upper bound
    u_ub = DIInput(acceleration_long=params.a_long_max, acceleration_lat=params.a_lat_max)

    return u_lb, u_ub
_system_matrices() staticmethod

Returns:

Type Description
Tuple[array, array]

system and input matrix - arrays of dimension (self._nx, self._nx) and (self._nx, self._nu)
Source code in commonroad_control/vehicle_dynamics/double_integrator/double_integrator.py 
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
@staticmethod
def _system_matrices() -> Tuple[np.array, np.array]:
    """
    :return: system and input matrix - arrays of dimension (self._nx, self._nx) and (self._nx, self._nu)
    """

    # system matrix
    sys_mat = np.zeros((DIStateIndices.dim, DIStateIndices.dim))
    sys_mat[DIStateIndices.position_long, DIStateIndices.velocity_long] = 1.0
    sys_mat[DIStateIndices.position_lat, DIStateIndices.velocity_lat] = 1.0

    # input matrix
    input_mat = np.zeros((DIStateIndices.dim, DIInputIndices.dim))
    input_mat[DIStateIndices.velocity_long, DIInputIndices.acceleration_long] = 1.0
    input_mat[DIStateIndices.velocity_lat, DIInputIndices.acceleration_lat] = 1.0

    return sys_mat, input_mat
compute_normalized_acceleration(x, u)

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

Parameters:

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

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

 required
u Union[DIInput, sym, array]

control input - Input/ 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/double_integrator/double_integrator.py 
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
def compute_normalized_acceleration(
    self,
    x: Union[DIState, cas.SX.sym, np.array],
    u: Union[DIInput, 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 - State/ CasADi symbolic/ array of dimension (self._nx,)
    :param u: control input - Input/ CasADi symbolic/ array of dimension (self._nu,)
    :return: normalized longitudinal and lateral acceleration - float/ CasADi symbolic
    """

    # extract control input
    if isinstance(u, DIInput):
        u = u.convert_to_array()
    a_long = u[DIInputIndices.acceleration_long]
    a_lat = u[DIInputIndices.acceleration_lat]

    # normalized acceleration
    a_long_norm = a_long / self._a_long_max
    a_lat_norm = a_lat / self._a_lat_max

    return a_long_norm, a_lat_norm
factory_method(params, delta_t) classmethod

Factory method to generate class

Parameters:

Name Type Description Default
params VehicleParameters

CommonRoad-Control vehicle params

 required
delta_t float

sampling time

 required

Returns:

Type Description
DoubleIntegrator

instance
Source code in commonroad_control/vehicle_dynamics/double_integrator/double_integrator.py 
29
30
31
32
33
34
35
36
37
@classmethod
def factory_method(cls, params: VehicleParameters, delta_t: float) -> "DoubleIntegrator":
    """
    Factory method to generate class
    :param params: CommonRoad-Control vehicle params
    :param delta_t: sampling time
    :return: instance
    """
    return DoubleIntegrator(params=params, delta_t=delta_t)
state_bounds() staticmethod

Returns:

Type Description
Tuple[array, array, array, array]
Source code in commonroad_control/vehicle_dynamics/double_integrator/double_integrator.py 
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
@staticmethod
def state_bounds(self) -> Tuple[np.array, np.array, np.array, np.array]:
    """

    :return:
    """
    # lower bound lb <= mat_lb*x
    mat_lb = np.zeros((2, DIStateIndices.dim), dtype=float)
    mat_lb[0, DIStateIndices.velocity_long] = 1.0
    mat_lb[1, DIStateIndices.velocity_lat] = 1.0

    lb = np.zeros((DIStateIndices.dim, 1), dtype=float)
    lb[DIStateIndices.velocity_long] = 0.0
    lb[DIStateIndices.velocity_lat] = -2.0
    lb = mat_lb @ lb

    # uppber bound mat_ub*x <= ub
    mat_ub = mat_lb

    ub = np.zeros((DIStateIndices.dim, 1), dtype=float)
    ub[DIStateIndices.velocity_long] = 10.0
    ub[DIStateIndices.velocity_lat] = 2.0
    ub = mat_ub @ ub

    return mat_lb, lb, mat_ub, ub

commonroad_control.vehicle_dynamics.double_integrator.di_state

DIState dataclass

Bases: StateInterface

Dataclass storing the states of the double integrator model.

Source code in commonroad_control/vehicle_dynamics/double_integrator/di_state.py 
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
@dataclass
class DIState(StateInterface):
    """
    Dataclass storing the states of the double integrator model.
    """

    position_long: float = None
    position_lat: float = None
    velocity_long: float = None
    velocity_lat: float = None

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

    def convert_to_array(self) -> np.ndarray:
        """
        Converts instance of class to numpy array.
        :return: np.ndarray of dimension (self.dim,)
        """
        x_np = np.zeros((self.dim,))
        x_np[DIStateIndices.position_long] = self.position_long
        x_np[DIStateIndices.position_lat] = self.position_lat
        x_np[DIStateIndices.velocity_long] = self.velocity_long
        x_np[DIStateIndices.velocity_lat] = self.velocity_lat

        return x_np

    def to_cr_initial_state(self, time_step: int) -> InitialState:
        """
        Convert to CommonRoad initial state
        :param time_step: time step - int
        :return: CommonRoad InitialState
        """
        raise NotImplementedError("to_cr_initial_state() has not been implemented yet.")

    def to_cr_custom_state(self, time_step: int) -> CustomState:
        """
        Convert to CommonRoad custom state
        :param time_step: time step - int
        :return: CommonRoad custom state
        """
        raise NotImplementedError("to_cr_custom_state() has not been implemented yet.")
dim property

Returns:

Type Description
int

state dimension
convert_to_array()

Converts instance of class to numpy array.

Returns:

Type Description
ndarray

np.ndarray of dimension (self.dim,)
Source code in commonroad_control/vehicle_dynamics/double_integrator/di_state.py 
43
44
45
46
47
48
49
50
51
52
53
54
def convert_to_array(self) -> np.ndarray:
    """
    Converts instance of class to numpy array.
    :return: np.ndarray of dimension (self.dim,)
    """
    x_np = np.zeros((self.dim,))
    x_np[DIStateIndices.position_long] = self.position_long
    x_np[DIStateIndices.position_lat] = self.position_lat
    x_np[DIStateIndices.velocity_long] = self.velocity_long
    x_np[DIStateIndices.velocity_lat] = self.velocity_lat

    return x_np
to_cr_custom_state(time_step)

Convert to CommonRoad custom state

Parameters:

Name Type Description Default
time_step int

time step - int

 required

Returns:

Type Description
CustomState

CommonRoad custom state
Source code in commonroad_control/vehicle_dynamics/double_integrator/di_state.py 
64
65
66
67
68
69
70
def to_cr_custom_state(self, time_step: int) -> CustomState:
    """
    Convert to CommonRoad custom state
    :param time_step: time step - int
    :return: CommonRoad custom state
    """
    raise NotImplementedError("to_cr_custom_state() has not been implemented yet.")
to_cr_initial_state(time_step)

Convert to CommonRoad initial state

Parameters:

Name Type Description Default
time_step int

time step - int

 required

Returns:

Type Description
InitialState

CommonRoad InitialState
Source code in commonroad_control/vehicle_dynamics/double_integrator/di_state.py 
56
57
58
59
60
61
62
def to_cr_initial_state(self, time_step: int) -> InitialState:
    """
    Convert to CommonRoad initial state
    :param time_step: time step - int
    :return: CommonRoad InitialState
    """
    raise NotImplementedError("to_cr_initial_state() has not been implemented yet.")

DIStateIndices dataclass

Bases: StateInterfaceIndex

Indices of the states of the double integrator model.

Source code in commonroad_control/vehicle_dynamics/double_integrator/di_state.py 
12
13
14
15
16
17
18
19
20
21
22
@dataclass(frozen=True)
class DIStateIndices(StateInterfaceIndex):
    """
    Indices of the states of the double integrator model.
    """

    dim: int = 4
    position_long: int = 0
    position_lat: int = 1
    velocity_long: int = 2
    velocity_lat: int = 3

commonroad_control.vehicle_dynamics.double_integrator.di_input

DIInput dataclass

Bases: InputInterface

Dataclass storing the control input of the double integrator model.

Source code in commonroad_control/vehicle_dynamics/double_integrator/di_input.py 
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
@dataclass
class DIInput(InputInterface):
    """
    Dataclass storing the control input of the double integrator model.
    """

    acceleration_long: float = None
    acceleration_lat: float = None

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

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

        u_np = np.zeros((self.dim,))
        u_np[DIInputIndices.acceleration_long] = self.acceleration_long
        u_np[DIInputIndices.acceleration_lat] = self.acceleration_lat

        return u_np
dim property

Returns:

Type Description
int

control input dimension
convert_to_array()

Converts instance of class to numpy array.

Returns:

Type Description
ndarray

np.ndarray of dimension (self.dim,)
Source code in commonroad_control/vehicle_dynamics/double_integrator/di_input.py 
38
39
40
41
42
43
44
45
46
47
48
def convert_to_array(self) -> np.ndarray:
    """
    Converts instance of class to numpy array.
    :return: np.ndarray of dimension (self.dim,)
    """

    u_np = np.zeros((self.dim,))
    u_np[DIInputIndices.acceleration_long] = self.acceleration_long
    u_np[DIInputIndices.acceleration_lat] = self.acceleration_lat

    return u_np

DIInputIndices dataclass

Bases: InputInterfaceIndex

Indices of the control inputs of the double integrator model.

Source code in commonroad_control/vehicle_dynamics/double_integrator/di_input.py 
11
12
13
14
15
16
17
18
19
@dataclass(frozen=True)
class DIInputIndices(InputInterfaceIndex):
    """
    Indices of the control inputs of the double integrator model.
    """

    dim: int = 2
    acceleration_long: int = 0
    acceleration_lat: int = 1

commonroad_control.vehicle_dynamics.double_integrator.di_trajectory

DITrajectory dataclass

Bases: TrajectoryInterface

Double integrator model Trajectory

Source code in commonroad_control/vehicle_dynamics/double_integrator/di_trajectory.py 
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
@dataclass
class DITrajectory(TrajectoryInterface):
    """
    Double integrator model Trajectory
    """

    def to_cr_dynamic_obstacle(
        self,
        vehicle_width: float,
        vehicle_length: float,
        vehicle_id: int,
    ):
        """
        Converts state trajectory to CommonRoad dynamic obstacles for plotting
        :param vehicle_width: vehicle width
        :param vehicle_length: vehicle length
        :param vehicle_id: vehicle id
        :return: CommonRoad dynamic obstacle
        """
        raise NotImplementedError("to_cr_dynamic_obstacle() has not been implemented yet.")
to_cr_dynamic_obstacle(vehicle_width, vehicle_length, vehicle_id)

Converts state trajectory to CommonRoad dynamic obstacles for plotting

Parameters:

Name Type Description Default
vehicle_width float

vehicle width

 required
vehicle_length float

vehicle length

 required
vehicle_id int

vehicle id

 required

Returns:

Type Description

CommonRoad dynamic obstacle
Source code in commonroad_control/vehicle_dynamics/double_integrator/di_trajectory.py 
17
18
19
20
21
22
23
24
25
26
27
28
29
30
def to_cr_dynamic_obstacle(
    self,
    vehicle_width: float,
    vehicle_length: float,
    vehicle_id: int,
):
    """
    Converts state trajectory to CommonRoad dynamic obstacles for plotting
    :param vehicle_width: vehicle width
    :param vehicle_length: vehicle length
    :param vehicle_id: vehicle id
    :return: CommonRoad dynamic obstacle
    """
    raise NotImplementedError("to_cr_dynamic_obstacle() has not been implemented yet.")

commonroad_control.vehicle_dynamics.double_integrator.di_disturbance

DIDisturbance dataclass

Bases: DisturbanceInterface

Dataclass storing the disturbances of the double integrator model.

Source code in commonroad_control/vehicle_dynamics/double_integrator/di_disturbance.py 
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
@dataclass
class DIDisturbance(DisturbanceInterface):
    """
    Dataclass storing the disturbances of the double integrator model.
    """

    position_long: float = 0.0
    position_lat: float = 0.0
    velocity_long: float = 0.0
    velocity_lat: float = 0.0

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

    def convert_to_array(self) -> np.ndarray:
        """
        Converts instance of class to numpy array.
        :return: np.ndarray of dimension (self.dim,)
        """
        w_np = np.zeros((self.dim,))
        w_np[DIDisturbanceIndices.position_long] = self.position_long
        w_np[DIDisturbanceIndices.position_lat] = self.position_lat
        w_np[DIDisturbanceIndices.velocity_long] = self.velocity_long
        w_np[DIDisturbanceIndices.velocity_lat] = self.velocity_lat

        return w_np
dim property

Returns:

Type Description
int

disturbance dimension
convert_to_array()

Converts instance of class to numpy array.

Returns:

Type Description
ndarray

np.ndarray of dimension (self.dim,)
Source code in commonroad_control/vehicle_dynamics/double_integrator/di_disturbance.py 
42
43
44
45
46
47
48
49
50
51
52
53
def convert_to_array(self) -> np.ndarray:
    """
    Converts instance of class to numpy array.
    :return: np.ndarray of dimension (self.dim,)
    """
    w_np = np.zeros((self.dim,))
    w_np[DIDisturbanceIndices.position_long] = self.position_long
    w_np[DIDisturbanceIndices.position_lat] = self.position_lat
    w_np[DIDisturbanceIndices.velocity_long] = self.velocity_long
    w_np[DIDisturbanceIndices.velocity_lat] = self.velocity_lat

    return w_np

DIDisturbanceIndices dataclass

Bases: DisturbanceInterfaceIndex

Indices of the disturbances of the double integrator model.

Source code in commonroad_control/vehicle_dynamics/double_integrator/di_disturbance.py 
11
12
13
14
15
16
17
18
19
20
21
@dataclass(frozen=True)
class DIDisturbanceIndices(DisturbanceInterfaceIndex):
    """
    Indices of the disturbances of the double integrator model.
    """

    dim: int = 4
    position_long: int = 0
    position_lat: int = 1
    velocity_long: int = 2
    velocity_lat: int = 3

commonroad_control.vehicle_dynamics.double_integrator.di_noise

DINoise dataclass

Bases: FullStateNoiseInterface

Full state noise of the double integrator model - required for the full state feedback sensor model.

Source code in commonroad_control/vehicle_dynamics/double_integrator/di_noise.py 
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
@dataclass
class DINoise(FullStateNoiseInterface):
    """
    Full state noise of the double integrator model - required for the full state feedback sensor model.
    """

    position_long: float = 0.0
    position_lat: float = 0.0
    velocity_long: float = 0.0
    velocity_lat: float = 0.0

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

    def convert_to_array(self) -> np.ndarray:
        """
        Converts instance of class to numpy array.
        :return: np.ndarray of dimension (self.dim,)
        """
        y_np = np.zeros((self.dim,))
        y_np[DINoiseIndices.position_long] = self.position_long
        y_np[DINoiseIndices.position_lat] = self.position_lat
        y_np[DINoiseIndices.velocity_long] = self.velocity_long
        y_np[DINoiseIndices.velocity_lat] = self.velocity_lat

        return y_np
dim property

Returns:

Type Description
int

noise dimension
convert_to_array()

Converts instance of class to numpy array.

Returns:

Type Description
ndarray

np.ndarray of dimension (self.dim,)
Source code in commonroad_control/vehicle_dynamics/double_integrator/di_noise.py 
42
43
44
45
46
47
48
49
50
51
52
53
def convert_to_array(self) -> np.ndarray:
    """
    Converts instance of class to numpy array.
    :return: np.ndarray of dimension (self.dim,)
    """
    y_np = np.zeros((self.dim,))
    y_np[DINoiseIndices.position_long] = self.position_long
    y_np[DINoiseIndices.position_lat] = self.position_lat
    y_np[DINoiseIndices.velocity_long] = self.velocity_long
    y_np[DINoiseIndices.velocity_lat] = self.velocity_lat

    return y_np

DINoiseIndices dataclass

Bases: FullStateNoiseInterfaceIndex

Indices of the noise variables.

Source code in commonroad_control/vehicle_dynamics/double_integrator/di_noise.py 
11
12
13
14
15
16
17
18
19
20
21
@dataclass(frozen=True)
class DINoiseIndices(FullStateNoiseInterfaceIndex):
    """
    Indices of the noise variables.
    """

    dim: int = 4
    position_long: int = 0
    position_lat: int = 1
    velocity_long: int = 2
    velocity_lat: int = 3

commonroad_control.vehicle_dynamics.double_integrator.di_sidt_factory

DISIDTFactory

Bases: StateInputDisturbanceTrajectoryFactoryInterface

Factory for creating double integrator model State, Input, Disturbance, and Trajectory.

Source code in commonroad_control/vehicle_dynamics/double_integrator/di_sidt_factory.py 
 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
class DISIDTFactory(StateInputDisturbanceTrajectoryFactoryInterface):
    """
    Factory for creating double integrator model State, Input, Disturbance, and Trajectory.
    """

    state_dimension = DIStateIndices.dim
    input_dimension = DIInputIndices.dim
    disturbance_dimension = DIDisturbanceIndices.dim

    @classmethod
    def state_from_args(
        cls,
        position_long: float,
        position_lat: float,
        velocity_long: float,
        velocity_lat: float,
    ) -> Union["DIState"]:
        """
        Create State from args
        :param position_long: longitudinal position
        :param position_lat: lateral position
        :param velocity_long: longitudinal velocity
        :param velocity_lat: lateral velocity
        :return: DIState
        """
        return DIState(
            position_long=position_long,
            position_lat=position_lat,
            velocity_long=velocity_long,
            velocity_lat=velocity_lat,
        )

    @classmethod
    def input_from_args(cls, acceleration_long: float, acceleration_lat: float) -> Union["DIInput"]:
        """
        Create Input from args
        :param acceleration_long: longitudinal acceleration
        :param acceleration_lat: lateral acceleration
        :return: DIInput
        """
        return DIInput(acceleration_long=acceleration_long, acceleration_lat=acceleration_lat)

    @classmethod
    def disturbance_from_args(
        cls,
        position_long: float = 0.0,
        position_lat: float = 0.0,
        velocity_long: float = 0.0,
        velocity_lat: float = 0.0,
    ) -> Union["DIDisturbance"]:
        """
        Create Disturbance from args - the default value of all variables is zero.
        :param position_long: longitudinal position
        :param position_lat: lateral position
        :param velocity_long: longitudinal velocity
        :param velocity_lat: lateral velocity
        :return: DIDisturbance
        """
        return DIDisturbance(
            position_long=position_long,
            position_lat=position_lat,
            velocity_long=velocity_long,
            velocity_lat=velocity_lat,
        )

    @classmethod
    def state_from_numpy_array(cls, x_np: np.ndarray[tuple[float], np.dtype[np.float64]]) -> Union["DIState"]:
        """
        Create State from numpy array
        :param x_np: state vector - array of dimension (cls.state_dimension,)
        :return: DIState
        """

        if x_np.ndim > 1 or x_np.shape[0] != cls.state_dimension:
            logger.error(f"Size of np_array should be ({cls.state_dimension},) but is {x_np.ndim}")
            raise ValueError(f"Size of np_array should be ({cls.state_dimension},) but is {x_np.ndim}")

        return DIState(
            position_long=x_np[DIStateIndices.position_long],
            position_lat=x_np[DIStateIndices.position_lat],
            velocity_long=x_np[DIStateIndices.velocity_long],
            velocity_lat=x_np[DIStateIndices.velocity_lat],
        )

    @classmethod
    def input_from_numpy_array(cls, u_np: np.ndarray[tuple[float], np.dtype[np.float64]]) -> Union["DIInput"]:
        """
        Create Input from numpy array
        :param u_np: control input - array of dimension (cls.input_dimension,)
        :return: DIInput
        """

        if u_np.ndim > 1 or u_np.shape[0] != cls.input_dimension:
            logger.error(f"Size of np_array should be ({cls.input_dimension},) but is {u_np.ndim}")
            raise ValueError(f"Size of np_array should be ({cls.input_dimension},) but is {u_np.ndim}")

        return DIInput(
            acceleration_long=u_np[DIInputIndices.acceleration_long],
            acceleration_lat=u_np[DIInputIndices.acceleration_lat],
        )

    @classmethod
    def disturbance_from_numpy_array(
        cls, w_np: np.ndarray[tuple[float], np.dtype[np.float64]]
    ) -> Union["DIDisturbance"]:
        """
        Create Disturbance from numpy array
        :param w_np: disturbance - array of dimension (cls.disturbance_dimension,)
        :return: DIDisturbance
        """

        if w_np.ndim > 1 or w_np.shape[0] != cls.disturbance_dimension:
            logger.error(f"Size of np_array should be ({cls.disturbance_dimension},) but is {w_np.shape}")
            raise ValueError(f"Size of np_array should be ({cls.disturbance_dimension},) but is {w_np.shape}")

        return DIDisturbance(
            position_long=w_np[DIDisturbanceIndices.position_long],
            position_lat=w_np[DIDisturbanceIndices.position_lat],
            velocity_long=w_np[DIDisturbanceIndices.velocity_long],
            velocity_lat=w_np[DIDisturbanceIndices.velocity_lat],
        )

    @classmethod
    def trajectory_from_points(
        cls,
        trajectory_dict: Union[Dict[int, DIState], Dict[int, DIInput]],
        mode: TrajectoryMode,
        t_0: float,
        delta_t: float,
    ) -> DITrajectory:
        """
        Create State, Input, or Disturbance Trajectory from list of DI points.
        :param trajectory_dict: dict of time steps to kb points
        :param mode: type of points (State, Input, or Disturbance)
        :param t_0: initial time - float
        :param delta_t: sampling time - float
        :return: DITrajectory
        """
        return DITrajectory(points=trajectory_dict, mode=mode, t_0=t_0, delta_t=delta_t)

    @classmethod
    def trajectory_from_numpy_array(
        cls,
        traj_np: np.ndarray[tuple[float, float], np.dtype[np.float64]],
        mode: TrajectoryMode,
        time_steps: List[int],
        t_0: float,
        delta_t: float,
    ) -> DITrajectory:
        """
        Create State, Input, or Disturbance Trajectory from numpy array.
        :param traj_np: numpy array storing the values of the point variables
        :param mode: type of points (State, Input, or Disturbance)
        :param time_steps: time steps of the points in the columns of traj_np
        :param t_0: initial time - float
        :param delta_t: sampling time - float
        :return: DITrajectory
        """

        # convert trajectory points to State/Input/DisturbanceInterface
        points_val = []
        for kk in range(len(time_steps)):
            if mode == TrajectoryMode.State:
                points_val.append(cls.state_from_numpy_array(traj_np[:, kk]))
            elif mode == TrajectoryMode.Input:
                points_val.append(cls.input_from_numpy_array(traj_np[:, kk]))
            elif mode == TrajectoryMode.Disturbance:
                points_val.append(cls.disturbance_from_numpy_array(traj_np[:, kk]))

        return DITrajectory(
            points=dict(zip(time_steps, points_val)),
            mode=mode,
            delta_t=delta_t,
            t_0=t_0,
        )
disturbance_from_args(position_long=0.0, position_lat=0.0, velocity_long=0.0, velocity_lat=0.0) classmethod

Create Disturbance from args - the default value of all variables is zero.

Parameters:

Name Type Description Default
position_long float

longitudinal position

 0.0
position_lat float

lateral position

 0.0
velocity_long float

longitudinal velocity

 0.0
velocity_lat float

lateral velocity

 0.0

Returns:

Type Description
Union[DIDisturbance]

DIDisturbance
Source code in commonroad_control/vehicle_dynamics/double_integrator/di_sidt_factory.py 
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
@classmethod
def disturbance_from_args(
    cls,
    position_long: float = 0.0,
    position_lat: float = 0.0,
    velocity_long: float = 0.0,
    velocity_lat: float = 0.0,
) -> Union["DIDisturbance"]:
    """
    Create Disturbance from args - the default value of all variables is zero.
    :param position_long: longitudinal position
    :param position_lat: lateral position
    :param velocity_long: longitudinal velocity
    :param velocity_lat: lateral velocity
    :return: DIDisturbance
    """
    return DIDisturbance(
        position_long=position_long,
        position_lat=position_lat,
        velocity_long=velocity_long,
        velocity_lat=velocity_lat,
    )
disturbance_from_numpy_array(w_np) classmethod

Create Disturbance from numpy array

Parameters:

Name Type Description Default
w_np ndarray[tuple[float], dtype[float64]]

disturbance - array of dimension (cls.disturbance_dimension,)

 required

Returns:

Type Description
Union[DIDisturbance]

DIDisturbance
Source code in commonroad_control/vehicle_dynamics/double_integrator/di_sidt_factory.py 
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
@classmethod
def disturbance_from_numpy_array(
    cls, w_np: np.ndarray[tuple[float], np.dtype[np.float64]]
) -> Union["DIDisturbance"]:
    """
    Create Disturbance from numpy array
    :param w_np: disturbance - array of dimension (cls.disturbance_dimension,)
    :return: DIDisturbance
    """

    if w_np.ndim > 1 or w_np.shape[0] != cls.disturbance_dimension:
        logger.error(f"Size of np_array should be ({cls.disturbance_dimension},) but is {w_np.shape}")
        raise ValueError(f"Size of np_array should be ({cls.disturbance_dimension},) but is {w_np.shape}")

    return DIDisturbance(
        position_long=w_np[DIDisturbanceIndices.position_long],
        position_lat=w_np[DIDisturbanceIndices.position_lat],
        velocity_long=w_np[DIDisturbanceIndices.velocity_long],
        velocity_lat=w_np[DIDisturbanceIndices.velocity_lat],
    )
input_from_args(acceleration_long, acceleration_lat) classmethod

Create Input from args

Parameters:

Name Type Description Default
acceleration_long float

longitudinal acceleration

 required
acceleration_lat float

lateral acceleration

 required

Returns:

Type Description
Union[DIInput]

DIInput
Source code in commonroad_control/vehicle_dynamics/double_integrator/di_sidt_factory.py 
61
62
63
64
65
66
67
68
69
@classmethod
def input_from_args(cls, acceleration_long: float, acceleration_lat: float) -> Union["DIInput"]:
    """
    Create Input from args
    :param acceleration_long: longitudinal acceleration
    :param acceleration_lat: lateral acceleration
    :return: DIInput
    """
    return DIInput(acceleration_long=acceleration_long, acceleration_lat=acceleration_lat)
input_from_numpy_array(u_np) classmethod

Create Input from numpy array

Parameters:

Name Type Description Default
u_np ndarray[tuple[float], dtype[float64]]

control input - array of dimension (cls.input_dimension,)

 required

Returns:

Type Description
Union[DIInput]

DIInput
Source code in commonroad_control/vehicle_dynamics/double_integrator/di_sidt_factory.py 
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
@classmethod
def input_from_numpy_array(cls, u_np: np.ndarray[tuple[float], np.dtype[np.float64]]) -> Union["DIInput"]:
    """
    Create Input from numpy array
    :param u_np: control input - array of dimension (cls.input_dimension,)
    :return: DIInput
    """

    if u_np.ndim > 1 or u_np.shape[0] != cls.input_dimension:
        logger.error(f"Size of np_array should be ({cls.input_dimension},) but is {u_np.ndim}")
        raise ValueError(f"Size of np_array should be ({cls.input_dimension},) but is {u_np.ndim}")

    return DIInput(
        acceleration_long=u_np[DIInputIndices.acceleration_long],
        acceleration_lat=u_np[DIInputIndices.acceleration_lat],
    )
state_from_args(position_long, position_lat, velocity_long, velocity_lat) classmethod

Create State from args

Parameters:

Name Type Description Default
position_long float

longitudinal position

 required
position_lat float

lateral position

 required
velocity_long float

longitudinal velocity

 required
velocity_lat float

lateral velocity

 required

Returns:

Type Description
Union[DIState]

DIState
Source code in commonroad_control/vehicle_dynamics/double_integrator/di_sidt_factory.py 
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
@classmethod
def state_from_args(
    cls,
    position_long: float,
    position_lat: float,
    velocity_long: float,
    velocity_lat: float,
) -> Union["DIState"]:
    """
    Create State from args
    :param position_long: longitudinal position
    :param position_lat: lateral position
    :param velocity_long: longitudinal velocity
    :param velocity_lat: lateral velocity
    :return: DIState
    """
    return DIState(
        position_long=position_long,
        position_lat=position_lat,
        velocity_long=velocity_long,
        velocity_lat=velocity_lat,
    )
state_from_numpy_array(x_np) classmethod

Create State from numpy array

Parameters:

Name Type Description Default
x_np ndarray[tuple[float], dtype[float64]]

state vector - array of dimension (cls.state_dimension,)

 required

Returns:

Type Description
Union[DIState]

DIState
Source code in commonroad_control/vehicle_dynamics/double_integrator/di_sidt_factory.py 
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
@classmethod
def state_from_numpy_array(cls, x_np: np.ndarray[tuple[float], np.dtype[np.float64]]) -> Union["DIState"]:
    """
    Create State from numpy array
    :param x_np: state vector - array of dimension (cls.state_dimension,)
    :return: DIState
    """

    if x_np.ndim > 1 or x_np.shape[0] != cls.state_dimension:
        logger.error(f"Size of np_array should be ({cls.state_dimension},) but is {x_np.ndim}")
        raise ValueError(f"Size of np_array should be ({cls.state_dimension},) but is {x_np.ndim}")

    return DIState(
        position_long=x_np[DIStateIndices.position_long],
        position_lat=x_np[DIStateIndices.position_lat],
        velocity_long=x_np[DIStateIndices.velocity_long],
        velocity_lat=x_np[DIStateIndices.velocity_lat],
    )
trajectory_from_numpy_array(traj_np, mode, time_steps, t_0, delta_t) classmethod

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

Parameters:

Name Type Description Default
traj_np ndarray[tuple[float, float], dtype[float64]]

numpy array storing the values of the point variables

 required
mode TrajectoryMode

type of points (State, Input, or Disturbance)

 required
time_steps List[int]

time steps of the points in the columns of traj_np

 required
t_0 float

initial time - float

 required
delta_t float

sampling time - float

 required

Returns:

Type Description
DITrajectory

DITrajectory
Source code in commonroad_control/vehicle_dynamics/double_integrator/di_sidt_factory.py 
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
@classmethod
def trajectory_from_numpy_array(
    cls,
    traj_np: np.ndarray[tuple[float, float], np.dtype[np.float64]],
    mode: TrajectoryMode,
    time_steps: List[int],
    t_0: float,
    delta_t: float,
) -> DITrajectory:
    """
    Create State, Input, or Disturbance Trajectory from numpy array.
    :param traj_np: numpy array storing the values of the point variables
    :param mode: type of points (State, Input, or Disturbance)
    :param time_steps: time steps of the points in the columns of traj_np
    :param t_0: initial time - float
    :param delta_t: sampling time - float
    :return: DITrajectory
    """

    # convert trajectory points to State/Input/DisturbanceInterface
    points_val = []
    for kk in range(len(time_steps)):
        if mode == TrajectoryMode.State:
            points_val.append(cls.state_from_numpy_array(traj_np[:, kk]))
        elif mode == TrajectoryMode.Input:
            points_val.append(cls.input_from_numpy_array(traj_np[:, kk]))
        elif mode == TrajectoryMode.Disturbance:
            points_val.append(cls.disturbance_from_numpy_array(traj_np[:, kk]))

    return DITrajectory(
        points=dict(zip(time_steps, points_val)),
        mode=mode,
        delta_t=delta_t,
        t_0=t_0,
    )
trajectory_from_points(trajectory_dict, mode, t_0, delta_t) classmethod

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

Parameters:

Name Type Description Default
trajectory_dict Union[Dict[int, DIState], Dict[int, DIInput]]

dict of time steps to kb points

 required
mode TrajectoryMode

type of points (State, Input, or Disturbance)

 required
t_0 float

initial time - float

 required
delta_t float

sampling time - float

 required

Returns:

Type Description
DITrajectory

DITrajectory
Source code in commonroad_control/vehicle_dynamics/double_integrator/di_sidt_factory.py 
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
@classmethod
def trajectory_from_points(
    cls,
    trajectory_dict: Union[Dict[int, DIState], Dict[int, DIInput]],
    mode: TrajectoryMode,
    t_0: float,
    delta_t: float,
) -> DITrajectory:
    """
    Create State, Input, or Disturbance Trajectory from list of DI points.
    :param trajectory_dict: dict of time steps to kb points
    :param mode: type of points (State, Input, or Disturbance)
    :param t_0: initial time - float
    :param delta_t: sampling time - float
    :return: DITrajectory
    """
    return DITrajectory(points=trajectory_dict, mode=mode, t_0=t_0, delta_t=delta_t)