Skip to content

Reactive Planner Converter

commonroad_control.planning_converter.reactive_planner_converter

ReactivePlannerConverter

Bases: PlanningConverterInterface

TODO add docstring
Source code in commonroad_control/planning_converter/reactive_planner_converter.py 
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
class ReactivePlannerConverter(PlanningConverterInterface):
    """
    #TODO add docstring
    """

    def __init__(
        self,
        kb_factory: Union[KBSIDTFactory, Any] = KBSIDTFactory(),
        db_factory: Union[DBSIDTFactory, Any] = DBSIDTFactory(),
        vehicle_params: Union[BMW3seriesParams, Any] = BMW3seriesParams(),
    ) -> None:
        """
        Converter for CommonRoad reactive planner to different vehicle models
        :param kb_factory: kb Factory
        :param db_factory: db Factory
        :param vehicle_params: vehicle params
        """
        super().__init__(
            kb_factory=kb_factory,
            db_factory=db_factory,
            vehicle_params=vehicle_params,
        )

    # --- kb ---
    def trajectory_p2c_kb(
        self,
        planner_traj: Union[List["ReactivePlannerState"], List[InputState]],
        mode: TrajectoryMode,
        t_0: float = 0.0,
        dt: float = 0.1,
    ) -> KBTrajectory:
        """
        Convert reactive planner state or input trajectory to KB trajectory
        :param planner_traj: planner state or input trajectory
        :param mode: state or input mode
        :param t_0: starting time of trajectory
        :param dt: time step size
        :return: KBTrajectory
        """
        kb_dict: Dict[int, Union[KBState, KBInput]] = dict()
        for kb_point in planner_traj:
            kb_dict[kb_point.time_step] = self.sample_p2c_kb(planner_state=kb_point, mode=mode)
        return self._kb_factory.trajectory_from_points(trajectory_dict=kb_dict, mode=mode, t_0=t_0, delta_t=dt)

    def sample_p2c_kb(
        self,
        planner_state: Union[ReactivePlannerState, InputState],
        mode: TrajectoryMode,
        *args,
        **kwargs,
    ) -> Union[KBState, KBInput]:
        """
        Convert one state or input of reactive planner to kb
        :param planner_state: planner state or input
        :param mode: state or input
        :return: KBState or KBInput object
        """
        if mode.value == TrajectoryMode.State.value:
            # compute velocity at center of gravity
            v_cog = map_velocity_from_ra_to_cog(
                l_wb=self._vehicle_params.l_wb,
                l_r=self._vehicle_params.l_r,
                velocity_ra=planner_state.velocity,
                steering_angle=planner_state.steering_angle,
            )
            # compute position of the center of gravity
            position_x_cog, position_y_cog = compute_position_of_cog_from_ra_cc(
                position_ra_x=planner_state.position[0],
                position_ra_y=planner_state.position[1],
                heading=planner_state.orientation,
                l_r=self._vehicle_params.l_r,
            )
            retval: KBState = self._kb_factory.state_from_args(
                position_x=position_x_cog,
                position_y=position_y_cog,
                velocity=v_cog,
                heading=planner_state.orientation,
                steering_angle=planner_state.steering_angle,
            )
        elif mode.value == TrajectoryMode.Input.value:
            retval: KBInput = self._kb_factory.input_from_args(
                acceleration=planner_state.acceleration,
                steering_angle_velocity=planner_state.steering_angle_speed,
            )
        else:
            logger.error(f"{mode} not implemented")
            raise NotImplementedError(f"{mode} not implemented")

        return retval

    def trajectory_c2p_kb(
        self,
        kb_traj: KBTrajectory,
        mode: TrajectoryMode,
    ) -> Union[List[ReactivePlannerState], List[InputState]]:
        """
        Convert kinematic bicycle state or input trajectory to reactive planner state or input trajectory
        :param kb_traj: KB trajectory
        :param mode: state or input mode
        :return: Reactive planner state or input trajectory
        """
        ordered_points_by_step = dict(sorted(kb_traj.points.items()))
        retval: List[ReactivePlannerState] = list()
        for step, point in ordered_points_by_step.items():
            retval.append(self.sample_c2p_kb(kb_state=point, mode=mode, time_step=step))
        return retval

    def sample_c2p_kb(
        self, kb_state: Union[KBState, KBInput], mode: TrajectoryMode, time_step: int
    ) -> Union[ReactivePlannerState, InputState]:
        """
        Convert kinematic bicycle state or input to reactive planner state or input at time step.
        :param kb_state: KB state or input
        :param mode: state or input mode
        :param time_step: time step
        :return: Reactive planner state or input
        """
        if mode == TrajectoryMode.State:
            # transform velocity to rear axle
            v_ra = map_velocity_from_cog_to_ra(
                l_wb=self._vehicle_params.l_wb,
                l_r=self._vehicle_params.l_r,
                velocity_cog=kb_state.velocity,
                steering_angle=kb_state.steering_angle,
            )

            # compute position of the center of gravity
            position_x_ra, position_y_ra = compute_position_of_ra_from_cog_cartesian(
                position_cog_x=kb_state.position_x,
                position_cog_y=kb_state.position_y,
                heading=kb_state.heading,
                l_r=self._vehicle_params.l_r,
            )

            retval: ReactivePlannerState = ReactivePlannerState(
                time_step=time_step,
                position=np.asarray([position_x_ra, position_y_ra]),
                velocity=v_ra,
                orientation=kb_state.heading,
                steering_angle=kb_state.steering_angle,
                yaw_rate=0,
            )
        elif mode == TrajectoryMode.Input:
            retval: InputState = InputState(
                steering_angle_speed=kb_state.steering_angle_velocity,
                acceleration=kb_state.acceleration,
                time_step=time_step,
            )
        else:
            logger.error(f"mode {mode} not implemented")
            raise NotImplementedError(f"mode {mode} not implemented")
        return retval

    # --- db ---
    def trajectory_p2c_db(
        self,
        planner_traj: Union[List["ReactivePlannerState"], List[InputState]],
        mode: TrajectoryMode,
        t_0: float = 0.0,
        dt: float = 0.1,
    ) -> DBTrajectory:
        """
        Convert reactive planner state or input trajectory to dynamic bicycle trajectory
        :param planner_traj: reactive planner state or input trajectory
        :param mode: state or input mode
        :param t_0: starting time of trajectory
        :param dt: time step size
        :return: Dynamic bicycle state or input trajectory
        """
        db_dict: Dict[int, Union[DBState, DBInput]] = dict()
        for db_point in planner_traj:
            db_dict[db_point.time_step] = self.sample_p2c_db(planner_state=db_point, mode=mode)
        return self._db_factory.trajectory_from_points(trajectory_dict=db_dict, mode=mode, t_0=t_0, delta_t=dt)

    def sample_p2c_db(
        self,
        planner_state: Union[ReactivePlannerState, InputState],
        mode: TrajectoryMode,
    ) -> Union[DBState, DBInput]:
        """
        Convert reactive planner state or input to dynamic bicycle state or input
        :param planner_state: reactive planner state or input
        :param mode: state or input mode
        :return: Dynamic bicycle state or input
        """
        if mode == TrajectoryMode.State:
            # compute velocity at center of gravity
            v_cog = map_velocity_from_ra_to_cog(
                l_wb=self._vehicle_params.l_wb,
                l_r=self._vehicle_params.l_r,
                velocity_ra=planner_state.velocity,
                steering_angle=planner_state.steering_angle,
            )
            v_cog_lon, v_cog_lat = compute_velocity_components_from_steering_angle_in_cog(
                steering_angle=planner_state.steering_angle,
                velocity_cog=v_cog,
                l_wb=self.vehicle_params.l_wb,
                l_r=self.vehicle_params.l_r,
            )
            # compute position of the center of gravity
            position_x_cog, position_y_cog = compute_position_of_cog_from_ra_cc(
                position_ra_x=planner_state.position[0],
                position_ra_y=planner_state.position[1],
                heading=planner_state.orientation,
                l_r=self._vehicle_params.l_r,
            )

            retval: DBState = self._db_factory.state_from_args(
                position_x=position_x_cog,
                position_y=position_y_cog,
                velocity_long=v_cog_lon,
                velocity_lat=v_cog_lat,
                yaw_rate=planner_state.yaw_rate,
                steering_angle=planner_state.steering_angle,
                heading=planner_state.orientation,
            )
        elif mode == TrajectoryMode.Input:
            retval: DBInput = self._db_factory.input_from_args(
                acceleration=planner_state.acceleration,
                steering_angle_velocity=planner_state.steering_angle_speed,
            )
        else:
            logger.error(f"mode {mode} not implemented")
            raise NotImplementedError(f"mode {mode} not implemented")

        return retval

    def trajectory_c2p_db(self, db_traj: DBTrajectory, mode: TrajectoryMode) -> Any:
        """
        NOT IMPLEMENTED!
        :param db_traj:
        :param mode:
        :return:
        """
        logger.error("Currently not implemented")
        raise NotImplementedError("Currently not implemented")

    def sample_c2p_db(
        self,
        db_state: Union[DBState, DBInput],
        mode: TrajectoryMode,
        time_step: int,
    ) -> Union[ReactivePlannerState, InputState]:
        """
        Convert dynamic bycicle state or input to reactive planner state or input at time step.
        :param db_state: Dynamic bicycle state or input
        :param mode: state or input mode
        :param time_step: time step
        :return: reactive planner state or input
        """
        if mode == TrajectoryMode.State:
            retval: ReactivePlannerState = ReactivePlannerState(
                time_step=time_step,
                position=np.asarray([db_state.position_x, db_state.position_y]),
                velocity=compute_velocity_from_components(db_state.velocity_long, db_state.velocity_lat),
                orientation=db_state.heading,
                steering_angle=db_state.steering_angle,
                yaw_rate=db_state.yaw_rate,
            )
        elif mode == TrajectoryMode.Input:
            retval: InputState = InputState(
                steering_angle_speed=db_state.steering_angle_velocity,
                acceleration=db_state.acceleration,
                time_step=time_step,
            )
        return retval

    @staticmethod
    def time_shift_state_input_list(
        si_list: Union[List[ReactivePlannerState], List[InputState]], t_0: int
    ) -> Union[List[ReactivePlannerState], List[InputState]]:
        """
        Time-shift list of reactive planner states or inputs to initial state.
        Updates each entries .time_step attribute
        :param si_list: state or input list
        :param t_0: new initial time step
        :return: new instance of reactive planenr state or input list
        """
        time_shifted_si_list = copy.copy(si_list)
        for idx in range(len(si_list)):
            time_shifted_si_list[idx].time_step = idx + t_0

        return time_shifted_si_list
__init__(kb_factory=KBSIDTFactory(), db_factory=DBSIDTFactory(), vehicle_params=BMW3seriesParams())

Converter for CommonRoad reactive planner to different vehicle models

Parameters:

Name Type Description Default
kb_factory Union[KBSIDTFactory, Any]

kb Factory

 KBSIDTFactory()
db_factory Union[DBSIDTFactory, Any]

db Factory

 DBSIDTFactory()
vehicle_params Union[BMW3seriesParams, Any]

vehicle params

 BMW3seriesParams()
Source code in commonroad_control/planning_converter/reactive_planner_converter.py 
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
def __init__(
    self,
    kb_factory: Union[KBSIDTFactory, Any] = KBSIDTFactory(),
    db_factory: Union[DBSIDTFactory, Any] = DBSIDTFactory(),
    vehicle_params: Union[BMW3seriesParams, Any] = BMW3seriesParams(),
) -> None:
    """
    Converter for CommonRoad reactive planner to different vehicle models
    :param kb_factory: kb Factory
    :param db_factory: db Factory
    :param vehicle_params: vehicle params
    """
    super().__init__(
        kb_factory=kb_factory,
        db_factory=db_factory,
        vehicle_params=vehicle_params,
    )
sample_c2p_db(db_state, mode, time_step)

Convert dynamic bycicle state or input to reactive planner state or input at time step.

Parameters:

Name Type Description Default
db_state Union[DBState, DBInput]

Dynamic bicycle state or input

 required
mode TrajectoryMode

state or input mode

 required
time_step int

time step

 required

Returns:

Type Description
Union[ReactivePlannerState, InputState]

reactive planner state or input
Source code in commonroad_control/planning_converter/reactive_planner_converter.py 
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
def sample_c2p_db(
    self,
    db_state: Union[DBState, DBInput],
    mode: TrajectoryMode,
    time_step: int,
) -> Union[ReactivePlannerState, InputState]:
    """
    Convert dynamic bycicle state or input to reactive planner state or input at time step.
    :param db_state: Dynamic bicycle state or input
    :param mode: state or input mode
    :param time_step: time step
    :return: reactive planner state or input
    """
    if mode == TrajectoryMode.State:
        retval: ReactivePlannerState = ReactivePlannerState(
            time_step=time_step,
            position=np.asarray([db_state.position_x, db_state.position_y]),
            velocity=compute_velocity_from_components(db_state.velocity_long, db_state.velocity_lat),
            orientation=db_state.heading,
            steering_angle=db_state.steering_angle,
            yaw_rate=db_state.yaw_rate,
        )
    elif mode == TrajectoryMode.Input:
        retval: InputState = InputState(
            steering_angle_speed=db_state.steering_angle_velocity,
            acceleration=db_state.acceleration,
            time_step=time_step,
        )
    return retval
sample_c2p_kb(kb_state, mode, time_step)

Convert kinematic bicycle state or input to reactive planner state or input at time step.

Parameters:

Name Type Description Default
kb_state Union[KBState, KBInput]

KB state or input

 required
mode TrajectoryMode

state or input mode

 required
time_step int

time step

 required

Returns:

Type Description
Union[ReactivePlannerState, InputState]

Reactive planner state or input
Source code in commonroad_control/planning_converter/reactive_planner_converter.py 
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
def sample_c2p_kb(
    self, kb_state: Union[KBState, KBInput], mode: TrajectoryMode, time_step: int
) -> Union[ReactivePlannerState, InputState]:
    """
    Convert kinematic bicycle state or input to reactive planner state or input at time step.
    :param kb_state: KB state or input
    :param mode: state or input mode
    :param time_step: time step
    :return: Reactive planner state or input
    """
    if mode == TrajectoryMode.State:
        # transform velocity to rear axle
        v_ra = map_velocity_from_cog_to_ra(
            l_wb=self._vehicle_params.l_wb,
            l_r=self._vehicle_params.l_r,
            velocity_cog=kb_state.velocity,
            steering_angle=kb_state.steering_angle,
        )

        # compute position of the center of gravity
        position_x_ra, position_y_ra = compute_position_of_ra_from_cog_cartesian(
            position_cog_x=kb_state.position_x,
            position_cog_y=kb_state.position_y,
            heading=kb_state.heading,
            l_r=self._vehicle_params.l_r,
        )

        retval: ReactivePlannerState = ReactivePlannerState(
            time_step=time_step,
            position=np.asarray([position_x_ra, position_y_ra]),
            velocity=v_ra,
            orientation=kb_state.heading,
            steering_angle=kb_state.steering_angle,
            yaw_rate=0,
        )
    elif mode == TrajectoryMode.Input:
        retval: InputState = InputState(
            steering_angle_speed=kb_state.steering_angle_velocity,
            acceleration=kb_state.acceleration,
            time_step=time_step,
        )
    else:
        logger.error(f"mode {mode} not implemented")
        raise NotImplementedError(f"mode {mode} not implemented")
    return retval
sample_p2c_db(planner_state, mode)

Convert reactive planner state or input to dynamic bicycle state or input

Parameters:

Name Type Description Default
planner_state Union[ReactivePlannerState, InputState]

reactive planner state or input

 required
mode TrajectoryMode

state or input mode

 required

Returns:

Type Description
Union[DBState, DBInput]

Dynamic bicycle state or input
Source code in commonroad_control/planning_converter/reactive_planner_converter.py 
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
def sample_p2c_db(
    self,
    planner_state: Union[ReactivePlannerState, InputState],
    mode: TrajectoryMode,
) -> Union[DBState, DBInput]:
    """
    Convert reactive planner state or input to dynamic bicycle state or input
    :param planner_state: reactive planner state or input
    :param mode: state or input mode
    :return: Dynamic bicycle state or input
    """
    if mode == TrajectoryMode.State:
        # compute velocity at center of gravity
        v_cog = map_velocity_from_ra_to_cog(
            l_wb=self._vehicle_params.l_wb,
            l_r=self._vehicle_params.l_r,
            velocity_ra=planner_state.velocity,
            steering_angle=planner_state.steering_angle,
        )
        v_cog_lon, v_cog_lat = compute_velocity_components_from_steering_angle_in_cog(
            steering_angle=planner_state.steering_angle,
            velocity_cog=v_cog,
            l_wb=self.vehicle_params.l_wb,
            l_r=self.vehicle_params.l_r,
        )
        # compute position of the center of gravity
        position_x_cog, position_y_cog = compute_position_of_cog_from_ra_cc(
            position_ra_x=planner_state.position[0],
            position_ra_y=planner_state.position[1],
            heading=planner_state.orientation,
            l_r=self._vehicle_params.l_r,
        )

        retval: DBState = self._db_factory.state_from_args(
            position_x=position_x_cog,
            position_y=position_y_cog,
            velocity_long=v_cog_lon,
            velocity_lat=v_cog_lat,
            yaw_rate=planner_state.yaw_rate,
            steering_angle=planner_state.steering_angle,
            heading=planner_state.orientation,
        )
    elif mode == TrajectoryMode.Input:
        retval: DBInput = self._db_factory.input_from_args(
            acceleration=planner_state.acceleration,
            steering_angle_velocity=planner_state.steering_angle_speed,
        )
    else:
        logger.error(f"mode {mode} not implemented")
        raise NotImplementedError(f"mode {mode} not implemented")

    return retval
sample_p2c_kb(planner_state, mode, *args, **kwargs)

Convert one state or input of reactive planner to kb

Parameters:

Name Type Description Default
planner_state Union[ReactivePlannerState, InputState]

planner state or input

 required
mode TrajectoryMode

state or input

 required

Returns:

Type Description
Union[KBState, KBInput]

KBState or KBInput object
Source code in commonroad_control/planning_converter/reactive_planner_converter.py 
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
def sample_p2c_kb(
    self,
    planner_state: Union[ReactivePlannerState, InputState],
    mode: TrajectoryMode,
    *args,
    **kwargs,
) -> Union[KBState, KBInput]:
    """
    Convert one state or input of reactive planner to kb
    :param planner_state: planner state or input
    :param mode: state or input
    :return: KBState or KBInput object
    """
    if mode.value == TrajectoryMode.State.value:
        # compute velocity at center of gravity
        v_cog = map_velocity_from_ra_to_cog(
            l_wb=self._vehicle_params.l_wb,
            l_r=self._vehicle_params.l_r,
            velocity_ra=planner_state.velocity,
            steering_angle=planner_state.steering_angle,
        )
        # compute position of the center of gravity
        position_x_cog, position_y_cog = compute_position_of_cog_from_ra_cc(
            position_ra_x=planner_state.position[0],
            position_ra_y=planner_state.position[1],
            heading=planner_state.orientation,
            l_r=self._vehicle_params.l_r,
        )
        retval: KBState = self._kb_factory.state_from_args(
            position_x=position_x_cog,
            position_y=position_y_cog,
            velocity=v_cog,
            heading=planner_state.orientation,
            steering_angle=planner_state.steering_angle,
        )
    elif mode.value == TrajectoryMode.Input.value:
        retval: KBInput = self._kb_factory.input_from_args(
            acceleration=planner_state.acceleration,
            steering_angle_velocity=planner_state.steering_angle_speed,
        )
    else:
        logger.error(f"{mode} not implemented")
        raise NotImplementedError(f"{mode} not implemented")

    return retval
time_shift_state_input_list(si_list, t_0) staticmethod

Time-shift list of reactive planner states or inputs to initial state. Updates each entries .time_step attribute

Parameters:

Name Type Description Default
si_list Union[List[ReactivePlannerState], List[InputState]]

state or input list

 required
t_0 int

new initial time step

 required

Returns:

Type Description
Union[List[ReactivePlannerState], List[InputState]]

new instance of reactive planenr state or input list
Source code in commonroad_control/planning_converter/reactive_planner_converter.py 
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
@staticmethod
def time_shift_state_input_list(
    si_list: Union[List[ReactivePlannerState], List[InputState]], t_0: int
) -> Union[List[ReactivePlannerState], List[InputState]]:
    """
    Time-shift list of reactive planner states or inputs to initial state.
    Updates each entries .time_step attribute
    :param si_list: state or input list
    :param t_0: new initial time step
    :return: new instance of reactive planenr state or input list
    """
    time_shifted_si_list = copy.copy(si_list)
    for idx in range(len(si_list)):
        time_shifted_si_list[idx].time_step = idx + t_0

    return time_shifted_si_list
trajectory_c2p_db(db_traj, mode)

NOT IMPLEMENTED!

Parameters:

Name Type Description Default
db_traj DBTrajectory
required
mode TrajectoryMode
required

Returns:

Type Description
Any
Source code in commonroad_control/planning_converter/reactive_planner_converter.py 
272
273
274
275
276
277
278
279
280
def trajectory_c2p_db(self, db_traj: DBTrajectory, mode: TrajectoryMode) -> Any:
    """
    NOT IMPLEMENTED!
    :param db_traj:
    :param mode:
    :return:
    """
    logger.error("Currently not implemented")
    raise NotImplementedError("Currently not implemented")
trajectory_c2p_kb(kb_traj, mode)

Convert kinematic bicycle state or input trajectory to reactive planner state or input trajectory

Parameters:

Name Type Description Default
kb_traj KBTrajectory

KB trajectory

 required
mode TrajectoryMode

state or input mode

 required

Returns:

Type Description
Union[List[ReactivePlannerState], List[InputState]]

Reactive planner state or input trajectory
Source code in commonroad_control/planning_converter/reactive_planner_converter.py 
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
def trajectory_c2p_kb(
    self,
    kb_traj: KBTrajectory,
    mode: TrajectoryMode,
) -> Union[List[ReactivePlannerState], List[InputState]]:
    """
    Convert kinematic bicycle state or input trajectory to reactive planner state or input trajectory
    :param kb_traj: KB trajectory
    :param mode: state or input mode
    :return: Reactive planner state or input trajectory
    """
    ordered_points_by_step = dict(sorted(kb_traj.points.items()))
    retval: List[ReactivePlannerState] = list()
    for step, point in ordered_points_by_step.items():
        retval.append(self.sample_c2p_kb(kb_state=point, mode=mode, time_step=step))
    return retval
trajectory_p2c_db(planner_traj, mode, t_0=0.0, dt=0.1)

Convert reactive planner state or input trajectory to dynamic bicycle trajectory

Parameters:

Name Type Description Default
planner_traj Union[List[ReactivePlannerState], List[InputState]]

reactive planner state or input trajectory

 required
mode TrajectoryMode

state or input mode

 required
t_0 float

starting time of trajectory

 0.0
dt float

time step size

 0.1

Returns:

Type Description
DBTrajectory

Dynamic bicycle state or input trajectory
Source code in commonroad_control/planning_converter/reactive_planner_converter.py 
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
def trajectory_p2c_db(
    self,
    planner_traj: Union[List["ReactivePlannerState"], List[InputState]],
    mode: TrajectoryMode,
    t_0: float = 0.0,
    dt: float = 0.1,
) -> DBTrajectory:
    """
    Convert reactive planner state or input trajectory to dynamic bicycle trajectory
    :param planner_traj: reactive planner state or input trajectory
    :param mode: state or input mode
    :param t_0: starting time of trajectory
    :param dt: time step size
    :return: Dynamic bicycle state or input trajectory
    """
    db_dict: Dict[int, Union[DBState, DBInput]] = dict()
    for db_point in planner_traj:
        db_dict[db_point.time_step] = self.sample_p2c_db(planner_state=db_point, mode=mode)
    return self._db_factory.trajectory_from_points(trajectory_dict=db_dict, mode=mode, t_0=t_0, delta_t=dt)
trajectory_p2c_kb(planner_traj, mode, t_0=0.0, dt=0.1)

Convert reactive planner state or input trajectory to KB trajectory

Parameters:

Name Type Description Default
planner_traj Union[List[ReactivePlannerState], List[InputState]]

planner state or input trajectory

 required
mode TrajectoryMode

state or input mode

 required
t_0 float

starting time of trajectory

 0.0
dt float

time step size

 0.1

Returns:

Type Description
KBTrajectory

KBTrajectory
Source code in commonroad_control/planning_converter/reactive_planner_converter.py 
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
def trajectory_p2c_kb(
    self,
    planner_traj: Union[List["ReactivePlannerState"], List[InputState]],
    mode: TrajectoryMode,
    t_0: float = 0.0,
    dt: float = 0.1,
) -> KBTrajectory:
    """
    Convert reactive planner state or input trajectory to KB trajectory
    :param planner_traj: planner state or input trajectory
    :param mode: state or input mode
    :param t_0: starting time of trajectory
    :param dt: time step size
    :return: KBTrajectory
    """
    kb_dict: Dict[int, Union[KBState, KBInput]] = dict()
    for kb_point in planner_traj:
        kb_dict[kb_point.time_step] = self.sample_p2c_kb(planner_state=kb_point, mode=mode)
    return self._kb_factory.trajectory_from_points(trajectory_dict=kb_dict, mode=mode, t_0=t_0, delta_t=dt)