Refactor lateral control

scripts/run_lateral_control_riccati.py

@@ -3,57 +3,71 @@
 
 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.lateral_control_riccati.lateral_control_riccati import (
+    DynamicVehicleState,
+)
 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,
 )
 
 
-def main():
+def main() -> None:
     print("Running simulation...")
     radius = 500
-    vars_0 = [0.0, -radius, 0.0, 0.0, 0.0]
-    v_0 = 33.0
+    initial_state = DynamicVehicleState(
+        x=0.0,
+        y=float(-radius),
+        heading=0.0,
+        sideslip_angle=0.0,
+        yaw_rate=0.0,
+    )
+    initial_velocity = 33.0
 
     curve = ref.generate_reference_curve(
-        [0, radius, 0, -radius, 0], [-radius, 0, radius, 0, radius], 10.0
+        np.array([0, radius, 0, -radius, 0]),
+        np.array([-radius, 0, radius, 0, radius]),
+        10.0,
     )
-    ti = np.arange(0, 40, 0.1)
+    time_vector = np.arange(0, 40, 0.1)
 
-    # r = 10  # hectic steering behavior
-    r = 10000  # fairly calm steering behavior
+    # control_weight = 10  # hectic steering behavior
+    control_weight = 10000  # fairly calm steering behavior
 
     model = cl.LateralControlRiccati(
-        initial_condition=vars_0,
+        initial_state=initial_state,
         curve=curve,
         vehicle_params=DEFAULT_VEHICLE_PARAMS,
-        initial_velocity=v_0,
-        r=r,
+        initial_velocity=initial_velocity,
+        control_weight=control_weight,
+    )
+    trajectory = model.simulate(
+        time_vector, velocity=initial_velocity, time_step=0.1
     )
-    sol = model.simulate(ti, v=v_0, t_step=0.1)
-    x = sol[:, 0]
-    y = sol[:, 1]
-    psi = sol[:, 2]
-    delta = sol[:, 5]
+    x = trajectory[:, 0]
+    y = trajectory[:, 1]
+    psi = trajectory[:, 2]
+    delta = trajectory[:, 5]
 
     fig, ax = plt.subplots()
 
-    plt.plot(curve["x"], curve["y"], "r-", linewidth=0.5)
+    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:])]
+    def update(i: int, *fargs: object) -> None:
+        for line in reversed(ax.lines[1:]):
+            line.remove()
         ax.plot(x[: i + 1], y[: i + 1], "b-", linewidth=0.5)
         point1.set_data(x[i : i + 1], y[i : i + 1])
         pv.plot_vehicle(ax, x[i], y[i], psi[i], delta[i])
         for farg in fargs:
             print(farg)
 
-    vz.Vizard(figure=fig, update_func=update, time_vec=ti)
+    vz.Vizard(figure=fig, update_func=update, time_vec=time_vector)
     plt.show()
 
 

scripts/run_lateral_control_state_based.py

@@ -3,42 +3,54 @@
 
 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.lateral_control_state_based.lateral_control_state_based import (
+    KinematicVehicleState,
+)
 from behavior_generation_lecture_python.utils.plot_vehicle import plot_vehicle as pv
 from behavior_generation_lecture_python.utils.vizard import vizard as vz
 
 
-def main():
+def main() -> None:
     print("Running simulation...")
     radius = 20
-    vars_0 = [0.1, -radius, 0.0]
+    initial_state = KinematicVehicleState(
+        x=0.1,
+        y=float(-radius),
+        heading=0.0,
+    )
     curve = ref.generate_reference_curve(
-        [0, radius, 0, -radius, 0], [-radius, 0, radius, 0, radius], 1.0
+        np.array([0, radius, 0, -radius, 0]),
+        np.array([-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]
+    time_vector = np.arange(0, 100, 0.1)
+    model = cl.LateralControlStateBased(initial_state, curve)
+    trajectory = model.simulate(time_vector, velocity=1)
+
+    # Extract data from ControllerOutput list
+    x = np.array([out.x for out in trajectory])
+    y = np.array([out.y for out in trajectory])
+    psi = np.array([out.heading for out in trajectory])
+    delta = np.array([out.steering_angle for out in trajectory])
 
     fig, ax = plt.subplots()
 
-    plt.plot(curve["x"], curve["y"], "r-", linewidth=0.5)
+    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:])]
+    def update(i: int, *fargs: object) -> None:
+        for line in reversed(ax.lines[1:]):
+            line.remove()
         ax.plot(x[:i], y[:i], "b-", linewidth=0.5)
         point1.set_data(x[i : i + 1], y[i : i + 1])
         pv.plot_vehicle(ax, x[i], y[i], psi[i], delta[i])
         for farg in fargs:
             print(farg)
 
-    vz.Vizard(figure=fig, update_func=update, time_vec=ti)
+    vz.Vizard(figure=fig, update_func=update, time_vec=time_vector)
     plt.show()