diff --git a/notebooks/lateral_control_riccati_notebook.ipynb b/notebooks/lateral_control_riccati_notebook.ipynb index 9a96004..2c3b231 100644 --- a/notebooks/lateral_control_riccati_notebook.ipynb +++ b/notebooks/lateral_control_riccati_notebook.ipynb @@ -1,118 +1,163 @@ { - "cells": [ - { - "cell_type": "code", - "execution_count": null, - "id": "cee10281", - "metadata": {}, - "outputs": [], - "source": [ - "import os\n", - "import numpy as np\n", - "import matplotlib.pyplot as plt\n", - "\n", - "import behavior_generation_lecture_python.lateral_control_riccati.lateral_control_riccati as cl\n", - "import behavior_generation_lecture_python.utils.generate_reference_curve as ref\n", - "from behavior_generation_lecture_python.utils.plot_vehicle import plot_vehicle as pv\n", - "from behavior_generation_lecture_python.utils.vizard import vizard as vz\n", - "from behavior_generation_lecture_python.vehicle_models.vehicle_parameters import (\n", - " DEFAULT_VEHICLE_PARAMS,\n", - ")\n", - "\n", - "interactive_widgets = not os.getenv(\"CI\") == \"true\"\n", - "if interactive_widgets:\n", - " # Use widget backend locally, to be able to interact with the plots\n", - " %matplotlib widget\n", - "else:\n", - " # Use inline backend in CI, to render the notebooks for the hosted docs\n", - " %matplotlib inline" - ] + "cells": [ + { + "cell_type": "code", + "execution_count": null, + "id": "cee10281", + "metadata": {}, + "outputs": [], + "source": [ + "import os\n", + "import numpy as np\n", + "import matplotlib.pyplot as plt\n", + "\n", + "import behavior_generation_lecture_python.lateral_control_riccati.lateral_control_riccati as cl\n", + "import behavior_generation_lecture_python.utils.generate_reference_curve as ref\n", + "from behavior_generation_lecture_python.lateral_control_riccati.lateral_control_riccati import (\n", + " DynamicVehicleState,\n", + ")\n", + "from behavior_generation_lecture_python.utils.plot_vehicle import plot_vehicle as pv\n", + "from behavior_generation_lecture_python.utils.vizard import vizard as vz\n", + "from behavior_generation_lecture_python.vehicle_models.vehicle_parameters import (\n", + " DEFAULT_VEHICLE_PARAMS,\n", + ")\n", + "\n", + "interactive_widgets = not os.getenv(\"CI\") == \"true\"\n", + "if interactive_widgets:\n", + " # Use widget backend locally, to be able to interact with the plots\n", + " %matplotlib widget\n", + "else:\n", + " # Use inline backend in CI, to render the notebooks for the hosted docs\n", + " %matplotlib inline" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "4813563b", + "metadata": {}, + "outputs": [], + "source": [ + "def main() -> None:\n", + " print(\"Running simulation...\")\n", + " radius = 500\n", + " initial_state = DynamicVehicleState(\n", + " x=0.0,\n", + " y=float(-radius),\n", + " heading=0.0,\n", + " sideslip_angle=0.0,\n", + " yaw_rate=0.0,\n", + " )\n", + " initial_velocity = 33.0\n", + "\n", + " curve = ref.generate_reference_curve(\n", + " np.array([0, radius, 0, -radius, 0]),\n", + " np.array([-radius, 0, radius, 0, radius]),\n", + " 10.0,\n", + " )\n", + " time_vector = np.arange(0, 40, 0.1)\n", + "\n", + " # control_weight = 10 # hectic steering behavior\n", + " control_weight = 10000 # fairly calm steering behavior\n", + "\n", + " model = cl.LateralControlRiccati(\n", + " initial_state=initial_state,\n", + " curve=curve,\n", + " vehicle_params=DEFAULT_VEHICLE_PARAMS,\n", + " initial_velocity=initial_velocity,\n", + " control_weight=control_weight,\n", + " )\n", + "\n", + " trajectory = model.simulate(time_vector, velocity=initial_velocity, time_step=0.1)\n", + " x = trajectory[:, 0]\n", + " y = trajectory[:, 1]\n", + " psi = trajectory[:, 2]\n", + " delta = trajectory[:, 5]\n", + "\n", + " fig, ax = plt.subplots()\n", + "\n", + " plt.plot(curve.x, curve.y, \"r-\", linewidth=0.5)\n", + " plt.plot(x, y, \"b-\")\n", + " plt.axis(\"equal\")\n", + "\n", + " (point1,) = ax.plot([], [], marker=\"o\", color=\"blue\", ms=5)\n", + "\n", + " def update(i: int, *fargs: object) -> None:\n", + " for line in reversed(ax.lines[1:]):\n", + " line.remove()\n", + " ax.plot(x[: i + 1], y[: i + 1], \"b-\", linewidth=0.5)\n", + " point1.set_data(x[i : i + 1], y[i : i + 1])\n", + " pv.plot_vehicle(ax, x[i], y[i], psi[i], delta[i])\n", + " for farg in fargs:\n", + " print(farg)\n", + "\n", + " _ = vz.Vizard(fig, update, time_vector)\n", + " plt.show()" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "id": "e8e63277", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Running simulation...\n" + ] + }, + { + "data": { + "application/vnd.jupyter.widget-view+json": { + "model_id": "3e25af31f764488baa5e3374012989e0", + "version_major": 2, + "version_minor": 0 + }, + "image/png": "iVBORw0KGgoAAAANSUhEUgAAAoAAAAHgCAYAAAA10dzkAAAAOXRFWHRTb2Z0d2FyZQBNYXRwbG90bGliIHZlcnNpb24zLjcuNSwgaHR0cHM6Ly9tYXRwbG90bGliLm9yZy/xnp5ZAAAACXBIWXMAAA9hAAAPYQGoP6dpAABHkUlEQVR4nO3deXxU5aH/8e9MkpnsGwRCIEDiEkRAEK4YvSBaChW1cr33ooJYKj80FTdQC7m2InZhE1BRa+0VyqUoCiKtragUhKoEVCCyyCIgexJASCbrZJnz+yNmZMhClklmhvN5v17nBXPOc848Tw6Z+fI85zzHYhiGIQAAAJiG1dcVAAAAQNsiAAIAAJgMARAAAMBkCIAAAAAmQwAEAAAwGQIgAACAyRAAAQAATIYACAAAYDIEQAAAAJMhAAIAAJgMARAAAMBkCIAAAAAmQwAEAAAwGQIgAACAyRAAAQAATIYACAAAYDIEQAAAAJMhAAIAAJgMARAAAMBkCIAAAAAmQwAEAAAwGQIgAACAyRAAAQAATIYACAAAYDIEQAAAAJMhAAIAAJgMARAAAMBkCIAAAAAmQwAEAAAwGQIgAACAyRAAAQAATIYACAAAYDIEQAAAAJMhAAIAAJgMARAAAMBkCIAAAAAmQwAEAAAwGQIgAACAyRAAAQAATIYACAAAYDIEQAAAAJMhAAIAAJgMARAAAMBkCIAAAAAmQwAEAAAwGQIgAACAyRAAAQAATIYACAAAYDIEQAAAAJMhAAIAAJgMARAAAMBkCIAAAAAmQwAEAAAwGQIgAACAyRAAAQAATIYACAAAYDLBvq5AIHO5XDpx4oSioqJksVh8XR0AANAIhmGosLBQSUlJslrN2RdGAGyBEydOKDk52dfVAAAAzXD06FF16dLF19XwCQJgC0RFRUmq/gcUHR3t49oAAIDGcDgcSk5Odn+PmxEBsAVqhn2jo6MJgAAABBgzX75lzoFvAAAAEyMAAgAAmAwBEAAAwGQCOgA6nU717dtXFotF2dnZHtvefvtt9e3bV+Hh4erWrZvmzJlT73E+++wzBQcHq2/fvq1bYQAAAD8Q0AHwl7/8pZKSkmqtX716tcaMGaOMjAzt3LlTr7zyiubPn6+XXnqpVtn8/Hzde++9+tGPftQWVQYAAPC5gA2Aq1ev1kcffaTnnnuu1rYlS5Zo5MiRysjIUGpqqm655RZlZmZq1qxZMgzDo2xGRoZGjx6t9PT0tqo6AACATwVkAMzLy9OECRO0ZMkShYeH19rudDoVGhrqsS4sLEzHjh3T4cOH3esWLVqkgwcPatq0aa1eZwAAAH8RcAHQMAyNGzdOGRkZGjBgQJ1lhg8frpUrV2rt2rVyuVzat2+f5s6dK0nKycmRJH3zzTeaOnWq/vKXvyg4uHHTITqdTjkcDo8FAAAg0PhNAJw6daosFkuDy549e7RgwQIVFhYqMzOz3mNNmDBBDz30kG699VbZbDZde+21uuuuuyRJVqtVVVVVGj16tKZPn67LL7+80XWcMWOGYmJi3AuPgQMAAIHIYpx/UZyPnDp1St99912DZVJTUzVq1Ci99957HrN3V1VVKSgoSGPGjNHixYs91ufm5iohIUFr167ViBEjdPLkSYWEhCguLk5BQUHusi6XS4ZhKCgoSB999JFuuummWu/vdDrldDrdr2seJVNQUMCTQAAACBAOh0MxMTGm/v72mwDYWEeOHPEYej1x4oSGDx+uFStWaODAgfU+1Pnee+/V/v37tXHjRrlcLn399dce21955RWtW7dOK1asUEpKiiIiIi5YF/4BAQAQePj+DsBnAXft2tXjdWRkpCTpkksucYe/06dPa8WKFRoyZIjKysq0aNEiLV++XBs2bJBUPQzcq1cvj+N06NBBoaGhtdYDAABcbPzmGkBvW7x4sQYMGKDrr79eu3bt0vr163XNNdf4uloAAAA+F3BDwP6ELmQAAAIP398XcQ8gAAAA6kYABAAAMBkCIAAAgMkQAAEAAEyGAAgAAGAyBEAAAACTIQACAACYDAEQAADAZAiAAAAAJkMABAAAMBkCIAAAgMkQAAEAAEyGAAgAAGAyBEAAAACTIQACAACYDAEQAADAZAiAAAAAJkMABAAAMBkCIAAAgMkQAAEAAEyGAAgAAGAyBEAAAACTIQACAACYDAEQAADAZAiAAAAAJkMABAAAMBkCIAAAgMkQAAEAAEyGAAgAAGAyBEAAAACTIQACAACYDAEQAADAZAiAAAAAJkMABAAAMBkCIAAAgMkQAAEAAEyGAAgAAGAyBEAAAACTIQACAACYDAEQAADAZAI6ADqdTvXt21cWi0XZ2dke295++2317dtX4eHh6tatm+bMmVPn/k899ZS6desmu92u7t27a+HChW1UewAAAN8I9nUFWuKXv/ylkpKS9NVXX3msX716tcaMGaMFCxZo2LBh2r17tyZMmKCwsDA99NBD7nKjRo1SXl6eXn/9dV166aXKycmRy+Vq62YAAAC0qYANgKtXr9ZHH32kd955R6tXr/bYtmTJEo0cOVIZGRmSpNTUVGVmZmrWrFmaOHGiLBaLPvjgA23YsEEHDx5UfHy8JKl79+5t3QwAAIA2F5BDwHl5eZowYYKWLFmi8PDwWtudTqdCQ0M91oWFhenYsWM6fPiwJOlvf/ubBgwYoNmzZ6tz5866/PLL9cQTT6i0tLRN2gAAAOArARcADcPQuHHjlJGRoQEDBtRZZvjw4Vq5cqXWrl0rl8ulffv2ae7cuZKknJwcSdLBgwf16aefaufOnXr33Xf1/PPPa8WKFXrwwQfrfW+n0ymHw+GxAAAABBq/CYBTp06VxWJpcNmzZ48WLFigwsJCZWZm1nusCRMm6KGHHtKtt94qm82ma6+9VnfddZckyWqtbrLL5ZLFYtHSpUt1zTXXaMSIEZo3b54WL15cby/gjBkzFBMT416Sk5O9/4MAAABoZRbDMAxfV0KSTp06pe+++67BMqmpqRo1apTee+89WSwW9/qqqioFBQVpzJgxWrx4scf63NxcJSQkaO3atRoxYoROnjyphIQE/exnP9Nnn32m/fv3u8vv3r1bPXv21L59+3TZZZfVen+n0ymn0+l+7XA4lJycrIKCAkVHR7ek+QAAoI04HA7FxMSY+vvbb24CSUhIUEJCwgXLvfjii/rtb3/rfn3ixAkNHz5cb731lgYOHOhRNigoSJ07d5Ykvfnmm0pPT3e/x/XXX6/ly5erqKhIkZGRkqR9+/bJarWqS5cudb633W6X3W5vVvsAAAD8hd8EwMbq2rWrx+ua8HbJJZe4g9vp06e1YsUKDRkyRGVlZVq0aJGWL1+uDRs2uPcbPXq0fvOb3+jnP/+5pk+frtOnT+vJJ5/Ufffdp7CwsLZrEAAAQBvzm2sAvW3x4sUaMGCArr/+eu3atUvr16/XNddc494eGRmpNWvWKD8/XwMGDNCYMWN022236cUXX/RhrQEAAFqf31wDGIi4hgAAgMDD9/dF3AMIAACAuhEAAQAATIYACAAAYDIEQAAAAJMhAAIAAJgMARAAAMBkCIAAAAAmQwAEAAAwGQIgAACAyRAAAQAATIYACAAAYDIEQAAAAJMhAAIAAJgMARAAAMBkCIAAAAAmQwAEAAAwGQIgAACAyRAAAQAATIYACAAAYDIEQAAAAJMhAAIAAJgMARAAAMBkCIAAAAAmQwAEAAAwGQIgAACAyRAAAQAATIYACAAAYDIEQAAAAJMhAAIAAJgMARAAAMBkCIAAAAAmQwAEAAAwGQIgAACAyRAAAQAATIYACAAAYDIEQAAAAJMhAAIAAJgMARAAAMBkCIAAAAAmQwAEAAAwmYAOgE6nU3379pXFYlF2drbHtrffflt9+/ZVeHi4unXrpjlz5tTaf+nSpbrqqqsUHh6uTp066b777tN3333XRrUHAADwjYAOgL/85S+VlJRUa/3q1as1ZswYZWRkaOfOnXrllVc0f/58vfTSS+4yn332me69916NHz9eu3bt0vLly/X5559rwoQJbdkEAACANhewAXD16tX66KOP9Nxzz9XatmTJEo0cOVIZGRlKTU3VLbfcoszMTM2aNUuGYUiSsrKy1L17dz3yyCNKSUnRv//7v+uBBx7Q559/3tZNAQAAaFMBGQDz8vI0YcIELVmyROHh4bW2O51OhYaGeqwLCwvTsWPHdPjwYUlSenq6jh49qvfff1+GYSgvL08rVqzQiBEj6n1fp9Mph8PhsQAAAASagAuAhmFo3LhxysjI0IABA+osM3z4cK1cuVJr166Vy+XSvn37NHfuXElSTk6OJOn666/X0qVLdeedd8pmsykxMVExMTF6+eWX633vGTNmKCYmxr0kJyd7v4EAAACtzG8C4NSpU2WxWBpc9uzZowULFqiwsFCZmZn1HmvChAl66KGHdOutt8pms+naa6/VXXfdJUmyWqub/PXXX+vRRx/V008/rS1btuiDDz7QoUOHlJGRUe9xMzMzVVBQ4F6OHj3q3R8CAABAG7AYNRfF+dipU6cueAduamqqRo0apffee08Wi8W9vqqqSkFBQRozZowWL17ssT43N1cJCQlau3atRowYoZMnTyohIUFjx45VWVmZli9f7i7/6aefatCgQTpx4oQ6dep0wTo7HA7FxMSooKBA0dHRzWg1AABoa3x/S8G+rkCNhIQEJSQkXLDciy++qN/+9rfu1ydOnNDw4cP11ltvaeDAgR5lg4KC1LlzZ0nSm2++qfT0dPd7lJSUKDg4uFZ5SfKTTAwAANAq/CYANlbXrl09XkdGRkqSLrnkEnXp0kWSdPr0aa1YsUJDhgxRWVmZFi1apOXLl2vDhg3u/W677TZNmDBBf/jDHzR8+HDl5OToscce0zXXXFPn1DIAAAAXi4ALgI21ePFiPfHEEzIMQ+np6Vq/fr2uueYa9/Zx48apsLBQL730kh5//HHFxsbqpptu0qxZs3xYawAAgNbnN9cABiKuIQAAIPDw/e1HdwEDAACgbRAAAQAATIYACAAAYDIEQAAAAJMhAAIAAJgMARAAAMBkCIAAAAAmQwAEAAAwGQIgAACAyRAAAQAATIYACAAAYDIEQAAAAJMhAAIAAJgMARAAAMBkCIAAAAAmQwAEAAAwGQIgAACAyRAAAcBLDn9XrA925uqbvEJfVwUAGkQABAAvqagytDvHob9vz9HWI2d9XR0AqBcBEAC85NIOkUpNiJAk/WvfKe0/SU8gAP8U7OsKALj4uVyGThc5ledw6lRRmbYdyVdBaUWtcpYLHMdikTrFhKl9pL1R72u50AGbWVaSLPXUtqS8SpJkGNKHu/K064SjaQc+z7GzpcovKf/+Peuoh8Uiq1WyWiwKsloU9P2ftmCrurcL1xWdYhQdGqKY8JAW1QPAxYUACMCrDMPQ2ZIK5TnKlOsoU15BmU4VOlXpMtxlduc4VFhW2azjl1a43CHL35VXunTwVHGLjrH/ZKFOF5U3a99OMaHadaK6FzLSHqyk2DCltI/Q5R0jFRzEABBgZgRAAC1SXunSsbMlOpFfHfhOFpbJWeHydbVwniJnpfblFWpfXqE27AvSNSlx6pccJ6u1iV2fAC4KBEAAzXKysEyff3tGB08Vq+qc3j34v7KKKv1r32kdPVOqEb07yRZMbyBgNvzWA2iyzQe/05ubj+qbvCLCXzNUVLlUWeX7XtJvTxdr3Z6Tvq4GAB+gBxBAk2w++J02HvjO19UIKM7KKhWWVcpRWqHCskqVVlQp2GpRz6Rohdt8+zG8O8ehXp2j1SUu3Kf1ANC2CIAAGu3Y2RJlHWx++DOM6t7CKzpFu//eYPk61lmberuuDzgrquQoq5SjrDrwlVXUvmml0mXom7wi9e4S02CburePULd2EXX+LFTPz7BmbVAjr+/bcvgsARAwGQIggEbbdPBMfZmjQUXOSh05UyLH91O/BFstsgcHKdRmVXhIsMJsQQoNsSosJEiWAAh45yurqJKjtMId+sorGze8W1pRpZMOpxJjQustE2xt/St1jp4pUWWVizuDARMhAAJolLPF5Tp6pqTJ+zlKK7Qnt1Cuc5JjpctQZXmlisul7/TDFCcWixQaEqTwkCCF2YIU9v2foSFBftXzV+ly6bui8uoevtJKlbfger6ThWUNBsC2UFFl6Hh+qbq1i/BpPQC0HQIggEY5eLqoyfsYhqGDp4s9wl/D5aXS8iqVlldJ50yflxBl1yUJkU1+/9ZSUFKhb0+3bH6/GiXlVSqrqFJoSJBXjtdch74rIQACJkJ/P4BGOXa2tMn7nC2pqPP6t6aKj7C1+BjeFBMWcsGnljRFfkntp6K0teb07gIIXARAAI2S5yhr8j5nipv3BItzWS0WxYT612PMgoOsCrd7bwClsMz3AfC7onI5KwPjCSsAWo4ACOCCSsurVOxsejhweCHYRNiD/PJpFZFeDIDFfvBoO5dh6ER+00M+gMBEAARwQd8VO5u8j7OyqtF3wzYkzObba+PqExrivY/Psooqv5gY+tNvTsnFxN6AKRAAAVxQQWnTe/LKvPQ8YHuwvwZA79bLH3oBTxeVK6cZQ/0AAg8BEMAFFZVVNnkfb/T+SZLNT+em83a9/OX6uzNFLb9uE4D/889PVgB+pcjZ9ADorUBjC/bPjylv16ui0j+GXvNLCYCAGfjnJysAv9KcAOitHsBgP7wBRJJCgqxenZy6JZNJe1NzbvYBEHgIgAAuqHk9gN4JNI19nq0vhAR5MQB66efVUiXlTT/XAAJPQAbA7t27y2KxeCwzZ870KLN9+3YNGjRIoaGhSk5O1uzZs2sdZ/ny5erRo4dCQ0PVu3dvvf/++23VBCCgNCecVHipR8ufHgF3Pm8OA3vr59VS/nAzCoDWF5ABUJKeffZZ5eTkuJeHH37Yvc3hcGjYsGHq1q2btmzZojlz5uiZZ57Ra6+95i6zceNG3X333Ro/fry2bdumkSNHauTIkdq5c6cvmgP4tapmTA3irdlErH78KWXxYjitauTj8lpbhZ/0RAJoXQH7LOCoqCglJibWuW3p0qUqLy/XwoULZbPZdOWVVyo7O1vz5s3T/fffL0l64YUX9JOf/ERPPvmkJOk3v/mN1qxZo5deekmvvvpqm7UDCASVzQmAXkqAFq8+dM1/+cv8e5UuAiBgBn78f+uGzZw5U+3atVO/fv00Z84cVVb+cN1KVlaWBg8eLJvth+eHDh8+XHv37tXZs2fdZYYOHepxzOHDhysrK6ttGgAEkOZMUuwvPVqBwl9+XBVVflIRAK0qIHsAH3nkEV199dWKj4/Xxo0blZmZqZycHM2bN0+SlJubq5SUFI99Onbs6N4WFxen3Nxc97pzy+Tm5tb7vk6nU07nD09EcDgc3moSLgKrMjdr7YeVks123hIihZy3LiREsljU1BFEX5XfeqS9VEcuaOj4OQWRdezQ+LrEJJUo5bpTjd8hwPlLYK4kAAKm4DcBcOrUqZo1a1aDZXbv3q0ePXpo8uTJ7nV9+vSRzWbTAw88oBkzZshut7daHWfMmKHp06e32vER2G64P039hp6WioqkokKpsFAqLq5+XVhY/WdxsXS2er37+95i+aH7JzhYioz0XKKipMhIGeER7r97LDZbnUmsqXmiofKLNxbUug7wQsfffizf8zrAJtYn2G6umxH8ZaDbj++5AeBFfhMAH3/8cY0bN67BMqmpqXWuHzhwoCorK3Xo0CGlpaUpMTFReXl5HmVqXtdcN1hfmfquK5SkzMxMj/DpcDiUnJzcYJ1hHnEpsYpLiW3ZQcrLq8NiYaHkcPywFBZKjpPS8fPXOaSy7x/ddW6QlKSICCk6ujo0Rkf/sJz/umZdVJQUVPfjzToeqWzyncAxlU56k5rA6ifT3QR7cWobAP7LbwJgQkKCEhISmrVvdna2rFarOnToIElKT0/XU089pYqKCoWEhEiS1qxZo7S0NMXFxbnLrF27Vo899pj7OGvWrFF6enq972O321u1hxGQzSa1a1e9tIRhSCUl9QRJh3TyZO11hYVSVVXtIBkUpKGlFpXaw1UeHqny8Eg5wyNUER4h5/evy8MjVR4RqfKwCJWHR6rSHvr99C0EwMbyl+lu/PXRewC8y28CYGNlZWVp8+bNuvHGGxUVFaWsrCxNmjRJ99xzjzvcjR49WtOnT9f48eM1ZcoU7dy5Uy+88ILmz5/vPs6jjz6qG264QXPnztUtt9yiZcuW6csvv/SYKgYIWBZLdQ9gRITUQK92o1RWavNHO1V+Nl+24iLZSr5fSosVVnBWMTnHZC8pUkhpsezfbw92lmpIaWWt69rKQ+wqCw1XSWi4SkMjVBoWoVJ7ePWfodVLSViEyuzh1X+GhsswDPnPAGnr8ZMOQL998goA7wq4AGi327Vs2TI988wzcjqdSklJ0aRJkzyGZmNiYvTRRx9p4sSJ6t+/v9q3b6+nn37aPQWMJF133XV644039Ktf/Ur/8z//o8suu0yrVq1Sr169fNEswH8FB8uIjVVhSHiTdttxvEDF5z5BxDAUUlmusNJihZWVKLysWGGlxQp1lii8tFgJ3+UorKx6W1hZscJKq8vE2a3Vw6PnhEnDYlVFWLi757GmJ7Ii7PteyYhIlYdHVPdIRny/LTTc6xe4Nefu6PoE+8mEh/aQui8DAHBxsRiGn9x6FoAcDodiYmJUUFCg6OhoX1cHaDVvf3FUx/NLm7TP3txCnS0pb/F7X5Ucq7DzQ4nLJVtpyQ+9kSVFspUUu/9uLymq7q0sLXZvCykrqd73vI88V7BNzoiIH4aywyM9gmP1EuEe+i4Pj5QrpHqKqS2Hz3rtCR7tIm26rEOUV47VEqkJEbq9b2dfVwNoVXx/B2APIIC2F2Fv+keF3UuPSatzgmSrtTqgRdQx1UwTBZWXK6S0SPaS4lpBMvJUrnu42x0ui4tlrawOtoNKKtzHMSQ57WHuoezSsHD38HaJe53nkHeZPUzG9z1//nLtXWQzzjWAwMNvOoALigxt+keFt56TW17lUoRXjlS3KptNVbZ4lcXEN2k/Z0WVth3Nd7+2uFyylZdVD21/P4Qd5iz5fsi7WPEFpxX6/fB2+PfrQp2lsnzfIxkeEiR7iFWyWOSyBp3X++jZQ+mMOGe4+/sh76qQuqcDaqrmhH0AgYffdAAXFGlv+nVh3uoB9Ndn05afN/RrWK1yhobLGRqus2r6jAaXdohU+8jqWQaslRUKKSmuHsqu6YEsru6dDD97WrHHD3sMf9tLihRU7qzzuJX2sDpC4zlD3N/fuV3zOs7eoek/DAABhwAI4IKiQkOavI83ewD9UVmFd+t17nWOruAQOaNj5YyObdlBDUPBzrJ6r5MMzz/jsc5WUqSUhYZkqXvYvcH5JOuaXzIsjJmlAT9FAARwQXHhtgsXOk+YLUjemAmwtMI/nwjirPRevawWi8JtrXD3rcWiytAwVYaGqST+wr2SaYlR6tm7U90bKyurn2Zz7tyRNcvp09K333qucziq56I8n2FUB8OmBMmoqOrHJwLwGgIggAuKCw+R1WKRqwmTBgRbrQq3B3tOBdMMJeX+GQC9Wa9wW5AsftBTNqBbXP0bg4Ol2NjqpSUMQ3I6a09EXrMcOVI7SBYWShUVtY9lsfwQGBsbJCMiqnszAZMjAAK4oOAgq2LDQ3SmuGnTukSHtjwAlpZXqbzS5bUhZW8pKmtZu87lD3fe2oKt7msQW5XFIoWGVi8dWni9octVf69kQYF07FjtIFlUVPeDrG22hoNjXa95MhQCmO8/dQAEhKTYsCYHwIQou3IKylr83mdLytUxOrTFx/GWkvJKr16bGB3m++HNTjGhfvM84kY797rElnI6634Ot8MhnTgh7dlTu9fSWfeNN4qMbFqQjIys9zncQGshAAJolOT4MO08XtCkfcJtwWoXYdN3TQyONYKtFoXZgvzuicLevAHEarEoxg8CYNf4pj3p5aJjt1cv7du37DiGIRUX190rWVgo5ebWXldYWN2beb7g4KYFyejo6p5VP7icAP6PAAigUVLbR8oWbFV5E6dlSWkfofIqlwobGDINCbIqLCRIYbYgjz/9bdi3RnyETQO6xclRVqnCsgo5SitVXN68IeGYsBAF+UHPW/f2rTnboolYLNU9epEtn6RcFRV190oWFkonT0r799fulSyt54k94eGNv06y5s9gIsLFjLMLoFFswVb16hyjrYfPNmm/4CCrenaK1pnichWUVV/IH2K1yh5sVagtSOEhQQr2k6dgNEVwkFXxETbFR1TfIV1Z5VKhs1KO0go5yipV4qxsVM9lUqzvh7ajQoPb5vo/NE1IiBQfX720hGFUB8O6hrcLC6WDB+u+KaeqjhudrNamBcno6OrwSa+k3yEAAmi0f+sep2/yChvszauLxWJRu0i72n0fMk4VlqmiylBRWWWTb6aICQ9RhM3/PrqCg6yKC7e5p8ypdFX3ejpKK1RYVqniOgJhQpT9gnMs5peUq+z7XtfGfoXWlAu3BzfqBpNLO3ihtwr+y2KpDmHh4VLHji07VlXVDzfenB8kz5yRDh2qHSaLi+s+lt3etCAZFVV9sw68wv8+RQH4rXBbsH7aN0nvbDmushbMz5frcDb77uBUa4RfBsDzBVs9A2GVy6geLv5+2NgWZFVKuwsPu54sdDb55psanWPDLhgAQ4Is6t/Q9C/AuYKCpJiY6qUlaqYDqmt4+/zpgM4tU990QJGRTZuovK5rLk3G/z9FAfiVDlGhuvuaZH30dZ6On63neiPUEmS1KDbcpthmTKrdmgZfntCsJ70ALXLudEAJTX90ogeX64cbb+oKkzXTAZ0bJM827VKWixEBEECTxYbbNGpAso6eKdHe3EIdPlMiR2kd/zOH37JaLLohLUF9usT6uipAy9RclxgVJXXu3Lh9HI6W92IGOAIggGZLjg9X8vfThxSWVSjP4VSeo0y5BWXKKyyT08vPy0XLWS0WXdIhQump7dzXZAIwHwIgAK+ICg1RVGiI+4YCwzCUX1KhXEeZch1lyiso06lCpypd/jar38Ut2GpRTHiIOsWEKSk2VN3bRSjCD548AsC3+BQA0CosFoviImyKi7Dpik7VT2qochk6W1Kuo2dKVFxeJeMCzxauvdlQVGiIwi9wE4jRhKmjm/B440aVPfxdsfbkFkqSEmNC1adL/cNMjTnegO5xKvv+ucM1xc/fzWqxyGqpvs4w2GpRkNWi0OAgJcaGqlNM2IXfBIDpEAABtJkgq0XtI+0X7Zxz5ZUubTxwWlL1831v7dOJGywA+KXAm30VAPzUF4fOqLCsUhH2IN1xdWfCHwC/RQ8gAHjJ1V3j1CkmVEmxYQoNCfJ1dQCgXgRAAPCSMFuQUhN4qgYA/8cQMAAAgMkQAAEAAEyGAAgAAGAyBEAAAACTIQACAACYDAEQAADAZAiAAAAAJkMABAAAMBkCIAAAgMkQAAEAAEyGAAgAAGAyBEAAAACTIQACAACYDAEQAADAZAiAAAAAJkMABAAAMBkCIAAAgMkQAAEAAEwmIANg9+7dZbFYPJaZM2d6lNm+fbsGDRqk0NBQJScna/bs2R7b//SnP2nQoEGKi4tTXFychg4dqs8//7wtmwEAAOATARkAJenZZ59VTk6Oe3n44Yfd2xwOh4YNG6Zu3bppy5YtmjNnjp555hm99tpr7jLr16/X3XffrY8//lhZWVlKTk7WsGHDdPz4cV80BwAAoM0E+7oCzRUVFaXExMQ6ty1dulTl5eVauHChbDabrrzySmVnZ2vevHm6//773WXO9b//+7965513tHbtWt17772tXn8AAABfCdgewJkzZ6pdu3bq16+f5syZo8rKSve2rKwsDR48WDabzb1u+PDh2rt3r86ePVvn8UpKSlRRUaH4+Ph639PpdMrhcHgsAAAAgSYgewAfeeQRXX311YqPj9fGjRuVmZmpnJwczZs3T5KUm5urlJQUj306duzo3hYXF1frmFOmTFFSUpKGDh1a7/vOmDFD06dP92JLAAAA2p7f9ABOnTq11o0d5y979uyRJE2ePFlDhgxRnz59lJGRoblz52rBggVyOp3Neu+ZM2dq2bJlevfddxUaGlpvuczMTBUUFLiXo0ePNuv9AAAAfMlvegAff/xxjRs3rsEyqampda4fOHCgKisrdejQIaWlpSkxMVF5eXkeZWpen3/d4HPPPaeZM2fqn//8p/r06dPg+9vtdtnt9gu0BAAAwL/5TQBMSEhQQkJCs/bNzs6W1WpVhw4dJEnp6el66qmnVFFRoZCQEEnSmjVrlJaW5jH8O3v2bP3ud7/Thx9+qAEDBrS8EQAAAAHAb4aAGysrK0vPP/+8vvrqKx08eFBLly7VpEmTdM8997jD3ejRo2Wz2TR+/Hjt2rVLb731ll544QVNnjzZfZxZs2bp17/+tRYuXKju3bsrNzdXubm5Kioq8lXTAAAA2oTFMAzD15Voiq1bt+rBBx/Unj175HQ6lZKSorFjx2ry5Mkew7Pbt2/XxIkT9cUXX6h9+/Z6+OGHNWXKFPf27t276/Dhw7WOP23aND3zzDONqovD4VBMTIwKCgoUHR3d4rYBAIDWx/d3AAZAf8I/IAAAAg/f3wE4BAwAAICWIQACAACYDAEQAADAZAiAAAAAJkMABAAAMBkCIAAAgMkQAAEAAEyGAAgAAGAyBEAAAACTIQACAACYDAEQAADAZAiAAAAAJkMABAAAMBkCIAAAgMkQAAEAAEyGAAgAAGAyBEAAAACTIQACAACYDAEQAADAZAiAAAAAJkMABAAAMBkCIAAAgMkQAAEAAEyGAAgAAGAyBEAAAACTIQACAACYDAEQAADAZAiAAAAAJkMABAAAMBkCIAAAgMkQAAEAAEyGAAgAAGAyBEAAAACTIQACAACYDAEQAADAZAiAAAAAJkMABAAAMBkCIAAAgMkQAAEAAEyGAAgAAGAyARkAu3fvLovF4rHMnDnTo8z27ds1aNAghYaGKjk5WbNnz673eMuWLZPFYtHIkSNbueYAAAC+F+zrCjTXs88+qwkTJrhfR0VFuf/ucDg0bNgwDR06VK+++qp27Nih++67T7Gxsbr//vs9jnPo0CE98cQTGjRoUJvVHQAAwJcCNgBGRUUpMTGxzm1Lly5VeXm5Fi5cKJvNpiuvvFLZ2dmaN2+eRwCsqqrSmDFjNH36dH3yySfKz89vo9oDAAD4TkAOAUvSzJkz1a5dO/Xr109z5sxRZWWle1tWVpYGDx4sm83mXjd8+HDt3btXZ8+eda979tln1aFDB40fP75R7+l0OuVwODwWAACAQBOQPYCPPPKIrr76asXHx2vjxo3KzMxUTk6O5s2bJ0nKzc1VSkqKxz4dO3Z0b4uLi9Onn36q119/XdnZ2Y1+3xkzZmj69OleawcAAIAv+E0P4NSpU2vd2HH+smfPHknS5MmTNWTIEPXp00cZGRmaO3euFixYIKfT2aj3Kiws1NixY/WnP/1J7du3b3QdMzMzVVBQ4F6OHj3arLYCAAD4kt/0AD7++OMaN25cg2VSU1PrXD9w4EBVVlbq0KFDSktLU2JiovLy8jzK1LxOTEzUgQMHdOjQId12223u7S6XS5IUHBysvXv36pJLLqn1Pna7XXa7vSnNAgAA8Dt+EwATEhKUkJDQrH2zs7NltVrVoUMHSVJ6erqeeuopVVRUKCQkRJK0Zs0apaWlKS4uTmFhYdqxY4fHMX71q1+psLBQL7zwgpKTk1vWGAAAAD/mNwGwsbKysrR582bdeOONioqKUlZWliZNmqR77rlHcXFxkqTRo0dr+vTpGj9+vKZMmaKdO3fqhRde0Pz58yVJoaGh6tWrl8dxY2NjJanWegAAgItNwAVAu92uZcuW6ZlnnpHT6VRKSoomTZqkyZMnu8vExMToo48+0sSJE9W/f3+1b99eTz/9dK05AAEAAMzIYhiG4etKBCqHw6GYmBgVFBQoOjra19UBAACNwPe3H90FDAAAgLZBAAQAADAZAiAAAIDJEAABAABMhgAIAABgMgRAAAAAkyEAAgAAmAwBEAAAwGQIgAAAACZDAAQAADAZAiAAAIDJEAABAABMhgAIAABgMgRAAAAAkyEAAgAAmAwBEAAAwGQIgAAAACZDAAQAADAZAiAAAIDJEAABAABMhgAIAABgMgRAAAAAkyEAAgAAmAwBEAAAwGQIgAAAACZDAAQAADAZAiAAAIDJEAABAABMhgAIAABgMgRAAAAAkyEAAgAAmAwBEAAAwGQIgAAAACZDAAQAADAZAiAAAIDJEAABAABMhgAIAABgMgRAAAAAkyEAAgAAmExABsDu3bvLYrF4LDNnzvQos337dg0aNEihoaFKTk7W7Nmzax0nPz9fEydOVKdOnWS323X55Zfr/fffb6tmAAAA+ESwryvQXM8++6wmTJjgfh0VFeX+u8Ph0LBhwzR06FC9+uqr2rFjh+677z7Fxsbq/vvvlySVl5frxz/+sTp06KAVK1aoc+fOOnz4sGJjY9u6KQAAAG0qYANgVFSUEhMT69y2dOlSlZeXa+HChbLZbLryyiuVnZ2tefPmuQPgwoULdebMGW3cuFEhISGSqnsWAQAALnYBOQQsSTNnzlS7du3Ur18/zZkzR5WVle5tWVlZGjx4sGw2m3vd8OHDtXfvXp09e1aS9Le//U3p6emaOHGiOnbsqF69eun3v/+9qqqq2rwtAAAAbSkgewAfeeQRXX311YqPj9fGjRuVmZmpnJwczZs3T5KUm5urlJQUj306duzo3hYXF6eDBw9q3bp1GjNmjN5//33t379fDz74oCoqKjRt2rQ639fpdMrpdLpfOxyOVmohAABA6/GbHsCpU6fWurHj/GXPnj2SpMmTJ2vIkCHq06ePMjIyNHfuXC1YsMAjnF2Iy+VShw4d9Nprr6l///6688479dRTT+nVV1+td58ZM2YoJibGvSQnJ7e43QAAAG3Nb3oAH3/8cY0bN67BMqmpqXWuHzhwoCorK3Xo0CGlpaUpMTFReXl5HmVqXtdcN9ipUyeFhIQoKCjIXeaKK65Qbm6uysvLPYaPa2RmZmry5Mnu1w6HgxAIAAACjt8EwISEBCUkJDRr3+zsbFmtVnXo0EGSlJ6erqeeekoVFRXuGzzWrFmjtLQ0xcXFSZKuv/56vfHGG3K5XLJaqztC9+3bp06dOtUZ/iTJbrfLbrc3q44AAAD+wm+GgBsrKytLzz//vL766isdPHhQS5cu1aRJk3TPPfe4w93o0aNls9k0fvx47dq1S2+99ZZeeOEFj967X/ziFzpz5oweffRR7du3T//4xz/0+9//XhMnTvRV0wAAANqExTAMw9eVaIqtW7fqwQcf1J49e+R0OpWSkqKxY8dq8uTJHr1z27dv18SJE/XFF1+offv2evjhhzVlyhSPY2VlZWnSpEnKzs5W586dNX78eE2ZMsVjWLghDodDMTExKigoUHR0tFfbCQAAWgff3wEYAP0J/4AAAAg8fH8H4BAwAAAAWoYACAAAYDIEQAAAAJMhAAIAAJiM38wDGIhq7p/hkXAAAASOmu9tM98HSwBsgcLCQkniaSAAAASgwsJCxcTE+LoaPsE0MC3gcrl04sQJRUVFyWKxSPrh8XBHjx415a3lZm4/baftZmu7ZO720/bAbbthGCosLFRSUpL7aWBmQw9gC1itVnXp0qXObdHR0QH5S+EtZm4/baftZmTm9tP2wGy7WXv+apgz9gIAAJgYARAAAMBkCIBeZrfbNW3aNI/nEpuJmdtP22m7GZm5/bTdnG2/WHATCAAAgMnQAwgAAGAyBEAAAACTIQACAACYDAEQAADAZAiAXtC9e3dZLBaPZebMmQ3uU1ZWpokTJ6pdu3aKjIzUf/7nfyovL6+Nauwdhw4d0vjx45WSkqKwsDBdcsklmjZtmsrLyxvcb8iQIbV+XhkZGW1U65Z5+eWX1b17d4WGhmrgwIH6/PPPGyy/fPly9ejRQ6Ghoerdu7fef//9Nqqp98yYMUP/9m//pqioKHXo0EEjR47U3r17G9znz3/+c61zHBoa2kY19p5nnnmmVjt69OjR4D4XwzmvUddnm8Vi0cSJE+ssH8jn/V//+pduu+02JSUlyWKxaNWqVR7bDcPQ008/rU6dOiksLExDhw7VN998c8HjNvUzwxcaantFRYWmTJmi3r17KyIiQklJSbr33nt14sSJBo/ZnN8dtC0CoJc8++yzysnJcS8PP/xwg+UnTZqk9957T8uXL9eGDRt04sQJ3XHHHW1UW+/Ys2ePXC6X/vjHP2rXrl2aP3++Xn31Vf3P//zPBfedMGGCx89r9uzZbVDjlnnrrbc0efJkTZs2TVu3btVVV12l4cOH6+TJk3WW37hxo+6++26NHz9e27Zt08iRIzVy5Ejt3LmzjWveMhs2bNDEiRO1adMmrVmzRhUVFRo2bJiKi4sb3C86OtrjHB8+fLiNauxdV155pUc7Pv3003rLXiznvMYXX3zh0fY1a9ZIkv77v/+73n0C9bwXFxfrqquu0ssvv1zn9tmzZ+vFF1/Uq6++qs2bNysiIkLDhw9XWVlZvcds6meGrzTU9pKSEm3dulW//vWvtXXrVq1cuVJ79+7VT3/60wsetym/O/ABAy3WrVs3Y/78+Y0un5+fb4SEhBjLly93r9u9e7chycjKymqFGrad2bNnGykpKQ2WueGGG4xHH320bSrkRddcc40xceJE9+uqqiojKSnJmDFjRp3lR40aZdxyyy0e6wYOHGg88MADrVrP1nby5ElDkrFhw4Z6yyxatMiIiYlpu0q1kmnTphlXXXVVo8tfrOe8xqOPPmpccsklhsvlqnP7xXLeJRnvvvuu+7XL5TISExONOXPmuNfl5+cbdrvdePPNN+s9TlM/M/zB+W2vy+eff25IMg4fPlxvmab+7qDt0QPoJTNnzlS7du3Ur18/zZkzR5WVlfWW3bJliyoqKjR06FD3uh49eqhr167Kyspqi+q2moKCAsXHx1+w3NKlS9W+fXv16tVLmZmZKikpaYPaNV95ebm2bNnicc6sVquGDh1a7znLysryKC9Jw4cPvyjOsaQLnueioiJ169ZNycnJuv3227Vr1662qJ7XffPNN0pKSlJqaqrGjBmjI0eO1Fv2Yj3nUvXvwF/+8hfdd999slgs9Za7WM77ub799lvl5uZ6nNuYmBgNHDiw3nPbnM+MQFFQUCCLxaLY2NgGyzXldwdtL9jXFbgYPPLII7r66qsVHx+vjRs3KjMzUzk5OZo3b16d5XNzc2Wz2Wr98nTs2FG5ubltUOPWsX//fi1YsEDPPfdcg+VGjx6tbt26KSkpSdu3b9eUKVO0d+9erVy5so1q2nSnT59WVVWVOnbs6LG+Y8eO2rNnT5375Obm1lk+kM+xy+XSY489puuvv169evWqt1xaWpoWLlyoPn36qKCgQM8995yuu+467dq1S126dGnDGrfMwIED9ec//1lpaWnKycnR9OnTNWjQIO3cuVNRUVG1yl+M57zGqlWrlJ+fr3HjxtVb5mI57+erOX9NObfN+cwIBGVlZZoyZYruvvtuRUdH11uuqb87aHsEwHpMnTpVs2bNarDM7t271aNHD02ePNm9rk+fPrLZbHrggQc0Y8aMgHxMTlPaXuP48eP6yU9+ov/+7//WhAkTGtz3/vvvd/+9d+/e6tSpk370ox/pwIEDuuSSS1pWebSqiRMnaufOnRe8lic9PV3p6enu19ddd52uuOIK/fGPf9RvfvOb1q6m19x8883uv/fp00cDBw5Ut27d9Pbbb2v8+PE+rFnbe/3113XzzTcrKSmp3jIXy3lH3SoqKjRq1CgZhqE//OEPDZbld8f/EQDr8fjjjzf4P11JSk1NrXP9wIEDVVlZqUOHDiktLa3W9sTERJWXlys/P9+jFzAvL0+JiYktqbZXNLXtJ06c0I033qjrrrtOr732WpPfb+DAgZKqexD9NQC2b99eQUFBte7UbuicJSYmNqm8v3vooYf097//Xf/617+a3JsTEhKifv36af/+/a1Uu7YRGxuryy+/vN52XGznvMbhw4f1z3/+s8m99BfLea85f3l5eerUqZN7fV5envr27VvnPs35zPBnNeHv8OHDWrduXYO9f3W50O8O2h7XANYjISFBPXr0aHCx2Wx17pudnS2r1aoOHTrUub1///4KCQnR2rVr3ev27t2rI0eOePzv2Vea0vbjx49ryJAh6t+/vxYtWiSrten/pLKzsyXJ44PV39hsNvXv39/jnLlcLq1du7bec5aenu5RXpLWrFnjF+e4KQzD0EMPPaR3331X69atU0pKSpOPUVVVpR07dvj1OW6MoqIiHThwoN52XCzn/HyLFi1Shw4ddMsttzRpv4vlvKekpCgxMdHj3DocDm3evLnec9uczwx/VRP+vvnmG/3zn/9Uu3btmnyMC/3uwAd8fRdKoNu4caMxf/58Izs72zhw4IDxl7/8xUhISDDuvfded5ljx44ZaWlpxubNm93rMjIyjK5duxrr1q0zvvzySyM9Pd1IT0/3RROa7dixY8all15q/OhHPzKOHTtm5OTkuJdzy5zb9v379xvPPvus8eWXXxrffvut8de//tVITU01Bg8e7KtmNNqyZcsMu91u/PnPfza+/vpr4/777zdiY2ON3NxcwzAMY+zYscbUqVPd5T/77DMjODjYeO6554zdu3cb06ZNM0JCQowdO3b4qgnN8otf/MKIiYkx1q9f73GOS0pK3GXOb/v06dONDz/80Dhw4ICxZcsW46677jJCQ0ONXbt2+aIJzfb4448b69evN7799lvjs88+M4YOHWq0b9/eOHnypGEYF+85P1dVVZXRtWtXY8qUKbW2XUznvbCw0Ni2bZuxbds2Q5Ixb948Y9u2be47XWfOnGnExsYaf/3rX43t27cbt99+u5GSkmKUlpa6j3HTTTcZCxYscL++0GeGv2io7eXl5cZPf/pTo0uXLkZ2drbHZ4DT6XQf4/y2X+h3B75HAGyhLVu2GAMHDjRiYmKM0NBQ44orrjB+//vfG2VlZe4y3377rSHJ+Pjjj93rSktLjQcffNCIi4szwsPDjf/4j//wCE6BYNGiRYakOpca57f9yJEjxuDBg434+HjDbrcbl156qfHkk08aBQUFPmpF0yxYsMDo2rWrYbPZjGuuucbYtGmTe9sNN9xg/OxnP/Mo//bbbxuXX365YbPZjCuvvNL4xz/+0cY1brn6zvGiRYvcZc5v+2OPPeb+OXXs2NEYMWKEsXXr1ravfAvdeeedRqdOnQybzWZ07tzZuPPOO439+/e7t1+s5/xcH374oSHJ2Lt3b61tF9N5//jjj+v8d17TPpfLZfz61782OnbsaNjtduNHP/pRrZ9Jt27djGnTpnmsa+gzw1801Paaz/C6lnO/085v+4V+d+B7FsMwjFbvZgQAAIDf4BpAAAAAkyEAAgAAmAwBEAAAwGQIgAAAACZDAAQAADAZAiAAAIDJEAABAABMhgAIAABgMgRAAAAAkyEAAgAAmAwBEAAAwGQIgAAAACZDAAQAADAZAiAAAIDJEAABAABMhgAIAABgMgRAAAAAkyEAAgAAmAwBEAAAwGQIgAAAACZDAAQAADAZAiAAAIDJEAABAABMhgAIAABgMgRAAAAAkyEAAgAAmAwBEAAAwGQIgAAAACZDAAQAADAZAiAAAIDJEAABAABMhgAIAABgMgRAAAAAkwn25sFcLpdOnDihqKgoWSwWbx66TRmGocLCQiUlJclqrc7ItM3/ma1tAAA0l1cD4IkTJ5ScnOzNQ/rU0aNH1aVLF0m0LZCYpW0AADSXVwNgVFSUJGnDhg2KjIz05qHbVFFRkW644QZ3eyTaFgjM1jYAAJrLqwGwZogtMjIyoL9sa5w7ZEjbAodZ2naxMAxDFRUVqqqqUlBQkEJCQi7KdgKAP/FqAMSFvfLKK3rhhRc81q1du7bOYb0f//jHOnLkiPv1NddcoyVLlrR6HYG2UFFRoby8PB0/flylpaXu9WFhYercubM6duyokJAQH9YQAC5eXE0OwOv+9a9/6bbbblNSUpIsFotWrVrlsf3MmTPatGmTDhw4oMjISPXs2VN9+vRRz549FRkZqQMHDmjTpk06c+aMbxoAoM384Q9/UJ8+fRQdHa3o6Gilp6dr9erV7u0HDhzQf/zHfyghIUHR0dEaNWqU8vLyPI6xdetW/fjHP1ZsbKzatWun+++/X0VFRW3dlIBCDyB8IicnR1FRUdq6dau6dOmi1NRUX1ep0Xbu3KmysjKlpKTIYrEoPj7e11XyO8XFxbrqqqt033336Y477vDYdubMGe3YsUNxcXFKS0uT3W732N6hQwc5nU7t3btX27dvV3R0tGw2W1tWH0Ar6NWrV53ru3TpopkzZ+qyyy6TYRhavHixbr/9dm3btk3du3fXsGHDdNVVV2ndunWSpF//+te67bbbtGnTJlmtVp04cUJDhw7VnXfeqZdeekkOh0OPPfaYxo0bpxUrVrRlEwOK3wfAsWPH6vPPP79guZSUFH3wwQdtUCM0l2EYysrK0htvvKF169bpvffe05/+9CeNGDEioALgBx98oNOnT2vEiBGaOHGiRowYoTFjxqhPnz6+rprfuPnmm3XzzTfXWl9RUaFdu3YpLi5OvXr1qndKG7vdrl69emnnzp06e/as4uLimP4GuEjddtttHq9/97vf6Q9/+IM2bdqk48eP69ChQ9q2bZuio6MlSYsXL1ZcXJzWrVunoUOH6u9//7tCQkL08ssvuz8nXn31VfXp00f79+/XpZde2uZtCgR8oqLVFRYW6v/+7/908803a+LEiYqPj1dVVZWvq+UV4eHhqqio0OjRo/Wf//mfWrlypZxOp6+r5bfy8vLkcrmUlpZ2wUBntVqVlpYmwzD4mQImUVVVpWXLlqm4uFjp6elyOp2yWCweIwWhoaGyWq369NNPJUlOp1M2m83jMyUsLEyS3GVQGwEQrWbPnj16+umnNXjwYC1btkxjxozRJ598omeffdbXVfOaiIgIzZs3Txs2bNDQoUO1YMECDR48WLNmzdLRo0d9XT2/YhiGjh8/rvbt29ca9q2P3W5XQkKCysrKZBhGK9cQgK/s2LFDkZGRstvtysjI0LvvvquePXvq2muvVUREhKZMmaKSkhIVFxfriSeeUFVVlXJyciRJN910k3JzczVnzhyVl5fr7Nmzmjp1qiS5y6A2AiBaxeuvv67bb79dR44c0SuvvKL3339fY8eOvSimYqlLu3bt9Itf/EL//Oc/9bvf/U6bN2/WsGHD9P777/u6an6joqJCpaWlSkhIaNJ+CQkJqqqqIgACF7G0tDRlZ2dr8+bN+sUvfqGf/exn+vrrr5WQkKDly5frvffeU2RkpGJiYpSfn6+rr77a3eN35ZVXavHixZo7d67Cw8OVmJiolJQUdezYkUtHGuD31wAiMPXs2VM9e/bU1q1btWrVKoWHh+uqq67ydbValWEY2rx5s1atWqW9e/eqf//+AXVtY2urGfYPDm7ax05NeQIgcPGy2Wzua/X69++vL774Qi+88IL++Mc/atiwYTpw4IBOnz6t4OBgxcbGKjEx0ePzdfTo0Ro9erTy8vIUEREhi8WiefPm8RncAAIgWkV6erreffddbdu2TUuXLtWYMWN0+eWXa/To0br11lt9XT2vcjgcWrlypd58802dPHlSt99+u1atWqXLLrvM11XzK0FBQZKkysrKJu1XU57JoQHzcLlcta79bd++vSRp3bp1OnnypH7605/W2q9jx46SpIULFyo0NFQ//vGPW7+yAYoAiFbVr18/9evXT5mZmVq+fLlefvllzZo1y9fV8pqTJ09q8ODBSkpK0tixYzVy5MiLdpi7KYqKirR//37362+//Va7du1SQUGBTp06pQ4dOjT6WKdOnVJQUBABELhIZWZm6uabb1bXrl1VWFioN954Q+vXr9eHH34oSVq0aJGuuOIKJSQkKCsrS48++qgmTZqktLQ09zFeeuklXXfddYqMjNSaNWv05JNPaubMmYqNjfVRq/wfARBtol27dsrIyNCECRP08ccf64033gj4azNCQkJ04403avTo0UpPT/d1dfzKl19+qRtvvNH9evLkyZKk//qv/9JDDz0kp9PZqBtBnE6nTp065R7SAXDxOXnypO69917l5OQoJiZGffr00Ycffujuvdu7d68yMzN15swZde/eXU899ZQmTZrkcYzPP/9c06ZNU1FRkXr06KE//vGPGjt2rC+aEzAIgGhTQUFBGjp0qIYOHSpJAflouyeeeML9d4Jf3YYMGVLnNXsVFRXatGmT9u7d2+A8gFL1ENDevXtrTQEB4OLy+uuvN7h95syZmjlzZoNl/u///s+bVTIFAiCANhMSEqIrr7xSO3bs0M6dO+t8Eogk95NAzp49q969e/O0FQDwMr8PgL17927UY6A6derUBrUB0FLx8fHq3bu3du3apU2bNql9+/ZKSEhQcHCwKisrderUKZ0+fVpWq5XwBwCtxO8D4C9/+UtfV8GrHnzwQT344IONKrtmzZpWrg3gG/Hx8br22muVl5en48eP69SpU+5tYWFhuuSSS5SYmNjkKWMAAI3DpysAnwgJCVGXLl3UuXNnVVZWqrKyUsHBwQoODuaGDwBoZV4NgDUXfRcVFXnzsG2upv7nXsRO2/yf2dp2sbBYLAoJCVFISIivqwIApuHVAFhYWChJuuGGG7x5WJ8pLCxUTEyM++8SbQsEZmkbAADNZTG82KXgcrl04sQJRUVFBfQQjmEYKiwsVFJSknuaCtrm/8zWNgAAmsurARAAAAD+j64EAAAAkyEAAgDgBePGjZPFYlFGRkatbRMnTpTFYtG4cePavmJAHZgGBgAAL0lOTtayZcs0f/58hYWFSZLKysr0xhtvqGvXru5yvr4+ua7risvLyxv14AVcHAiAAAB4ydVXX60DBw5o5cqVGjNmjCRp5cqV6tq1q1JSUtzljh07pm7duvmqmm4/+9nPFBsbq7/85S/q3bu3Pv74Y+3cuVNPPvmkPvnkE0VERGjYsGGaP3++2rdv7+vqwosYAgYAwIvuu+8+LVq0yP164cKF+vnPf+5R5k9/+pMkac6cOfrrX/+qadOmyWaz6bXXXtOWLVu0adMmTZgwQUuWLNHf/vY3/eY3v1FoaKhmzJihLVu2aPPmzYqMjNTYsWO1atUqrVixQs8884z+/ve/a8uWLbr11lt1ww03aMuWLe7l7rvvVv/+/bVlyxZt2LBBkvTOO+/IZrPps88+06uvvqr8/HzddNNN6tevn7788kt98MEHysvL06hRo9ruB4g2QQ8gAABedM899ygzM1OHDx+WJH322WdatmyZ1q9fL0lyOp2aN2+eJOmmm25SZGSkevTooV27dulvf/ube97SJ554wn3MtLQ07dmzR+vXr9cdd9yh/Px8FRUVadiwYbriiiskSb1793aXDwkJUXBwsCIjIz3WBQUFeaxLTU3V7Nmz3a9/+9vfql+/fvr973/vXrdw4UIlJydr3759uvzyy731Y4KPEQABAPCihIQE3XLLLfrzn/8swzB0yy23eAyf7t+/XyUlJZKkf//3f3dfA1hRUeEOc5K0dOlSvfPOOzpx4oScTqcqKirUo0cPSVJsbKzuuOMOjR8/Xtdff73S09N18803q0OHDk2qa9++fT1ef/XVV/r44489QmKNAwcOEAAvIgRAAAC87L777tNDDz0kSXr55Zc9tp37aMo333xT4eHh7tc1N2H84x//0KxZszRlyhT169dPERERev311/XVV1+5y86YMUNjx47VJ598otWrV+v555/XokWL1LdvX1ksllqPjqysrKxVz4iIiFp1u+222zRr1qxaZTt16tTY5iMAEAABAPCyn/zkJyovL5fFYtHw4cM9tvXs2VN2u11Op1PJycl19rZt3bpV/fr1c99IIklHjhypVa5nz57q2bOnHnjgAd155536+9//rr59+yo+Pl7ffPONR9ndu3df8JnbV199td555x11795dwcFEhIsZN4EAAOBlQUFB2r17t77++msFBQV5bIuKitLDDz8sSXrvvfd05MgR7dq1S0uWLNG7774rSerWrZt27typTz75RN9++62ef/557dixw32Mo0ePau7cudq2bZuOHz+uTz/9VIcOHVJqaqok6dprr9XOnTu1atUqHTp0SC+++GKtQFiXiRMn6syZM7r77rv1xRdf6MCBA/rwww/185//XFVVVd768cAPEAABAGgF0dHRio6OrnPbr371K0nSokWLNGLECP2///f/tH79enXp0kWSdNddd2nYsGGaNGmSRo0apfz8fI0ePdq9f1hYmA4ePKiHH35Yw4cP19NPP60xY8borrvukiQNGjRIDz74oObMmaP/+q//UnFxsUaOHHnBOiclJemzzz5TVVWVhg0bpt69e+uxxx5TbGwszyG/yPAsYAAA2lhBQYFiY2O1YcOGOoeAW1tRUZFuuOEG5efnKyYmps3fH77HAD8AAG2ssLBQktxTvviyHgRAc6IHEACANuaPj4KDuRAAAQAATIbYDwAAYDIEQAAAAJP5/7UcEY82zonkAAAAAElFTkSuQmCC", + "text/html": [ + "\n", + "
\n", + "
\n", + " Figure\n", + "
\n", + " \n", + "
\n", + " " + ], + "text/plain": [ + "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "main()" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": ".venv", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.11" + } }, - { - "cell_type": "code", - "execution_count": null, - "id": "4813563b", - "metadata": {}, - "outputs": [], - "source": [ - "def main():\n", - " print(\"Running simulation...\")\n", - " radius = 500\n", - " vars_0 = [0.0, -radius, 0.0, 0.0, 0.0]\n", - " v_0 = 33.0\n", - "\n", - " curve = ref.generate_reference_curve(\n", - " [0, radius, 0, -radius, 0], [-radius, 0, radius, 0, radius], 10.0\n", - " )\n", - " ti = np.arange(0, 40, 0.1)\n", - "\n", - " # r = 10 # hectic steering behavior\n", - " r = 10000 # fairly calm steering behavior\n", - "\n", - " model = cl.LateralControlRiccati(\n", - " initial_condition=vars_0,\n", - " curve=curve,\n", - " vehicle_params=DEFAULT_VEHICLE_PARAMS,\n", - " initial_velocity=v_0,\n", - " r=r,\n", - " )\n", - "\n", - " sol = model.simulate(ti, v=v_0, t_step=0.1)\n", - " x = sol[:, 0]\n", - " y = sol[:, 1]\n", - " psi = sol[:, 2]\n", - " delta = sol[:, 5]\n", - "\n", - " fig, ax = plt.subplots()\n", - "\n", - " plt.plot(curve[\"x\"], curve[\"y\"], \"r-\", linewidth=0.5)\n", - " plt.plot(x, y, \"b-\")\n", - " plt.axis(\"equal\")\n", - "\n", - " (point1,) = ax.plot([], [], marker=\"o\", color=\"blue\", ms=5)\n", - "\n", - " def update(i, *fargs):\n", - " [l.remove() for l in reversed(ax.lines[1:])]\n", - " ax.plot(x[: i + 1], y[: i + 1], \"b-\", linewidth=0.5)\n", - " point1.set_data(x[i : i + 1], y[i : i + 1])\n", - " pv.plot_vehicle(ax, x[i], y[i], psi[i], delta[i])\n", - " for farg in fargs:\n", - " print(farg)\n", - "\n", - " viz = vz.Vizard(fig, update, ti)\n", - " plt.show()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "e8e63277", - "metadata": {}, - "outputs": [], - "source": [ - "main()" - ] - } - ], - "metadata": { - "kernelspec": { - "display_name": "behavior_generation_lecture", - "language": "python", - "name": "behavior_generation_lecture" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.9.6" - } - }, - "nbformat": 4, - "nbformat_minor": 5 + "nbformat": 4, + "nbformat_minor": 5 } diff --git a/notebooks/lateral_control_state_based_notebook.ipynb b/notebooks/lateral_control_state_based_notebook.ipynb index d760899..c510244 100644 --- a/notebooks/lateral_control_state_based_notebook.ipynb +++ b/notebooks/lateral_control_state_based_notebook.ipynb @@ -1,105 +1,147 @@ { - "cells": [ - { - "cell_type": "code", - "execution_count": null, - "id": "14606e50", - "metadata": {}, - "outputs": [], - "source": [ - "import os\n", - "import numpy as np\n", - "import matplotlib.pyplot as plt\n", - "\n", - "import behavior_generation_lecture_python.lateral_control_state_based.lateral_control_state_based as cl\n", - "import behavior_generation_lecture_python.utils.generate_reference_curve as ref\n", - "from behavior_generation_lecture_python.utils.plot_vehicle import plot_vehicle as pv\n", - "from behavior_generation_lecture_python.utils.vizard import vizard as vz\n", - "\n", - "interactive_widgets = not os.getenv(\"CI\") == \"true\"\n", - "if interactive_widgets:\n", - " # Use widget backend locally, to be able to interact with the plots\n", - " %matplotlib widget\n", - "else:\n", - " # Use inline backend in CI, to render the notebooks for the hosted docs\n", - " %matplotlib inline" - ] + "cells": [ + { + "cell_type": "code", + "execution_count": null, + "id": "14606e50", + "metadata": {}, + "outputs": [], + "source": [ + "import os\n", + "import numpy as np\n", + "import matplotlib.pyplot as plt\n", + "\n", + "import behavior_generation_lecture_python.lateral_control_state_based.lateral_control_state_based as cl\n", + "import behavior_generation_lecture_python.utils.generate_reference_curve as ref\n", + "from behavior_generation_lecture_python.lateral_control_state_based.lateral_control_state_based import (\n", + " KinematicVehicleState,\n", + ")\n", + "from behavior_generation_lecture_python.utils.plot_vehicle import plot_vehicle as pv\n", + "from behavior_generation_lecture_python.utils.vizard import vizard as vz\n", + "\n", + "interactive_widgets = not os.getenv(\"CI\") == \"true\"\n", + "if interactive_widgets:\n", + " # Use widget backend locally, to be able to interact with the plots\n", + " %matplotlib widget\n", + "else:\n", + " # Use inline backend in CI, to render the notebooks for the hosted docs\n", + " %matplotlib inline" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "08eeef08", + "metadata": {}, + "outputs": [], + "source": [ + "def main() -> None:\n", + " print(\"Running simulation...\")\n", + " radius = 20\n", + " initial_state = KinematicVehicleState(\n", + " x=0.1,\n", + " y=float(-radius),\n", + " heading=0.0,\n", + " )\n", + " curve = ref.generate_reference_curve(\n", + " np.array([0, radius, 0, -radius, 0]),\n", + " np.array([-radius, 0, radius, 0, radius]),\n", + " 1.0,\n", + " )\n", + " time_vector = np.arange(0, 100, 0.1)\n", + " model = cl.LateralControlStateBased(initial_state, curve)\n", + " trajectory = model.simulate(time_vector, velocity=1)\n", + "\n", + " # Extract data from ControllerOutput list\n", + " x = np.array([out.x for out in trajectory])\n", + " y = np.array([out.y for out in trajectory])\n", + " psi = np.array([out.heading for out in trajectory])\n", + " delta = np.array([out.steering_angle for out in trajectory])\n", + "\n", + " fig, ax = plt.subplots()\n", + "\n", + " plt.plot(curve.x, curve.y, \"r-\", linewidth=0.5)\n", + " plt.plot(x, y, \"b-\", linewidth=0.5)\n", + " plt.axis(\"equal\")\n", + "\n", + " (point1,) = ax.plot([], [], marker=\"o\", color=\"blue\", ms=5)\n", + "\n", + " def update(i: int, *fargs: object) -> None:\n", + " for line in reversed(ax.lines[1:]):\n", + " line.remove()\n", + " ax.plot(x[:i], y[:i], \"b-\", linewidth=0.5)\n", + " point1.set_data(x[i : i + 1], y[i : i + 1])\n", + " pv.plot_vehicle(ax, x[i], y[i], psi[i], delta[i])\n", + " for farg in fargs:\n", + " print(farg)\n", + "\n", + " _ = vz.Vizard(fig, update, time_vector)\n", + " plt.show()" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "id": "d2c31289", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Running simulation...\n" + ] + }, + { + "data": { + "application/vnd.jupyter.widget-view+json": { + "model_id": "d3ee1cff400d442fa919da03a7be1681", + "version_major": 2, + "version_minor": 0 + }, + "image/png": "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", + "text/html": [ + "\n", + "
\n", + "
\n", + " Figure\n", + "
\n", + " \n", + "
\n", + " " + ], + "text/plain": [ + "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "main()" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": ".venv", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.11" + } }, - { - "cell_type": "code", - "execution_count": null, - "id": "08eeef08", - "metadata": {}, - "outputs": [], - "source": [ - "def main():\n", - " print(\"Running simulation...\")\n", - " radius = 20\n", - " vars_0 = [0.1, -radius, 0.0]\n", - " curve = ref.generate_reference_curve(\n", - " [0, radius, 0, -radius, 0], [-radius, 0, radius, 0, radius], 1.0\n", - " )\n", - " ti = np.arange(0, 100, 0.1)\n", - " model = cl.LateralControlStateBased(vars_0, curve)\n", - " sol = model.simulate(ti, v=1)\n", - " x = sol[:, 0]\n", - " y = sol[:, 1]\n", - " psi = sol[:, 2]\n", - " delta = sol[:, 4]\n", - "\n", - " fig, ax = plt.subplots()\n", - "\n", - " plt.plot(curve[\"x\"], curve[\"y\"], \"r-\", linewidth=0.5)\n", - " plt.plot(x, y, \"b-\", linewidth=0.5)\n", - " plt.axis(\"equal\")\n", - "\n", - " (point1,) = ax.plot([], [], marker=\"o\", color=\"blue\", ms=5)\n", - "\n", - " def update(i, *fargs):\n", - " [l.remove() for l in reversed(ax.lines[1:])]\n", - " ax.plot(x[:i], y[:i], \"b-\", linewidth=0.5)\n", - " point1.set_data(x[i : i + 1], y[i : i + 1])\n", - " pv.plot_vehicle(ax, x[i], y[i], psi[i], delta[i])\n", - " for farg in fargs:\n", - " print(farg)\n", - "\n", - " viz = vz.Vizard(fig, update, ti)\n", - " plt.show()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "d2c31289", - "metadata": {}, - "outputs": [], - "source": [ - "main()" - ] - } - ], - "metadata": { - "interpreter": { - "hash": "ea80bdc8d9eecdfb0ad4850befec70bcf98ec6f56b32ef8090165e65e0e9c093" - }, - "kernelspec": { - "display_name": "behavior_generation_lecture", - "language": "python", - "name": "behavior_generation_lecture" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.9.6" - } - }, - "nbformat": 4, - "nbformat_minor": 5 + "nbformat": 4, + "nbformat_minor": 5 } diff --git a/scripts/run_lateral_control_riccati.py b/scripts/run_lateral_control_riccati.py index a83a9b8..2e62b2a 100644 --- a/scripts/run_lateral_control_riccati.py +++ b/scripts/run_lateral_control_riccati.py @@ -3,6 +3,9 @@ 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 ( @@ -10,50 +13,61 @@ ) -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() diff --git a/scripts/run_lateral_control_state_based.py b/scripts/run_lateral_control_state_based.py index 2966549..c2cd2b8 100644 --- a/scripts/run_lateral_control_state_based.py +++ b/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() diff --git a/src/behavior_generation_lecture_python/lateral_control_riccati/lateral_control_riccati.py b/src/behavior_generation_lecture_python/lateral_control_riccati/lateral_control_riccati.py index 066d1ec..dc92dc3 100644 --- a/src/behavior_generation_lecture_python/lateral_control_riccati/lateral_control_riccati.py +++ b/src/behavior_generation_lecture_python/lateral_control_riccati/lateral_control_riccati.py @@ -1,5 +1,8 @@ +"""Lateral vehicle control using LQR (Linear Quadratic Regulator) with Riccati equation.""" + import math from dataclasses import dataclass +from typing import Any import numpy as np import scipy.linalg @@ -9,6 +12,7 @@ import behavior_generation_lecture_python.lateral_control_riccati.riccati_controller as con import behavior_generation_lecture_python.utils.projection as pro import behavior_generation_lecture_python.vehicle_models.dynamic_one_track_model as dotm +from behavior_generation_lecture_python.utils.reference_curve import ReferenceCurve from behavior_generation_lecture_python.vehicle_models.vehicle_parameters import ( VehicleParameters, ) @@ -16,172 +20,500 @@ @dataclass class ControlParameters: - l_s: float - k_lqr: np.ndarray - k_dist_comp: float + """Parameters for the LQR lateral controller. + + Attributes: + lookahead_distance: Look-ahead distance for the controller [m] + lqr_gain: LQR feedback gain vector [k_lateral, k_heading, k_beta, k_yaw_rate] + disturbance_compensation_gain: Gain for curvature feedforward compensation + """ + + lookahead_distance: float + lqr_gain: np.ndarray[Any, Any] + disturbance_compensation_gain: float + + +@dataclass +class LQRSolution: + """Solution of the continuous-time LQR problem. + + Attributes: + feedback_gain: State feedback gain vector K + riccati_solution: Solution matrix X of the algebraic Riccati equation + closed_loop_eigenvalues: Eigenvalues of the closed-loop system (A - BK) + """ + + feedback_gain: np.ndarray[Any, Any] + riccati_solution: np.ndarray[Any, Any] + closed_loop_eigenvalues: np.ndarray[Any, Any] + + +@dataclass +class ActuatorDynamicsOutput: + """Output of the PT2 actuator dynamics computation. + + Attributes: + steering_angle_derivative: Rate of change of steering angle [rad/s] + steering_rate_derivative: Rate of change of steering rate [rad/s^2] + actual_steering_angle: Current actual steering angle after actuator dynamics [rad] + """ + + steering_angle_derivative: float + steering_rate_derivative: float + actual_steering_angle: float + + +@dataclass +class DynamicVehicleState: + """State of a vehicle using the dynamic one-track model. + + Attributes: + x: X-position in global coordinates [m] + y: Y-position in global coordinates [m] + heading: Vehicle heading angle (yaw) [rad] + sideslip_angle: Sideslip angle (beta) at center of gravity [rad] + yaw_rate: Angular velocity around vertical axis [rad/s] + """ + + x: float + y: float + heading: float + sideslip_angle: float + yaw_rate: float + def to_list(self) -> list[float]: + """Convert to list for use with numerical integrators.""" + return [self.x, self.y, self.heading, self.sideslip_angle, self.yaw_rate] -def get_control_params(vehicle_params: VehicleParameters, velocity: float, r: float): - v_0 = velocity - c_v = vehicle_params.A_v * vehicle_params.B_v * vehicle_params.C_v - c_h = vehicle_params.A_h * vehicle_params.B_h * vehicle_params.C_h +@dataclass +class SimulationState: + """Full simulation state including vehicle dynamics and actuator states. + + Attributes: + x: X-position in global coordinates [m] + y: Y-position in global coordinates [m] + heading: Vehicle heading angle (yaw) [rad] + sideslip_angle: Sideslip angle (beta) at center of gravity [rad] + yaw_rate: Angular velocity around vertical axis [rad/s] + steering_angle: Current steering angle [rad] + steering_rate: Current rate of change of steering angle [rad/s] + """ + + x: float + y: float + heading: float + sideslip_angle: float + yaw_rate: float + steering_angle: float + steering_rate: float + + @classmethod + def from_vehicle_state( + cls, + vehicle_state: DynamicVehicleState, + steering_angle: float = 0.0, + steering_rate: float = 0.0, + ) -> "SimulationState": + """Create simulation state from vehicle state with initial actuator values.""" + return cls( + x=vehicle_state.x, + y=vehicle_state.y, + heading=vehicle_state.heading, + sideslip_angle=vehicle_state.sideslip_angle, + yaw_rate=vehicle_state.yaw_rate, + steering_angle=steering_angle, + steering_rate=steering_rate, + ) + + def to_list(self) -> list[float]: + """Convert to list for use with numerical integrators.""" + return [ + self.x, + self.y, + self.heading, + self.sideslip_angle, + self.yaw_rate, + self.steering_angle, + self.steering_rate, + ] + + +@dataclass +class StateDerivatives: + """Time derivatives of the simulation state. + + Attributes: + x_dot: Velocity in x-direction [m/s] + y_dot: Velocity in y-direction [m/s] + heading_dot: Yaw rate [rad/s] + sideslip_angle_dot: Rate of change of sideslip angle [rad/s] + yaw_rate_dot: Angular acceleration [rad/s^2] + steering_angle_dot: Rate of change of steering angle [rad/s] + steering_rate_dot: Steering acceleration [rad/s^2] + """ + + x_dot: float + y_dot: float + heading_dot: float + sideslip_angle_dot: float + yaw_rate_dot: float + steering_angle_dot: float + steering_rate_dot: float + + def to_tuple(self) -> tuple[float, float, float, float, float, float, float]: + """Convert to tuple for use with numerical integrators like odeint.""" + return ( + self.x_dot, + self.y_dot, + self.heading_dot, + self.sideslip_angle_dot, + self.yaw_rate_dot, + self.steering_angle_dot, + self.steering_rate_dot, + ) + + +def get_control_params( + vehicle_params: VehicleParameters, velocity: float, control_weight: float +) -> ControlParameters: + """Compute LQR control parameters for the given vehicle and velocity. + + This function linearizes the vehicle dynamics at the given velocity and + solves the LQR problem to obtain optimal feedback gains. + + Args: + vehicle_params: Physical parameters of the vehicle + velocity: Longitudinal velocity for linearization [m/s] + control_weight: Weight on control effort in LQR cost (higher = less aggressive) + + Returns: + ControlParameters containing LQR gains and disturbance compensation. + """ + # Compute cornering stiffness from tire parameters (Pacejka magic formula coefficients) + cornering_stiffness_front = ( + vehicle_params.A_v * vehicle_params.B_v * vehicle_params.C_v + ) + cornering_stiffness_rear = ( + vehicle_params.A_h * vehicle_params.B_h * vehicle_params.C_h + ) - a11 = -(c_h + c_v) / (vehicle_params.m * v_0) - a12 = -1 + (c_h * vehicle_params.l_h - c_v * vehicle_params.l_v) / ( - vehicle_params.m * np.power(v_0, 2) + # Linearized lateral dynamics matrix elements + a11 = -(cornering_stiffness_rear + cornering_stiffness_front) / ( + vehicle_params.m * velocity ) - a21 = (c_h * vehicle_params.l_h - c_v * vehicle_params.l_v) / vehicle_params.J + a12 = -1 + ( + cornering_stiffness_rear * vehicle_params.l_h + - cornering_stiffness_front * vehicle_params.l_v + ) / (vehicle_params.m * np.power(velocity, 2)) + a21 = ( + cornering_stiffness_rear * vehicle_params.l_h + - cornering_stiffness_front * vehicle_params.l_v + ) / vehicle_params.J a22 = -( - c_v * np.power(vehicle_params.l_v, 2) + c_h * np.power(vehicle_params.l_h, 2) - ) / (vehicle_params.J * v_0) + cornering_stiffness_front * np.power(vehicle_params.l_v, 2) + + cornering_stiffness_rear * np.power(vehicle_params.l_h, 2) + ) / (vehicle_params.J * velocity) - A_lOTM = np.array([[a11, a12], [a21, a22]]) - b_lOTM = np.array( + A_lateral_dynamics = np.array([[a11, a12], [a21, a22]]) + B_lateral_dynamics = np.array( [ - c_v / (vehicle_params.m * v_0), - c_v * vehicle_params.l_v / vehicle_params.J, + cornering_stiffness_front / (vehicle_params.m * velocity), + cornering_stiffness_front * vehicle_params.l_v / vehicle_params.J, ] ) - A = np.array( + # Augmented system matrix for error dynamics + # State: [lateral_error, heading_error, beta, yaw_rate] + A_augmented = np.array( [ - [0, v_0, v_0, vehicle_params.l_s], + [0, velocity, velocity, vehicle_params.l_s], [0, 0, 0, 1], - [0, 0, A_lOTM[0, 0], A_lOTM[0, 1]], - [0, 0, A_lOTM[1, 0], A_lOTM[1, 1]], + [0, 0, A_lateral_dynamics[0, 0], A_lateral_dynamics[0, 1]], + [0, 0, A_lateral_dynamics[1, 0], A_lateral_dynamics[1, 1]], ] ) - b = (np.array([0, 0, b_lOTM[0], b_lOTM[1]])[np.newaxis]).transpose() + B_augmented = ( + np.array([0, 0, B_lateral_dynamics[0], B_lateral_dynamics[1]])[np.newaxis] + ).transpose() - Q = np.zeros((4, 4)) - np.fill_diagonal(Q, 1) + # LQR state weighting matrix (identity = equal weight on all states) + Q_state_weight = np.zeros((4, 4)) + np.fill_diagonal(Q_state_weight, 1) - k_lqr, _, _ = lqr(A=A, b=b, Q=Q, r=r) + lqr_solution = lqr(A=A_augmented, B=B_augmented, Q=Q_state_weight, R=control_weight) - l = vehicle_params.l_h + vehicle_params.l_v - EG = vehicle_params.m / l * (vehicle_params.l_h / c_v - vehicle_params.l_v / c_h) - k_dist_comp = l + EG * np.power(v_0, 2) + # Compute disturbance compensation gain (understeer gradient compensation) + wheelbase = vehicle_params.l_h + vehicle_params.l_v + understeer_gradient = ( + vehicle_params.m + / wheelbase + * ( + vehicle_params.l_h / cornering_stiffness_front + - vehicle_params.l_v / cornering_stiffness_rear + ) + ) + disturbance_compensation_gain = wheelbase + understeer_gradient * np.power( + velocity, 2 + ) return ControlParameters( - l_s=vehicle_params.l_s, k_lqr=k_lqr, k_dist_comp=k_dist_comp + lookahead_distance=vehicle_params.l_s, + lqr_gain=lqr_solution.feedback_gain, + disturbance_compensation_gain=disturbance_compensation_gain, ) -def lqr(A, b, Q, r): - X = scipy.linalg.solve_continuous_are(A, b, Q, r) +def lqr( + A: np.ndarray[Any, Any], B: np.ndarray[Any, Any], Q: np.ndarray[Any, Any], R: float +) -> LQRSolution: + """Solve the continuous-time Linear Quadratic Regulator (LQR) problem. + + Finds the optimal state-feedback gain K that minimizes the cost function: + J = integral(x'Qx + u'Ru) dt - K = (1 / r) * np.dot(b.T, X) - K = np.array([K[0, 0], K[0, 1], K[0, 2], K[0, 3]]) + Args: + A: System dynamics matrix (n x n) + B: Input matrix (n x 1) + Q: State weighting matrix (n x n), must be positive semi-definite + R: Control weighting scalar, must be positive + + Returns: + LQRSolution containing feedback gain, Riccati solution, and closed-loop eigenvalues. + """ + riccati_solution = scipy.linalg.solve_continuous_are(A, B, Q, R) + + feedback_gain_matrix = (1 / R) * np.dot(B.T, riccati_solution) + feedback_gain = np.array( + [ + feedback_gain_matrix[0, 0], + feedback_gain_matrix[0, 1], + feedback_gain_matrix[0, 2], + feedback_gain_matrix[0, 3], + ] + ) - eig_vals, eig_vecs = scipy.linalg.eig(A - b * K) + closed_loop_eigenvalues, _ = scipy.linalg.eig(A - B * feedback_gain) - return K, X, eig_vals + return LQRSolution( + feedback_gain=feedback_gain, + riccati_solution=riccati_solution, + closed_loop_eigenvalues=closed_loop_eigenvalues, + ) class LateralControlRiccati: + """Lateral vehicle controller using LQR with dynamic one-track model. + + This controller uses a Linear Quadratic Regulator (LQR) design based on a + linearized dynamic one-track (bicycle) model. It includes: + - State feedback for lateral error, heading error, sideslip, and yaw rate + - Curvature feedforward compensation for steady-state cornering + - PT2 actuator dynamics for realistic steering response + - Measurement noise simulation + """ + def __init__( self, - initial_condition: np.array, - curve: dict, + initial_state: DynamicVehicleState, + curve: ReferenceCurve, vehicle_params: VehicleParameters, initial_velocity: float, - r: float, + control_weight: float, ): - self.vars_0 = initial_condition - self.vars_0.append(0.0) - self.vars_0.append(0.0) - self.curve = curve - self.v = initial_velocity + """Initialize the LQR lateral controller. + + Args: + initial_state: Initial vehicle state (position, heading, sideslip, yaw rate) + curve: Reference curve to follow + vehicle_params: Physical parameters of the vehicle + initial_velocity: Initial longitudinal velocity [m/s] + control_weight: LQR control weight (higher = less aggressive steering) + """ + self.initial_simulation_state = SimulationState.from_vehicle_state( + initial_state, steering_angle=0.0, steering_rate=0.0 + ) + self.reference_curve = curve + self.velocity = initial_velocity self.vehicle_params = vehicle_params - self.params = get_control_params( - vehicle_params=vehicle_params, velocity=initial_velocity, r=r + self.control_params = get_control_params( + vehicle_params=vehicle_params, + velocity=initial_velocity, + control_weight=control_weight, ) - num = [1] - den = [2 * np.power(0.05, 2), 2 * 0.05, 1] - self.tf_ss = signal.TransferFunction(num, den).to_ss() - - def simulate(self, t_vector, v=1, t_step=0.1): - self.v = v - state_trajectory = odeint( - self._f_system_dynamics, self.vars_0, t_vector, args=(t_step,) + # PT2 actuator dynamics (second-order low-pass filter for steering) + actuator_time_constant = 0.05 + numerator = [1] + denominator = [ + 2 * np.power(actuator_time_constant, 2), + 2 * actuator_time_constant, + 1, + ] + self.actuator_state_space = signal.TransferFunction( + numerator, denominator + ).to_ss() + + def simulate( + self, + time_vector: np.ndarray[Any, Any], + velocity: float = 1, + time_step: float = 0.1, + ) -> np.ndarray[Any, Any]: + """Simulate the closed-loop vehicle trajectory. + + Args: + time_vector: Array of time points for simulation [s] + velocity: Constant longitudinal velocity [m/s] + time_step: Time step for noise generation [s] + + Returns: + State trajectory array with shape (len(time_vector), 7). + Columns: [x, y, psi, beta, yaw_rate, steering_angle, steering_rate] + """ + self.velocity = velocity + state_trajectory: np.ndarray[Any, Any] = odeint( + self._compute_state_derivatives, + self.initial_simulation_state.to_list(), + time_vector, + args=(time_step,), ) return state_trajectory @staticmethod - def __position_noise(val, seed): - position_noise = 0.01 - mu = 0 - sigma = position_noise + def _add_position_noise(value: float, seed: int) -> float: + """Add Gaussian noise to simulate position measurement uncertainty. - np.random.seed(seed) - noise = np.random.normal(mu, sigma) - result = val + noise + Args: + value: True position value + seed: Random seed for reproducibility - return result + Returns: + Noisy position value + """ + position_noise_std = 0.01 # meters + np.random.seed(seed) + noise = np.random.normal(0, position_noise_std) + return value + noise @staticmethod - def __orientation_noise(val, seed): - orientation_noise = 1.0 / 180 * math.pi - mu = 0 - sigma = orientation_noise + def _add_orientation_noise(value: float, seed: int) -> float: + """Add Gaussian noise to simulate orientation measurement uncertainty. + + Args: + value: True orientation value [rad] + seed: Random seed for reproducibility + Returns: + Noisy orientation value [rad] + """ + orientation_noise_std = 1.0 / 180 * math.pi # 1 degree in radians np.random.seed(seed) - noise = np.random.normal(mu, sigma) - result = val + noise + noise = np.random.normal(0, orientation_noise_std) + return value + noise - return result + def _compute_actuator_dynamics( + self, + steering_angle: float, + steering_rate: float, + steering_command: float, + ) -> ActuatorDynamicsOutput: + """Compute PT2 actuator dynamics for the steering system. - def __pt2_motor_dynamic(self, vars_, t, delta_in): - state_1, state_2 = vars_ - state = np.matrix([state_1, state_2]).T - dvarsdt = np.dot(self.tf_ss.A, state) + np.dot(self.tf_ss.B, delta_in) - delta = np.dot(self.tf_ss.C, state) + np.dot(self.tf_ss.D, delta_in) - return dvarsdt[0, 0], dvarsdt[1, 0], delta[0, 0] + Args: + steering_angle: Current steering angle [rad] + steering_rate: Current steering rate [rad/s] + steering_command: Commanded steering angle [rad] - @staticmethod - def __delta_pt1(delta): - delta = delta * 18 - return delta - - def _f_system_dynamics(self, vars_, t, t_step): - x, y, psi, beta, r, delta, delta_dot = vars_ - _, _, _, e_l, e_psi, kappa_r = pro.project2curve_with_lookahead( - self.curve["s"], - self.curve["x"], - self.curve["y"], - self.curve["theta"], - self.curve["kappa"], - self.params.l_s, - x, - y, - psi, + Returns: + ActuatorDynamicsOutput with derivatives and actual steering angle + """ + state = np.array([[steering_angle], [steering_rate]]) + state_derivative = np.dot(self.actuator_state_space.A, state) + np.dot( + self.actuator_state_space.B, steering_command ) - seed = math.floor(t / t_step) - e_l = self.__position_noise(e_l, seed) - e_psi = self.__orientation_noise(e_psi, seed) - delta_in = con.feedback_law( - self.params.k_lqr, - self.params.k_dist_comp, - e_l, - e_psi, - kappa_r, - beta, - r, + actual_steering = np.dot(self.actuator_state_space.C, state) + np.dot( + self.actuator_state_space.D, steering_command ) - state_1_dot, state_2_dot, delta = self.__pt2_motor_dynamic( - [delta, delta_dot], t, delta_in + return ActuatorDynamicsOutput( + steering_angle_derivative=state_derivative[0, 0], + steering_rate_derivative=state_derivative[1, 0], + actual_steering_angle=actual_steering[0, 0], ) - v = self.v # const velocity - vars_dot = dotm.DynamicOneTrackModel(self.vehicle_params).system_dynamics( - vars_[:5], t, v, delta + + def _compute_state_derivatives( + self, state: np.ndarray[Any, Any], time: float, time_step: float + ) -> tuple[float, float, float, float, float, float, float]: + """Compute state derivatives for the closed-loop system. + + Args: + state: Current state [x, y, psi, beta, yaw_rate, steering_angle, steering_rate] + time: Current simulation time [s] + time_step: Time step for noise generation [s] + + Returns: + State derivatives as tuple (required by odeint) + """ + current_state = SimulationState( + x=state[0], + y=state[1], + heading=state[2], + sideslip_angle=state[3], + yaw_rate=state[4], + steering_angle=state[5], + steering_rate=state[6], ) - return ( - vars_dot[0], - vars_dot[1], - vars_dot[2], - vars_dot[3], - vars_dot[4], - state_1_dot, - state_2_dot, + + # Project vehicle position onto reference curve with look-ahead + projection = pro.project2curve_with_lookahead( + self.reference_curve.arc_length, + self.reference_curve.x, + self.reference_curve.y, + self.reference_curve.heading, + self.reference_curve.curvature, + self.control_params.lookahead_distance, + current_state.x, + current_state.y, + current_state.heading, + ) + + # Add measurement noise (synchronized by time step) + noise_seed = math.floor(time / time_step) + lateral_error = self._add_position_noise(projection.lateral_error, noise_seed) + heading_error = self._add_orientation_noise(projection.heading, noise_seed) + + # Compute steering command from feedback law + steering_command = con.feedback_law( + self.control_params.lqr_gain, + self.control_params.disturbance_compensation_gain, + lateral_error, + heading_error, + projection.curvature, + current_state.sideslip_angle, + current_state.yaw_rate, + ) + + # Apply actuator dynamics + actuator_output = self._compute_actuator_dynamics( + current_state.steering_angle, + current_state.steering_rate, + steering_command, + ) + + # Compute vehicle dynamics (vehicle model is not fully typed yet) + vehicle_state = state[:5] + vehicle_derivatives = dotm.DynamicOneTrackModel( # type: ignore[no-untyped-call] + self.vehicle_params + ).system_dynamics( + vehicle_state, time, self.velocity, actuator_output.actual_steering_angle + ) + + derivatives = StateDerivatives( + x_dot=vehicle_derivatives[0], + y_dot=vehicle_derivatives[1], + heading_dot=vehicle_derivatives[2], + sideslip_angle_dot=vehicle_derivatives[3], + yaw_rate_dot=vehicle_derivatives[4], + steering_angle_dot=actuator_output.steering_angle_derivative, + steering_rate_dot=actuator_output.steering_rate_derivative, ) + return derivatives.to_tuple() diff --git a/src/behavior_generation_lecture_python/lateral_control_riccati/riccati_controller.py b/src/behavior_generation_lecture_python/lateral_control_riccati/riccati_controller.py index 0207b6f..6162bce 100644 --- a/src/behavior_generation_lecture_python/lateral_control_riccati/riccati_controller.py +++ b/src/behavior_generation_lecture_python/lateral_control_riccati/riccati_controller.py @@ -1,8 +1,38 @@ +"""LQR-based feedback controller for lateral vehicle control.""" + +from typing import Any + import numpy as np -def feedback_law(k_lqr, k_dist_comp, e_l, e_psi, kappa_r, beta, r): - x = np.array([e_l, e_psi, beta, r]) - delta = np.dot(-k_lqr, x) + k_dist_comp * kappa_r +def feedback_law( + k_lqr: np.ndarray[Any, Any], + k_dist_comp: float, + lateral_error: float, + heading_error: float, + reference_curvature: float, + beta: float, + yaw_rate: float, +) -> float: + """Compute steering angle using LQR feedback with disturbance compensation. + + The control law combines state feedback (LQR) with a feedforward term for + curvature compensation. The state vector consists of lateral error, heading + error, sideslip angle (beta), and yaw rate. + + Args: + k_lqr: LQR gain vector [k_lateral, k_heading, k_beta, k_yaw_rate] + k_dist_comp: Disturbance compensation gain for curvature feedforward + lateral_error: Distance from vehicle to reference curve [m] + heading_error: Difference between vehicle heading and reference heading [rad] + reference_curvature: Curvature of the reference curve at the projection point [1/m] + beta: Vehicle sideslip angle [rad] + yaw_rate: Vehicle yaw rate [rad/s] + + Returns: + Steering angle command [rad] + """ + state = np.array([lateral_error, heading_error, beta, yaw_rate]) + steering_angle: float = float(np.dot(-k_lqr, state) + k_dist_comp * reference_curvature) - return delta + return steering_angle diff --git a/src/behavior_generation_lecture_python/lateral_control_state_based/lateral_control_state_based.py b/src/behavior_generation_lecture_python/lateral_control_state_based/lateral_control_state_based.py index 2b679c2..da34580 100644 --- a/src/behavior_generation_lecture_python/lateral_control_state_based/lateral_control_state_based.py +++ b/src/behavior_generation_lecture_python/lateral_control_state_based/lateral_control_state_based.py @@ -1,52 +1,156 @@ +"""Lateral vehicle control using state-based feedback with kinematic model.""" + +from dataclasses import dataclass +from typing import Any + import numpy as np from scipy.integrate import odeint import behavior_generation_lecture_python.lateral_control_state_based.state_based_controller as con import behavior_generation_lecture_python.utils.projection as pro import behavior_generation_lecture_python.vehicle_models.kinematic_one_track_model as kotm +from behavior_generation_lecture_python.utils.reference_curve import ReferenceCurve + + +@dataclass +class KinematicVehicleState: + """State of a vehicle using the kinematic one-track model. + + Attributes: + x: X-position in global coordinates [m] + y: Y-position in global coordinates [m] + heading: Vehicle heading angle (yaw) [rad] + """ + + x: float + y: float + heading: float + + def to_list(self) -> list[float]: + """Convert to list for use with numerical integrators.""" + return [self.x, self.y, self.heading] + + +@dataclass +class ControllerOutput: + """Output of the state-based lateral controller at a single time step. + + Attributes: + x: Vehicle x-position [m] + y: Vehicle y-position [m] + heading: Vehicle heading angle [rad] + lateral_error: Distance from vehicle to reference curve [m] + steering_angle: Commanded steering angle [rad] + """ + + x: float + y: float + heading: float + lateral_error: float + steering_angle: float class LateralControlStateBased: - def __init__(self, initial_condition, curve): - self.vars_0 = initial_condition - self.curve = curve - self.v = 1 - - def simulate(self, t_vector, v=1): - self.v = v - state_trajectory = odeint(self._f_system_dynamics, self.vars_0, t_vector) - output_trajectory = np.array( - [self._g_system_output(x) for x in state_trajectory] + """Lateral vehicle controller using state-based feedback with kinematic model. + + This controller uses a simple state feedback design based on a kinematic + one-track (bicycle) model. It computes steering commands based on: + - Lateral error (distance to reference curve) + - Heading error (difference from reference heading) + - Curvature feedforward for steady-state cornering + """ + + def __init__( + self, + initial_state: KinematicVehicleState, + curve: ReferenceCurve, + ): + """Initialize the state-based lateral controller. + + Args: + initial_state: Initial vehicle state (position and heading) + curve: Reference curve to follow + """ + self.initial_state = initial_state + self.reference_curve = curve + self.velocity = 1.0 + + def simulate( + self, time_vector: np.ndarray[Any, Any], velocity: float = 1.0 + ) -> list[ControllerOutput]: + """Simulate the closed-loop vehicle trajectory. + + Args: + time_vector: Array of time points for simulation [s] + velocity: Constant longitudinal velocity [m/s] + + Returns: + List of ControllerOutput for each time step + """ + self.velocity = velocity + state_trajectory = odeint( + self._compute_state_derivatives, self.initial_state.to_list(), time_vector ) - return output_trajectory - - def _f_system_dynamics(self, vars_, t): - x, y, psi = vars_ - _, _, _, d, theta_r, kappa_r = pro.project2curve( - self.curve["s"], - self.curve["x"], - self.curve["y"], - self.curve["theta"], - self.curve["kappa"], + return [self._compute_output(state) for state in state_trajectory] + + def _compute_state_derivatives( + self, state: np.ndarray[Any, Any], time: float + ) -> np.ndarray[Any, Any]: + """Compute state derivatives for the closed-loop system. + + Args: + state: Current state [x, y, psi] + time: Current simulation time [s] + + Returns: + State derivatives [x_dot, y_dot, psi_dot] + """ + x, y, psi = state + projection = pro.project2curve( + self.reference_curve.arc_length, + self.reference_curve.x, + self.reference_curve.y, + self.reference_curve.heading, + self.reference_curve.curvature, x, y, ) - delta = con.feedback_law(d, psi, theta_r, kappa_r) - v = self.v # const velocity - vars_dot = kotm.KinematicOneTrackModel().system_dynamics(vars_, t, v, delta) - return vars_dot - - def _g_system_output(self, vars_): - x, y, psi = vars_ - _, _, _, d, theta_r, kappa_r = pro.project2curve( - self.curve["s"], - self.curve["x"], - self.curve["y"], - self.curve["theta"], - self.curve["kappa"], + steering_angle = con.feedback_law( + projection.lateral_error, psi, projection.heading, projection.curvature + ) + # Vehicle model is not fully typed yet + state_derivatives: np.ndarray[Any, Any] = kotm.KinematicOneTrackModel().system_dynamics( # type: ignore[no-untyped-call] + state, time, self.velocity, steering_angle + ) + return state_derivatives + + def _compute_output(self, state: np.ndarray[Any, Any]) -> ControllerOutput: + """Compute output variables for the current state. + + Args: + state: Current state [x, y, psi] + + Returns: + ControllerOutput with position, heading, lateral error, and steering angle + """ + x, y, psi = state + projection = pro.project2curve( + self.reference_curve.arc_length, + self.reference_curve.x, + self.reference_curve.y, + self.reference_curve.heading, + self.reference_curve.curvature, x, y, ) - delta = con.feedback_law(d, psi, theta_r, kappa_r) + steering_angle = con.feedback_law( + projection.lateral_error, psi, projection.heading, projection.curvature + ) - return [x, y, psi, d, delta] + return ControllerOutput( + x=x, + y=y, + heading=psi, + lateral_error=projection.lateral_error, + steering_angle=steering_angle, + ) diff --git a/src/behavior_generation_lecture_python/lateral_control_state_based/state_based_controller.py b/src/behavior_generation_lecture_python/lateral_control_state_based/state_based_controller.py index 0d89dd4..3a38abf 100644 --- a/src/behavior_generation_lecture_python/lateral_control_state_based/state_based_controller.py +++ b/src/behavior_generation_lecture_python/lateral_control_state_based/state_based_controller.py @@ -1,27 +1,48 @@ +"""State-based feedback controller for lateral vehicle control.""" + import numpy as np import behavior_generation_lecture_python.utils.normalize_angle as na -def feedback_law(d, psi, theta_r, kappa_r): - """Feedback law for the state-based controller +def feedback_law( + lateral_error: float, + vehicle_heading: float, + reference_heading: float, + reference_curvature: float, +) -> float: + """Compute steering angle using state-based feedback control. + + This controller uses a linearized kinematic bicycle model and computes + a steering command based on lateral error, heading error, and curvature + feedforward. Args: - d: Distance of the vehicle to the reference curve - psi: Heading of the vehicle - theta_r: Heading of the reference line - kappa_r: Curvature of the reference line + lateral_error: Distance from vehicle to reference curve [m], + positive if vehicle is left of the curve + vehicle_heading: Heading angle of the vehicle [rad] + reference_heading: Heading angle of the reference curve at the + projection point [rad] + reference_curvature: Curvature of the reference curve at the + projection point [1/m] Returns: - Steering angle + Steering angle command [rad] """ - axis_distance = 2.9680 - k_0 = 0.2 - k_1 = 1.0 + wheelbase = 2.9680 # Distance between front and rear axle [m] + lateral_error_gain = 0.2 + heading_error_gain = 1.0 + + # Compute heading error with angle normalization + heading_error = na.normalize_angle(vehicle_heading - reference_heading) - # Stabilization - u = kappa_r - k_0 * d - k_1 * na.normalize_angle(psi - theta_r) + # State feedback with curvature feedforward + curvature_command = ( + reference_curvature + - lateral_error_gain * lateral_error + - heading_error_gain * heading_error + ) - # Re-substitution - delta = np.arctan(axis_distance * u) - return delta + # Convert curvature to steering angle (inverse kinematic bicycle model) + steering_angle: float = float(np.arctan(wheelbase * curvature_command)) + return steering_angle diff --git a/src/behavior_generation_lecture_python/py.typed b/src/behavior_generation_lecture_python/py.typed new file mode 100644 index 0000000..e69de29 diff --git a/src/behavior_generation_lecture_python/utils/__init__.py b/src/behavior_generation_lecture_python/utils/__init__.py index e69de29..ab8ff30 100644 --- a/src/behavior_generation_lecture_python/utils/__init__.py +++ b/src/behavior_generation_lecture_python/utils/__init__.py @@ -0,0 +1,6 @@ +"""Utility modules for behavior generation lecture.""" + +from behavior_generation_lecture_python.utils.projection import CurveProjection +from behavior_generation_lecture_python.utils.reference_curve import ReferenceCurve + +__all__ = ["CurveProjection", "ReferenceCurve"] diff --git a/src/behavior_generation_lecture_python/utils/generate_reference_curve.py b/src/behavior_generation_lecture_python/utils/generate_reference_curve.py index def7230..28f691d 100644 --- a/src/behavior_generation_lecture_python/utils/generate_reference_curve.py +++ b/src/behavior_generation_lecture_python/utils/generate_reference_curve.py @@ -1,9 +1,20 @@ +"""Generate reference curves from input points using spline interpolation.""" + +from typing import Any + import matplotlib.pyplot as plt import numpy as np from scipy import interpolate +from behavior_generation_lecture_python.utils.reference_curve import ReferenceCurve + + +def pick_points_from_plot() -> ReferenceCurve: + """Interactively pick points from a plot to generate a reference curve. -def pick_points_from_plot(): + Returns: + A ReferenceCurve generated from the selected points. + """ fig, ax = plt.subplots() ax.set_xlim([0, 100]) ax.set_ylim([0, 100]) @@ -20,7 +31,7 @@ def pick_points_from_plot(): x_input = xy[:, 0] y_input = xy[:, 1] curve = generate_reference_curve(x_input, y_input, 1) - ax.plot(curve["x"], curve["y"], "r-") + ax.plot(curve.x, curve.y, "r-") ax.plot(x_input, y_input, "bo") plt.draw() print("Press any key to exit") @@ -29,54 +40,66 @@ def pick_points_from_plot(): return curve -def generate_reference_curve(xx, yy, delta): - assert len(xx) == len(yy) >= 4 - delta_s = np.sqrt(np.diff(xx) ** 2 + np.diff(yy) ** 2) - chords = np.cumsum(np.concatenate([[0], delta_s])) +def generate_reference_curve( + x_points: np.ndarray[Any, Any], y_points: np.ndarray[Any, Any], sampling_distance: float +) -> ReferenceCurve: + """Generate a reference curve from input points using spline interpolation. - # Generate spline for xx(s) and yy(s) - sp_x = interpolate.splrep(chords, xx) - sp_y = interpolate.splrep(chords, yy) + Args: + x_points: X-coordinates of the input points (at least 4 points required) + y_points: Y-coordinates of the input points (at least 4 points required) + sampling_distance: Distance between sampled points on the output curve [m] - # At every delta meter, evaluate spline... - s_sampled = np.arange(0, max(chords) + delta, delta) - x_curve = interpolate.splev(s_sampled, sp_x, der=0) - y_curve = interpolate.splev(s_sampled, sp_y, der=0) + Returns: + A ReferenceCurve with arc length, x, y, heading, and curvature arrays. + """ + assert len(x_points) == len(y_points) >= 4 + segment_lengths = np.sqrt(np.diff(x_points) ** 2 + np.diff(y_points) ** 2) + chord_lengths = np.cumsum(np.concatenate([[0], segment_lengths])) - # ... and its first ... - x_prime = interpolate.splev(s_sampled, sp_x, der=1) - y_prime = interpolate.splev(s_sampled, sp_y, der=1) + # Generate spline for x(s) and y(s) + spline_x = interpolate.splrep(chord_lengths, x_points) + spline_y = interpolate.splrep(chord_lengths, y_points) + + # At every sampling_distance meter, evaluate spline... + arc_length_sampled = np.arange( + 0, max(chord_lengths) + sampling_distance, sampling_distance + ) + x_curve = interpolate.splev(arc_length_sampled, spline_x, der=0) + y_curve = interpolate.splev(arc_length_sampled, spline_y, der=0) + + # ... and its first derivative ... + dx_ds = interpolate.splev(arc_length_sampled, spline_x, der=1) + dy_ds = interpolate.splev(arc_length_sampled, spline_y, der=1) # ... and its second derivative ... - x_pprime = interpolate.splev(s_sampled, sp_x, der=2) - y_pprime = interpolate.splev(s_sampled, sp_y, der=2) + d2x_ds2 = interpolate.splev(arc_length_sampled, spline_x, der=2) + d2y_ds2 = interpolate.splev(arc_length_sampled, spline_y, der=2) - # delta_s = sqrt(delta_x² + delta_y²) (add zero at the first - s_curve = np.concatenate( + # Compute arc length: delta_s = sqrt(delta_x^2 + delta_y^2) + arc_length = np.concatenate( ( np.array([0]), np.cumsum(np.sqrt(np.diff(x_curve) ** 2 + np.diff(y_curve) ** 2)), ) ) - # tan(theta) = dy/dx = (dy/ds) / (dx/ds) - theta_curve = np.arctan2(y_prime, x_prime) + # Heading: tan(theta) = dy/dx = (dy/ds) / (dx/ds) + heading = np.arctan2(dy_ds, dx_ds) - # kappa = (x'y'' - y'x'')/(x'² + y'²)^(3/2) - kappa_curve = (x_prime * y_pprime - y_prime * x_pprime) / ( - x_prime**2 + y_prime**2 - ) ** (3 / 2) + # Curvature: kappa = (x'y'' - y'x'') / (x'^2 + y'^2)^(3/2) + curvature = (dx_ds * d2y_ds2 - dy_ds * d2x_ds2) / (dx_ds**2 + dy_ds**2) ** (3 / 2) - return { - "s": s_curve, - "x": x_curve, - "y": y_curve, - "theta": theta_curve, - "kappa": kappa_curve, - } + return ReferenceCurve( + arc_length=arc_length, + x=x_curve, + y=y_curve, + heading=heading, + curvature=curvature, + ) -def main(): +def main() -> None: curve = pick_points_from_plot() print(curve) diff --git a/src/behavior_generation_lecture_python/utils/normalize_angle.py b/src/behavior_generation_lecture_python/utils/normalize_angle.py index a260fc1..bf038dc 100644 --- a/src/behavior_generation_lecture_python/utils/normalize_angle.py +++ b/src/behavior_generation_lecture_python/utils/normalize_angle.py @@ -1,6 +1,16 @@ +"""Angle normalization utilities.""" + import numpy as np -def normalize_angle(angle): - """Normalize angle to be between -pi and pi""" - return (angle + np.pi) % (2 * np.pi) - np.pi +def normalize_angle(angle: float) -> float: + """Normalize angle to be between -pi and pi. + + Args: + angle: Angle in radians + + Returns: + Normalized angle in the range [-pi, pi] + """ + result: float = (angle + np.pi) % (2 * np.pi) - np.pi + return result diff --git a/src/behavior_generation_lecture_python/utils/plot_vehicle/plot_vehicle.py b/src/behavior_generation_lecture_python/utils/plot_vehicle/plot_vehicle.py index 7518a0a..0c84540 100644 --- a/src/behavior_generation_lecture_python/utils/plot_vehicle/plot_vehicle.py +++ b/src/behavior_generation_lecture_python/utils/plot_vehicle/plot_vehicle.py @@ -1,14 +1,26 @@ +"""Vehicle plotting utilities.""" + import os +from typing import Any import matplotlib as mpl import numpy as np +from matplotlib.axes import Axes from matplotlib.patches import Polygon NP_FILE = os.path.join(os.path.dirname(__file__), "f10.npy") -geo = np.load(NP_FILE, allow_pickle=True)[()] +geo: dict[str, Any] = np.load(NP_FILE, allow_pickle=True)[()] -def plot_vehicle(ax, x, y, psi, delta=0, length=4.899, width=2.094): +def plot_vehicle( + ax: Axes, + x: float, + y: float, + psi: float, + delta: float = 0, + length: float = 4.899, + width: float = 2.094, +) -> None: global geo [p.remove() for p in reversed(ax.patches)] diff --git a/src/behavior_generation_lecture_python/utils/projection.py b/src/behavior_generation_lecture_python/utils/projection.py index b718424..52c120b 100644 --- a/src/behavior_generation_lecture_python/utils/projection.py +++ b/src/behavior_generation_lecture_python/utils/projection.py @@ -1,4 +1,8 @@ +"""Projection utilities for projecting points onto reference curves.""" + import warnings +from dataclasses import dataclass +from typing import Any import numpy as np from scipy import spatial @@ -6,37 +10,107 @@ import behavior_generation_lecture_python.utils.normalize_angle as na -def project2curve_with_lookahead(s_c, x_c, y_c, theta_c, kappa_c, l_v, x, y, psi): +@dataclass +class CurveProjection: + """Result of projecting a point onto a reference curve. + + Attributes: + x: X-coordinate of the projected point [m] + y: Y-coordinate of the projected point [m] + arc_length: Arc length along the curve at the projection point [m] + lateral_error: Signed distance from original point to curve [m], + positive if point is left of the curve + heading: Heading angle at the projection point [rad] + curvature: Curvature at the projection point [1/m] + """ + + x: float + y: float + arc_length: float + lateral_error: float + heading: float + curvature: float + + +def project2curve_with_lookahead( + s_c: np.ndarray[Any, Any], + x_c: np.ndarray[Any, Any], + y_c: np.ndarray[Any, Any], + theta_c: np.ndarray[Any, Any], + kappa_c: np.ndarray[Any, Any], + lookahead_distance: float, + x: float, + y: float, + psi: float, +) -> CurveProjection: + """Project a point with look-ahead onto a reference curve. + + Computes the look-ahead sensor point and projects it onto the curve. + The heading in the result is the heading error (vehicle heading - reference heading). + + Args: + s_c: Arc length of the curve points + x_c: X-coordinates of the curve points + y_c: Y-coordinates of the curve points + theta_c: Heading at the curve points + kappa_c: Curvature at the curve points + lookahead_distance: Look-ahead distance from vehicle position [m] + x: X-coordinate of the vehicle + y: Y-coordinate of the vehicle + psi: Heading of the vehicle [rad] + + Returns: + CurveProjection with heading representing heading error (psi - reference_heading) + """ # Calculate look-ahead sensor point - x = x + l_v * np.cos(psi) - y = y + l_v * np.sin(psi) + x_lookahead = x + lookahead_distance * np.cos(psi) + y_lookahead = y + lookahead_distance * np.sin(psi) projection = project2curve( - s_c=s_c, x_c=x_c, y_c=y_c, theta_c=theta_c, kappa_c=kappa_c, x=x, y=y + s_c=s_c, + x_c=x_c, + y_c=y_c, + theta_c=theta_c, + kappa_c=kappa_c, + x=x_lookahead, + y=y_lookahead, ) - # Simulate camera view - projection[4] = psi - projection[4] + # Simulate camera view: return heading error instead of reference heading + heading_error = psi - projection.heading - return projection + return CurveProjection( + x=projection.x, + y=projection.y, + arc_length=projection.arc_length, + lateral_error=projection.lateral_error, + heading=heading_error, + curvature=projection.curvature, + ) -def project2curve(s_c, x_c, y_c, theta_c, kappa_c, x, y): - """Project a point onto a curve (defined as a polygonal chain/ sequence of points/ line string) +def project2curve( + s_c: np.ndarray[Any, Any], + x_c: np.ndarray[Any, Any], + y_c: np.ndarray[Any, Any], + theta_c: np.ndarray[Any, Any], + kappa_c: np.ndarray[Any, Any], + x: float, + y: float, +) -> CurveProjection: + """Project a point onto a curve (defined as a polygonal chain). Args: - s_c: Arc lenght of the curve - x_c: x-coordinates of the curve points - y_c: y-coordinates of the curve points - theta_c: heading at the curve points - kappa_c: curvature at the curve points - x: x-coordinates of the point to be projected - y: y-coordinates of the point to be projected + s_c: Arc length of the curve points + x_c: X-coordinates of the curve points + y_c: Y-coordinates of the curve points + theta_c: Heading at the curve points + kappa_c: Curvature at the curve points + x: X-coordinate of the point to be projected + y: Y-coordinate of the point to be projected Returns: - properties of the projected point as list: x-coordinate, - y-coordinate, arc length, distance to original point, heading, - curvature + CurveProjection containing projected point properties """ # Find the closest curve point to [x, y] distance, mindex = spatial.KDTree(np.array([x_c, y_c]).transpose()).query([x, y]) @@ -81,24 +155,39 @@ def project2curve(s_c, x_c, y_c, theta_c, kappa_c, x, y): kappa2 = kappa_c[start_index + 1] kappa_p = lambda_ * kappa2 + (1.0 - lambda_) * kappa1 - return [x_p, y_p, s_p, d, theta_p, kappa_p] + return CurveProjection( + x=x_p, + y=y_p, + arc_length=s_p, + lateral_error=d, + heading=theta_p, + curvature=kappa_p, + ) -def pseudo_projection(start_index, x, y, x_c, y_c, theta_c): - """Project a point onto a segment of a curve (defined as a polygonal chain/ sequence of points/ line string) +def pseudo_projection( + start_index: int, + x: float, + y: float, + x_c: np.ndarray[Any, Any], + y_c: np.ndarray[Any, Any], + theta_c: np.ndarray[Any, Any], +) -> tuple[np.ndarray[Any, Any], float, int]: + """Project a point onto a segment of a curve. Args: start_index: Start index of the segment to be projected to - x_c: x-coordinates of the curve points - y_c: y-coordinates of the curve points - theta_c: heading at the curve points - x: x-coordinates of the point to be projected - y: y-coordinates of the point to be projected + x: X-coordinate of the point to be projected + y: Y-coordinate of the point to be projected + x_c: X-coordinates of the curve points + y_c: Y-coordinates of the curve points + theta_c: Heading at the curve points Returns: - properties of the projected point as list: point ([x-coordinate, - y-coordinate]), lambda: interpolation scale, sgn: sign of the - projection (-1 or 1) + Tuple of (projected_point, lambda, sign) where: + - projected_point: [x, y] coordinates of projection + - lambda: interpolation parameter (0 to 1) + - sign: +1 if point is left of curve, -1 if right, 0 if on curve """ p1 = np.array([x_c[start_index], y_c[start_index]]) p2 = np.array([x_c[start_index + 1], y_c[start_index + 1]]) @@ -129,4 +218,4 @@ def pseudo_projection(start_index, x, y, x_c, y_c, theta_c): elif x_[1] < 0: sgn = -1 - return [px, lambda_, sgn] + return (px, float(lambda_), sgn) diff --git a/src/behavior_generation_lecture_python/utils/reference_curve.py b/src/behavior_generation_lecture_python/utils/reference_curve.py new file mode 100644 index 0000000..cf15a39 --- /dev/null +++ b/src/behavior_generation_lecture_python/utils/reference_curve.py @@ -0,0 +1,25 @@ +"""Reference curve dataclass for path following controllers.""" + +from dataclasses import dataclass +from typing import Any + +import numpy as np + + +@dataclass +class ReferenceCurve: + """A reference curve for path following, defined by sampled points. + + Attributes: + arc_length: Arc length values along the curve [m] + x: X-coordinates of curve points [m] + y: Y-coordinates of curve points [m] + heading: Heading angle at each point [rad] + curvature: Curvature at each point [1/m] + """ + + arc_length: np.ndarray[Any, Any] + x: np.ndarray[Any, Any] + y: np.ndarray[Any, Any] + heading: np.ndarray[Any, Any] + curvature: np.ndarray[Any, Any] diff --git a/src/behavior_generation_lecture_python/utils/vizard/vizard.py b/src/behavior_generation_lecture_python/utils/vizard/vizard.py index 7f4e1f6..617e184 100755 --- a/src/behavior_generation_lecture_python/utils/vizard/vizard.py +++ b/src/behavior_generation_lecture_python/utils/vizard/vizard.py @@ -1,28 +1,39 @@ +"""Animation control utilities for matplotlib figures.""" + import math import warnings +from typing import Any, Callable, Optional -import matplotlib.pyplot as plt -import matplotlib.widgets -from matplotlib.animation import FuncAnimation -from mpl_toolkits.axes_grid1 import Divider, Size -from mpl_toolkits.axes_grid1.mpl_axes import Axes +import matplotlib.figure # type: ignore[import-untyped] +import matplotlib.pyplot as plt # type: ignore[import-untyped] +import matplotlib.widgets # type: ignore[import-untyped] +import numpy as np +from matplotlib.animation import FuncAnimation # type: ignore[import-untyped] +from mpl_toolkits.axes_grid1 import Divider, Size # type: ignore[import-untyped] +from mpl_toolkits.axes_grid1.mpl_axes import Axes # type: ignore[import-untyped] -class Vizard(object): +class Vizard: """Class around matplotlib's FuncAnimation for managing and controlling plot animations via GUI elements. """ - def __init__(self, figure, update_func, time_vec): + def __init__( + self, + figure: matplotlib.figure.Figure, + update_func: Callable[..., Any], + time_vec: np.ndarray, + ) -> None: """Constructor Args: - interval: Update interval of animation's event source - (timer). + figure: Matplotlib figure to animate + update_func: Function called to update the plot at each frame + time_vec: Array of time values for the animation """ - self.plots = [] - self.func_animations = [] - self.event_source = None + self.plots: list[Vizard.VizardPlot] = [] + self.func_animations: list[FuncAnimation] = [] + self.event_source: Optional[Any] = None self.i = 0 self.min = 0 @@ -37,30 +48,37 @@ def __init__(self, figure, update_func, time_vec): self.go_to(1) - class VizardPlot(object): + class VizardPlot: """Helper class for storing plot fixtures.""" - def __init__(self): - self.figure = None - self.init_func = None - self.update_func = None - self.time_vec = None - self.anim = None - self.controls = None + def __init__(self) -> None: + self.figure: Optional[matplotlib.figure.Figure] = None + self.init_func: Optional[Callable[..., Any]] = None + self.update_func: Optional[Callable[..., Any]] = None + self.time_vec: Optional[np.ndarray] = None + self.anim: Optional[FuncAnimation] = None + self.controls: Optional["Vizard.VizardControls"] = None - class VizardControls(object): + class VizardControls: """Helper class for storing animation controls.""" - def __init__(self): - self.button_stop = None - self.button_playpause = None - self.button_oneback = None - self.button_oneforward = None - self.slider = None - self.button_measure = None - self.text_measure = None - - def register_plot(self, figure, update_func, time_vec, init_func=None, blit=False): + def __init__(self) -> None: + self.button_stop: Optional[matplotlib.widgets.Button] = None + self.button_playpause: Optional[matplotlib.widgets.Button] = None + self.button_oneback: Optional[matplotlib.widgets.Button] = None + self.button_oneforward: Optional[matplotlib.widgets.Button] = None + self.slider: Optional[matplotlib.widgets.Slider] = None + self.button_measure: Optional[matplotlib.widgets.Button] = None + self.text_measure: Optional[Any] = None + + def register_plot( + self, + figure: matplotlib.figure.Figure, + update_func: Callable[..., Any], + time_vec: np.ndarray, + init_func: Optional[Callable[..., Any]] = None, + blit: bool = False, + ) -> None: """Create new FuncAnimation instance with given plot class. """ diff --git a/tests/test_lateral_control_riccati.py b/tests/test_lateral_control_riccati.py index 79ccc2c..fa5bd1a 100644 --- a/tests/test_lateral_control_riccati.py +++ b/tests/test_lateral_control_riccati.py @@ -2,45 +2,332 @@ import pytest import behavior_generation_lecture_python.lateral_control_riccati.lateral_control_riccati as cl +import behavior_generation_lecture_python.lateral_control_riccati.riccati_controller as con 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.projection import project2curve from behavior_generation_lecture_python.vehicle_models.vehicle_parameters import ( DEFAULT_VEHICLE_PARAMS, ) -@pytest.mark.parametrize("test_r,error_factor", [(10000, 1), (10, 0.5)]) -def test_lateral_control_riccati(test_r, error_factor): +@pytest.mark.parametrize("control_weight,error_factor", [(10000, 1), (10, 0.5)]) +def test_lateral_control_riccati(control_weight, error_factor): radius = 500 - vars_0 = [0.0, -radius, 0.0, 0.0, 0.0] - v_0 = 33.0 + initial_state = DynamicVehicleState( + x=0.0, + y=-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) model = cl.LateralControlRiccati( - initial_condition=vars_0, + initial_state=initial_state, curve=curve, vehicle_params=DEFAULT_VEHICLE_PARAMS, - initial_velocity=v_0, - r=test_r, + initial_velocity=initial_velocity, + control_weight=control_weight, ) - sol = model.simulate(ti, v=v_0, t_step=0.1) + trajectory = model.simulate(time_vector, velocity=initial_velocity, time_step=0.1) errors = [] - for state in sol: - _, _, _, d, _, _ = project2curve( - s_c=curve["s"], - x_c=curve["x"], - y_c=curve["y"], - theta_c=curve["theta"], - kappa_c=curve["kappa"], + for state in trajectory: + projection = project2curve( + s_c=curve.arc_length, + x_c=curve.x, + y_c=curve.y, + theta_c=curve.heading, + kappa_c=curve.curvature, x=state[0], y=state[1], ) - errors.append(abs(d)) + errors.append(abs(projection.lateral_error)) + + assert np.sum(errors) > len(trajectory) * 0.1 * error_factor + assert np.sum(errors) < len(trajectory) * 0.2 * error_factor + + +# Unit tests for riccati_controller.feedback_law() +class TestFeedbackLaw: + def test_feedback_law_zero_error(self): + """Zero steering when on reference with no curvature and no dynamics""" + k_lqr = np.array([1.0, 1.0, 1.0, 1.0]) + steering = con.feedback_law( + k_lqr=k_lqr, + k_dist_comp=1.0, + lateral_error=0, + heading_error=0, + reference_curvature=0, + beta=0, + yaw_rate=0, + ) + assert steering == pytest.approx(0) + + def test_feedback_law_lateral_error_positive(self): + """Negative steering to correct positive lateral error (left of reference)""" + k_lqr = np.array([1.0, 0.0, 0.0, 0.0]) + steering = con.feedback_law( + k_lqr=k_lqr, + k_dist_comp=0.0, + lateral_error=1.0, + heading_error=0, + reference_curvature=0, + beta=0, + yaw_rate=0, + ) + assert steering < 0, "Should steer right (negative) to correct left error" + + def test_feedback_law_lateral_error_negative(self): + """Positive steering to correct negative lateral error (right of reference)""" + k_lqr = np.array([1.0, 0.0, 0.0, 0.0]) + steering = con.feedback_law( + k_lqr=k_lqr, + k_dist_comp=0.0, + lateral_error=-1.0, + heading_error=0, + reference_curvature=0, + beta=0, + yaw_rate=0, + ) + assert steering > 0, "Should steer left (positive) to correct right error" + + def test_feedback_law_heading_error_positive(self): + """Steering correction for positive heading error""" + k_lqr = np.array([0.0, 1.0, 0.0, 0.0]) + steering = con.feedback_law( + k_lqr=k_lqr, + k_dist_comp=0.0, + lateral_error=0, + heading_error=0.1, + reference_curvature=0, + beta=0, + yaw_rate=0, + ) + assert steering < 0, "Should steer right to correct heading pointing left" + + def test_feedback_law_heading_error_negative(self): + """Steering correction for negative heading error""" + k_lqr = np.array([0.0, 1.0, 0.0, 0.0]) + steering = con.feedback_law( + k_lqr=k_lqr, + k_dist_comp=0.0, + lateral_error=0, + heading_error=-0.1, + reference_curvature=0, + beta=0, + yaw_rate=0, + ) + assert steering > 0, "Should steer left to correct heading pointing right" + + def test_feedback_law_curvature_feedforward_positive(self): + """Positive curvature should add positive steering component""" + k_lqr = np.array([0.0, 0.0, 0.0, 0.0]) + steering = con.feedback_law( + k_lqr=k_lqr, + k_dist_comp=1.0, + lateral_error=0, + heading_error=0, + reference_curvature=0.1, + beta=0, + yaw_rate=0, + ) + assert steering > 0, "Positive curvature should cause positive steering" + + def test_feedback_law_curvature_feedforward_negative(self): + """Negative curvature should add negative steering component""" + k_lqr = np.array([0.0, 0.0, 0.0, 0.0]) + steering = con.feedback_law( + k_lqr=k_lqr, + k_dist_comp=1.0, + lateral_error=0, + heading_error=0, + reference_curvature=-0.1, + beta=0, + yaw_rate=0, + ) + assert steering < 0, "Negative curvature should cause negative steering" + + def test_feedback_law_beta_correction(self): + """Sideslip angle (beta) should be corrected""" + k_lqr = np.array([0.0, 0.0, 1.0, 0.0]) + steering = con.feedback_law( + k_lqr=k_lqr, + k_dist_comp=0.0, + lateral_error=0, + heading_error=0, + reference_curvature=0, + beta=0.1, + yaw_rate=0, + ) + assert steering < 0, "Positive beta should cause negative steering correction" + + def test_feedback_law_yaw_rate_correction(self): + """Yaw rate should be corrected""" + k_lqr = np.array([0.0, 0.0, 0.0, 1.0]) + steering = con.feedback_law( + k_lqr=k_lqr, + k_dist_comp=0.0, + lateral_error=0, + heading_error=0, + reference_curvature=0, + beta=0, + yaw_rate=0.1, + ) + assert ( + steering < 0 + ), "Positive yaw rate should cause negative steering correction" + - assert np.sum(errors) > len(sol) * 0.1 * error_factor - assert np.sum(errors) < len(sol) * 0.2 * error_factor +# Unit tests for lqr() function +# Note: The lqr() function is specifically designed for 4-state systems +class TestLQR: + @pytest.fixture + def sample_4d_system(self): + """A sample 4D system similar to vehicle lateral control""" + A = np.array([[0, 1, 1, 0], [0, 0, 0, 1], [0, 0, -1, 0.5], [0, 0, 0.5, -1]]) + B = np.array([[0], [0], [1], [0.5]]) + Q = np.eye(4) + return A, B, Q + + def test_lqr_4d_system_stability(self, sample_4d_system): + """Test LQR on 4D system - closed loop should be stable""" + A, B, Q = sample_4d_system + R = 1.0 + solution = cl.lqr(A, B, Q, R) + # Verify closed-loop eigenvalues are stable (negative real parts) + assert all( + np.real(solution.closed_loop_eigenvalues) < 0 + ), "Closed-loop system should have stable eigenvalues" + + def test_lqr_gain_shape(self, sample_4d_system): + """Verify feedback_gain has correct shape for 4D system""" + A, B, Q = sample_4d_system + R = 1.0 + solution = cl.lqr(A, B, Q, R) + assert solution.feedback_gain.shape == ( + 4, + ), "K should have 4 elements for 4D system" + + def test_lqr_riccati_solution_shape(self, sample_4d_system): + """Verify riccati_solution has correct shape""" + A, B, Q = sample_4d_system + R = 1.0 + solution = cl.lqr(A, B, Q, R) + assert solution.riccati_solution.shape == ( + 4, + 4, + ), "X should be 4x4 for 4D system" + + def test_lqr_riccati_solution_symmetric(self, sample_4d_system): + """Verify riccati_solution is symmetric""" + A, B, Q = sample_4d_system + R = 1.0 + solution = cl.lqr(A, B, Q, R) + assert np.allclose( + solution.riccati_solution, solution.riccati_solution.T + ), "Riccati solution should be symmetric" + + def test_lqr_riccati_solution_positive_semidefinite(self, sample_4d_system): + """Verify riccati_solution is positive semi-definite""" + A, B, Q = sample_4d_system + R = 1.0 + solution = cl.lqr(A, B, Q, R) + eigenvalues_X = np.linalg.eigvals(solution.riccati_solution) + assert all( + eigenvalues_X >= -1e-10 + ), "Riccati solution should be positive semi-definite" + + def test_lqr_higher_control_cost_smaller_gains(self, sample_4d_system): + """Higher control cost R should result in smaller gains""" + A, B, Q = sample_4d_system + solution_low_R = cl.lqr(A, B, Q, R=0.1) + solution_high_R = cl.lqr(A, B, Q, R=10.0) + assert np.linalg.norm(solution_high_R.feedback_gain) < np.linalg.norm( + solution_low_R.feedback_gain + ), "Higher R should result in smaller gains" + + def test_lqr_eigenvalues_count(self, sample_4d_system): + """Verify correct number of eigenvalues returned""" + A, B, Q = sample_4d_system + R = 1.0 + solution = cl.lqr(A, B, Q, R) + assert ( + len(solution.closed_loop_eigenvalues) == 4 + ), "Should have 4 eigenvalues for 4D system" + + +# Unit tests for get_control_params() function +class TestGetControlParams: + def test_get_control_params_basic(self): + """Verify control parameters are computed correctly""" + params = cl.get_control_params( + DEFAULT_VEHICLE_PARAMS, velocity=10.0, control_weight=1.0 + ) + assert params.lookahead_distance == DEFAULT_VEHICLE_PARAMS.l_s + assert params.lqr_gain.shape == (4,) + assert params.disturbance_compensation_gain > 0 + + def test_get_control_params_gain_shape(self): + """Verify LQR gain has correct shape""" + params = cl.get_control_params( + DEFAULT_VEHICLE_PARAMS, velocity=20.0, control_weight=1.0 + ) + assert params.lqr_gain.shape == (4,), "LQR gain should have 4 elements" + + def test_get_control_params_different_velocities(self): + """Control parameters should change with velocity""" + params_slow = cl.get_control_params( + DEFAULT_VEHICLE_PARAMS, velocity=5.0, control_weight=1.0 + ) + params_fast = cl.get_control_params( + DEFAULT_VEHICLE_PARAMS, velocity=30.0, control_weight=1.0 + ) + # Gains should be different for different velocities + assert not np.allclose( + params_slow.lqr_gain, params_fast.lqr_gain + ), "Gains should differ for different velocities" + # Disturbance compensation should also differ + assert params_slow.disturbance_compensation_gain != pytest.approx( + params_fast.disturbance_compensation_gain + ), "Disturbance compensation should differ for different velocities" + + def test_get_control_params_different_control_weights(self): + """Higher control_weight should result in smaller gains (less aggressive)""" + params_low_weight = cl.get_control_params( + DEFAULT_VEHICLE_PARAMS, velocity=20.0, control_weight=0.1 + ) + params_high_weight = cl.get_control_params( + DEFAULT_VEHICLE_PARAMS, velocity=20.0, control_weight=100.0 + ) + assert np.linalg.norm(params_high_weight.lqr_gain) < np.linalg.norm( + params_low_weight.lqr_gain + ), "Higher control_weight should result in smaller gains" + + def test_get_control_params_disturbance_compensation_positive(self): + """Disturbance compensation should be positive for typical parameters""" + params = cl.get_control_params( + DEFAULT_VEHICLE_PARAMS, velocity=20.0, control_weight=1.0 + ) + assert ( + params.disturbance_compensation_gain > 0 + ), "Disturbance compensation should be positive" + + def test_get_control_params_lookahead_distance(self): + """Lookahead distance should match vehicle parameters""" + params = cl.get_control_params( + DEFAULT_VEHICLE_PARAMS, velocity=20.0, control_weight=1.0 + ) + assert params.lookahead_distance == pytest.approx( + DEFAULT_VEHICLE_PARAMS.l_s + ), "Lookahead distance should match vehicle params" diff --git a/tests/test_lateral_control_state_based.py b/tests/test_lateral_control_state_based.py index 92515a1..541b089 100644 --- a/tests/test_lateral_control_state_based.py +++ b/tests/test_lateral_control_state_based.py @@ -2,30 +2,24 @@ 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.projection import project2curve +from behavior_generation_lecture_python.lateral_control_state_based.lateral_control_state_based import ( + KinematicVehicleState, +) def test_lateral_control_state_based(): radius = 20 - vars_0 = [0.1, -radius, 0.0] + initial_state = KinematicVehicleState(x=0.1, y=-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) + time_vector = np.arange(0, 100, 0.1) + model = cl.LateralControlStateBased(initial_state, curve) + trajectory = model.simulate(time_vector, velocity=1) - errors = [] - for state in sol: - _, _, _, d, _, _ = project2curve( - s_c=curve["s"], - x_c=curve["x"], - y_c=curve["y"], - theta_c=curve["theta"], - kappa_c=curve["kappa"], - x=state[0], - y=state[1], - ) - errors.append(abs(d)) + # trajectory is now a list of ControllerOutput dataclasses + errors = [abs(output.lateral_error) for output in trajectory] - assert np.sum(errors) < len(sol) * 0.01 + assert np.sum(errors) < len(trajectory) * 0.01 diff --git a/tests/test_state_based_control.py b/tests/test_state_based_control.py index 464381d..dbb4d17 100644 --- a/tests/test_state_based_control.py +++ b/tests/test_state_based_control.py @@ -1,3 +1,5 @@ +import math + import pytest from behavior_generation_lecture_python.lateral_control_state_based import ( @@ -7,29 +9,197 @@ def test_feedback_law(): assert state_based_controller.feedback_law( - d=0, psi=0, theta_r=0, kappa_r=0 + lateral_error=0, vehicle_heading=0, reference_heading=0, reference_curvature=0 ) == pytest.approx(0), "zero steering for going straight" assert ( - state_based_controller.feedback_law(d=0.1, psi=0, theta_r=0, kappa_r=0) < 0 + state_based_controller.feedback_law( + lateral_error=0.1, + vehicle_heading=0, + reference_heading=0, + reference_curvature=0, + ) + < 0 ), "neg. steering (to the right) if left of reference curve" assert ( - state_based_controller.feedback_law(d=-0.1, psi=0, theta_r=0, kappa_r=0) > 0 + state_based_controller.feedback_law( + lateral_error=-0.1, + vehicle_heading=0, + reference_heading=0, + reference_curvature=0, + ) + > 0 ), "pos. steering (to the left) if right of reference curve" assert ( - state_based_controller.feedback_law(d=0, psi=0, theta_r=0, kappa_r=0.1) > 0 + state_based_controller.feedback_law( + lateral_error=0, + vehicle_heading=0, + reference_heading=0, + reference_curvature=0.1, + ) + > 0 ), "positive steering (to the left) if on reference curve and ref curve has positive curvature" assert ( - state_based_controller.feedback_law(d=0, psi=0, theta_r=0, kappa_r=-0.1) < 0 + state_based_controller.feedback_law( + lateral_error=0, + vehicle_heading=0, + reference_heading=0, + reference_curvature=-0.1, + ) + < 0 ), "negative steering (to the right) if on reference curve and ref curve has negative curvature" assert ( - state_based_controller.feedback_law(d=0, psi=0.1, theta_r=0.2, kappa_r=0) > 0 + state_based_controller.feedback_law( + lateral_error=0, + vehicle_heading=0.1, + reference_heading=0.2, + reference_curvature=0, + ) + > 0 ), "positive steering (to the left) if reference curve heads further left" assert ( - state_based_controller.feedback_law(d=0, psi=-0.1, theta_r=-0.2, kappa_r=0) < 0 + state_based_controller.feedback_law( + lateral_error=0, + vehicle_heading=-0.1, + reference_heading=-0.2, + reference_curvature=0, + ) + < 0 ), "negative steering (to the right) if reference curve heads further right" + + +def test_feedback_law_angle_wrapping(): + """Test behavior when angles are near +/- pi boundaries""" + # psi near +pi, theta_r near -pi (they are actually close due to wrapping) + # The normalized difference should be small, so steering should be small + steering = state_based_controller.feedback_law( + lateral_error=0, + vehicle_heading=3.0, + reference_heading=-3.0, + reference_curvature=0, + ) + # The angle difference after normalization: 3.0 - (-3.0) = 6.0 + # but normalized: 6.0 wraps to about -0.28 radians + # So steering should be positive (steer left) but small + assert abs(steering) < 1.0, "Angle wrapping should result in reasonable steering" + + # Test exact pi boundary + steering_at_pi = state_based_controller.feedback_law( + lateral_error=0, + vehicle_heading=math.pi - 0.1, + reference_heading=-math.pi + 0.1, + reference_curvature=0, + ) + # These angles are close (differ by ~0.2 radians across the pi boundary) + assert abs(steering_at_pi) < 0.6, "Steering near pi boundary should be reasonable" + + +def test_feedback_law_straight_line(): + """Test with zero curvature (straight reference)""" + # Heading error to the left, should steer right + steering = state_based_controller.feedback_law( + lateral_error=0, + vehicle_heading=0.1, + reference_heading=0, + reference_curvature=0, + ) + assert steering < 0, "Should steer right to correct left heading error" + + # Heading error to the right, should steer left + steering = state_based_controller.feedback_law( + lateral_error=0, + vehicle_heading=-0.1, + reference_heading=0, + reference_curvature=0, + ) + assert steering > 0, "Should steer left to correct right heading error" + + +def test_feedback_law_combined_errors(): + """Test with multiple error sources combined""" + # Left of reference (lateral_error > 0) and heading left (psi > theta_r) + # Both errors suggest steering right (negative) + steering = state_based_controller.feedback_law( + lateral_error=1.0, + vehicle_heading=0.2, + reference_heading=0, + reference_curvature=0, + ) + assert steering < 0, "Combined errors should reinforce steering direction" + + # Right of reference (lateral_error < 0) and heading right (psi < theta_r) + # Both errors suggest steering left (positive) + steering = state_based_controller.feedback_law( + lateral_error=-1.0, + vehicle_heading=-0.2, + reference_heading=0, + reference_curvature=0, + ) + assert steering > 0, "Combined errors should reinforce steering direction" + + +def test_feedback_law_opposing_errors(): + """Test with opposing error sources""" + # Left of reference (lateral_error > 0) but heading right (psi < theta_r) + # Errors partially cancel + steering_opposing = state_based_controller.feedback_law( + lateral_error=0.5, + vehicle_heading=-0.1, + reference_heading=0, + reference_curvature=0, + ) + steering_lateral_only = state_based_controller.feedback_law( + lateral_error=0.5, + vehicle_heading=0, + reference_heading=0, + reference_curvature=0, + ) + # Opposing heading should reduce the steering magnitude + assert abs(steering_opposing) < abs( + steering_lateral_only + ), "Opposing errors should reduce steering" + + +def test_feedback_law_large_lateral_error(): + """Test with large lateral errors""" + steering_large = state_based_controller.feedback_law( + lateral_error=10.0, + vehicle_heading=0, + reference_heading=0, + reference_curvature=0, + ) + steering_small = state_based_controller.feedback_law( + lateral_error=0.1, + vehicle_heading=0, + reference_heading=0, + reference_curvature=0, + ) + assert abs(steering_large) > abs( + steering_small + ), "Larger lateral error should cause larger steering" + # Steering should be bounded by arctan + assert abs(steering_large) < math.pi / 2, "Steering should be bounded" + + +def test_feedback_law_large_curvature(): + """Test with large reference curvature""" + steering_large_kappa = state_based_controller.feedback_law( + lateral_error=0, + vehicle_heading=0, + reference_heading=0, + reference_curvature=1.0, + ) + steering_small_kappa = state_based_controller.feedback_law( + lateral_error=0, + vehicle_heading=0, + reference_heading=0, + reference_curvature=0.01, + ) + assert abs(steering_large_kappa) > abs( + steering_small_kappa + ), "Larger curvature should cause larger steering" diff --git a/tests/utils/test_generate_reference_curve.py b/tests/utils/test_generate_reference_curve.py index 18c1412..4298190 100644 --- a/tests/utils/test_generate_reference_curve.py +++ b/tests/utils/test_generate_reference_curve.py @@ -5,10 +5,10 @@ def test_straight_line(): - x_input = [0, 1, 2, 3] - y_input = [0, 1, 2, 3] + x_input = np.array([0, 1, 2, 3]) + y_input = np.array([0, 1, 2, 3]) curve = generate_reference_curve.generate_reference_curve(x_input, y_input, 1.0) - assert np.allclose(curve["x"], curve["y"]) - assert curve["s"][2] == pytest.approx(2.0) - assert np.allclose(curve["kappa"], np.array([0.0] * len(curve["kappa"]))) - assert np.allclose(curve["theta"], np.array([np.pi / 4.0] * len(curve["theta"]))) + assert np.allclose(curve.x, curve.y) + assert curve.arc_length[2] == pytest.approx(2.0) + assert np.allclose(curve.curvature, np.array([0.0] * len(curve.curvature))) + assert np.allclose(curve.heading, np.array([np.pi / 4.0] * len(curve.heading))) diff --git a/tests/utils/test_projection.py b/tests/utils/test_projection.py index 7aa550b..ebcd823 100644 --- a/tests/utils/test_projection.py +++ b/tests/utils/test_projection.py @@ -35,7 +35,7 @@ def test_project2curve(): with pytest.warns( UserWarning, match="Extrapolating over start of reference curve!" ): - projected_point = projection.project2curve( + result = projection.project2curve( s_c=sc.curve_points_s, x_c=sc.curve_points_x, y_c=sc.curve_points_y, @@ -44,17 +44,16 @@ def test_project2curve(): x=target_point[0], y=target_point[1], ) - x_p, y_p, s_p, d, theta_p, kappa_p = projected_point - assert x_p == -1 - assert y_p == 0 - assert s_p == -1 - assert d == pytest.approx(0) - assert kappa_p == -10 - # theta is not meaningful here + assert result.x == -1 + assert result.y == 0 + assert result.arc_length == -1 + assert result.lateral_error == pytest.approx(0) + assert result.curvature == -10 + # heading is not meaningful here target_point = [4, 3] with pytest.warns(UserWarning, match="Extrapolating over end of reference curve!"): - projected_point = projection.project2curve( + result = projection.project2curve( s_c=sc.curve_points_s, x_c=sc.curve_points_x, y_c=sc.curve_points_y, @@ -63,17 +62,16 @@ def test_project2curve(): x=target_point[0], y=target_point[1], ) - x_p, y_p, s_p, d, theta_p, kappa_p = projected_point - assert x_p == pytest.approx(3.2, abs=0.1) - assert y_p == pytest.approx(3.4, abs=0.1) - assert s_p == pytest.approx(3.4, abs=0.1) - assert d == pytest.approx(-0.9, abs=0.1) - assert theta_p == pytest.approx(np.arctan(2)) - assert kappa_p == -10 + assert result.x == pytest.approx(3.2, abs=0.1) + assert result.y == pytest.approx(3.4, abs=0.1) + assert result.arc_length == pytest.approx(3.4, abs=0.1) + assert result.lateral_error == pytest.approx(-0.9, abs=0.1) + assert result.heading == pytest.approx(np.arctan(2)) + assert result.curvature == -10 target_point = [1, 1] with warnings.catch_warnings(): - projected_point = projection.project2curve( + result = projection.project2curve( s_c=sc.curve_points_s, x_c=sc.curve_points_x, y_c=sc.curve_points_y, @@ -82,5 +80,4 @@ def test_project2curve(): x=target_point[0], y=target_point[1], ) - x_p, y_p, s_p, d, theta_p, kappa_p = projected_point # todo: assertions