Skip to content

Dynamic Bicycle

commonroad_control.vehicle_dynamics.dynamic_bicycle.dynamic_bicycle

DynamicBicycle

Bases: VehicleModelInterface

Dynamic bicycle model with linear tyre model. Reference point for the vehicle dynamics: center of gravity.

Source code in commonroad_control/vehicle_dynamics/dynamic_bicycle/dynamic_bicycle.py 
 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
class DynamicBicycle(VehicleModelInterface):
    """
    Dynamic bicycle model with linear tyre model.
    Reference point for the vehicle dynamics: center of gravity.
    """

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

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

        # set vehicle parameters
        self._g = params.g
        self._m = params.m
        self._l_wb = params.l_wb
        self._l_f = params.l_f
        self._l_r = params.l_r
        self._I_zz = params.I_zz
        self._C_f = params.C_f
        self._C_r = params.C_r
        self._h_cog = params.h_cog
        self._a_long_max = params.a_long_max
        self._a_lat_max = params.a_lat_max

        # init base class
        super().__init__(
            params=params,
            nx=DBStateIndices.dim,
            nu=DBInputIndices.dim,
            nw=DBDisturbanceIndices.dim,
            delta_t=delta_t,
        )

    def _set_input_bounds(self, params: VehicleParameters) -> Tuple[DBInput, DBInput]:
        """
        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 - DBInputs
        """

        # lower bound
        u_lb = DBInput(
            acceleration=-params.a_long_max,
            steering_angle_velocity=-params.steering_angle_velocity_max,
        )

        # upper bound
        u_ub = DBInput(
            acceleration=params.a_long_max,
            steering_angle_velocity=params.steering_angle_velocity_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]]],
    ) -> Union[cas.SX.sym, np.array]:
        """
        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,)
        """

        # extract state
        v_bx = x[DBStateIndices.velocity_long]
        v_by = x[DBStateIndices.velocity_lat]
        psi = x[DBStateIndices.heading]
        psi_dot = x[DBStateIndices.yaw_rate]
        delta = x[DBStateIndices.steering_angle]

        # extract control input
        a = u[DBInputIndices.acceleration]
        delta_dot = u[DBInputIndices.steering_angle_velocity]

        # compute lateral tyre forces
        fc_f, fc_r = self._compute_lateral_tyre_forces(x, u)

        # dynamics
        position_x_dot = v_bx * cas.cos(psi) - v_by * cas.sin(psi)
        position_y_dot = v_bx * cas.sin(psi) + v_by * cas.cos(psi)
        velocity_long_dot = psi_dot * v_by + a - (fc_f * cas.sin(delta)) / self._m
        velocity_lat_dot = -psi_dot * v_bx + (fc_f * cas.cos(delta) + fc_r) / self._m
        heading_dot = psi_dot
        yaw_rate_dot = (self._l_f * fc_f * cas.cos(delta) - self._l_r * fc_r) / self._I_zz
        steering_angle_dot = delta_dot

        f = cas.vertcat(
            position_x_dot,
            position_y_dot,
            velocity_long_dot,
            velocity_lat_dot,
            heading_dot,
            yaw_rate_dot,
            steering_angle_dot,
        )

        # add disturbances
        f = f + np.reshape(w, shape=(w.size, 1))

        return f

    def compute_normalized_acceleration(
        self,
        x: Union[DBState, cas.SX.sym, np.array],
        u: Union[DBInput, 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
        """

        # extract state
        if isinstance(x, DBState):
            x = x.convert_to_array()
        delta = x[DBStateIndices.steering_angle]

        # extract control input
        if isinstance(u, DBInput):
            u = u.convert_to_array()
        a = u[DBInputIndices.acceleration]

        # compute lateral tyre forces
        fc_f, fc_r = self._compute_lateral_tyre_forces(x, u)

        # normalized acceleration
        a_long_norm = (a - fc_f * cas.sin(delta) / self._m) / self._a_long_max
        a_lat_norm = ((fc_f * cas.cos(delta) + fc_r) / self._m) / self._a_lat_max

        return a_long_norm, a_lat_norm

    def _compute_lateral_tyre_forces(
        self, x: Union[cas.SX.sym, np.array], u: Union[cas.SX.sym, np.array]
    ) -> Tuple[Union[float, cas.SX.sym], Union[float, cas.SX.sym]]:
        """
        Computes the lateral tyre forces at the front and rear axle.
        :param x: state - CasADi symbolic/ array of dimension (self._nx,)
        :param u: control input - CasADi symbolic/ array of dimension (self._nu,)
        :return: lateral tyre forces at front and rear axle - float/ CasADi symbolic
        """
        # extract state
        v_bx = x[DBStateIndices.velocity_long]
        v_by = x[DBStateIndices.velocity_lat]
        psi_dot = x[DBStateIndices.yaw_rate]
        delta = x[DBStateIndices.steering_angle]

        # extract control input
        a = u[DBInputIndices.acceleration]

        # (tyre) slip angles
        alpha_f = cas.atan((v_by + self._l_f * psi_dot) / v_bx) - delta
        alpha_r = cas.atan((v_by - self._l_r * psi_dot) / v_bx)

        # compute normal forces per axle (including longitudinal load transfer)
        fz_f = (self._m * self._g * self._l_r - self._m * a * self._h_cog) / self._l_wb
        fz_r = (self._m * self._g * self._l_f + self._m * a * self._h_cog) / self._l_wb

        # lateral tyre forces per axle
        fc_f = -self._C_f * alpha_f * fz_f
        fc_r = -self._C_r * alpha_r * fz_r

        return fc_f, fc_r
_compute_lateral_tyre_forces(x, u)

Computes the lateral tyre forces at the front and rear axle.

Parameters:

Name Type Description Default
x Union[sym, array]

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

 required
u Union[sym, array]

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

 required

Returns:

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

lateral tyre forces at front and rear axle - float/ CasADi symbolic
Source code in commonroad_control/vehicle_dynamics/dynamic_bicycle/dynamic_bicycle.py 
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
def _compute_lateral_tyre_forces(
    self, x: Union[cas.SX.sym, np.array], u: Union[cas.SX.sym, np.array]
) -> Tuple[Union[float, cas.SX.sym], Union[float, cas.SX.sym]]:
    """
    Computes the lateral tyre forces at the front and rear axle.
    :param x: state - CasADi symbolic/ array of dimension (self._nx,)
    :param u: control input - CasADi symbolic/ array of dimension (self._nu,)
    :return: lateral tyre forces at front and rear axle - float/ CasADi symbolic
    """
    # extract state
    v_bx = x[DBStateIndices.velocity_long]
    v_by = x[DBStateIndices.velocity_lat]
    psi_dot = x[DBStateIndices.yaw_rate]
    delta = x[DBStateIndices.steering_angle]

    # extract control input
    a = u[DBInputIndices.acceleration]

    # (tyre) slip angles
    alpha_f = cas.atan((v_by + self._l_f * psi_dot) / v_bx) - delta
    alpha_r = cas.atan((v_by - self._l_r * psi_dot) / v_bx)

    # compute normal forces per axle (including longitudinal load transfer)
    fz_f = (self._m * self._g * self._l_r - self._m * a * self._h_cog) / self._l_wb
    fz_r = (self._m * self._g * self._l_f + self._m * a * self._h_cog) / self._l_wb

    # lateral tyre forces per axle
    fc_f = -self._C_f * alpha_f * fz_f
    fc_r = -self._C_r * alpha_r * fz_r

    return fc_f, fc_r
_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
Union[sym, array]

dynamics function evaluated at (x,u,w) - CasADi symbolic/ array of dimension (self._nx,)
Source code in commonroad_control/vehicle_dynamics/dynamic_bicycle/dynamic_bicycle.py 
 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
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.array]:
    """
    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,)
    """

    # extract state
    v_bx = x[DBStateIndices.velocity_long]
    v_by = x[DBStateIndices.velocity_lat]
    psi = x[DBStateIndices.heading]
    psi_dot = x[DBStateIndices.yaw_rate]
    delta = x[DBStateIndices.steering_angle]

    # extract control input
    a = u[DBInputIndices.acceleration]
    delta_dot = u[DBInputIndices.steering_angle_velocity]

    # compute lateral tyre forces
    fc_f, fc_r = self._compute_lateral_tyre_forces(x, u)

    # dynamics
    position_x_dot = v_bx * cas.cos(psi) - v_by * cas.sin(psi)
    position_y_dot = v_bx * cas.sin(psi) + v_by * cas.cos(psi)
    velocity_long_dot = psi_dot * v_by + a - (fc_f * cas.sin(delta)) / self._m
    velocity_lat_dot = -psi_dot * v_bx + (fc_f * cas.cos(delta) + fc_r) / self._m
    heading_dot = psi_dot
    yaw_rate_dot = (self._l_f * fc_f * cas.cos(delta) - self._l_r * fc_r) / self._I_zz
    steering_angle_dot = delta_dot

    f = cas.vertcat(
        position_x_dot,
        position_y_dot,
        velocity_long_dot,
        velocity_lat_dot,
        heading_dot,
        yaw_rate_dot,
        steering_angle_dot,
    )

    # add disturbances
    f = f + np.reshape(w, shape=(w.size, 1))

    return f
_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[DBInput, DBInput]

lower and upper bound on the inputs - DBInputs
Source code in commonroad_control/vehicle_dynamics/dynamic_bicycle/dynamic_bicycle.py 
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
def _set_input_bounds(self, params: VehicleParameters) -> Tuple[DBInput, DBInput]:
    """
    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 - DBInputs
    """

    # lower bound
    u_lb = DBInput(
        acceleration=-params.a_long_max,
        steering_angle_velocity=-params.steering_angle_velocity_max,
    )

    # upper bound
    u_ub = DBInput(
        acceleration=params.a_long_max,
        steering_angle_velocity=params.steering_angle_velocity_max,
    )

    return u_lb, u_ub
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[DBState, sym, array]

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

 required
u Union[DBInput, 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/dynamic_bicycle/dynamic_bicycle.py 
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
def compute_normalized_acceleration(
    self,
    x: Union[DBState, cas.SX.sym, np.array],
    u: Union[DBInput, 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
    """

    # extract state
    if isinstance(x, DBState):
        x = x.convert_to_array()
    delta = x[DBStateIndices.steering_angle]

    # extract control input
    if isinstance(u, DBInput):
        u = u.convert_to_array()
    a = u[DBInputIndices.acceleration]

    # compute lateral tyre forces
    fc_f, fc_r = self._compute_lateral_tyre_forces(x, u)

    # normalized acceleration
    a_long_norm = (a - fc_f * cas.sin(delta) / self._m) / self._a_long_max
    a_lat_norm = ((fc_f * cas.cos(delta) + fc_r) / self._m) / 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
DynamicBicycle

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

commonroad_control.vehicle_dynamics.dynamic_bicycle.db_state

DBState dataclass

Bases: StateInterface

Dataclass storing the states of the dynamic bicycle model.

Source code in commonroad_control/vehicle_dynamics/dynamic_bicycle/db_state.py 
 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
@dataclass
class DBState(StateInterface):
    """
    Dataclass storing the states of the dynamic bicycle model.
    """

    position_x: float = None
    position_y: float = None
    velocity_long: float = None
    velocity_lat: float = None
    heading: float = None
    yaw_rate: float = None
    steering_angle: float = None

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

    @property
    def velocity(self) -> float:
        """
        :return: absolute value of velocity of the vehicle
        """
        return sqrt(self.velocity_long**2 + self.velocity_lat**2)

    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[DBStateIndices.position_x] = self.position_x
        x_np[DBStateIndices.position_y] = self.position_y
        x_np[DBStateIndices.velocity_long] = self.velocity_long
        x_np[DBStateIndices.velocity_lat] = self.velocity_lat
        x_np[DBStateIndices.heading] = self.heading
        x_np[DBStateIndices.yaw_rate] = self.yaw_rate
        x_np[DBStateIndices.steering_angle] = self.steering_angle

        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
        """
        return InitialState(
            position=np.asarray([self.position_x, self.position_y]),
            velocity=compute_velocity_from_components(v_long=self.velocity_long, v_lat=self.velocity_lat),
            orientation=self.heading,
            acceleration=0,
            yaw_rate=self.yaw_rate,
            slip_angle=compute_slip_angle_from_velocity_components(self.velocity_long, self.velocity_lat),
            time_step=time_step,
        )

    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
        """
        return CustomState(
            position=np.asarray([self.position_x, self.position_y]),
            velocity=compute_velocity_from_components(v_long=self.velocity_long, v_lat=self.velocity_lat),
            orientation=self.heading,
            acceleration=0,
            yaw_rate=0,
            slip_angle=compute_slip_angle_from_velocity_components(self.velocity_long, self.velocity_lat),
            time_step=time_step,
        )
dim property

Returns:

Type Description
int

state dimension
velocity property

Returns:

Type Description
float

absolute value of velocity of the vehicle
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/dynamic_bicycle/db_state.py 
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
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[DBStateIndices.position_x] = self.position_x
    x_np[DBStateIndices.position_y] = self.position_y
    x_np[DBStateIndices.velocity_long] = self.velocity_long
    x_np[DBStateIndices.velocity_lat] = self.velocity_lat
    x_np[DBStateIndices.heading] = self.heading
    x_np[DBStateIndices.yaw_rate] = self.yaw_rate
    x_np[DBStateIndices.steering_angle] = self.steering_angle

    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/dynamic_bicycle/db_state.py 
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
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
    """
    return CustomState(
        position=np.asarray([self.position_x, self.position_y]),
        velocity=compute_velocity_from_components(v_long=self.velocity_long, v_lat=self.velocity_lat),
        orientation=self.heading,
        acceleration=0,
        yaw_rate=0,
        slip_angle=compute_slip_angle_from_velocity_components(self.velocity_long, self.velocity_lat),
        time_step=time_step,
    )
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/dynamic_bicycle/db_state.py 
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
def to_cr_initial_state(self, time_step: int) -> InitialState:
    """
    Convert to CommonRoad initial state
    :param time_step: time step - int
    :return: CommonRoad InitialState
    """
    return InitialState(
        position=np.asarray([self.position_x, self.position_y]),
        velocity=compute_velocity_from_components(v_long=self.velocity_long, v_lat=self.velocity_lat),
        orientation=self.heading,
        acceleration=0,
        yaw_rate=self.yaw_rate,
        slip_angle=compute_slip_angle_from_velocity_components(self.velocity_long, self.velocity_lat),
        time_step=time_step,
    )

DBStateIndices dataclass

Bases: StateInterfaceIndex

Indices of the states of the dynamic bicycle model.

Source code in commonroad_control/vehicle_dynamics/dynamic_bicycle/db_state.py 
17
18
19
20
21
22
23
24
25
26
27
28
29
30
@dataclass(frozen=True)
class DBStateIndices(StateInterfaceIndex):
    """
    Indices of the states of the dynamic bicycle model.
    """

    dim: int = 7
    position_x: int = 0
    position_y: int = 1
    velocity_long: int = 2
    velocity_lat: int = 3
    heading: int = 4
    yaw_rate: int = 5
    steering_angle: int = 6

commonroad_control.vehicle_dynamics.dynamic_bicycle.db_input

DBInput dataclass

Bases: InputInterface

Dataclass storing the control input of the dynamic bicycle model.

Source code in commonroad_control/vehicle_dynamics/dynamic_bicycle/db_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 DBInput(InputInterface):
    """
    Dataclass storing the control input of the dynamic bicycle model.
    """

    acceleration: float = None
    steering_angle_velocity: float = None

    @property
    def dim(self) -> int:
        """
        :return: control input dimension
        """
        return DBInputIndices.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[DBInputIndices.acceleration] = self.acceleration
        u_np[DBInputIndices.steering_angle_velocity] = self.steering_angle_velocity

        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/dynamic_bicycle/db_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[DBInputIndices.acceleration] = self.acceleration
    u_np[DBInputIndices.steering_angle_velocity] = self.steering_angle_velocity

    return u_np

DBInputIndices dataclass

Bases: InputInterfaceIndex

Indices of the control inputs of the dynamic bicycle model.

Source code in commonroad_control/vehicle_dynamics/dynamic_bicycle/db_input.py 
11
12
13
14
15
16
17
18
19
@dataclass(frozen=True)
class DBInputIndices(InputInterfaceIndex):
    """
    Indices of the control inputs of the dynamic bicycle model.
    """

    dim: int = 2
    acceleration: int = 0
    steering_angle_velocity: int = 1

commonroad_control.vehicle_dynamics.dynamic_bicycle.db_trajectory

DBTrajectory dataclass

Bases: TrajectoryInterface

Dynamic bicycle model Trajectory

Source code in commonroad_control/vehicle_dynamics/dynamic_bicycle/db_trajectory.py 
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
@dataclass
class DBTrajectory(TrajectoryInterface):
    """
    Dynamic bicycle model Trajectory
    """

    def to_cr_dynamic_obstacle(
        self,
        vehicle_width: float,
        vehicle_length: float,
        vehicle_id: int,
    ) -> DynamicObstacle:
        """
        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
        """

        if self.mode is not TrajectoryMode.State:
            logger.error(
                f"Conversion to dynamic obstacle for plotting not admissible for trajectory points of type {self.mode}"
            )
            raise TypeError(
                f"Conversion to dynamic obstacle for plotting not admissible for trajectory points of type {self.mode}"
            )

        if not self.points:
            logger.error(f"Trajectory.points={self.points} is empty")
            raise ValueError(f"Trajectory.points={self.points} is empty")
        else:
            # convert to CR obstacle
            initial_state: InitialState = self.initial_point.to_cr_initial_state(time_step=min(self.points.keys()))
            state_list: List[CustomState] = [
                state.to_cr_custom_state(time_step=step) for step, state in self.points.items()
            ]

            cr_trajectory = Trajectory(state_list[0].time_step, state_list)
            shape = Rectangle(width=vehicle_width, length=vehicle_length)

            trajectory_prediction = TrajectoryPrediction(trajectory=cr_trajectory, shape=shape)
            # obstacle generation
            return DynamicObstacle(
                obstacle_id=vehicle_id,
                obstacle_type=ObstacleType.CAR,
                obstacle_shape=shape,
                initial_state=initial_state,
                prediction=trajectory_prediction,
            )
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
DynamicObstacle

CommonRoad dynamic obstacle
Source code in commonroad_control/vehicle_dynamics/dynamic_bicycle/db_trajectory.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
def to_cr_dynamic_obstacle(
    self,
    vehicle_width: float,
    vehicle_length: float,
    vehicle_id: int,
) -> DynamicObstacle:
    """
    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
    """

    if self.mode is not TrajectoryMode.State:
        logger.error(
            f"Conversion to dynamic obstacle for plotting not admissible for trajectory points of type {self.mode}"
        )
        raise TypeError(
            f"Conversion to dynamic obstacle for plotting not admissible for trajectory points of type {self.mode}"
        )

    if not self.points:
        logger.error(f"Trajectory.points={self.points} is empty")
        raise ValueError(f"Trajectory.points={self.points} is empty")
    else:
        # convert to CR obstacle
        initial_state: InitialState = self.initial_point.to_cr_initial_state(time_step=min(self.points.keys()))
        state_list: List[CustomState] = [
            state.to_cr_custom_state(time_step=step) for step, state in self.points.items()
        ]

        cr_trajectory = Trajectory(state_list[0].time_step, state_list)
        shape = Rectangle(width=vehicle_width, length=vehicle_length)

        trajectory_prediction = TrajectoryPrediction(trajectory=cr_trajectory, shape=shape)
        # obstacle generation
        return DynamicObstacle(
            obstacle_id=vehicle_id,
            obstacle_type=ObstacleType.CAR,
            obstacle_shape=shape,
            initial_state=initial_state,
            prediction=trajectory_prediction,
        )

commonroad_control.vehicle_dynamics.dynamic_bicycle.db_disturbance

DBDisturbance dataclass

Bases: DisturbanceInterface

Dataclass storing the disturbances of the kinematic bicycle model.

Source code in commonroad_control/vehicle_dynamics/dynamic_bicycle/db_disturbance.py 
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
@dataclass
class DBDisturbance(DisturbanceInterface):
    """
    Dataclass storing the disturbances of the kinematic bicycle model.
    """

    position_x: float = 0.0
    position_y: float = 0.0
    velocity_long: float = 0.0
    velocity_lat: float = 0.0
    heading: float = 0.0
    yaw_rate: float = 0.0
    steering_angle: float = 0.0

    @property
    def dim(self) -> int:
        """
        :return: disturbance dimension
        """
        return DBDisturbanceIndices.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[DBDisturbanceIndices.position_x] = self.position_x
        w_np[DBDisturbanceIndices.position_y] = self.position_y
        w_np[DBDisturbanceIndices.velocity_long] = self.velocity_long
        w_np[DBDisturbanceIndices.velocity_lat] = self.velocity_lat
        w_np[DBDisturbanceIndices.heading] = self.heading
        w_np[DBDisturbanceIndices.yaw_rate] = self.yaw_rate
        w_np[DBDisturbanceIndices.steering_angle] = self.steering_angle

        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/dynamic_bicycle/db_disturbance.py 
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
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[DBDisturbanceIndices.position_x] = self.position_x
    w_np[DBDisturbanceIndices.position_y] = self.position_y
    w_np[DBDisturbanceIndices.velocity_long] = self.velocity_long
    w_np[DBDisturbanceIndices.velocity_lat] = self.velocity_lat
    w_np[DBDisturbanceIndices.heading] = self.heading
    w_np[DBDisturbanceIndices.yaw_rate] = self.yaw_rate
    w_np[DBDisturbanceIndices.steering_angle] = self.steering_angle

    return w_np

DBDisturbanceIndices dataclass

Bases: DisturbanceInterfaceIndex

Indices of the disturbances of the dynamic bicycle model.

Source code in commonroad_control/vehicle_dynamics/dynamic_bicycle/db_disturbance.py 
11
12
13
14
15
16
17
18
19
20
21
22
23
24
@dataclass(frozen=True)
class DBDisturbanceIndices(DisturbanceInterfaceIndex):
    """
    Indices of the disturbances of the dynamic bicycle model.
    """

    dim: int = 7
    position_x: int = 0
    position_y: int = 1
    velocity_long: int = 2
    velocity_lat: int = 3
    heading: int = 4
    yaw_rate: int = 5
    steering_angle: int = 6

commonroad_control.vehicle_dynamics.dynamic_bicycle.db_noise

DBNoise dataclass

Bases: FullStateNoiseInterface

Full state noise of the dynamic bicycle model - required for the full state feedback sensor model.

Source code in commonroad_control/vehicle_dynamics/dynamic_bicycle/db_noise.py 
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
@dataclass
class DBNoise(FullStateNoiseInterface):
    """
    Full state noise of the dynamic bicycle model - required for the full state feedback sensor model.
    """

    position_x: float = 0.0
    position_y: float = 0.0
    velocity_long: float = 0.0
    velocity_lat: float = 0.0
    heading: float = 0.0
    yaw_rate: float = 0.0
    steering_angle: float = 0.0

    @property
    def dim(self) -> int:
        """
        :return: noise dimension
        """
        return DBNoiseIndices.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[DBNoiseIndices.position_x] = self.position_x
        y_np[DBNoiseIndices.position_y] = self.position_y
        y_np[DBNoiseIndices.velocity_long] = self.velocity_long
        y_np[DBNoiseIndices.velocity_lat] = self.velocity_lat
        y_np[DBNoiseIndices.heading] = self.heading
        y_np[DBNoiseIndices.yaw_rate] = self.yaw_rate
        y_np[DBNoiseIndices.steering_angle] = self.steering_angle

        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/dynamic_bicycle/db_noise.py 
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
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[DBNoiseIndices.position_x] = self.position_x
    y_np[DBNoiseIndices.position_y] = self.position_y
    y_np[DBNoiseIndices.velocity_long] = self.velocity_long
    y_np[DBNoiseIndices.velocity_lat] = self.velocity_lat
    y_np[DBNoiseIndices.heading] = self.heading
    y_np[DBNoiseIndices.yaw_rate] = self.yaw_rate
    y_np[DBNoiseIndices.steering_angle] = self.steering_angle

    return y_np

DBNoiseIndices dataclass

Bases: FullStateNoiseInterfaceIndex

Indices of the noise variables.

Source code in commonroad_control/vehicle_dynamics/dynamic_bicycle/db_noise.py 
11
12
13
14
15
16
17
18
19
20
21
22
23
24
@dataclass(frozen=True)
class DBNoiseIndices(FullStateNoiseInterfaceIndex):
    """
    Indices of the noise variables.
    """

    dim: int = 7
    position_x: int = 0
    position_y: int = 1
    velocity_long: int = 2
    velocity_lat: int = 3
    heading: int = 4
    yaw_rate: int = 5
    steering_angle: int = 6

commonroad_control.vehicle_dynamics.dynamic_bicycle.db_sidt_factory

DBSIDTFactory

Bases: StateInputDisturbanceTrajectoryFactoryInterface

Factory for creating dynamic bicycle model State, Input, Disturbance, and Trajectory.

Source code in commonroad_control/vehicle_dynamics/dynamic_bicycle/db_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
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
class DBSIDTFactory(StateInputDisturbanceTrajectoryFactoryInterface):
    """
    Factory for creating dynamic bicycle model State, Input, Disturbance, and Trajectory.
    """

    state_dimension: int = DBStateIndices.dim
    input_dimension: int = DBInputIndices.dim
    disturbance_dimension: int = DBDisturbanceIndices.dim

    @classmethod
    def state_from_args(
        cls,
        position_x: float,
        position_y: float,
        velocity_long: float,
        velocity_lat: float,
        heading: float,
        yaw_rate: float,
        steering_angle: float,
    ) -> Union["DBState"]:
        """
        Create State from args
        :param position_x: position x of center of gravity (Cartesian coordinates)
        :param position_y: position y of center of gravity (Cartesian coordinates)
        :param velocity_long: longitudinal velocity (body frame)
        :param velocity_lat: lateral velocity (body frame)
        :param heading: heading
        :param yaw_rate: yaw rate
        :param steering_angle: steering angle
        :return: DBState
        """
        return DBState(
            position_x=position_x,
            position_y=position_y,
            velocity_long=velocity_long,
            velocity_lat=velocity_lat,
            heading=heading,
            yaw_rate=yaw_rate,
            steering_angle=steering_angle,
        )

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

    @classmethod
    def disturbance_from_args(
        cls,
        position_x: float = 0.0,
        position_y: float = 0.0,
        velocity_long: float = 0.0,
        velocity_lat: float = 0.0,
        heading: float = 0.0,
        yaw_rate: float = 0.0,
        steering_angle: float = 0.0,
    ) -> Union["DBDisturbance"]:
        """
        Create Disturbance from args - the default value of all variables is zero.
        :param position_x: position x of center of gravity (Cartesian coordinates)
        :param position_y: position y of center of gravity (Cartesian coordinates)
        :param velocity_long: longitudinal velocity (body frame)
        :param velocity_lat: lateral velocity (body frame)
        :param heading: heading
        :param yaw_rate: yaw rate
        :param steering_angle: steering angle
        :return: DBDisturbance
        """
        return DBDisturbance(
            position_x=position_x,
            position_y=position_y,
            velocity_long=velocity_long,
            velocity_lat=velocity_lat,
            heading=heading,
            yaw_rate=yaw_rate,
            steering_angle=steering_angle,
        )

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

        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 DBState(
            position_x=x_np[DBStateIndices.position_x],
            position_y=x_np[DBStateIndices.position_y],
            velocity_long=x_np[DBStateIndices.velocity_long],
            velocity_lat=x_np[DBStateIndices.velocity_lat],
            heading=x_np[DBStateIndices.heading],
            yaw_rate=x_np[DBStateIndices.yaw_rate],
            steering_angle=x_np[DBStateIndices.steering_angle],
        )

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

        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 DBInput(
            acceleration=u_np[DBInputIndices.acceleration],
            steering_angle_velocity=u_np[DBInputIndices.steering_angle_velocity],
        )

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

        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 DBDisturbance(
            position_x=w_np[DBDisturbanceIndices.position_x],
            position_y=w_np[DBDisturbanceIndices.position_y],
            velocity_long=w_np[DBDisturbanceIndices.velocity_long],
            velocity_lat=w_np[DBDisturbanceIndices.velocity_lat],
            heading=w_np[DBDisturbanceIndices.heading],
            yaw_rate=w_np[DBDisturbanceIndices.yaw_rate],
            steering_angle=w_np[DBDisturbanceIndices.steering_angle],
        )

    @classmethod
    def trajectory_from_points(
        cls,
        trajectory_dict: Union[Dict[int, DBState], Dict[int, DBInput]],
        mode: TrajectoryMode,
        t_0: float,
        delta_t: float,
    ) -> DBTrajectory:
        """
        Create State, Input, or Disturbance Trajectory from list of DB 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: DBTrajectory
        """
        return DBTrajectory(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,
    ) -> DBTrajectory:
        """
        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: DBTrajectory
        """

        # 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 DBTrajectory(
            points=dict(zip(time_steps, points_val)),
            mode=mode,
            delta_t=delta_t,
            t_0=t_0,
        )
disturbance_from_args(position_x=0.0, position_y=0.0, velocity_long=0.0, velocity_lat=0.0, heading=0.0, yaw_rate=0.0, steering_angle=0.0) classmethod

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

Parameters:

Name Type Description Default
position_x float

position x of center of gravity (Cartesian coordinates)

 0.0
position_y float

position y of center of gravity (Cartesian coordinates)

 0.0
velocity_long float

longitudinal velocity (body frame)

 0.0
velocity_lat float

lateral velocity (body frame)

 0.0
heading float

heading

 0.0
yaw_rate float

yaw rate

 0.0
steering_angle float

steering angle

 0.0

Returns:

Type Description
Union[DBDisturbance]

DBDisturbance
Source code in commonroad_control/vehicle_dynamics/dynamic_bicycle/db_sidt_factory.py 
 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
@classmethod
def disturbance_from_args(
    cls,
    position_x: float = 0.0,
    position_y: float = 0.0,
    velocity_long: float = 0.0,
    velocity_lat: float = 0.0,
    heading: float = 0.0,
    yaw_rate: float = 0.0,
    steering_angle: float = 0.0,
) -> Union["DBDisturbance"]:
    """
    Create Disturbance from args - the default value of all variables is zero.
    :param position_x: position x of center of gravity (Cartesian coordinates)
    :param position_y: position y of center of gravity (Cartesian coordinates)
    :param velocity_long: longitudinal velocity (body frame)
    :param velocity_lat: lateral velocity (body frame)
    :param heading: heading
    :param yaw_rate: yaw rate
    :param steering_angle: steering angle
    :return: DBDisturbance
    """
    return DBDisturbance(
        position_x=position_x,
        position_y=position_y,
        velocity_long=velocity_long,
        velocity_lat=velocity_lat,
        heading=heading,
        yaw_rate=yaw_rate,
        steering_angle=steering_angle,
    )
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[DBDisturbance]

DBDisturbance
Source code in commonroad_control/vehicle_dynamics/dynamic_bicycle/db_sidt_factory.py 
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
@classmethod
def disturbance_from_numpy_array(
    cls, w_np: np.ndarray[tuple[float], np.dtype[np.float64]]
) -> Union["DBDisturbance"]:
    """
    Create Disturbance from numpy array
    :param w_np: disturbance - array of dimension (cls.disturbance_dimension,)
    :return: DBDisturbance
    """

    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 DBDisturbance(
        position_x=w_np[DBDisturbanceIndices.position_x],
        position_y=w_np[DBDisturbanceIndices.position_y],
        velocity_long=w_np[DBDisturbanceIndices.velocity_long],
        velocity_lat=w_np[DBDisturbanceIndices.velocity_lat],
        heading=w_np[DBDisturbanceIndices.heading],
        yaw_rate=w_np[DBDisturbanceIndices.yaw_rate],
        steering_angle=w_np[DBDisturbanceIndices.steering_angle],
    )
input_from_args(acceleration, steering_angle_velocity) classmethod

Create Input from args

Parameters:

Name Type Description Default
acceleration float

longitudinal acceleration

 required
steering_angle_velocity float

lateral acceleration

 required

Returns:

Type Description
Union[DBInput]

DBInput
Source code in commonroad_control/vehicle_dynamics/dynamic_bicycle/db_sidt_factory.py 
70
71
72
73
74
75
76
77
78
@classmethod
def input_from_args(cls, acceleration: float, steering_angle_velocity: float) -> Union["DBInput"]:
    """
    Create Input from args
    :param acceleration: longitudinal acceleration
    :param steering_angle_velocity: lateral acceleration
    :return: DBInput
    """
    return DBInput(acceleration=acceleration, steering_angle_velocity=steering_angle_velocity)
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[DBInput]

DBInput
Source code in commonroad_control/vehicle_dynamics/dynamic_bicycle/db_sidt_factory.py 
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
@classmethod
def input_from_numpy_array(cls, u_np: np.ndarray[tuple[float], np.dtype[np.float64]]) -> Union["DBInput"]:
    """
    Create Input from numpy array
    :param u_np: control input - array of dimension (cls.input_dimension,)
    :return: DBInput
    """

    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 DBInput(
        acceleration=u_np[DBInputIndices.acceleration],
        steering_angle_velocity=u_np[DBInputIndices.steering_angle_velocity],
    )
state_from_args(position_x, position_y, velocity_long, velocity_lat, heading, yaw_rate, steering_angle) classmethod

Create State from args

Parameters:

Name Type Description Default
position_x float

position x of center of gravity (Cartesian coordinates)

 required
position_y float

position y of center of gravity (Cartesian coordinates)

 required
velocity_long float

longitudinal velocity (body frame)

 required
velocity_lat float

lateral velocity (body frame)

 required
heading float

heading

 required
yaw_rate float

yaw rate

 required
steering_angle float

steering angle

 required

Returns:

Type Description
Union[DBState]

DBState
Source code in commonroad_control/vehicle_dynamics/dynamic_bicycle/db_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
60
61
62
63
64
65
66
67
68
@classmethod
def state_from_args(
    cls,
    position_x: float,
    position_y: float,
    velocity_long: float,
    velocity_lat: float,
    heading: float,
    yaw_rate: float,
    steering_angle: float,
) -> Union["DBState"]:
    """
    Create State from args
    :param position_x: position x of center of gravity (Cartesian coordinates)
    :param position_y: position y of center of gravity (Cartesian coordinates)
    :param velocity_long: longitudinal velocity (body frame)
    :param velocity_lat: lateral velocity (body frame)
    :param heading: heading
    :param yaw_rate: yaw rate
    :param steering_angle: steering angle
    :return: DBState
    """
    return DBState(
        position_x=position_x,
        position_y=position_y,
        velocity_long=velocity_long,
        velocity_lat=velocity_lat,
        heading=heading,
        yaw_rate=yaw_rate,
        steering_angle=steering_angle,
    )
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[DBState]

DBState
Source code in commonroad_control/vehicle_dynamics/dynamic_bicycle/db_sidt_factory.py 
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
@classmethod
def state_from_numpy_array(
    cls,
    x_np: np.ndarray[tuple[float], np.dtype[np.float64]],
) -> Union["DBState"]:
    """
    Create State from numpy array
    :param x_np: state vector - array of dimension (cls.state_dimension,)
    :return: DBState
    """

    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 DBState(
        position_x=x_np[DBStateIndices.position_x],
        position_y=x_np[DBStateIndices.position_y],
        velocity_long=x_np[DBStateIndices.velocity_long],
        velocity_lat=x_np[DBStateIndices.velocity_lat],
        heading=x_np[DBStateIndices.heading],
        yaw_rate=x_np[DBStateIndices.yaw_rate],
        steering_angle=x_np[DBStateIndices.steering_angle],
    )
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
DBTrajectory

DBTrajectory
Source code in commonroad_control/vehicle_dynamics/dynamic_bicycle/db_sidt_factory.py 
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
@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,
) -> DBTrajectory:
    """
    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: DBTrajectory
    """

    # 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 DBTrajectory(
        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 DB points.

Parameters:

Name Type Description Default
trajectory_dict Union[Dict[int, DBState], Dict[int, DBInput]]

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
DBTrajectory

DBTrajectory
Source code in commonroad_control/vehicle_dynamics/dynamic_bicycle/db_sidt_factory.py 
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
@classmethod
def trajectory_from_points(
    cls,
    trajectory_dict: Union[Dict[int, DBState], Dict[int, DBInput]],
    mode: TrajectoryMode,
    t_0: float,
    delta_t: float,
) -> DBTrajectory:
    """
    Create State, Input, or Disturbance Trajectory from list of DB 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: DBTrajectory
    """
    return DBTrajectory(points=trajectory_dict, mode=mode, t_0=t_0, delta_t=delta_t)