Lateral Control (Riccati)

In [1]:

import numpy as np
import matplotlib.pyplot as plt

import behavior_generation_lecture_python.lateral_control_riccati.lateral_control_riccati as cl
import behavior_generation_lecture_python.utils.generate_reference_curve as ref
from behavior_generation_lecture_python.utils.plot_vehicle import plot_vehicle as pv
from behavior_generation_lecture_python.utils.vizard import vizard as vz
from behavior_generation_lecture_python.vehicle_models.vehicle_parameters import (
    DEFAULT_VEHICLE_PARAMS,
)

%matplotlib inline
# use '%matplotlib notebook' to interact with the vizard

import numpy as np import matplotlib.pyplot as plt import behavior_generation_lecture_python.lateral_control_riccati.lateral_control_riccati as cl import behavior_generation_lecture_python.utils.generate_reference_curve as ref from behavior_generation_lecture_python.utils.plot_vehicle import plot_vehicle as pv from behavior_generation_lecture_python.utils.vizard import vizard as vz from behavior_generation_lecture_python.vehicle_models.vehicle_parameters import ( DEFAULT_VEHICLE_PARAMS, ) %matplotlib inline # use '%matplotlib notebook' to interact with the vizard

In [2]:

def main():
    print("Running simulation...")
    radius = 500
    vars_0 = [0.0, -radius, 0.0, 0.0, 0.0]
    v_0 = 33.0

    curve = ref.generate_reference_curve(
        [0, radius, 0, -radius, 0], [-radius, 0, radius, 0, radius], 10.0
    )
    ti = np.arange(0, 40, 0.1)

    # r = 10  # hectic steering behavior
    r = 10000  # fairly calm steering behavior

    model = cl.LateralControlRiccati(
        initial_condition=vars_0,
        curve=curve,
        vehicle_params=DEFAULT_VEHICLE_PARAMS,
        initial_velocity=v_0,
        r=r,
    )

    sol = model.simulate(ti, v=v_0, t_step=0.1)
    x = sol[:, 0]
    y = sol[:, 1]
    psi = sol[:, 2]
    delta = sol[:, 5]

    fig, ax = plt.subplots()

    plt.plot(curve["x"], curve["y"], "r-", linewidth=0.5)
    plt.plot(x, y, "b-")
    plt.axis("equal")

    (point1,) = ax.plot([], [], marker="o", color="blue", ms=5)

    def update(i, *fargs):
        [l.remove() for l in reversed(ax.lines[1:])]
        ax.plot(x[: i + 1], y[: i + 1], "b-", linewidth=0.5)
        point1.set_data(x[i], y[i])
        pv.plot_vehicle(ax, x[i], y[i], psi[i], delta[i])
        for farg in fargs:
            print(farg)

    viz = vz.Vizard(fig, update, ti)
    plt.show()

def main(): print("Running simulation...") radius = 500 vars_0 = [0.0, -radius, 0.0, 0.0, 0.0] v_0 = 33.0 curve = ref.generate_reference_curve( [0, radius, 0, -radius, 0], [-radius, 0, radius, 0, radius], 10.0 ) ti = np.arange(0, 40, 0.1) # r = 10 # hectic steering behavior r = 10000 # fairly calm steering behavior model = cl.LateralControlRiccati( initial_condition=vars_0, curve=curve, vehicle_params=DEFAULT_VEHICLE_PARAMS, initial_velocity=v_0, r=r, ) sol = model.simulate(ti, v=v_0, t_step=0.1) x = sol[:, 0] y = sol[:, 1] psi = sol[:, 2] delta = sol[:, 5] fig, ax = plt.subplots() plt.plot(curve["x"], curve["y"], "r-", linewidth=0.5) plt.plot(x, y, "b-") plt.axis("equal") (point1,) = ax.plot([], [], marker="o", color="blue", ms=5) def update(i, *fargs): [l.remove() for l in reversed(ax.lines[1:])] ax.plot(x[: i + 1], y[: i + 1], "b-", linewidth=0.5) point1.set_data(x[i], y[i]) pv.plot_vehicle(ax, x[i], y[i], psi[i], delta[i]) for farg in fargs: print(farg) viz = vz.Vizard(fig, update, ti) plt.show()

In [3]:

main()

main()

Running simulation...

/home/runner/work/behavior_generation_lecture_python/behavior_generation_lecture_python/src/behavior_generation_lecture_python/utils/vizard/vizard.py:84: UserWarning: frames=<bound method Vizard.frames of <behavior_generation_lecture_python.utils.vizard.vizard.Vizard object at 0x7f7b09468a60>> which we can infer the length of, did not pass an explicit *save_count* and passed cache_frame_data=True.  To avoid a possibly unbounded cache, frame data caching has been disabled. To suppress this warning either pass `cache_frame_data=False` or `save_count=MAX_FRAMES`.
  anim = FuncAnimation(
/tmp/ipykernel_1927/4124110550.py:40: MatplotlibDeprecationWarning: Setting data with a non sequence type is deprecated since 3.7 and will be remove two minor releases later
  point1.set_data(x[i], y[i])