PID

We have a module offering different wrappers for our decoupled longitudinal-lateral PID controller.

commonroad_control.cr_control_easy_api.pid_for_dedicated_planner

pid_with_lookahead_for_planner(scenario, planning_problem, state_trajectory, input_trajectory, planner_converter, kp_long=1.0, ki_long=0.0, kd_long=0.0, kp_lat=0.05, ki_lat=0.0, kd_lat=0.1, dt_controller=0.1, t_look_ahead=0.5, vehicle_params=BMW3seriesParams(), disturbance_model_typename=GaussianDistribution, noise_model_typename=GaussianDistribution, sit_factory_sim=DBSIDTFactory(), vehicle_model_typename=DynamicBicycle, sensor_model_typename=FullStateFeedback, func_convert_planner2controller_state=convert_state_kb2db, func_convert_controller2planner_state=convert_state_db2kb, ivp_method='RK45', visualize_scenario=False, visualize_control=False, save_imgs=False, img_saving_path=None)

Decoupled longitudinal-lateral PID controller with look-ahead for a planner. Longitudinal velocity and lateral offset are calculated with respect to the lookahead time given a planner trajectory.

Parameters:

Name	Type	Description	Default
scenario	Scenario	CommonRoad scenario	required
planning_problem	PlanningProblem	CommonRoad planning problem	required
state_trajectory	Any	planner state trajectory	required
input_trajectory	Any	planner input trajectory	required
kp_long	float	proportional gain longitudinal velocity	1.0
ki_long	float	integral gain longitudinal velocity	0.0
kd_long	float	derivative gain longitudinal velocity	0.0
kp_lat	float	proportional gain lateral offset	0.05
ki_lat	float	integral gain lateral offset	0.0
kd_lat	float	derivative gain lateral offset	0.1
dt_controller	float	controller time step size in seconds	0.1
t_look_ahead	float	look-ahead in seconds	0.5
vehicle_params		vehicle parameters object	BMW3seriesParams()
planner_converter	Union[ReactivePlannerConverter, PlanningConverterInterface]	planner converter	required
disturbance_model_typename	Type[Union[UncertaintyModelInterface, GaussianDistribution]]	typename (=class) of the disturbance	GaussianDistribution
noise_model_typename	Type[Union[UncertaintyModelInterface, GaussianDistribution]]	typename (=class) of the noise	GaussianDistribution
sit_factory_sim	StateInputDisturbanceTrajectoryFactoryInterface	StateInputTrajectory factory for a given dynamics model	DBSIDTFactory()
vehicle_model_typename	Type[VehicleModelInterface]	typename (=class) of the vehicle dynamics model	DynamicBicycle
sensor_model_typename	Type[Union[SensorModelInterface, FullStateFeedback]]	typename (=class) of the sensor model / state feedback	FullStateFeedback
func_convert_planner2controller_state	Callable[[StateInterface, VehicleParameters], StateInterface]	function to convert a planner state into a controller state	convert_state_kb2db
func_convert_controller2planner_state	Callable[[StateInterface], StateInterface]	function to convert a controller state into a planner state	convert_state_db2kb
ivp_method	Union[str, OdeSolver, None]	IVP-Method used by the ODE-Solver	'RK45'
visualize_scenario	bool	If true, visualizes the scenario	False
visualize_control	bool	If true, visualizes control and error outputs	False
save_imgs	bool	If true and img_saving_path is given, saves visualizations instead of displaying them	False
img_saving_path	Union[Path, str]	If given and save_imgs=true, saves visualizations instead of displaying them	None

Returns:

Type	Description
Tuple[Dict[int, StateInterface], Dict[int, StateInterface], Dict[int, InputInterface]]	measured trajectory, trajectory without noise, trajectory without noise and without disturbance

Source code in commonroad_control/cr_control_easy_api/pid_for_dedicated_planner.py

def pid_with_lookahead_for_planner(
    scenario: Scenario,
    planning_problem: PlanningProblem,
    state_trajectory: Any,
    input_trajectory: Any,
    planner_converter: Union[ReactivePlannerConverter, PlanningConverterInterface],
    kp_long: float = 1.0,
    ki_long: float = 0.0,
    kd_long: float = 0.0,
    kp_lat: float = 0.05,
    ki_lat: float = 0.0,
    kd_lat: float = 0.1,
    dt_controller: float = 0.1,
    t_look_ahead: float = 0.5,
    vehicle_params=BMW3seriesParams(),
    disturbance_model_typename: Type[Union[UncertaintyModelInterface, GaussianDistribution]] = GaussianDistribution,
    noise_model_typename: Type[Union[UncertaintyModelInterface, GaussianDistribution]] = GaussianDistribution,
    sit_factory_sim: StateInputDisturbanceTrajectoryFactoryInterface = DBSIDTFactory(),
    vehicle_model_typename: Type[VehicleModelInterface] = DynamicBicycle,
    sensor_model_typename: Type[Union[SensorModelInterface, FullStateFeedback]] = FullStateFeedback,
    func_convert_planner2controller_state: Callable[
        [StateInterface, VehicleParameters], StateInterface
    ] = convert_state_kb2db,
    func_convert_controller2planner_state: Callable[[StateInterface], StateInterface] = convert_state_db2kb,
    ivp_method: Union[str, OdeSolver, None] = "RK45",
    visualize_scenario: bool = False,
    visualize_control: bool = False,
    save_imgs: bool = False,
    img_saving_path: Union[Path, str] = None,
) -> Tuple[Dict[int, StateInterface], Dict[int, StateInterface], Dict[int, InputInterface]]:
    """
    Decoupled longitudinal-lateral PID controller with look-ahead for a planner. Longitudinal velocity and lateral offset are
    calculated with respect to the lookahead time given a planner trajectory.
    :param scenario: CommonRoad scenario
    :param planning_problem: CommonRoad planning problem
    :param state_trajectory: planner state trajectory
    :param input_trajectory: planner input trajectory
    :param kp_long: proportional gain longitudinal velocity
    :param ki_long: integral gain longitudinal velocity
    :param kd_long: derivative gain longitudinal velocity
    :param kp_lat: proportional gain lateral offset
    :param ki_lat: integral gain lateral offset
    :param kd_lat: derivative gain lateral offset
    :param dt_controller: controller time step size in seconds
    :param t_look_ahead: look-ahead in seconds
    :param vehicle_params: vehicle parameters object
    :param planner_converter: planner converter
    :param disturbance_model_typename: typename (=class) of the disturbance
    :param noise_model_typename: typename (=class) of the noise
    :param sit_factory_sim: StateInputTrajectory factory for a given dynamics model
    :param vehicle_model_typename: typename (=class) of the vehicle dynamics model
    :param sensor_model_typename: typename (=class) of the sensor model / state feedback
    :param func_convert_planner2controller_state: function to convert a planner state into a controller state
    :param func_convert_controller2planner_state: function to convert a controller state into a planner state
    :param ivp_method: IVP-Method used by the ODE-Solver
    :param visualize_scenario: If true, visualizes the scenario
    :param visualize_control: If true, visualizes control and error outputs
    :param save_imgs: If true and img_saving_path is given, saves visualizations instead of displaying them
    :param img_saving_path: If given and save_imgs=true, saves visualizations instead of displaying them
    :return: measured trajectory, trajectory without noise, trajectory without noise and without disturbance
    """
    x_ref = planner_converter.trajectory_p2c_kb(planner_traj=state_trajectory, mode=TrajectoryMode.State)
    u_ref = planner_converter.trajectory_p2c_kb(planner_traj=input_trajectory, mode=TrajectoryMode.Input)

    logger.debug("initialize simulation")
    # simulation
    # ... vehicle model
    vehicle_model_sim = vehicle_model_typename(params=vehicle_params, delta_t=dt_controller)
    # ... disturbances
    sim_disturbance_model = disturbance_model_typename(
        dim=vehicle_model_sim.disturbance_dimension,
        mean=vehicle_params.disturbance_gaussian_mean,
        std_deviation=vehicle_params.disturbance_gaussian_std,
    )
    # ... noise
    sim_noise_model = noise_model_typename(
        dim=vehicle_model_sim.disturbance_dimension,
        mean=vehicle_params.noise_gaussian_mean,
        std_deviation=vehicle_params.noise_gaussian_std,
    )
    # ... sensor model
    sensor_model = sensor_model_typename(
        noise_model=sim_noise_model,
        state_output_factory=sit_factory_sim,
        state_dimension=sit_factory_sim.state_dimension,
        input_dimension=sit_factory_sim.input_dimension,
    )
    # ... simulation
    simulation: Simulation = Simulation(
        vehicle_model=vehicle_model_sim,
        sidt_factory=sit_factory_sim,
        disturbance_model=sim_disturbance_model,
        random_disturbance=True,
        sensor_model=sensor_model,
        random_noise=True,
        delta_t_w=dt_controller,
    )

    # Lookahead
    # ... simulation
    look_ahead_sim: Simulation = Simulation(
        vehicle_model=vehicle_model_sim,
        sidt_factory=sit_factory_sim,
    )

    pid_controller: PIDLongLat = PIDLongLat(
        kp_long=kp_long,
        ki_long=ki_long,
        kd_long=kd_long,
        kp_lat=kp_lat,
        ki_lat=ki_lat,
        kd_lat=kd_lat,
        delta_t=dt_controller,
    )

    logger.debug("run controller")
    t_0 = time.perf_counter()
    eta: float = 0.0

    # extend reference trajectory
    horizon_look_ahead = ceil(t_look_ahead / dt_controller)
    clcs_traj, x_ref_ext, u_ref_ext = extend_kb_reference_trajectory_lane_following(
        x_ref=copy.copy(x_ref),
        u_ref=copy.copy(u_ref),
        lanelet_network=scenario.lanelet_network,
        vehicle_params=vehicle_params,
        delta_t=dt_controller,
        horizon=horizon_look_ahead,
    )
    reference_trajectory = ReferenceTrajectoryFactory(
        delta_t_controller=dt_controller,
        sidt_factory=KBSIDTFactory(),
        t_look_ahead=t_look_ahead,
    )
    reference_trajectory.set_reference_trajectory(state_ref=x_ref_ext, input_ref=u_ref_ext, t_0=0)
    ref_path: LineString = LineString(
        [(p.position_x, p.position_y) for p in reference_trajectory.state_trajectory.points.values()]
    )

    x_measured = func_convert_planner2controller_state(kb_state=x_ref.initial_point, vehicle_params=vehicle_params)

    x_disturbed = copy.copy(x_measured)
    traj_dict_measured = {0: x_measured}
    traj_dict_no_noise = {0: x_disturbed}
    input_dict = {}

    for kk_sim in range(len(u_ref.steps)):
        # extract reference trajectory
        tmp_x_ref, tmp_u_ref = reference_trajectory.get_reference_trajectory_at_time(t=kk_sim * dt_controller)

        u_look_ahead_sim = sit_factory_sim.input_from_args(
            acceleration=u_ref.points[kk_sim].acceleration,
            steering_angle_velocity=u_ref.points[kk_sim].steering_angle_velocity,
        )

        eta = eta + time.perf_counter() - t_0
        _, _, x_look_ahead = look_ahead_sim.simulate(
            x0=traj_dict_no_noise[kk_sim],
            u=u_look_ahead_sim,
            t_final=t_look_ahead,
            ivp_method=ivp_method,
        )
        t_0 = time.perf_counter()

        # convert simulated forward step state back to KB for control
        x0_kb = func_convert_controller2planner_state(x_look_ahead.final_point)
        lateral_offset_lookahead = signed_distance_point_to_linestring(
            point=Point(x0_kb.position_x, x0_kb.position_y), linestring=ref_path
        )

        u_vel, u_steer = pid_controller.compute_control_input(
            measured_v_long=x0_kb.velocity,
            reference_v_long=tmp_x_ref.points[0].velocity,
            measured_lat_offset=lateral_offset_lookahead,
            reference_lat_offset=0.0,
        )

        u_now = sit_factory_sim.input_from_args(
            acceleration=u_vel + u_ref.points[kk_sim].acceleration,
            steering_angle_velocity=u_steer + u_ref.points[kk_sim].steering_angle_velocity,
        )

        eta = eta + time.perf_counter() - t_0
        t_0 = time.perf_counter()
        # simulate
        x_measured, x_disturbed, x_nominal = simulation.simulate(
            x0=traj_dict_no_noise[kk_sim],
            u=u_now,
            t_final=dt_controller,
            ivp_method=ivp_method,
        )

        # update dicts
        input_dict[kk_sim] = u_now
        traj_dict_measured[kk_sim + 1] = x_measured
        traj_dict_no_noise[kk_sim + 1] = x_disturbed.final_point

    logger.debug(f"Control took: {eta * 1000} millisec.")
    simulated_traj = sit_factory_sim.trajectory_from_points(
        trajectory_dict=traj_dict_measured,
        mode=TrajectoryMode.State,
        t_0=0,
        delta_t=dt_controller,
    )

    if visualize_scenario:
        logger.info("visualization")
        visualize_trajectories(
            scenario=scenario,
            planning_problem=planning_problem,
            planner_trajectory=x_ref,
            controller_trajectory=simulated_traj,
            save_path=img_saving_path,
            save_img=save_imgs,
        )

        if save_imgs:
            make_gif(
                path_to_img_dir=img_saving_path,
                scenario_name=str(scenario.scenario_id),
                num_imgs=len(x_ref.points.values()),
            )

    if visualize_control:
        visualize_reference_vs_actual_states(
            reference_trajectory=x_ref,
            actual_trajectory=simulated_traj,
            time_steps=list(simulated_traj.points.keys())[:-2],
            save_img=save_imgs,
            save_path=img_saving_path,
        )

    return traj_dict_measured, traj_dict_no_noise, input_dict

pid_with_lookahead_for_reactive_planner(scenario, planning_problem, reactive_planner_state_trajectory, reactive_planner_input_trajectory, kp_long=1.0, ki_long=0.0, kd_long=0.0, kp_lat=0.05, ki_lat=0.0, kd_lat=0.1, dt_controller=0.1, look_ahead_s=0.5, vehicle_params=BMW3seriesParams(), planner_converter=ReactivePlannerConverter(), disturbance_model_typename=GaussianDistribution, noise_model_typename=GaussianDistribution, sit_factory_sim=DBSIDTFactory(), vehicle_model_typename=DynamicBicycle, sensor_model_typename=FullStateFeedback, func_convert_planner2controller_state=convert_state_kb2db, func_convert_controller2planner_state=convert_state_db2kb, ivp_method='RK45', visualize_scenario=False, visualize_control=False, save_imgs=False, img_saving_path=None)

Decoupled longitudinal-lateral PID controller with look-ahead for the CommonRoad reactive planner. Longitudinal velocity and lateral offset are calculated with respect to the lookahead time given a reactive planner trajectory. Wrapper around pid_with_lookahead_for_planner().

Parameters:

Name	Type	Description	Default
scenario	Scenario	CommonRoad scenario	required
planning_problem	PlanningProblem	CommonRoad planning problem	required
reactive_planner_state_trajectory	List[ReactivePlannerState]	CommonRoad reactive planner state trajectory	required
reactive_planner_input_trajectory	List[InputState]	CommonRoad reactive planner input trajectory	required
kp_long	float	proportional gain longitudinal velocity	1.0
ki_long	float	integral gain longitudinal velocity	0.0
kd_long	float	derivative gain longitudinal velocity	0.0
kp_lat	float	proportional gain lateral offset	0.05
ki_lat	float	integral gain lateral offset	0.0
kd_lat	float	derivative gain lateral offset	0.1
dt_controller	float	controller time step size in seconds	0.1
look_ahead_s	float	look-ahead in seconds	0.5
vehicle_params		vehicle parameters object	BMW3seriesParams()
planner_converter	Union[PlanningConverterInterface, ReactivePlannerConverter]	planner converter	ReactivePlannerConverter()
disturbance_model_typename	Type[UncertaintyModelInterface]	typename (=class) of the disturbance	GaussianDistribution
noise_model_typename	Type[UncertaintyModelInterface]	typename (=class) of the noise	GaussianDistribution
sit_factory_sim	StateInputDisturbanceTrajectoryFactoryInterface	StateInputTrajectory factory for a given dynamics model	DBSIDTFactory()
vehicle_model_typename	Type[VehicleModelInterface]	typename (=class) of the vehicle dynamics model	DynamicBicycle
sensor_model_typename	Type[Union[SensorModelInterface, FullStateFeedback]]	typename (=class) of the sensor model / state feedback	FullStateFeedback
func_convert_planner2controller_state	Callable[[StateInterface, VehicleParameters], StateInterface]	function to convert a planner state into a controller state	convert_state_kb2db
func_convert_controller2planner_state	Callable[[StateInterface], StateInterface]	function to convert a controller state into a planner state	convert_state_db2kb
ivp_method	Union[str, OdeSolver, None]	IVP-Method used by the ODE-Solver	'RK45'
visualize_scenario	bool	If true, visualizes the scenario	False
visualize_control	bool	If true, visualizes control and error outputs	False
save_imgs	bool	If true and img_saving_path is given, saves visualizations instead of displaying them	False
img_saving_path	Union[Path, str]	If given and save_imgs=true, saves visualizations instead of displaying them	None

Returns:

Type	Description
Tuple[Dict[int, StateInterface], Dict[int, StateInterface], Dict[int, InputInterface]]	measured trajectory, trajectory without noise, trajectory without noise and without disturbance

Source code in commonroad_control/cr_control_easy_api/pid_for_dedicated_planner.py

def pid_with_lookahead_for_reactive_planner(
    scenario: Scenario,
    planning_problem: PlanningProblem,
    reactive_planner_state_trajectory: List[ReactivePlannerState],
    reactive_planner_input_trajectory: List[InputState],
    kp_long: float = 1.0,
    ki_long: float = 0.0,
    kd_long: float = 0.0,
    kp_lat: float = 0.05,
    ki_lat: float = 0.0,
    kd_lat: float = 0.1,
    dt_controller: float = 0.1,
    look_ahead_s: float = 0.5,
    vehicle_params=BMW3seriesParams(),
    planner_converter: Union[PlanningConverterInterface, ReactivePlannerConverter] = ReactivePlannerConverter(),
    disturbance_model_typename: Type[UncertaintyModelInterface] = GaussianDistribution,
    noise_model_typename: Type[UncertaintyModelInterface] = GaussianDistribution,
    sit_factory_sim: StateInputDisturbanceTrajectoryFactoryInterface = DBSIDTFactory(),
    vehicle_model_typename: Type[VehicleModelInterface] = DynamicBicycle,
    sensor_model_typename: Type[Union[SensorModelInterface, FullStateFeedback]] = FullStateFeedback,
    func_convert_planner2controller_state: Callable[
        [StateInterface, VehicleParameters], StateInterface
    ] = convert_state_kb2db,
    func_convert_controller2planner_state: Callable[[StateInterface], StateInterface] = convert_state_db2kb,
    ivp_method: Union[str, OdeSolver, None] = "RK45",
    visualize_scenario: bool = False,
    visualize_control: bool = False,
    save_imgs: bool = False,
    img_saving_path: Union[Path, str] = None,
) -> Tuple[Dict[int, StateInterface], Dict[int, StateInterface], Dict[int, InputInterface]]:
    """
    Decoupled longitudinal-lateral PID controller with look-ahead for the CommonRoad reactive planner. Longitudinal velocity and lateral offset are
    calculated with respect to the lookahead time given a reactive planner trajectory. Wrapper around pid_with_lookahead_for_planner().
    :param scenario: CommonRoad scenario
    :param planning_problem: CommonRoad planning problem
    :param reactive_planner_state_trajectory: CommonRoad reactive planner state trajectory
    :param reactive_planner_input_trajectory: CommonRoad reactive planner input trajectory
    :param kp_long: proportional gain longitudinal velocity
    :param ki_long: integral gain longitudinal velocity
    :param kd_long: derivative gain longitudinal velocity
    :param kp_lat: proportional gain lateral offset
    :param ki_lat: integral gain lateral offset
    :param kd_lat: derivative gain lateral offset
    :param dt_controller: controller time step size in seconds
    :param look_ahead_s: look-ahead in seconds
    :param vehicle_params: vehicle parameters object
    :param planner_converter: planner converter
    :param disturbance_model_typename: typename (=class) of the disturbance
    :param noise_model_typename: typename (=class) of the noise
    :param sit_factory_sim: StateInputTrajectory factory for a given dynamics model
    :param vehicle_model_typename: typename (=class) of the vehicle dynamics model
    :param sensor_model_typename: typename (=class) of the sensor model / state feedback
    :param func_convert_planner2controller_state: function to convert a planner state into a controller state
    :param func_convert_controller2planner_state: function to convert a controller state into a planner state
    :param ivp_method: IVP-Method used by the ODE-Solver
    :param visualize_scenario: If true, visualizes the scenario
    :param visualize_control: If true, visualizes control and error outputs
    :param save_imgs: If true and img_saving_path is given, saves visualizations instead of displaying them
    :param img_saving_path: If given and save_imgs=true, saves visualizations instead of displaying them
    :return: measured trajectory, trajectory without noise, trajectory without noise and without disturbance
    """
    return pid_with_lookahead_for_planner(
        scenario=scenario,
        planning_problem=planning_problem,
        state_trajectory=reactive_planner_state_trajectory,
        input_trajectory=reactive_planner_input_trajectory,
        kp_long=kp_long,
        ki_long=ki_long,
        kd_long=kd_long,
        kp_lat=kp_lat,
        ki_lat=ki_lat,
        kd_lat=kd_lat,
        dt_controller=dt_controller,
        t_look_ahead=look_ahead_s,
        vehicle_params=vehicle_params,
        planner_converter=planner_converter,
        disturbance_model_typename=disturbance_model_typename,
        noise_model_typename=noise_model_typename,
        sit_factory_sim=sit_factory_sim,
        vehicle_model_typename=vehicle_model_typename,
        sensor_model_typename=sensor_model_typename,
        func_convert_planner2controller_state=func_convert_planner2controller_state,
        func_convert_controller2planner_state=func_convert_controller2planner_state,
        ivp_method=ivp_method,
        visualize_scenario=visualize_scenario,
        visualize_control=visualize_control,
        save_imgs=save_imgs,
        img_saving_path=img_saving_path,
    )

pid_with_lookahead_for_reactive_planner_no_uncertainty(scenario, planning_problem, reactive_planner_state_trajectory, reactive_planner_input_trajectory, kp_long=1.0, ki_long=0.0, kd_long=0.0, kp_lat=0.05, ki_lat=0.0, kd_lat=0.1, dt_controller=0.1, t_look_ahead=0.5, vehicle_params=BMW3seriesParams(), planner_converter=ReactivePlannerConverter(), sit_factory_sim=DBSIDTFactory(), vehicle_model_typename=DynamicBicycle, sensor_model_typename=FullStateFeedback, func_convert_planner2controller_state=convert_state_kb2db, func_convert_controller2planner_state=convert_state_db2kb, ivp_method='RK45', visualize_scenario=False, visualize_control=False, save_imgs=False, img_saving_path=None)

Decoupled longitudinal-lateral PID controller with look-ahead for the CommonRoad reactive planner using no noises or disturbances. Longitudinal velocity and lateral offset are calculated with respect to the lookahead time given a reactive planner trajectory. Wrapper around pid_with_lookahead_for_planner().

Parameters:

Name	Type	Description	Default
scenario	Scenario	CommonRoad scenario	required
planning_problem	PlanningProblem	CommonRoad planning problem	required
reactive_planner_state_trajectory	List[ReactivePlannerState]	CommonRoad reactive planner state trajectory	required
reactive_planner_input_trajectory	List[InputState]	CommonRoad reactive planner input trajectory	required
kp_long	float	proportional gain longitudinal velocity	1.0
ki_long	float	integral gain longitudinal velocity	0.0
kd_long	float	derivative gain longitudinal velocity	0.0
kp_lat	float	proportional gain lateral offset	0.05
ki_lat	float	integral gain lateral offset	0.0
kd_lat	float	derivative gain lateral offset	0.1
dt_controller	float	controller time step size in seconds	0.1
t_look_ahead	float	look-ahead in seconds	0.5
vehicle_params		vehicle parameters object	BMW3seriesParams()
planner_converter	Optional[PlanningConverterInterface]	planner converter	ReactivePlannerConverter()
sit_factory_sim	StateInputDisturbanceTrajectoryFactoryInterface	StateInputTrajectory factory for a given dynamics model	DBSIDTFactory()
vehicle_model_typename	Type[VehicleModelInterface]	typename (=class) of the vehicle dynamics model	DynamicBicycle
sensor_model_typename	Type[Union[SensorModelInterface, FullStateFeedback]]	typename (=class) of the sensor model / state feedback	FullStateFeedback
func_convert_planner2controller_state	Callable[[StateInterface, VehicleParameters], StateInterface]	function to convert a planner state into a controller state	convert_state_kb2db
func_convert_controller2planner_state	Callable[[StateInterface], StateInterface]	function to convert a controller state into a planner state	convert_state_db2kb
ivp_method	Union[str, OdeSolver, None]	IVP-Method used by the ODE-Solver	'RK45'
visualize_scenario	bool	If true, visualizes the scenario	False
visualize_control	bool	If true, visualizes control and error outputs	False
save_imgs	bool	If true and img_saving_path is given, saves visualizations instead of displaying them	False
img_saving_path	Union[Path, str]	If given and save_imgs=true, saves visualizations instead of displaying them	None

Returns:

Type	Description
Tuple[Dict[int, StateInterface], Dict[int, StateInterface], Dict[int, InputInterface]]	measured trajectory, trajectory without noise, trajectory without noise and without disturbance

Source code in commonroad_control/cr_control_easy_api/pid_for_dedicated_planner.py

def pid_with_lookahead_for_reactive_planner_no_uncertainty(
    scenario: Scenario,
    planning_problem: PlanningProblem,
    reactive_planner_state_trajectory: List[ReactivePlannerState],
    reactive_planner_input_trajectory: List[InputState],
    kp_long: float = 1.0,
    ki_long: float = 0.0,
    kd_long: float = 0.0,
    kp_lat: float = 0.05,
    ki_lat: float = 0.0,
    kd_lat: float = 0.1,
    dt_controller: float = 0.1,
    t_look_ahead: float = 0.5,
    vehicle_params=BMW3seriesParams(),
    planner_converter: Optional[PlanningConverterInterface] = ReactivePlannerConverter(),
    sit_factory_sim: StateInputDisturbanceTrajectoryFactoryInterface = DBSIDTFactory(),
    vehicle_model_typename: Type[VehicleModelInterface] = DynamicBicycle,
    sensor_model_typename: Type[Union[SensorModelInterface, FullStateFeedback]] = FullStateFeedback,
    func_convert_planner2controller_state: Callable[
        [StateInterface, VehicleParameters], StateInterface
    ] = convert_state_kb2db,
    func_convert_controller2planner_state: Callable[[StateInterface], StateInterface] = convert_state_db2kb,
    ivp_method: Union[str, OdeSolver, None] = "RK45",
    visualize_scenario: bool = False,
    visualize_control: bool = False,
    save_imgs: bool = False,
    img_saving_path: Union[Path, str] = None,
) -> Tuple[Dict[int, StateInterface], Dict[int, StateInterface], Dict[int, InputInterface]]:
    """
    Decoupled longitudinal-lateral PID controller with look-ahead for the CommonRoad reactive planner using no noises or disturbances.
    Longitudinal velocity and lateral offset are calculated with respect to the lookahead time given a reactive planner
    trajectory. Wrapper around pid_with_lookahead_for_planner().
    :param scenario: CommonRoad scenario
    :param planning_problem: CommonRoad planning problem
    :param reactive_planner_state_trajectory: CommonRoad reactive planner state trajectory
    :param reactive_planner_input_trajectory: CommonRoad reactive planner input trajectory
    :param kp_long: proportional gain longitudinal velocity
    :param ki_long: integral gain longitudinal velocity
    :param kd_long: derivative gain longitudinal velocity
    :param kp_lat: proportional gain lateral offset
    :param ki_lat: integral gain lateral offset
    :param kd_lat: derivative gain lateral offset
    :param dt_controller: controller time step size in seconds
    :param t_look_ahead: look-ahead in seconds
    :param vehicle_params: vehicle parameters object
    :param planner_converter: planner converter
    :param sit_factory_sim: StateInputTrajectory factory for a given dynamics model
    :param vehicle_model_typename: typename (=class) of the vehicle dynamics model
    :param sensor_model_typename: typename (=class) of the sensor model / state feedback
    :param func_convert_planner2controller_state: function to convert a planner state into a controller state
    :param func_convert_controller2planner_state: function to convert a controller state into a planner state
    :param ivp_method: IVP-Method used by the ODE-Solver
    :param visualize_scenario: If true, visualizes the scenario
    :param visualize_control: If true, visualizes control and error outputs
    :param save_imgs: If true and img_saving_path is given, saves visualizations instead of displaying them
    :param img_saving_path: If given and save_imgs=true, saves visualizations instead of displaying them
    :return: measured trajectory, trajectory without noise, trajectory without noise and without disturbance
    """
    return pid_with_lookahead_for_reactive_planner(
        scenario=scenario,
        planning_problem=planning_problem,
        reactive_planner_state_trajectory=reactive_planner_state_trajectory,
        reactive_planner_input_trajectory=reactive_planner_input_trajectory,
        kp_long=kp_long,
        ki_long=ki_long,
        kd_long=kd_long,
        kp_lat=kp_lat,
        ki_lat=ki_lat,
        kd_lat=kd_lat,
        dt_controller=dt_controller,
        look_ahead_s=t_look_ahead,
        vehicle_params=vehicle_params,
        planner_converter=planner_converter,
        disturbance_model_typename=NoUncertainty,
        noise_model_typename=NoUncertainty,
        sit_factory_sim=sit_factory_sim,
        vehicle_model_typename=vehicle_model_typename,
        sensor_model_typename=sensor_model_typename,
        func_convert_planner2controller_state=func_convert_planner2controller_state,
        func_convert_controller2planner_state=func_convert_controller2planner_state,
        ivp_method=ivp_method,
        visualize_scenario=visualize_scenario,
        visualize_control=visualize_control,
        save_imgs=save_imgs,
        img_saving_path=img_saving_path,
    )