Skip to content

Kinematic Bicycle

commonroad_control.vehicle_dynamics.kinematic_bicycle.kinematic_bicycle

KinematicBicycle

Bases: VehicleModelInterface

Kinematic bicycle model. Reference point for the vehicle dynamics: center of gravity.

Source code in commonroad_control/vehicle_dynamics/kinematic_bicycle/kinematic_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
class KinematicBicycle(VehicleModelInterface):
    """
    Kinematic bicycle model.
    Reference point for the vehicle dynamics: center of gravity.
    """

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

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

        # set vehicle parameters
        self._l_wb = params.l_wb
        self._l_r = params.l_r
        self._a_long_max = params.a_long_max
        self._a_lat_max = params.a_lat_max

        # init base class
        super().__init__(
            params=params,
            nx=KBStateIndices.dim,
            nu=KBInputIndices.dim,
            nw=KBDisturbanceIndices.dim,
            delta_t=delta_t,
        )

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

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

        # upper bound
        u_ub = KBInput(
            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 = x[KBStateIndices.velocity]
        psi = x[KBStateIndices.heading]
        delta = x[KBStateIndices.steering_angle]

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

        # compute slip angle
        beta = cas.atan(cas.tan(delta) * self._l_r / self._l_wb)

        # dynamics
        position_x_dot = v * cas.cos(psi + beta)
        position_y_dot = v * cas.sin(psi + beta)
        velocity_dot = a
        heading_dot = v * cas.sin(beta) / self._l_r
        steering_angle_dot = delta_dot

        f = cas.vertcat(
            position_x_dot,
            position_y_dot,
            velocity_dot,
            heading_dot,
            steering_angle_dot,
        )

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

        return f

    def compute_normalized_acceleration(
        self,
        x: Union[KBState, cas.SX.sym, np.array],
        u: Union[KBInput, 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, KBState):
            x = x.convert_to_array()
        v = x[KBStateIndices.velocity]
        delta = x[KBStateIndices.steering_angle]

        # compute slip angle
        beta = cas.atan(cas.tan(delta) * self._l_r / self._l_wb)
        # compute yaw rate
        heading_dot = v * cas.sin(beta) / self._l_r

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

        # normalized acceleration
        a_long_norm = a / self._a_long_max
        a_lat_norm = (v * heading_dot) / self._a_lat_max

        return a_long_norm, a_lat_norm
_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/kinematic_bicycle/kinematic_bicycle.py 
 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
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 = x[KBStateIndices.velocity]
    psi = x[KBStateIndices.heading]
    delta = x[KBStateIndices.steering_angle]

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

    # compute slip angle
    beta = cas.atan(cas.tan(delta) * self._l_r / self._l_wb)

    # dynamics
    position_x_dot = v * cas.cos(psi + beta)
    position_y_dot = v * cas.sin(psi + beta)
    velocity_dot = a
    heading_dot = v * cas.sin(beta) / self._l_r
    steering_angle_dot = delta_dot

    f = cas.vertcat(
        position_x_dot,
        position_y_dot,
        velocity_dot,
        heading_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[KBInput, KBInput]

lower and upper bound on the inputs - KBInputs
Source code in commonroad_control/vehicle_dynamics/kinematic_bicycle/kinematic_bicycle.py 
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
def _set_input_bounds(self, params: VehicleParameters) -> Tuple[KBInput, KBInput]:
    """
    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 - KBInputs
    """

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

    # upper bound
    u_ub = KBInput(
        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[KBState, sym, array]

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

 required
u Union[KBInput, 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/kinematic_bicycle/kinematic_bicycle.py 
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
def compute_normalized_acceleration(
    self,
    x: Union[KBState, cas.SX.sym, np.array],
    u: Union[KBInput, 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, KBState):
        x = x.convert_to_array()
    v = x[KBStateIndices.velocity]
    delta = x[KBStateIndices.steering_angle]

    # compute slip angle
    beta = cas.atan(cas.tan(delta) * self._l_r / self._l_wb)
    # compute yaw rate
    heading_dot = v * cas.sin(beta) / self._l_r

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

    # normalized acceleration
    a_long_norm = a / self._a_long_max
    a_lat_norm = (v * heading_dot) / 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 parameters

 required
delta_t float

sampling time

 required

Returns:

Type Description
KinematicBicycle

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

commonroad_control.vehicle_dynamics.kinematic_bicycle.kb_state

KBState dataclass

Bases: StateInterface

Dataclass storing the states of the kinematic bicycle model.

Source code in commonroad_control/vehicle_dynamics/kinematic_bicycle/kb_state.py 
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
@dataclass
class KBState(StateInterface):
    """
    Dataclass storing the states of the kinematic bicycle model.
    """

    position_x: float = None
    position_y: float = None
    velocity: float = None
    heading: float = None
    steering_angle: float = None

    @property
    def dim(self) -> int:
        """
        :return: state dimension
        """
        return KBStateIndices.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[KBStateIndices.position_x] = self.position_x
        x_np[KBStateIndices.position_y] = self.position_y
        x_np[KBStateIndices.velocity] = self.velocity
        x_np[KBStateIndices.heading] = self.heading
        x_np[KBStateIndices.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
        :return: CommonRoad InitialState
        """
        return InitialState(
            position=np.asarray([self.position_x, self.position_y]),
            velocity=self.velocity,
            orientation=self.heading,
            acceleration=0,
            yaw_rate=0,
            slip_angle=0,
            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=self.velocity,
            orientation=self.heading,
            acceleration=0,
            yaw_rate=0,
            slip_angle=0,
            time_step=time_step,
        )
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/kinematic_bicycle/kb_state.py 
45
46
47
48
49
50
51
52
53
54
55
56
57
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[KBStateIndices.position_x] = self.position_x
    x_np[KBStateIndices.position_y] = self.position_y
    x_np[KBStateIndices.velocity] = self.velocity
    x_np[KBStateIndices.heading] = self.heading
    x_np[KBStateIndices.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/kinematic_bicycle/kb_state.py 
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
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=self.velocity,
        orientation=self.heading,
        acceleration=0,
        yaw_rate=0,
        slip_angle=0,
        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

 required

Returns:

Type Description
InitialState

CommonRoad InitialState
Source code in commonroad_control/vehicle_dynamics/kinematic_bicycle/kb_state.py 
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
def to_cr_initial_state(self, time_step: int) -> InitialState:
    """
    Convert to CommonRoad initial state
    :param time_step: time step
    :return: CommonRoad InitialState
    """
    return InitialState(
        position=np.asarray([self.position_x, self.position_y]),
        velocity=self.velocity,
        orientation=self.heading,
        acceleration=0,
        yaw_rate=0,
        slip_angle=0,
        time_step=time_step,
    )

KBStateIndices dataclass

Bases: StateInterfaceIndex

Indices of the states of the kinematic bicycle model.

Source code in commonroad_control/vehicle_dynamics/kinematic_bicycle/kb_state.py 
12
13
14
15
16
17
18
19
20
21
22
23
@dataclass(frozen=True)
class KBStateIndices(StateInterfaceIndex):
    """
    Indices of the states of the kinematic bicycle model.
    """

    dim: int = 5
    position_x: int = 0
    position_y: int = 1
    velocity: int = 2
    heading: int = 3
    steering_angle: int = 4

commonroad_control.vehicle_dynamics.kinematic_bicycle.kb_input

KBInput dataclass

Bases: InputInterface

Dataclass storing the control input of the kinematic bicycle model.

Source code in commonroad_control/vehicle_dynamics/kinematic_bicycle/kb_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 KBInput(InputInterface):
    """
    Dataclass storing the control input of the kinematic bicycle model.
    """

    acceleration: float = None
    steering_angle_velocity: float = None

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

    return u_np

KBInputIndices dataclass

Bases: InputInterfaceIndex

Indices of the control inputs of the kinematic bicycle model.

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

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

commonroad_control.vehicle_dynamics.kinematic_bicycle.kb_trajectory

KBTrajectory dataclass

Bases: TrajectoryInterface

Kinematic bicycle model Trajectory.

Source code in commonroad_control/vehicle_dynamics/kinematic_bicycle/kb_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
68
@dataclass
class KBTrajectory(TrajectoryInterface):
    """
    Kinematic 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/kinematic_bicycle/kb_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
68
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.kinematic_bicycle.kb_disturbance

KBDisturbance dataclass

Bases: DisturbanceInterface

Dataclass storing the disturbances of the kinematic bicycle model.

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

    position_x: float = 0.0
    position_y: float = 0.0
    velocity: float = 0.0
    heading: float = 0.0
    steering_angle: float = 0.0

    @property
    def dim(self) -> int:
        """
        :return: disturbance dimension
        """
        return KBDisturbanceIndices.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[KBDisturbanceIndices.position_x] = self.position_x
        w_np[KBDisturbanceIndices.position_y] = self.position_y
        w_np[KBDisturbanceIndices.velocity] = self.velocity
        w_np[KBDisturbanceIndices.heading] = self.heading
        w_np[KBDisturbanceIndices.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/kinematic_bicycle/kb_disturbance.py 
44
45
46
47
48
49
50
51
52
53
54
55
56
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[KBDisturbanceIndices.position_x] = self.position_x
    w_np[KBDisturbanceIndices.position_y] = self.position_y
    w_np[KBDisturbanceIndices.velocity] = self.velocity
    w_np[KBDisturbanceIndices.heading] = self.heading
    w_np[KBDisturbanceIndices.steering_angle] = self.steering_angle

    return w_np

KBDisturbanceIndices dataclass

Bases: DisturbanceInterfaceIndex

Indices of the disturbances of the kinematic bicycle model.

Source code in commonroad_control/vehicle_dynamics/kinematic_bicycle/kb_disturbance.py 
11
12
13
14
15
16
17
18
19
20
21
22
@dataclass(frozen=True)
class KBDisturbanceIndices(DisturbanceInterfaceIndex):
    """
    Indices of the disturbances of the kinematic bicycle model.
    """

    dim: int = 5
    position_x: int = 0
    position_y: int = 1
    velocity: int = 2
    heading: int = 3
    steering_angle: int = 4

commonroad_control.vehicle_dynamics.kinematic_bicycle.kb_noise

KBNoise dataclass

Bases: FullStateNoiseInterface

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

Source code in commonroad_control/vehicle_dynamics/kinematic_bicycle/kb_noise.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
@dataclass
class KBNoise(FullStateNoiseInterface):
    """
    Full state noise of the kinematic bicycle model - required for the full state feedback sensor model.
    """

    position_x: float = 0.0
    position_y: float = 0.0
    velocity: float = 0.0
    heading: float = 0.0
    steering_angle: float = 0.0

    @property
    def dim(self) -> int:
        """
        :return: noise dimension
        """
        return KBNoiseIndices.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[KBNoiseIndices.position_x] = self.position_x
        w_np[KBNoiseIndices.position_y] = self.position_y
        w_np[KBNoiseIndices.velocity] = self.velocity
        w_np[KBNoiseIndices.heading] = self.heading
        w_np[KBNoiseIndices.steering_angle] = self.steering_angle

        return w_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/kinematic_bicycle/kb_noise.py 
44
45
46
47
48
49
50
51
52
53
54
55
56
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[KBNoiseIndices.position_x] = self.position_x
    w_np[KBNoiseIndices.position_y] = self.position_y
    w_np[KBNoiseIndices.velocity] = self.velocity
    w_np[KBNoiseIndices.heading] = self.heading
    w_np[KBNoiseIndices.steering_angle] = self.steering_angle

    return w_np

KBNoiseIndices dataclass

Bases: FullStateNoiseInterfaceIndex

Indices of the noise variables.

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

    dim: int = 5
    position_x: int = 0
    position_y: int = 1
    velocity: int = 2
    heading: int = 3
    steering_angle: int = 4

commonroad_control.vehicle_dynamics.kinematic_bicycle.kb_sidt_factory

KBSIDTFactory

Bases: StateInputDisturbanceTrajectoryFactoryInterface

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

Source code in commonroad_control/vehicle_dynamics/kinematic_bicycle/kb_sidt_factory.py 
 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
class KBSIDTFactory(StateInputDisturbanceTrajectoryFactoryInterface):
    """
    Factory for creating kinematic bicycle model State, Input, Disturbance, and Trajectory.
    """

    state_dimension: int = KBStateIndices.dim
    input_dimension: int = KBInputIndices.dim
    disturbance_dimension: int = KBDisturbanceIndices.dim

    @classmethod
    def state_from_args(
        cls,
        position_x: float,
        position_y: float,
        velocity: float,
        heading: float,
        steering_angle: float,
    ) -> Union["KBState"]:
        """
        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: velocity
        :param heading: heading
        :param steering_angle: steering angle
        :return: KBState
        """
        return KBState(
            position_x=position_x,
            position_y=position_y,
            velocity=velocity,
            heading=heading,
            steering_angle=steering_angle,
        )

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

    @staticmethod
    def disturbance_from_args(
        position_x: float = 0.0,
        position_y: float = 0.0,
        velocity: float = 0.0,
        heading: float = 0.0,
        steering_angle: float = 0.0,
    ) -> Union["KBDisturbance"]:
        """
        Create Disturbance from args - the default value of all variables is zero.
        :param position_x: position x of center of gravity
        :param position_y: position y of center of gravity
        :param velocity: velocity
        :param heading: heading
        :param steering_angle: steering angle
        :return: KBDisturbance
        """
        return KBDisturbance(
            position_x=position_x,
            position_y=position_y,
            velocity=velocity,
            heading=heading,
            steering_angle=steering_angle,
        )

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

        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 KBState(
            position_x=x_np[KBStateIndices.position_x],
            position_y=x_np[KBStateIndices.position_y],
            velocity=x_np[KBStateIndices.velocity],
            heading=x_np[KBStateIndices.heading],
            steering_angle=x_np[KBStateIndices.steering_angle],
        )

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

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

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

        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 KBDisturbance(
            position_x=w_np[KBDisturbanceIndices.position_x],
            position_y=w_np[KBDisturbanceIndices.position_y],
            velocity=w_np[KBDisturbanceIndices.velocity],
            heading=w_np[KBDisturbanceIndices.heading],
            steering_angle=w_np[KBDisturbanceIndices.steering_angle],
        )

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

        # 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 KBTrajectory(
            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=0.0, heading=0.0, steering_angle=0.0) staticmethod

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

 0.0
position_y float

position y of center of gravity

 0.0
velocity float

velocity

 0.0
heading float

heading

 0.0
steering_angle float

steering angle

 0.0

Returns:

Type Description
Union['KBDisturbance']

KBDisturbance
Source code in commonroad_control/vehicle_dynamics/kinematic_bicycle/kb_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
@staticmethod
def disturbance_from_args(
    position_x: float = 0.0,
    position_y: float = 0.0,
    velocity: float = 0.0,
    heading: float = 0.0,
    steering_angle: float = 0.0,
) -> Union["KBDisturbance"]:
    """
    Create Disturbance from args - the default value of all variables is zero.
    :param position_x: position x of center of gravity
    :param position_y: position y of center of gravity
    :param velocity: velocity
    :param heading: heading
    :param steering_angle: steering angle
    :return: KBDisturbance
    """
    return KBDisturbance(
        position_x=position_x,
        position_y=position_y,
        velocity=velocity,
        heading=heading,
        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['KBDisturbance']

KBDisturbance
Source code in commonroad_control/vehicle_dynamics/kinematic_bicycle/kb_sidt_factory.py 
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
@classmethod
def disturbance_from_numpy_array(
    cls, w_np: np.ndarray[tuple[float], np.dtype[np.float64]]
) -> Union["KBDisturbance"]:
    """
    Create Disturbance from numpy array
    :param w_np: disturbance - array of dimension (cls.disturbance_dimension,)
    :return: KBDisturbance
    """

    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 KBDisturbance(
        position_x=w_np[KBDisturbanceIndices.position_x],
        position_y=w_np[KBDisturbanceIndices.position_y],
        velocity=w_np[KBDisturbanceIndices.velocity],
        heading=w_np[KBDisturbanceIndices.heading],
        steering_angle=w_np[KBDisturbanceIndices.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

steering angle velocity

 required

Returns:

Type Description
Union['KBInput']

KBInput
Source code in commonroad_control/vehicle_dynamics/kinematic_bicycle/kb_sidt_factory.py 
66
67
68
69
70
71
72
73
74
75
76
77
78
@classmethod
def input_from_args(
    cls,
    acceleration: float,
    steering_angle_velocity,
) -> Union["KBInput"]:
    """
    Create Input from args
    :param acceleration: longitudinal acceleration
    :param steering_angle_velocity: steering angle velocity
    :return: KBInput
    """
    return KBInput(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['KBInput']

KBInput
Source code in commonroad_control/vehicle_dynamics/kinematic_bicycle/kb_sidt_factory.py 
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
@classmethod
def input_from_numpy_array(cls, u_np: np.ndarray[tuple[float], np.dtype[np.float64]]) -> Union["KBInput"]:
    """
    Create Input from numpy array
    :param u_np: control input - array of dimension (cls.input_dimension,)
    :return: KBInput
    """

    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 KBInput(
        acceleration=u_np[KBInputIndices.acceleration],
        steering_angle_velocity=u_np[KBInputIndices.steering_angle_velocity],
    )
state_from_args(position_x, position_y, velocity, heading, 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 float

velocity

 required
heading float

heading

 required
steering_angle float

steering angle

 required

Returns:

Type Description
Union['KBState']

KBState
Source code in commonroad_control/vehicle_dynamics/kinematic_bicycle/kb_sidt_factory.py 
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
@classmethod
def state_from_args(
    cls,
    position_x: float,
    position_y: float,
    velocity: float,
    heading: float,
    steering_angle: float,
) -> Union["KBState"]:
    """
    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: velocity
    :param heading: heading
    :param steering_angle: steering angle
    :return: KBState
    """
    return KBState(
        position_x=position_x,
        position_y=position_y,
        velocity=velocity,
        heading=heading,
        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['KBState']

KBState
Source code in commonroad_control/vehicle_dynamics/kinematic_bicycle/kb_sidt_factory.py 
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
@classmethod
def state_from_numpy_array(
    cls,
    x_np: np.ndarray[tuple[float], np.dtype[np.float64]],
) -> Union["KBState"]:
    """
    Create State from numpy array
    :param x_np: state vector - array of dimension (cls.state_dimension,)
    :return: KBState
    """

    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 KBState(
        position_x=x_np[KBStateIndices.position_x],
        position_y=x_np[KBStateIndices.position_y],
        velocity=x_np[KBStateIndices.velocity],
        heading=x_np[KBStateIndices.heading],
        steering_angle=x_np[KBStateIndices.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
'KBTrajectory'

KBTrajectory
Source code in commonroad_control/vehicle_dynamics/kinematic_bicycle/kb_sidt_factory.py 
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
@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,
) -> "KBTrajectory":
    """
    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: KBTrajectory
    """

    # 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 KBTrajectory(
        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 KB points.

Parameters:

Name Type Description Default
trajectory_dict Union[Dict[int, KBState], Dict[int, KBInput]]

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
'KBTrajectory'

KBTrajectory
Source code in commonroad_control/vehicle_dynamics/kinematic_bicycle/kb_sidt_factory.py 
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
@classmethod
def trajectory_from_points(
    cls,
    trajectory_dict: Union[Dict[int, KBState], Dict[int, KBInput]],
    mode: TrajectoryMode,
    t_0: float,
    delta_t: float,
) -> "KBTrajectory":
    """
    Create State, Input, or Disturbance Trajectory from list of KB 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: KBTrajectory
    """
    return KBTrajectory(points=trajectory_dict, mode=mode, t_0=t_0, delta_t=delta_t)