Lateral Control (state-based)

In [1]:

import numpy as np
import matplotlib.pyplot as plt

import behavior_generation_lecture_python.lateral_control_state_based.lateral_control_state_based 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

%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_state_based.lateral_control_state_based 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 %matplotlib inline # use '%matplotlib notebook' to interact with the vizard

In [2]:

def main():
    print("Running simulation...")
    radius = 20
    vars_0 = [0.1, -radius, 0.0]
    curve = ref.generate_reference_curve(
        [0, radius, 0, -radius, 0], [-radius, 0, radius, 0, radius], 1.0
    )
    ti = np.arange(0, 100, 0.1)
    model = cl.LateralControlStateBased(vars_0, curve)
    sol = model.simulate(ti, v=1)
    x = sol[:, 0]
    y = sol[:, 1]
    psi = sol[:, 2]
    delta = sol[:, 4]

    fig, ax = plt.subplots()

    plt.plot(curve["x"], curve["y"], "r-", linewidth=0.5)
    plt.plot(x, y, "b-", linewidth=0.5)
    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], y[:i], "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 = 20 vars_0 = [0.1, -radius, 0.0] curve = ref.generate_reference_curve( [0, radius, 0, -radius, 0], [-radius, 0, radius, 0, radius], 1.0 ) ti = np.arange(0, 100, 0.1) model = cl.LateralControlStateBased(vars_0, curve) sol = model.simulate(ti, v=1) x = sol[:, 0] y = sol[:, 1] psi = sol[:, 2] delta = sol[:, 4] fig, ax = plt.subplots() plt.plot(curve["x"], curve["y"], "r-", linewidth=0.5) plt.plot(x, y, "b-", linewidth=0.5) 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], y[:i], "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 0x7efbdcf081f0>> 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_2006/1816185659.py:27: 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])