Paper_code.ipynb

{
  "nbformat": 4,
  "nbformat_minor": 0,
  "metadata": {
    "kernelspec": {
      "display_name": "Python 3",
      "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.7.4"
    },
    "colab": {
      "provenance": []
    },
    "widgets": {
      "application/vnd.jupyter.widget-state+json": {
        "c835573a8fde448fbf339254ebf948f4": {
          "model_module": "@jupyter-widgets/controls",
          "model_name": "VBoxModel",
          "model_module_version": "1.5.0",
          "state": {
            "_dom_classes": [
              "widget-interact"
            ],
            "_model_module": "@jupyter-widgets/controls",
            "_model_module_version": "1.5.0",
            "_model_name": "VBoxModel",
            "_view_count": null,
            "_view_module": "@jupyter-widgets/controls",
            "_view_module_version": "1.5.0",
            "_view_name": "VBoxView",
            "box_style": "",
            "children": [
              "IPY_MODEL_4e36ee2ebf944eeaa806185ed3b5962b",
              "IPY_MODEL_732c7ea62b144cfd917969016e728390",
              "IPY_MODEL_fa29b1673a9d4dafb91a0f768c35165d",
              "IPY_MODEL_679f06be7a3845f0a4ba6de53f062765",
              "IPY_MODEL_464217ad0a6544ff9c72f1f2c57ee044"
            ],
            "layout": "IPY_MODEL_aa74d0b0cfab47d7abc58f8fe187ec61"
          }
        },
        "4e36ee2ebf944eeaa806185ed3b5962b": {
          "model_module": "@jupyter-widgets/controls",
          "model_name": "DropdownModel",
          "model_module_version": "1.5.0",
          "state": {
            "_dom_classes": [],
            "_model_module": "@jupyter-widgets/controls",
            "_model_module_version": "1.5.0",
            "_model_name": "DropdownModel",
            "_options_labels": [
              "Main",
              "Circle",
              "Low Energy",
              "Dubins",
              "Lyapunov",
              "High Energy",
              "RobinsMathew"
            ],
            "_view_count": null,
            "_view_module": "@jupyter-widgets/controls",
            "_view_module_version": "1.5.0",
            "_view_name": "DropdownView",
            "description": "experimTag",
            "description_tooltip": null,
            "disabled": false,
            "index": 0,
            "layout": "IPY_MODEL_b19b510633a74717828b80f40ad78d9e",
            "style": "IPY_MODEL_d6a58da568fa492cb5d3b98eddebf142"
          }
        },
        "732c7ea62b144cfd917969016e728390": {
          "model_module": "@jupyter-widgets/controls",
          "model_name": "DropdownModel",
          "model_module_version": "1.5.0",
          "state": {
            "_dom_classes": [],
            "_model_module": "@jupyter-widgets/controls",
            "_model_module_version": "1.5.0",
            "_model_name": "DropdownModel",
            "_options_labels": [
              "Square path",
              "Main",
              "Straightforward path",
              "Supportive",
              "Straightforward path with many waypoints",
              "Diamond-Shape path",
              "Sharp turn path",
              "Custom"
            ],
            "_view_count": null,
            "_view_module": "@jupyter-widgets/controls",
            "_view_module_version": "1.5.0",
            "_view_name": "DropdownView",
            "description": "pathTag",
            "description_tooltip": null,
            "disabled": false,
            "index": 0,
            "layout": "IPY_MODEL_6e22c9f4871946c3bd3159b4db148678",
            "style": "IPY_MODEL_5934939b10d0414cb5ac64c739fe1bbe"
          }
        },
        "fa29b1673a9d4dafb91a0f768c35165d": {
          "model_module": "@jupyter-widgets/controls",
          "model_name": "DropdownModel",
          "model_module_version": "1.5.0",
          "state": {
            "_dom_classes": [],
            "_model_module": "@jupyter-widgets/controls",
            "_model_module_version": "1.5.0",
            "_model_name": "DropdownModel",
            "_options_labels": [
              "text",
              "all",
              "path",
              "distance",
              "energy"
            ],
            "_view_count": null,
            "_view_module": "@jupyter-widgets/controls",
            "_view_module_version": "1.5.0",
            "_view_name": "DropdownView",
            "description": "visualizations",
            "description_tooltip": null,
            "disabled": false,
            "index": 0,
            "layout": "IPY_MODEL_5b8e5282ae2545c6ba4d88612f8416d1",
            "style": "IPY_MODEL_b88467ce98a1466f959a25211e6fe18a"
          }
        },
        "679f06be7a3845f0a4ba6de53f062765": {
          "model_module": "@jupyter-widgets/controls",
          "model_name": "CheckboxModel",
          "model_module_version": "1.5.0",
          "state": {
            "_dom_classes": [],
            "_model_module": "@jupyter-widgets/controls",
            "_model_module_version": "1.5.0",
            "_model_name": "CheckboxModel",
            "_view_count": null,
            "_view_module": "@jupyter-widgets/controls",
            "_view_module_version": "1.5.0",
            "_view_name": "CheckboxView",
            "description": "smallImages",
            "description_tooltip": null,
            "disabled": false,
            "indent": true,
            "layout": "IPY_MODEL_e557b3e70a5a4ab895b49f53e2e0d27e",
            "style": "IPY_MODEL_6ff6a3aa9cc94f35b04f8d158baefafe",
            "value": true
          }
        },
        "464217ad0a6544ff9c72f1f2c57ee044": {
          "model_module": "@jupyter-widgets/output",
          "model_name": "OutputModel",
          "model_module_version": "1.0.0",
          "state": {
            "_dom_classes": [],
            "_model_module": "@jupyter-widgets/output",
            "_model_module_version": "1.0.0",
            "_model_name": "OutputModel",
            "_view_count": null,
            "_view_module": "@jupyter-widgets/output",
            "_view_module_version": "1.0.0",
            "_view_name": "OutputView",
            "layout": "IPY_MODEL_cfb83205e55144758a1c4032d9f4f5fb",
            "msg_id": "",
            "outputs": [
              {
                "output_type": "stream",
                "name": "stdout",
                "text": [
                  "Simulating, please be patient\n"
                ]
              },
              {
                "output_type": "stream",
                "name": "stdout",
                "text": [
                  "******************************\n",
                  "* Path name: Square path\n",
                  "******************************\n"
                ]
              },
              {
                "output_type": "display_data",
                "data": {
                  "text/plain": "                Experiment      Total distance        Total energy  \\\n0  Circle [v = 1.0, g = 4]   42.95221338089532   4.552543532987671   \n1  Circle [v = 1.0, g = 8]   42.68933859842528   4.572306937022359   \n2                   Dubins   42.62717224241682   4.673627090143663   \n3                 Lyapunov   41.18647614665072  16.782510732550506   \n4                 Geometry   41.22299863009624  16.793426586958944   \n5             RobinsMathew  41.223065912911046  16.793482805437343   \n6                 Kanayama   41.26941004793312  16.801473907660508   \n\n   lowVelocityLimit  highVelocityLimit  lowOmegaLimit  highOmegaLimit  vri  \\\n0               0.2                1.0          -0.75            0.75  1.0   \n1               0.2                1.0          -0.75            0.75  1.0   \n2               0.2                1.0          -0.75            0.75  1.0   \n3               0.2                1.0          -0.75            0.75  1.0   \n4               0.2                1.0          -0.75            0.75  1.0   \n5               0.2                1.0          -0.75            0.75  1.0   \n6               0.2                1.0          -0.75            0.75  1.0   \n\n      k1    k2   k3  \n0  4.000   NaN  NaN  \n1  8.000   NaN  NaN  \n2  1.000   NaN  NaN  \n3  5.000  0.01  NaN  \n4  7.000   NaN  NaN  \n5  1.500  6.50  NaN  \n6  0.001  0.10  5.0  ",
                  "text/html": "\n  <div id=\"df-83724f54-e314-4927-a951-72ca1e76330b\">\n    <div class=\"colab-df-container\">\n      <div>\n<style scoped>\n    .dataframe tbody tr th:only-of-type {\n        vertical-align: middle;\n    }\n\n    .dataframe tbody tr th {\n        vertical-align: top;\n    }\n\n    .dataframe thead th {\n        text-align: right;\n    }\n</style>\n<table border=\"1\" class=\"dataframe\">\n  <thead>\n    <tr style=\"text-align: right;\">\n      <th></th>\n      <th>Experiment</th>\n      <th>Total distance</th>\n      <th>Total energy</th>\n      <th>lowVelocityLimit</th>\n      <th>highVelocityLimit</th>\n      <th>lowOmegaLimit</th>\n      <th>highOmegaLimit</th>\n      <th>vri</th>\n      <th>k1</th>\n      <th>k2</th>\n      <th>k3</th>\n    </tr>\n  </thead>\n  <tbody>\n    <tr>\n      <th>0</th>\n      <td>Circle [v = 1.0, g = 4]</td>\n      <td>42.95221338089532</td>\n      <td>4.552543532987671</td>\n      <td>0.2</td>\n      <td>1.0</td>\n      <td>-0.75</td>\n      <td>0.75</td>\n      <td>1.0</td>\n      <td>4.000</td>\n      <td>NaN</td>\n      <td>NaN</td>\n    </tr>\n    <tr>\n      <th>1</th>\n      <td>Circle [v = 1.0, g = 8]</td>\n      <td>42.68933859842528</td>\n      <td>4.572306937022359</td>\n      <td>0.2</td>\n      <td>1.0</td>\n      <td>-0.75</td>\n      <td>0.75</td>\n      <td>1.0</td>\n      <td>8.000</td>\n      <td>NaN</td>\n      <td>NaN</td>\n    </tr>\n    <tr>\n      <th>2</th>\n      <td>Dubins</td>\n      <td>42.62717224241682</td>\n      <td>4.673627090143663</td>\n      <td>0.2</td>\n      <td>1.0</td>\n      <td>-0.75</td>\n      <td>0.75</td>\n      <td>1.0</td>\n      <td>1.000</td>\n      <td>NaN</td>\n      <td>NaN</td>\n    </tr>\n    <tr>\n      <th>3</th>\n      <td>Lyapunov</td>\n      <td>41.18647614665072</td>\n      <td>16.782510732550506</td>\n      <td>0.2</td>\n      <td>1.0</td>\n      <td>-0.75</td>\n      <td>0.75</td>\n      <td>1.0</td>\n      <td>5.000</td>\n      <td>0.01</td>\n      <td>NaN</td>\n    </tr>\n    <tr>\n      <th>4</th>\n      <td>Geometry</td>\n      <td>41.22299863009624</td>\n      <td>16.793426586958944</td>\n      <td>0.2</td>\n      <td>1.0</td>\n      <td>-0.75</td>\n      <td>0.75</td>\n      <td>1.0</td>\n      <td>7.000</td>\n      <td>NaN</td>\n      <td>NaN</td>\n    </tr>\n    <tr>\n      <th>5</th>\n      <td>RobinsMathew</td>\n      <td>41.223065912911046</td>\n      <td>16.793482805437343</td>\n      <td>0.2</td>\n      <td>1.0</td>\n      <td>-0.75</td>\n      <td>0.75</td>\n      <td>1.0</td>\n      <td>1.500</td>\n      <td>6.50</td>\n      <td>NaN</td>\n    </tr>\n    <tr>\n      <th>6</th>\n      <td>Kanayama</td>\n      <td>41.26941004793312</td>\n      <td>16.801473907660508</td>\n      <td>0.2</td>\n      <td>1.0</td>\n      <td>-0.75</td>\n      <td>0.75</td>\n      <td>1.0</td>\n      <td>0.001</td>\n      <td>0.10</td>\n      <td>5.0</td>\n    </tr>\n  </tbody>\n</table>\n</div>\n      <button class=\"colab-df-convert\" onclick=\"convertToInteractive('df-83724f54-e314-4927-a951-72ca1e76330b')\"\n              title=\"Convert this dataframe to an interactive table.\"\n              style=\"display:none;\">\n        \n  <svg xmlns=\"http://www.w3.org/2000/svg\" height=\"24px\"viewBox=\"0 0 24 24\"\n       width=\"24px\">\n    <path d=\"M0 0h24v24H0V0z\" fill=\"none\"/>\n    <path d=\"M18.56 5.44l.94 2.06.94-2.06 2.06-.94-2.06-.94-.94-2.06-.94 2.06-2.06.94zm-11 1L8.5 8.5l.94-2.06 2.06-.94-2.06-.94L8.5 2.5l-.94 2.06-2.06.94zm10 10l.94 2.06.94-2.06 2.06-.94-2.06-.94-.94-2.06-.94 2.06-2.06.94z\"/><path d=\"M17.41 7.96l-1.37-1.37c-.4-.4-.92-.59-1.43-.59-.52 0-1.04.2-1.43.59L10.3 9.45l-7.72 7.72c-.78.78-.78 2.05 0 2.83L4 21.41c.39.39.9.59 1.41.59.51 0 1.02-.2 1.41-.59l7.78-7.78 2.81-2.81c.8-.78.8-2.07 0-2.86zM5.41 20L4 18.59l7.72-7.72 1.47 1.35L5.41 20z\"/>\n  </svg>\n      </button>\n      \n  <style>\n    .colab-df-container {\n      display:flex;\n      flex-wrap:wrap;\n      gap: 12px;\n    }\n\n    .colab-df-convert {\n      background-color: #E8F0FE;\n      border: none;\n      border-radius: 50%;\n      cursor: pointer;\n      display: none;\n      fill: #1967D2;\n      height: 32px;\n      padding: 0 0 0 0;\n      width: 32px;\n    }\n\n    .colab-df-convert:hover {\n      background-color: #E2EBFA;\n      box-shadow: 0px 1px 2px rgba(60, 64, 67, 0.3), 0px 1px 3px 1px rgba(60, 64, 67, 0.15);\n      fill: #174EA6;\n    }\n\n    [theme=dark] .colab-df-convert {\n      background-color: #3B4455;\n      fill: #D2E3FC;\n    }\n\n    [theme=dark] .colab-df-convert:hover {\n      background-color: #434B5C;\n      box-shadow: 0px 1px 3px 1px rgba(0, 0, 0, 0.15);\n      filter: drop-shadow(0px 1px 2px rgba(0, 0, 0, 0.3));\n      fill: #FFFFFF;\n    }\n  </style>\n\n      <script>\n        const buttonEl =\n          document.querySelector('#df-83724f54-e314-4927-a951-72ca1e76330b button.colab-df-convert');\n        buttonEl.style.display =\n          google.colab.kernel.accessAllowed ? 'block' : 'none';\n\n        async function convertToInteractive(key) {\n          const element = document.querySelector('#df-83724f54-e314-4927-a951-72ca1e76330b');\n          const dataTable =\n            await google.colab.kernel.invokeFunction('convertToInteractive',\n                                                     [key], {});\n          if (!dataTable) return;\n\n          const docLinkHtml = 'Like what you see? Visit the ' +\n            '<a target=\"_blank\" href=https://colab.research.google.com/notebooks/data_table.ipynb>data table notebook</a>'\n            + ' to learn more about interactive tables.';\n          element.innerHTML = '';\n          dataTable['output_type'] = 'display_data';\n          await google.colab.output.renderOutput(dataTable, element);\n          const docLink = document.createElement('div');\n          docLink.innerHTML = docLinkHtml;\n          element.appendChild(docLink);\n        }\n      </script>\n    </div>\n  </div>\n  "
                },
                "metadata": {}
              }
            ]
          }
        },
        "aa74d0b0cfab47d7abc58f8fe187ec61": {
          "model_module": "@jupyter-widgets/base",
          "model_name": "LayoutModel",
          "model_module_version": "1.2.0",
          "state": {
            "_model_module": "@jupyter-widgets/base",
            "_model_module_version": "1.2.0",
            "_model_name": "LayoutModel",
            "_view_count": null,
            "_view_module": "@jupyter-widgets/base",
            "_view_module_version": "1.2.0",
            "_view_name": "LayoutView",
            "align_content": null,
            "align_items": null,
            "align_self": null,
            "border": null,
            "bottom": null,
            "display": null,
            "flex": null,
            "flex_flow": null,
            "grid_area": null,
            "grid_auto_columns": null,
            "grid_auto_flow": null,
            "grid_auto_rows": null,
            "grid_column": null,
            "grid_gap": null,
            "grid_row": null,
            "grid_template_areas": null,
            "grid_template_columns": null,
            "grid_template_rows": null,
            "height": null,
            "justify_content": null,
            "justify_items": null,
            "left": null,
            "margin": null,
            "max_height": null,
            "max_width": null,
            "min_height": null,
            "min_width": null,
            "object_fit": null,
            "object_position": null,
            "order": null,
            "overflow": null,
            "overflow_x": null,
            "overflow_y": null,
            "padding": null,
            "right": null,
            "top": null,
            "visibility": null,
            "width": null
          }
        },
        "b19b510633a74717828b80f40ad78d9e": {
          "model_module": "@jupyter-widgets/base",
          "model_name": "LayoutModel",
          "model_module_version": "1.2.0",
          "state": {
            "_model_module": "@jupyter-widgets/base",
            "_model_module_version": "1.2.0",
            "_model_name": "LayoutModel",
            "_view_count": null,
            "_view_module": "@jupyter-widgets/base",
            "_view_module_version": "1.2.0",
            "_view_name": "LayoutView",
            "align_content": null,
            "align_items": null,
            "align_self": null,
            "border": null,
            "bottom": null,
            "display": null,
            "flex": null,
            "flex_flow": null,
            "grid_area": null,
            "grid_auto_columns": null,
            "grid_auto_flow": null,
            "grid_auto_rows": null,
            "grid_column": null,
            "grid_gap": null,
            "grid_row": null,
            "grid_template_areas": null,
            "grid_template_columns": null,
            "grid_template_rows": null,
            "height": null,
            "justify_content": null,
            "justify_items": null,
            "left": null,
            "margin": null,
            "max_height": null,
            "max_width": null,
            "min_height": null,
            "min_width": null,
            "object_fit": null,
            "object_position": null,
            "order": null,
            "overflow": null,
            "overflow_x": null,
            "overflow_y": null,
            "padding": null,
            "right": null,
            "top": null,
            "visibility": null,
            "width": null
          }
        },
        "d6a58da568fa492cb5d3b98eddebf142": {
          "model_module": "@jupyter-widgets/controls",
          "model_name": "DescriptionStyleModel",
          "model_module_version": "1.5.0",
          "state": {
            "_model_module": "@jupyter-widgets/controls",
            "_model_module_version": "1.5.0",
            "_model_name": "DescriptionStyleModel",
            "_view_count": null,
            "_view_module": "@jupyter-widgets/base",
            "_view_module_version": "1.2.0",
            "_view_name": "StyleView",
            "description_width": ""
          }
        },
        "6e22c9f4871946c3bd3159b4db148678": {
          "model_module": "@jupyter-widgets/base",
          "model_name": "LayoutModel",
          "model_module_version": "1.2.0",
          "state": {
            "_model_module": "@jupyter-widgets/base",
            "_model_module_version": "1.2.0",
            "_model_name": "LayoutModel",
            "_view_count": null,
            "_view_module": "@jupyter-widgets/base",
            "_view_module_version": "1.2.0",
            "_view_name": "LayoutView",
            "align_content": null,
            "align_items": null,
            "align_self": null,
            "border": null,
            "bottom": null,
            "display": null,
            "flex": null,
            "flex_flow": null,
            "grid_area": null,
            "grid_auto_columns": null,
            "grid_auto_flow": null,
            "grid_auto_rows": null,
            "grid_column": null,
            "grid_gap": null,
            "grid_row": null,
            "grid_template_areas": null,
            "grid_template_columns": null,
            "grid_template_rows": null,
            "height": null,
            "justify_content": null,
            "justify_items": null,
            "left": null,
            "margin": null,
            "max_height": null,
            "max_width": null,
            "min_height": null,
            "min_width": null,
            "object_fit": null,
            "object_position": null,
            "order": null,
            "overflow": null,
            "overflow_x": null,
            "overflow_y": null,
            "padding": null,
            "right": null,
            "top": null,
            "visibility": null,
            "width": null
          }
        },
        "5934939b10d0414cb5ac64c739fe1bbe": {
          "model_module": "@jupyter-widgets/controls",
          "model_name": "DescriptionStyleModel",
          "model_module_version": "1.5.0",
          "state": {
            "_model_module": "@jupyter-widgets/controls",
            "_model_module_version": "1.5.0",
            "_model_name": "DescriptionStyleModel",
            "_view_count": null,
            "_view_module": "@jupyter-widgets/base",
            "_view_module_version": "1.2.0",
            "_view_name": "StyleView",
            "description_width": ""
          }
        },
        "5b8e5282ae2545c6ba4d88612f8416d1": {
          "model_module": "@jupyter-widgets/base",
          "model_name": "LayoutModel",
          "model_module_version": "1.2.0",
          "state": {
            "_model_module": "@jupyter-widgets/base",
            "_model_module_version": "1.2.0",
            "_model_name": "LayoutModel",
            "_view_count": null,
            "_view_module": "@jupyter-widgets/base",
            "_view_module_version": "1.2.0",
            "_view_name": "LayoutView",
            "align_content": null,
            "align_items": null,
            "align_self": null,
            "border": null,
            "bottom": null,
            "display": null,
            "flex": null,
            "flex_flow": null,
            "grid_area": null,
            "grid_auto_columns": null,
            "grid_auto_flow": null,
            "grid_auto_rows": null,
            "grid_column": null,
            "grid_gap": null,
            "grid_row": null,
            "grid_template_areas": null,
            "grid_template_columns": null,
            "grid_template_rows": null,
            "height": null,
            "justify_content": null,
            "justify_items": null,
            "left": null,
            "margin": null,
            "max_height": null,
            "max_width": null,
            "min_height": null,
            "min_width": null,
            "object_fit": null,
            "object_position": null,
            "order": null,
            "overflow": null,
            "overflow_x": null,
            "overflow_y": null,
            "padding": null,
            "right": null,
            "top": null,
            "visibility": null,
            "width": null
          }
        },
        "b88467ce98a1466f959a25211e6fe18a": {
          "model_module": "@jupyter-widgets/controls",
          "model_name": "DescriptionStyleModel",
          "model_module_version": "1.5.0",
          "state": {
            "_model_module": "@jupyter-widgets/controls",
            "_model_module_version": "1.5.0",
            "_model_name": "DescriptionStyleModel",
            "_view_count": null,
            "_view_module": "@jupyter-widgets/base",
            "_view_module_version": "1.2.0",
            "_view_name": "StyleView",
            "description_width": ""
          }
        },
        "e557b3e70a5a4ab895b49f53e2e0d27e": {
          "model_module": "@jupyter-widgets/base",
          "model_name": "LayoutModel",
          "model_module_version": "1.2.0",
          "state": {
            "_model_module": "@jupyter-widgets/base",
            "_model_module_version": "1.2.0",
            "_model_name": "LayoutModel",
            "_view_count": null,
            "_view_module": "@jupyter-widgets/base",
            "_view_module_version": "1.2.0",
            "_view_name": "LayoutView",
            "align_content": null,
            "align_items": null,
            "align_self": null,
            "border": null,
            "bottom": null,
            "display": null,
            "flex": null,
            "flex_flow": null,
            "grid_area": null,
            "grid_auto_columns": null,
            "grid_auto_flow": null,
            "grid_auto_rows": null,
            "grid_column": null,
            "grid_gap": null,
            "grid_row": null,
            "grid_template_areas": null,
            "grid_template_columns": null,
            "grid_template_rows": null,
            "height": null,
            "justify_content": null,
            "justify_items": null,
            "left": null,
            "margin": null,
            "max_height": null,
            "max_width": null,
            "min_height": null,
            "min_width": null,
            "object_fit": null,
            "object_position": null,
            "order": null,
            "overflow": null,
            "overflow_x": null,
            "overflow_y": null,
            "padding": null,
            "right": null,
            "top": null,
            "visibility": null,
            "width": null
          }
        },
        "6ff6a3aa9cc94f35b04f8d158baefafe": {
          "model_module": "@jupyter-widgets/controls",
          "model_name": "DescriptionStyleModel",
          "model_module_version": "1.5.0",
          "state": {
            "_model_module": "@jupyter-widgets/controls",
            "_model_module_version": "1.5.0",
            "_model_name": "DescriptionStyleModel",
            "_view_count": null,
            "_view_module": "@jupyter-widgets/base",
            "_view_module_version": "1.2.0",
            "_view_name": "StyleView",
            "description_width": ""
          }
        },
        "cfb83205e55144758a1c4032d9f4f5fb": {
          "model_module": "@jupyter-widgets/base",
          "model_name": "LayoutModel",
          "model_module_version": "1.2.0",
          "state": {
            "_model_module": "@jupyter-widgets/base",
            "_model_module_version": "1.2.0",
            "_model_name": "LayoutModel",
            "_view_count": null,
            "_view_module": "@jupyter-widgets/base",
            "_view_module_version": "1.2.0",
            "_view_name": "LayoutView",
            "align_content": null,
            "align_items": null,
            "align_self": null,
            "border": null,
            "bottom": null,
            "display": null,
            "flex": null,
            "flex_flow": null,
            "grid_area": null,
            "grid_auto_columns": null,
            "grid_auto_flow": null,
            "grid_auto_rows": null,
            "grid_column": null,
            "grid_gap": null,
            "grid_row": null,
            "grid_template_areas": null,
            "grid_template_columns": null,
            "grid_template_rows": null,
            "height": null,
            "justify_content": null,
            "justify_items": null,
            "left": null,
            "margin": null,
            "max_height": null,
            "max_width": null,
            "min_height": null,
            "min_width": null,
            "object_fit": null,
            "object_position": null,
            "order": null,
            "overflow": null,
            "overflow_x": null,
            "overflow_y": null,
            "padding": null,
            "right": null,
            "top": null,
            "visibility": null,
            "width": null
          }
        }
      }
    }
  },
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "SS5dWeVcFtXe"
      },
      "source": [
        "#Energy Comparison of Controllers Used for Differential Drive Wheeled Mobile Robot"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "Ult-xen6YyfR"
      },
      "source": [
        "This notebook contains the implementation of the different control algorithms and there respective simulations. If you want to include your own controller you can go to the section **Controllers**, subsection **Define your controller here**.\n",
        "\n",
        "The last section is **Interactive Simulation** where you can choose different experiment sets on different paths. \n",
        "\n",
        "\n"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "ScfXVj8VmEFV"
      },
      "source": [
        "import numpy as np\n",
        "import math\n",
        "from math import *\n",
        "import scipy.integrate as integrate\n",
        "import matplotlib.pyplot as plt\n",
        "import pandas as pd\n",
        "%matplotlib inline"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "Usarjl5UGYLC"
      },
      "source": [
        "## Robot Model"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "TH62-ude0NAV"
      },
      "source": [
        "r = 0.0925\n",
        "b = 0.37\n",
        "m = 9  \n",
        "I = 0.16245\n",
        "start_x = 0\n",
        "start_y = 0\n",
        "start_theta =  45\n",
        "start_theta = (start_theta * math.pi) / 180"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "I2xPyyz6GD9g"
      },
      "source": [
        "### Robot model implementation\n",
        "Function\n",
        "```python\n",
        "def robot\n",
        "```\n",
        "is prepared for future differential equation expression as could be expressed as $f_r$.\n"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "9jV6xnId2t9A"
      },
      "source": [
        "#### Kinematic model"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "xgG1KvGZ4yxk"
      },
      "source": [
        "from math import sin, cos\n",
        "def robot(t, currentState, controller):\n",
        "  # ask controller for velocity and omega\n",
        "  velocity, omega = controller(t, currentState)\n",
        "\n",
        "  currentTheta = currentState[2]\n",
        "  cosTheta = cos(currentTheta)\n",
        "  sinTheta = sin(currentTheta)\n",
        "\n",
        "  x_dot = velocity * cosTheta\n",
        "  y_dot = velocity * sinTheta\n",
        "  theta_dot = omega\n",
        "\n",
        "  E = 0.5*(m*(velocity)*(velocity) + I*(omega)*(omega))\n",
        "\n",
        "  return [x_dot, y_dot, theta_dot, velocity, omega, E] #velocity, omega, E are returned for easy evaluation they are not needed for computation"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "-8OsewAO_m6G"
      },
      "source": [
        "#### Dynamic Model\n"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "y-VYt8Wu_mY2"
      },
      "source": [
        "#//////////////////////////////////////////////////////////////////////////////\n",
        "# Calculating the coefficients for DC motor\n",
        "J = 0.01\n",
        "B = 0.1\n",
        "Kt = Ke = K = 0.01\n",
        "Ra = 0.1\n",
        "La = 0.01\n",
        "b1 = K/(La*J)\n",
        "b0 = 0\n",
        "a1 = -(Ra*B + K*K)/(La*J)\n",
        "a0 = -(Ra*J + La*B)/(La*J)\n",
        "#//////////////////////////////////////////////////////////////////////////////\n",
        "def createFilter2ndOrder(b1, b0, a1, a0):\n",
        "  def filter2ndOrder(t, u, currentState):\n",
        "    x0 = currentState[0]\n",
        "    x1 = currentState[1]\n",
        "    dx0 = b0 * u + a0 * x0 + x1\n",
        "    dx1 = b1 * u + a1 * x0\n",
        "    return [dx0, dx1]\n",
        "  return filter2ndOrder\n",
        "\n",
        "def createRobotModelWithDynamic(b1, b0, a1, a0):\n",
        "  motorAsFilter = createFilter2ndOrder(b1, b0, a1, a0)\n",
        "  def robot(t, currentState, controller):\n",
        "    # ask controller for velocity and omega\n",
        "    velocity, omega = controller(t, currentState)\n",
        "    \n",
        "    delta = omega * b / 2\n",
        "    vL = velocity - delta\n",
        "    vR = velocity + delta\n",
        "    vLState = currentState[6:8]\n",
        "    vRState = currentState[8:10]\n",
        "    vLStateD = motorAsFilter(t, vL, vLState)\n",
        "    vRStateD = motorAsFilter(t, vR, vRState)\n",
        "    vLFiltered = vLState[0]\n",
        "    vRFiltered = vRState[0]\n",
        "\n",
        "    velocity = (vRFiltered + vLFiltered) / 2\n",
        "    delta = (vRFiltered - vLFiltered) / 2\n",
        "    omega = 2 * delta / b\n",
        "\n",
        "    currentTheta = currentState[2]\n",
        "    cosTheta = cos(currentTheta)\n",
        "    sinTheta = sin(currentTheta)\n",
        "\n",
        "    x_dot = velocity * cosTheta\n",
        "    y_dot = velocity * sinTheta\n",
        "    theta_dot = omega\n",
        "\n",
        "    E = 0.5*(m*(velocity)*(velocity) + I*(omega)*(omega))\n",
        "\n",
        "    return [x_dot, y_dot, theta_dot, velocity, omega, E, *vLStateD, *vRStateD] #velocity, omega, E are returned for easy evaluation they are not needed for computation\n",
        "  return robot\n",
        "  \n",
        "robotWithDynamic = createRobotModelWithDynamic(b1, b0, a1, a0)\n",
        "robot = robotWithDynamic # If delete this line, only the Kinematic model is taken into account."
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "bu7BoUC3LtlQ"
      },
      "source": [
        "### Dynamic Model Test"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "h2RiPj7CHeSp",
        "colab": {
          "base_uri": "https://localhost:8080/",
          "height": 297
        },
        "outputId": "2a16d36a-7d50-4f55-e2a5-c9ce15e83ea0"
      },
      "source": [
        "filter2ndOrder = createFilter2ndOrder(b1, b0, a1, a0)\n",
        "def compute(model, state0, t0 = 0.0, t_bound = 5, max_step = 0.05):\n",
        "  solver = integrate.RK45(fun = model, t0 = t0, y0 = state0, t_bound = t_bound, max_step = max_step)\n",
        "  cnt = 0\n",
        "  while True:\n",
        "    message = solver.step()\n",
        "    y = solver.y\n",
        "    currentItem = {'time': solver.t, 'y': y[0], 'x1': y[1]}\n",
        "    yield currentItem\n",
        "    if (not(solver.status == 'running')):\n",
        "        break\n",
        "  return\n",
        "\n",
        "filter2ndOrderStep = lambda t, state: filter2ndOrder(t, 10, state)\n",
        "result = compute(filter2ndOrderStep, state0 = [0, 0])\n",
        "df = pd.DataFrame(result)\n",
        "df.plot.line('time', 'y')"
      ],
      "execution_count": null,
      "outputs": [
        {
          "output_type": "execute_result",
          "data": {
            "text/plain": [
              "<matplotlib.axes._subplots.AxesSubplot at 0x7f25c933c0d0>"
            ]
          },
          "metadata": {},
          "execution_count": 5
        },
        {
          "output_type": "display_data",
          "data": {
            "text/plain": [
              "<Figure size 432x288 with 1 Axes>"
            ],
            "image/png": "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\n"
          },
          "metadata": {
            "needs_background": "light"
          }
        }
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "dZNvWB_gGMQT"
      },
      "source": [
        "## Simulation Tools"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "_9BPIqhX1lRK"
      },
      "source": [
        "### Simulation / Computation\n",
        "Function\n",
        "```python\n",
        "def compute\n",
        "```\n",
        "streams out (`yield`) the vector $r(t)$ which consists of\n",
        "\n",
        "|index|name|symbol|unit|\n",
        "|:-:|-|:-:|:-:|\n",
        "|0|time|$t$|$s$|\n",
        "|1|x position|$x$|$m$|\n",
        "|2|y position|$y$|$m$|\n",
        "|3|orientation|$\\theta$|$rad$|\n",
        "|4|distance|$s$|$m$|\n",
        "|5|orientation II|$\\theta$|$rad$|\n",
        "|6||$\\int E dt$||\n",
        "|7|x velocity|$v_x$|$ms^{-1}$|\n",
        "|8|y velocity|$v_y$|$ms^{-1}$|\n",
        "|9|angular velocity|$\\omega$|$rads^{-1}$|\n",
        "|10|velocity|$v$|$ms^{-1}$|\n",
        "|11|angular velocity II|$\\omega$|$rads^{-1}$|\n",
        "|12|current energy|$E$|$J$|\n",
        "|13|total energy|$E$|$J$|\n",
        "\n",
        "for numerical computation `RK45` function from `scipy` library is used.\n"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "SWZGXlRBBxpR"
      },
      "source": [
        "selectx = lambda item: item['y'][0]       # x position\n",
        "selecty = lambda item: item['y'][1]       # y position\n",
        "selectt = lambda item: item['time']       # time\n",
        "selectv = lambda item: item['dy'][3]      # velocity\n",
        "selectomega = lambda item: item['dy'][2]  # omega = theta_dot\n",
        "selecte = lambda item: item['TotalEnergy']# energy\n",
        "selects = lambda item: item['y'][3]       # displacement\n",
        "\n",
        "selectors = [selectt, selectx, selecty, selects, selectv, selectomega, selecte]\n",
        "#         yIndex=0, yIndex=1, yIndex=2, yIndex=3, yIndex=4, yIndex=5, yIndex=6\n",
        "def compute(model, state0, t0 = 0.0, t_bound = 10, max_step = 0.05):\n",
        "  solver = integrate.RK45(fun = model, t0 = t0, y0 = state0, t_bound = t_bound, max_step = max_step)\n",
        "  cnt = 0\n",
        "  lastEnergy = 0\n",
        "  totalEnergy = 0\n",
        "\n",
        "#names = ['t', 'x', 'y', 'θ', 's', 'θ2', 'IE', \"x'\", \"y'\", 'ω', 'v', 'ω2', 'E']\n",
        "  while True:\n",
        "    message = solver.step()\n",
        "    #currentItem = [solver.t, *solver.y, *model(solver.t, solver.y)]\n",
        "    currentItem = {'time': solver.t, 'y': solver.y, 'dy': model(solver.t, solver.y)}\n",
        "    #t, 'solver.y': x, y, theta, s, theta, intE 'model': x', y', theta', velocity, omega, E\n",
        "    #0,             1, 2, 3,     4,   5,    6,           7,  8,   9,        10,     11,   12  \n",
        "    # Energy calculation / energy sumation\n",
        "    currentEnergy = currentItem['dy'][5] #currentNamed['E']\n",
        "    deltaEnergy = currentEnergy - lastEnergy\n",
        "\n",
        "    if deltaEnergy > 0:\n",
        "      totalEnergy = totalEnergy + deltaEnergy\n",
        "\n",
        "    lastEnergy = currentEnergy\n",
        "    currentItem['TotalEnergy'] = totalEnergy\n",
        "\n",
        "    yield currentItem\n",
        "    if (not(solver.status == 'running')):\n",
        "        break\n",
        "  return"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "PKYv7tORgW0v"
      },
      "source": [
        "### Following given path\n",
        "Function\n",
        "```python\n",
        "def controllerForPath\n",
        "```\n",
        "extends given controller which is supposed to reach the goal defined by a single position. Every time when the robot reaches the goal, a new point from the path is readout until the queue of the point is exhausted."
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "SI5xAsojggYe"
      },
      "source": [
        "def controllerForPath(controller, path):\n",
        "  destinationX, destinationY, destinationOrietation = next(path)\n",
        "  destinationState = [destinationX, destinationY, destinationOrietation]\n",
        "  lastReached = False\n",
        "  #print('Destination set to', destinationState)\n",
        "  def result(t, currentState):\n",
        "    nonlocal destinationX # use parent variable\n",
        "    nonlocal destinationY # use parent variable\n",
        "    nonlocal destinationState # use parent variable\n",
        "    nonlocal lastReached # use parent variable\n",
        "\n",
        "    currentX = currentState[0]\n",
        "    currentY = currentState[1]\n",
        "    deltaX = destinationX - currentX\n",
        "    deltaY = destinationY - currentY\n",
        "    if (lastReached == False):\n",
        "      # last point in path was not reached\n",
        "      if (deltaX * deltaX + deltaY * deltaY < 1e-2):\n",
        "        # robot is close enought to currentDestination\n",
        "        try:\n",
        "          # try to get another point on path\n",
        "          destinationX, destinationY, destinationOrietation = next(path)\n",
        "          destinationState = [destinationX, destinationY, destinationOrietation]\n",
        "          #print('Destination set to', destinationState, 'while in state', currentState)\n",
        "        except StopIteration:\n",
        "          # there are no other points\n",
        "          lastReached = True\n",
        "    if (lastReached):\n",
        "      return (0, 0)\n",
        "    else:\n",
        "      return controller(t, currentState, destinationState)\n",
        "  return result"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "ojH4jVtUG8SD"
      },
      "source": [
        "### Robot Model and Controller Connection\n",
        "Function\n",
        "```python\n",
        "def robotModelCreator\n",
        "```\n",
        "creates function$f(t, x)$ which is part of differential equation\n",
        "\n",
        "$\\dot x=f(t, x)$\n",
        "\n",
        "returns `resultRMC` "
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "-le5YufJ3k5J"
      },
      "source": [
        "def robotModelCreator(controllerCreator, path, **kwargs):\n",
        "  controller_ = controllerCreator(**kwargs)\n",
        "  savedController = controllerForPath(controller_, path)\n",
        "  def resultRMC(t, currentState):\n",
        "    return robot(t, currentState, savedController)\n",
        "  return resultRMC"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "-ab96awcHDDo"
      },
      "source": [
        "### Closure Function for Simulation\n",
        "function\n",
        "```python\n",
        "def simpleCompute\n",
        "```"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "pqsV4QZ42pIn"
      },
      "source": [
        "def simpleCompute(computefunc, state0, t0 = 0, t_bound = 200, max_step = 0.05):\n",
        "  def resultSC(model):\n",
        "    return computefunc(\n",
        "        model, state0 = state0, t0 = t0, t_bound = t_bound, max_step = max_step)\n",
        "  return resultSC"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "GtWdz8gkHQqz"
      },
      "source": [
        "### Simulation Runner\n",
        "This function uses the simulation description parameter (`experimentsDescription`) and returns a 3D array of results where, first dimension is along robots, second dimension is along time and last dimension is along values."
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "c0nG3hCL2qCX"
      },
      "source": [
        "def runSimulations(\n",
        "    experimentsDescription, path, \n",
        "    state0, t0 = 0, t_bound = 200, max_step = 0.05):\n",
        "\n",
        "  solverfunc = simpleCompute(\n",
        "      compute, state0 = state0, \n",
        "      t0 = t0, t_bound = t_bound, max_step = max_step)\n",
        "\n",
        "  robots = []\n",
        "  for currentExperiment in experimentsDescription:\n",
        "    description = currentExperiment['description']\n",
        "    robotWithController = robotModelCreator(**description)\n",
        "    robots.append(solverfunc(robotWithController))\n",
        "\n",
        "  results = []\n",
        "  for currentRobot in robots:\n",
        "    result = []\n",
        "    for currentState in currentRobot: # readout all states from current moving robot\n",
        "      for _ in selectors:\n",
        "        result.append([])\n",
        "      for index, func in enumerate(selectors):\n",
        "        result[index].append(func(currentState))\n",
        "    results.append(result)\n",
        "  return results"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "SbT2XGoKHQCg"
      },
      "source": [
        "### Path Plotter"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "oYw5ZT6h7BrQ"
      },
      "source": [
        "def plotSimulationsPaths(\n",
        "    experimentsDescription, experimentsResults, path, show = True):\n",
        "  for currentResult, experiment in zip(experimentsResults, experimentsDescription):\n",
        "    currentStyle = experiment['plotstyle']\n",
        "\n",
        "    t_a = currentResult[0]\n",
        "    x_a = currentResult[1]\n",
        "    y_a = currentResult[2]\n",
        "        \n",
        "    plt.plot(x_a, y_a, **currentStyle)\n",
        "\n",
        "  pathx = []\n",
        "  pathy = []\n",
        "  for x, y, orientation in path:\n",
        "    pathx.append(x)\n",
        "    pathy.append(y)\n",
        "\n",
        "  plt.plot(pathx, pathy, 'b-', label = 'Planned path', lw = 1)\n",
        "  plt.grid(which = 'both')  \n",
        "  plt.gca().set_aspect('equal')\n",
        "  plt.legend(loc = 'best')\n",
        "  plt.xlabel('$x[m]$')\n",
        "  plt.ylabel('$y[m]$')\n",
        "  if show:\n",
        "    plt.show()    \n",
        "  return"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "e5baAe5RHaz7"
      },
      "source": [
        "### Value Plotter"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "WFQfKYYDExna"
      },
      "source": [
        "def plotValueAndTime(\n",
        "    experimentsDescription, experimentsResults, show = True, \n",
        "    xIndex = 0, xLabel = '$t[s]$',\n",
        "    yIndex = 3, yLabel = '$s[m]$', title = ''):\n",
        "  for currentResult, experiment in zip(experimentsResults, experimentsDescription):\n",
        "    t_a = currentResult[xIndex]\n",
        "    s_a = currentResult[yIndex]\n",
        "    currentStyle = experiment['plotstyle']\n",
        "\n",
        "    plt.plot(t_a, s_a, **currentStyle)\n",
        "\n",
        "  plt.grid(which = 'both')  \n",
        "  plt.xlabel(xLabel)\n",
        "  plt.ylabel(yLabel)\n",
        "  plt.title(title)\n",
        "  if show:\n",
        "    plt.show()    \n",
        "  return  "
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "o-h86IpYLzc8"
      },
      "source": [
        "### All in one Plotter"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "TlDSkp-5MnWO"
      },
      "source": [
        "def plotAllInOne(experimentsDescription, experimentsResults, path):\n",
        "  fig = plt.figure()# for save picture of figures\n",
        "\n",
        "  plt.subplot(3, 2, 1)\n",
        "  plt.title('Path')\n",
        "  plotSimulationsPaths(\n",
        "      experimentsDescription, experimentsResults, path, show = False)\n",
        "  \n",
        "  plt.subplot(3, 2, 2)\n",
        "  plt.title('Distance in Time')\n",
        "  plotValueAndTime(\n",
        "      experimentsDescription, experimentsResults, show = False, \n",
        "      yIndex = 3, yLabel = '$s[m]$')\n",
        "  plt.legend(loc = 'right')\n",
        "  \n",
        "  plt.subplot(3, 2, 3)\n",
        "  plt.title('Angular velocity in Time')\n",
        "  plotValueAndTime(\n",
        "      experimentsDescription, experimentsResults, show = False, \n",
        "      yIndex = 5, yLabel = '$\\omega[rads^{-1}]$')\n",
        "  plt.legend(loc = 'lower right')\n",
        "  \n",
        "  plt.subplot(3, 2, 4)\n",
        "  plt.title('Velocity in Time')\n",
        "  plotValueAndTime(\n",
        "      experimentsDescription, experimentsResults, show = False, \n",
        "      yIndex = 4, yLabel = '$v[ms^{-1}]$')\n",
        "  plt.legend(loc = 'right')\n",
        "\n",
        "  plt.subplot(3, 2, 5)\n",
        "  plt.title('Total Kinetic Energy loss in Time')\n",
        "  plotValueAndTime(\n",
        "      experimentsDescription, experimentsResults, show = False, \n",
        "      yIndex = 6, yLabel = '$E[J]$')\n",
        "  plt.legend(loc = 'right')\n",
        "      \n",
        "  fig.savefig('plot.png')\n",
        "  plt.show()    \n"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "HnVJFim5yOEP"
      },
      "source": [
        "### Data Extractor\n"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "SSOQbuttydaF"
      },
      "source": [
        "def ExtractData(experimentsDescription, experimentsResults, path):\n",
        "  for description, result in zip(experimentsDescription, experimentsResults):\n",
        "#controllerCreator\n",
        "    parameters = {**description['description']}\n",
        "    del parameters['controllerCreator']\n",
        "    del parameters['path']\n",
        "    del parameters['omega_ri']\n",
        "\n",
        "    yield {\n",
        "        'Experiment': description['name'], \n",
        "        'Total distance': str(result[3][-1]), \n",
        "        'Total energy': str(result[6][-1]),\n",
        "        **parameters\n",
        "        }\n",
        "  pass"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "2_eEd0Uml9Ff"
      },
      "source": [
        "### Table Display"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "9SpRvc4XmBe2"
      },
      "source": [
        "def displayTable(data):\n",
        "  df = pd.DataFrame(data)\n",
        "  display(df)"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "TVtqxqIXGOqg"
      },
      "source": [
        "## Controllers\n",
        "Following are the controllers which are implemented and compared:\n",
        "\n",
        "- Lyapunov controller\n",
        "\n",
        "- Clever Trigonometry Based Controller\n",
        "\n",
        "- Feedback base control – Circular path\n",
        "\n",
        "- Advanced controller By_RobinsMathew\n",
        "\n",
        "- Advanced controller - Circular path\n",
        "\n",
        "- Advanced controller - Dubins path\n",
        "\n",
        "- Controller proposed by Kanayama"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "ou0oV2KfFU5j"
      },
      "source": [
        "### Feedback-based controller for  circular path\n",
        "\n",
        "\n"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "TcNCbwYKFVPa"
      },
      "source": [
        "def createCircleControllerWithGain(omega_ri, vri, k1, \n",
        "  lowVelocityLimit, highVelocityLimit, lowOmegaLimit, highOmegaLimit):\n",
        "  def controller(t, currentState, destinationState):\n",
        "    currentX = currentState[0]\n",
        "    currentY = currentState[1]\n",
        "    currentTheta = currentState[2]\n",
        "\n",
        "    destinationX = destinationState[0]\n",
        "    destinationY = destinationState[1]\n",
        "\n",
        "    cosTheta = cos(currentTheta)\n",
        "    sinTheta = sin(currentTheta)\n",
        "\n",
        "    deltaX = destinationX - currentX\n",
        "    deltaY = destinationY - currentY\n",
        "\n",
        "    velocity = vri\n",
        "    omega = -2 * k1 * vri * (deltaX * sinTheta - deltaY * cosTheta) / (deltaX * deltaX + deltaY * deltaY)\n",
        "    \n",
        "    if velocity > highVelocityLimit:\n",
        "      velocity = highVelocityLimit\n",
        "    if (velocity < lowVelocityLimit):\n",
        "      velocity = lowVelocityLimit\n",
        "    if omega > highOmegaLimit:\n",
        "      omega = highOmegaLimit\n",
        "    if (omega < lowOmegaLimit):\n",
        "      omega = lowOmegaLimit\n",
        "\n",
        "    return velocity, omega\n",
        "  return controller"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "mZ-uB_wxLLRF"
      },
      "source": [
        "### Advanced controller by RobinsMathew\n",
        "\n"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "ljj_8gWjLuLV"
      },
      "source": [
        "def createController_By_RobinsMathew(omega_ri, vri, k1, k2,\n",
        "  lowVelocityLimit, highVelocityLimit, lowOmegaLimit, highOmegaLimit):\n",
        "                                     \n",
        "  def controller(t, currentState, destinationState):\n",
        "    currentX = currentState[0]\n",
        "    currentY = currentState[1]\n",
        "    currentTheta = currentState[2]\n",
        "\n",
        "    destinationX = destinationState[0]\n",
        "    destinationY = destinationState[1]\n",
        "\n",
        "    cosTheta = cos(currentTheta)\n",
        "    sinTheta = sin(currentTheta)\n",
        "    \n",
        "    deltaX = destinationX - currentX\n",
        "    deltaY = destinationY - currentY\n",
        "    theta_destination = atan2(deltaY, deltaX)\n",
        "    theta_error = theta_destination - currentTheta\n",
        "\n",
        "    Te = math.sin(theta_destination)*deltaX - math.cos(theta_destination)*deltaY\n",
        "    velocity = vri*math.cos(theta_error)\n",
        "    omega = omega_ri + k1*vri*Te + k2*vri*math.sin(theta_error)\n",
        "\n",
        "    if velocity > highVelocityLimit:\n",
        "      velocity = highVelocityLimit\n",
        "    if (velocity < lowVelocityLimit):\n",
        "      velocity = lowVelocityLimit\n",
        "    if omega > highOmegaLimit:\n",
        "      omega = highOmegaLimit\n",
        "    if (omega < lowOmegaLimit):\n",
        "      omega = lowOmegaLimit\n",
        "      \n",
        "    return velocity, omega\n",
        "  return controller"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "XGTqViYKCBKx"
      },
      "source": [
        "### Lyapunov controller\n",
        "\n"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "hKMGUrXdBpbR"
      },
      "source": [
        "def createLyapunovController(omega_ri, vri, k1, k2,\n",
        "  lowVelocityLimit, highVelocityLimit, lowOmegaLimit, highOmegaLimit):\n",
        "  def controller(t, currentState, destinationState):\n",
        "    currentX = currentState[0]\n",
        "    currentY = currentState[1]\n",
        "    currentTheta = currentState[2]\n",
        "    \n",
        "    destinationX = destinationState[0]\n",
        "    destinationY = destinationState[1]\n",
        "    \n",
        "    cosTheta = cos(currentTheta)\n",
        "    sinTheta = sin(currentTheta)\n",
        "    \n",
        "    deltaX = destinationX - currentX\n",
        "    deltaY = destinationY - currentY\n",
        "    theta_destination = atan2(deltaY, deltaX)\n",
        "    theta_error = theta_destination - currentTheta\n",
        "\n",
        "    if (theta_error > pi):\n",
        "      theta_error -= 2 * pi\n",
        "    if (theta_error < -pi):\n",
        "      theta_error += 2 * pi\n",
        "\n",
        "    velocity = vri* math.cos(theta_error)\n",
        "    omega = k1*theta_error + vri*math.sin(theta_error)*math.cos(theta_error)*(theta_error+k2*theta_destination)/theta_error\n",
        "    \n",
        "    if velocity > highVelocityLimit:\n",
        "      velocity = highVelocityLimit\n",
        "    if (velocity < lowVelocityLimit):\n",
        "      velocity = lowVelocityLimit\n",
        "    if omega > highOmegaLimit:\n",
        "      omega = highOmegaLimit\n",
        "    if (omega < lowOmegaLimit):\n",
        "      omega = lowOmegaLimit\n",
        " \n",
        "    return velocity, omega\n",
        "  return controller"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "f-A1_VMGDnA5"
      },
      "source": [
        "### Clever geometry controller\n",
        "\n"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "yooFaiZWDnbL"
      },
      "source": [
        "def createCleverGeometryController(omega_ri, vri, k1,\n",
        "  lowVelocityLimit, highVelocityLimit,lowOmegaLimit, highOmegaLimit):\n",
        "  def controller(t, currentState, destinationState):\n",
        "    currentX = currentState[0]\n",
        "    currentY = currentState[1]\n",
        "    currentTheta = currentState[2]\n",
        "    \n",
        "    destinationX = destinationState[0]\n",
        "    destinationY = destinationState[1]\n",
        "    \n",
        "    cosTheta = cos(currentTheta)\n",
        "    sinTheta = sin(currentTheta)\n",
        "    \n",
        "    deltaX = destinationX - currentX\n",
        "    deltaY = destinationY - currentY\n",
        "    theta_destination = atan2(deltaY, deltaX)\n",
        "    theta_error = theta_destination - currentTheta\n",
        "\n",
        "    if (theta_error > pi):\n",
        "      theta_error -= 2 * pi\n",
        "    if (theta_error < -pi):\n",
        "      theta_error += 2 * pi\n",
        "\n",
        "    velocity = vri *math.cos(theta_error)\n",
        "    omega = k1 * math.sin(theta_error)\n",
        "\n",
        "    if velocity > highVelocityLimit:\n",
        "      velocity = highVelocityLimit\n",
        "    if (velocity < lowVelocityLimit):\n",
        "      velocity = lowVelocityLimit\n",
        "    if omega > highOmegaLimit:\n",
        "      omega = highOmegaLimit\n",
        "    if (omega < lowOmegaLimit):\n",
        "      omega = lowOmegaLimit\n",
        "      \n",
        "    return velocity, omega\n",
        "  return controller"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "qVs4dmtYux42"
      },
      "source": [
        "### Advanced controller - Dubins path\n"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "pLw26--vu5SB"
      },
      "source": [
        "def createDubinsController(omega_ri, vri, k1, lowVelocityLimit, highVelocityLimit,lowOmegaLimit, highOmegaLimit):\n",
        "  minimal_radius = k1\n",
        "  angle_epsilon = 0.05 # radians\n",
        "  def controller(t, currentState, destinationState):\n",
        "    currentX = currentState[0]\n",
        "    currentY = currentState[1]\n",
        "    currentTheta = currentState[2]\n",
        "    \n",
        "    # hacky, because it could be outside interval <-PI, +PI> :(\n",
        "    while currentTheta < -2 * pi:\n",
        "      currentTheta = currentTheta + 2 * pi\n",
        "    while currentTheta > 2 * pi:\n",
        "      currentTheta = currentTheta - 2 * pi\n",
        "\n",
        "    destinationX = destinationState[0]\n",
        "    destinationY = destinationState[1]\n",
        "\n",
        "    deltaX = destinationX - currentX\n",
        "    deltaY = destinationY - currentY\n",
        "\n",
        "    angle_to_destination = atan2(deltaY, deltaX)\n",
        "\n",
        "\n",
        "    angle_delta = currentTheta - angle_to_destination\n",
        "\n",
        "    if (angle_delta > pi):\n",
        "      angle_delta -= 2 * pi\n",
        "    if (angle_delta < -pi):\n",
        "      angle_delta += 2 * pi\n",
        "\n",
        "    if (angle_delta < - angle_epsilon):\n",
        "      # turn on minimal radius\n",
        "      velocity = vri\n",
        "      omega = velocity / minimal_radius\n",
        "    elif (angle_delta > + angle_epsilon):\n",
        "      # turn on minimal radius\n",
        "      velocity = vri\n",
        "      omega = -velocity / minimal_radius\n",
        "    else:\n",
        "      # go straight\n",
        "      velocity = vri\n",
        "      omega = 0\n",
        "\n",
        "    if velocity > highVelocityLimit:\n",
        "      velocity = highVelocityLimit\n",
        "    if (velocity < lowVelocityLimit):\n",
        "      velocity = lowVelocityLimit\n",
        "    if omega > highOmegaLimit:\n",
        "      omega = highOmegaLimit\n",
        "    if (omega < lowOmegaLimit):\n",
        "      omega = lowOmegaLimit\n",
        "\n",
        "    return velocity, omega\n",
        "  return controller"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "OjIazxZYefYs"
      },
      "source": [
        "### Advanced controller by Kanayama Controller\n"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "uW-Np-rdejxW"
      },
      "source": [
        "def createController_By_Kanayama(omega_ri, vri, k1, k2, k3,\n",
        "  lowVelocityLimit, highVelocityLimit, lowOmegaLimit, highOmegaLimit):\n",
        "  def controller(t, currentState, destinationState):\n",
        "    currentX = currentState[0]\n",
        "    currentY = currentState[1]\n",
        "    currentTheta = currentState[2]\n",
        "\n",
        "    destinationX = destinationState[0]\n",
        "    destinationY = destinationState[1]\n",
        "\n",
        "    cosTheta = cos(currentTheta)\n",
        "    sinTheta = sin(currentTheta)\n",
        "    \n",
        "    deltaX = destinationX - currentX\n",
        "    deltaY = destinationY - currentY\n",
        "    theta_destination = atan2(deltaY, deltaX)\n",
        "    theta_error = theta_destination - currentTheta\n",
        "\n",
        "    x_ie = cosTheta*deltaX + sinTheta*deltaY\n",
        "    y_ie = cosTheta*deltaY - sinTheta*deltaX\n",
        "    velocity = vri*math.cos(theta_error) + k1*x_ie\n",
        "    omega = omega_ri + k2*vri*y_ie + k3*vri*math.sin(theta_error)\n",
        "\n",
        "    if velocity > highVelocityLimit:\n",
        "      velocity = highVelocityLimit\n",
        "    if (velocity < lowVelocityLimit):\n",
        "      velocity = lowVelocityLimit\n",
        "    if omega > highOmegaLimit:\n",
        "      omega = highOmegaLimit\n",
        "    if (omega < lowOmegaLimit):\n",
        "      omega = lowOmegaLimit\n",
        "          \n",
        "    return velocity, omega\n",
        "  return controller"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "pI9-KpSaExXU"
      },
      "source": [
        "## Simulation"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "OBpgwcqAuuZ8"
      },
      "source": [
        "### Paths Definition"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "iDTovkk1hDjq"
      },
      "source": [
        "# Square path\n",
        "def pathA():\n",
        "  yield 0, 0, 0  #X, Y, orientation\n",
        "  yield 10, 0, 0 #X, Y, orientation\n",
        "  yield 10, 10, 0#X, Y, orientation\n",
        "  yield 0, 10, 0 #X, Y, orientation\n",
        "  yield 0, 0, 0 #X, Y, orientation"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "ukFmy-xSuz-g"
      },
      "source": [
        "# StraightFoward path\n",
        "def pathB():\n",
        "  yield 0, 0, 0 #X, Y, orientation\n",
        "  yield 10, 0, 0 #X, Y, orientation\n",
        "  yield 20, 10, 0 #X, Y, orientation\n",
        "  yield 30, 10, 0 #X, Y, orientation\n",
        "  yield 25, 20, 0 #X, Y, orientation"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "_ia79twnZmHK"
      },
      "source": [
        "# StraightFoward path with many waypoint\n",
        "def pathC():\n",
        "  yield 0, 0, 0 #X, Y, orientation\n",
        "  x1 = 3\n",
        "  for x1 in range (3,10):\n",
        "    yield x1, 0, 0\n",
        "    x1 = x1 + 1\n",
        "  yield 10, 0, 0 #X, Y, orientation\n",
        "  x2 = 12\n",
        "  y2 = 2\n",
        "  for x2 in range (12,20):\n",
        "    yield x2, y2, 0\n",
        "    x2 = x2 + 1\n",
        "    y2 = y2 + 1\n",
        "  yield 20, 10, 0 #X, Y, orientation\n",
        "  x3 = 24\n",
        "  for x3 in range(24, 30):\n",
        "    yield x3, 10, 0\n",
        "    x3 = x3+1\n",
        "  yield 30, 10, 0 #X, Y, orientation\n",
        "  yield 27.5, 15, 0\n",
        "  yield 26.5, 17, 0\n",
        "  yield 25.5, 19, 0\n",
        "  yield 25, 20, 0 #X, Y, orientation"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "poDNn4JORWxS"
      },
      "source": [
        "# Diamond-Shape path\n",
        "def pathD():\n",
        "  yield 0, 0, 0 #X, Y, orientation\n",
        "  yield 10, -5, 0 #X, Y, orientation\n",
        "  yield 20, 0, 0 #X, Y, orientation\n",
        "  yield 10, 5, 0 #X, Y, orientation\n",
        "  yield 0, 0, 0 #X, Y, orientation"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "DoQiVJDztTb8"
      },
      "source": [
        "# Sharp turn path\n",
        "def pathE():\n",
        "  yield 0, 0, 0 #X, Y, orientation\n",
        "  yield 0, 5, 0 #X, Y, orientation\n",
        "  yield 5, 0, 0 #X, Y, orientation\n",
        "  yield 5, 10, 0 #X, Y, orientation\n",
        "  yield 25, 0, 0 #X, Y, orientation"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "0fhLTm08v5fe"
      },
      "source": [
        "### Define your path here"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "JbQZ3ugWv9FH"
      },
      "source": [
        "def pathF():\n",
        "  #for every single point on path express\n",
        "  yield 0, 0, 0 #X, Y, orientation\n",
        "  #repeat it until done\n",
        "  "
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "U4C0LiWqUSRV"
      },
      "source": [
        "paths = [ \n",
        "  {'name': 'Square path', 'func': pathA, \n",
        "   'tags' :['Square path', 'Main']},\n",
        "  {'name': 'Straightforward path', 'func': pathB, \n",
        "   'tags' :['Straightforward path', 'Main', 'Supportive']},\n",
        "  {'name': 'Straightforward path with many waypoints', 'func': pathC, \n",
        "   'tags': ['Straightforward path with many waypoints','Main', 'Supportive']},\n",
        "  {'name': 'Diamond-Shape path', 'func': pathD, \n",
        "   'tags': ['Diamond-Shape path','Main']},\n",
        "  {'name': 'Sharp turn path', 'func': pathE, \n",
        "   'tags': ['Sharp turn path','Main']},\n",
        "  {'name': 'Custom', 'func': pathF, 'tags': ['Custom']} \n",
        "   ]"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "eyS-090qJr8_"
      },
      "source": [
        "### Experiment Description"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "g5c5R5qKSuTN"
      },
      "source": [
        "experiments = [\n",
        "    { 'name': 'Circle [v = 1.0, g = 4]',\n",
        "      'description':\n",
        "        {'controllerCreator': createCircleControllerWithGain, \n",
        "        'omega_ri': 0,'vri': 1.0, 'k1': 4.0\n",
        "        },\n",
        "      'plotstyle':\n",
        "        {'linestyle': '-', 'color': 'g', 'lw': 1,\n",
        "        'label': 'Feedback-based controller $G = 4$'\n",
        "        },      \n",
        "      'tags': ['Main', 'Circle', 'Low Energy']\n",
        "    },\n",
        "    { 'name': 'Circle_2nd [v = 0.8, g = 4]',\n",
        "      'description':\n",
        "        {'controllerCreator': createCircleControllerWithGain, \n",
        "        'omega_ri': 0,'vri': 0.8, 'k1': 4.0,\n",
        "        'highVelocityLimit': 0.8\n",
        "        },\n",
        "      'plotstyle':\n",
        "        {'linestyle': '-', 'color': 'r', 'lw': 1,\n",
        "        'label': 'Feedback-based controller 2nd $G = 4$'\n",
        "        },      \n",
        "      'tags': ['Circle']\n",
        "    },\n",
        "    { 'name': 'Circle_3rd [v = 0.5, g = 4]',\n",
        "      'description':\n",
        "        {'controllerCreator': createCircleControllerWithGain, \n",
        "        'omega_ri': 0,'vri': 0.5, 'k1': 4.0,\n",
        "        'highVelocityLimit': 0.5\n",
        "        },\n",
        "      'plotstyle':\n",
        "        {'linestyle': '-', 'color': 'k', 'lw': 1,\n",
        "        'label': 'Feedback-based controller 3rd $G = 4$'\n",
        "        },      \n",
        "      'tags': ['Circle']\n",
        "    },\n",
        "    { 'name': 'Circle [v = 1.0, g = 8]',\n",
        "      'description':\n",
        "        {'controllerCreator': createCircleControllerWithGain, \n",
        "        'omega_ri': 0,'vri': 1.0, 'k1': 8.0,\n",
        "        },\n",
        "      'plotstyle':\n",
        "        {'linestyle': '--', 'color': 'g', 'lw': 1,\n",
        "        'label': 'Feedback-based controller $G = 8$'\n",
        "        },\n",
        "      'tags': ['Main', 'Low Energy']\n",
        "    },\n",
        "    { 'name': 'Dubins',\n",
        "      'description':\n",
        "        {'controllerCreator': createDubinsController, \n",
        "        'omega_ri': 0, 'vri': 1.0, 'k1': 1.0,\n",
        "        },\n",
        "      'plotstyle':\n",
        "        {'linestyle': '--', 'color': 'maroon', 'lw': 1, \n",
        "        'label': 'Dubins-based controller'\n",
        "        },\n",
        "      'tags': ['Main', 'Dubins', 'Low Energy']    \n",
        "    },\n",
        "    \n",
        "    { 'name': 'Dubins_2nd',\n",
        "      'description':\n",
        "        {'controllerCreator': createDubinsController, \n",
        "        'omega_ri': 0, 'vri': 1.1, 'k1': 1.0,\n",
        "        'highVelocityLimit': 1.1\n",
        "        },\n",
        "      'plotstyle':\n",
        "        {'linestyle': '-', 'color': 'g', 'lw': 1, \n",
        "        'label': 'Dubins-based controller 2nd'\n",
        "        },\n",
        "      'tags': ['Dubins']     \n",
        "    },\n",
        "    { 'name': 'Dubins_3rd',\n",
        "      'description':\n",
        "        {'controllerCreator': createDubinsController, \n",
        "        'omega_ri': 0, 'vri': 1.15, 'k1': 1.0,\n",
        "        'highVelocityLimit': 1.15\n",
        "        },\n",
        "      'plotstyle':\n",
        "        {'linestyle': '-', 'color': 'r', 'lw': 1, \n",
        "        'label': 'Dubins-based controller 3rd'\n",
        "        },\n",
        "      'tags': ['Dubins']     \n",
        "    },\n",
        "    { 'name': 'Lyapunov',\n",
        "      'description':\n",
        "        {'controllerCreator': createLyapunovController, \n",
        "        'omega_ri': 0,'vri': 1.0, 'k1': 5.0, 'k2': 0.01,\n",
        "        },\n",
        "      'plotstyle':\n",
        "        {'linestyle': '-', 'color': 'r', 'lw': 1,\n",
        "        'label': 'Lyapunov-based controller'\n",
        "        },\n",
        "      'tags': ['Main', 'Lyapunov', 'High Energy']\n",
        "    },    \n",
        "    { 'name': 'Lyapunov 2nd',\n",
        "      'description':\n",
        "        {'controllerCreator': createLyapunovController, \n",
        "        'omega_ri': 0,'vri': 1.1, 'k1': 5.0, 'k2': 0.01,\n",
        "        'highVelocityLimit': 1.1\n",
        "        },\n",
        "      'plotstyle':\n",
        "        {'linestyle': '--', 'color': 'g', 'lw': 1,\n",
        "        'label': 'Lyapunov-based controller vri = 1.1 m/s'\n",
        "        },\n",
        "      'tags': ['Lyapunov']\n",
        "    },\n",
        "    { 'name': 'Lyapunov 3rd',\n",
        "      'description':\n",
        "        {'controllerCreator': createLyapunovController, \n",
        "        'omega_ri': 0,'vri': 1.15, 'k1': 5.0, 'k2': 0.01,\n",
        "        'highVelocityLimit': 1.15\n",
        "        },\n",
        "      'plotstyle':\n",
        "        {'linestyle': '-', 'color': 'indigo', 'lw': 1,\n",
        "        'label': 'Lyapunov-based controller vri = 1.15 m/s'\n",
        "        },\n",
        "      'tags': ['Lyapunov']\n",
        "    },\n",
        "\n",
        "    { 'name': 'Geometry',\n",
        "      'description':\n",
        "        {'controllerCreator': createCleverGeometryController, \n",
        "        'omega_ri': 0,'vri': 1.0, 'k1': 7.0,\n",
        "        },\n",
        "      'plotstyle':\n",
        "        {'linestyle': '-', 'color': 'indigo', 'lw': 1,\n",
        "        'label': 'Geometry-based controller'\n",
        "        },\n",
        "      'tags': ['Main', 'High Energy']\n",
        "    },\n",
        "    { 'name': 'RobinsMathew',\n",
        "      'description':\n",
        "        {'controllerCreator': createController_By_RobinsMathew, \n",
        "        'omega_ri':0, 'vri': 1.0, 'k1': 1.5, 'k2': 6.5,\n",
        "        },\n",
        "      'plotstyle':\n",
        "        {'linestyle': '-', 'color': 'k', 'lw': 1, \n",
        "        'label': 'RobinsMathew-based controller'\n",
        "        },\n",
        "      'tags': ['Main', 'RobinsMathew', 'High Energy']    \n",
        "    },\n",
        "\n",
        "    { 'name': 'RobinsMathew 2nd',\n",
        "      'description':\n",
        "        {'controllerCreator': createController_By_RobinsMathew, \n",
        "        'omega_ri':0, 'vri': 1.1, 'k1': 1.5, 'k2': 6.5,\n",
        "        'highVelocityLimit': 1.1\n",
        "        },\n",
        "      'plotstyle':\n",
        "        {'linestyle': '-', 'color': 'r', 'lw': 1, \n",
        "        'label': 'RobinsMathew-based controller 2nd'\n",
        "        },\n",
        "      'tags': ['RobinsMathew']    \n",
        "    },\n",
        "\n",
        "    { 'name': 'RobinsMathew 3rd',\n",
        "      'description':\n",
        "        {'controllerCreator': createController_By_RobinsMathew, \n",
        "        'omega_ri':0, 'vri': 1.15, 'k1': 1.5, 'k2': 6.5,\n",
        "        'highVelocityLimit': 1.15\n",
        "        },\n",
        "      'plotstyle':\n",
        "        {'linestyle': '--', 'color': 'g', 'lw': 1, \n",
        "        'label': 'RobinsMathew-based controller 3rd'\n",
        "        },\n",
        "      'tags': ['RobinsMathew']    \n",
        "    },\n",
        "    \n",
        "    { 'name': 'Kanayama',\n",
        "      'description':\n",
        "        {'controllerCreator': createController_By_Kanayama, \n",
        "        'omega_ri':0, 'vri': 1.0, 'k1': 0.001, 'k2': 0.1, 'k3': 5,\n",
        "        },\n",
        "      'plotstyle':\n",
        "        {'linestyle': '--', 'color': 'k', 'lw': 1, \n",
        "        'label': 'Kanayama-based controller'\n",
        "        },\n",
        "      'tags': ['Main', 'High Energy']\n",
        "    }\n",
        "    #insert you new controller here\n",
        "    ]"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "QRSto5CaIXcq"
      },
      "source": [
        "def ApplyExperimentsOnPath(path, experiments = experiments,\n",
        "  #shared limits on linear and angular velocities\n",
        "  slVelocityLow = 0.2,\n",
        "  slVelocityHigh = 1.0,\n",
        "  slOmegaLow = -0.75,\n",
        "  slOmegaHigh = 0.75):\n",
        "\n",
        "  #Movement Limits could be redefined in controllers description\n",
        "  MovementLimits = {'lowVelocityLimit': slVelocityLow, \n",
        "    'highVelocityLimit': slVelocityHigh, \n",
        "    'lowOmegaLimit': slOmegaLow, \n",
        "    'highOmegaLimit': slOmegaHigh}\n",
        "\n",
        "  result = []\n",
        "  for item in experiments:\n",
        "    newExperiment = {\n",
        "        'name': item['name'],\n",
        "        'description': {**MovementLimits, **item['description'], 'path': path()},\n",
        "        'plotstyle': {**item['plotstyle']},\n",
        "        'tags': item['tags']\n",
        "    }\n",
        "    result.append(newExperiment)\n",
        "\n",
        "  return result"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "OvZ94OkjJyHz"
      },
      "source": [
        "### Execution"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "twsZ24lQJ1uP"
      },
      "source": [
        "\n",
        "#just those paths whose tags attribute cointains tagValuePaths will be used fro simulations\n",
        "tagValuePaths = 'main'\n",
        "selectedPaths = list(filter(lambda item: tagValuePaths in item['tags'], paths))\n",
        "#just those described experiment which contains inside tags tagValue will be executed\n",
        "tagValue = 'Main' #main experiment, \n",
        "selectedExperimentsDescription = list(filter(lambda item: tagValue in item['tags'], experiments))\n",
        "\n",
        "def executeSelectedExperiments(\n",
        "    experimentsDescription = selectedExperimentsDescription,\n",
        "    paths = selectedPaths):\n",
        "  allResults = [] #storage for all experiments\n",
        "\n",
        "  for pathDesc in paths: # for all paths\n",
        "    currentPath = pathDesc['func'] \n",
        "    experiments = ApplyExperimentsOnPath(\n",
        "        path = currentPath, \n",
        "        experiments = experimentsDescription) # create experiments for currentPath\n",
        "\n",
        "    p0 = None\n",
        "    if robot == robotWithDynamic:\n",
        "      p0 = np.array([start_x, start_y, start_theta, 0, 0, 0, 0, 0, 0, 0]) # x0=0, y0=0, theta\n",
        "    else:\n",
        "      p0 = np.array([start_x, start_y, start_theta, 0, 0, 0]) # x0=0, y0=0,theta\n",
        "\n",
        "    #execute all experiments\n",
        "    results = runSimulations(\n",
        "      experimentsDescription = experiments, path = currentPath(),\n",
        "      state0 = p0, t0 = 0, t_bound = 100, max_step = 0.05) \n",
        "    \n",
        "    #retrieve text values from experiments\n",
        "    text = list(ExtractData(experimentsDescription = experiments, \n",
        "                            experimentsResults = results, path = currentPath()))   \n",
        "    \n",
        "    #store experiments results\n",
        "    allResults.append({\n",
        "        'pathDesc': pathDesc,\n",
        "        'description': experimentsDescription,\n",
        "        'results': results,\n",
        "        'texts': text})\n",
        "  return allResults\n",
        "  \n",
        "allResults = executeSelectedExperiments(\n",
        "    selectedExperimentsDescription, selectedPaths)"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "y9pOaGLnV93S"
      },
      "source": [
        "## Simulation results"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "g9nJFdODQS1D"
      },
      "source": [
        "### Simulation text results"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "HA5FxgP8KQL6"
      },
      "source": [
        "def textOutput(allResults):\n",
        "  for item in allResults:\n",
        "    pathDesc = item['pathDesc']\n",
        "    #experimentsDescription = item['description']\n",
        "    #results = item['results']  \n",
        "    text = item['texts']\n",
        "\n",
        "    print('*' * 30)\n",
        "    print('*', 'Path name:', pathDesc['name'])\n",
        "    print('*' * 30)\n",
        "    displayTable(text)\n",
        "    \n",
        "textOutput(allResults)"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "8WXrDf0FRJgp"
      },
      "source": [
        "### Simulation image output size settings"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "E720H9wxRR9s"
      },
      "source": [
        "smallImages = False\n",
        "if smallImages:\n",
        "  plt.rcParams[\"font.size\"] = \"10\"\n",
        "  plt.rcParams[\"figure.figsize\"] = (15, 10)\n",
        "else:\n",
        "  plt.rcParams[\"font.size\"] = \"18\"\n",
        "  plt.rcParams[\"figure.figsize\"] = (25, 24)"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "8tpUPWTMQXe8"
      },
      "source": [
        "### Simulation all visualizations"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "awxAh8VuP0xc"
      },
      "source": [
        "def allVisualizations(allResults):\n",
        "  for item in allResults:\n",
        "    #path = item['path']\n",
        "    pathDesc = item['pathDesc']\n",
        "    experimentsDescription = item['description']\n",
        "    results = item['results']  \n",
        "    #text = item['texts']\n",
        "    plotAllInOne(\n",
        "      experimentsDescription = experimentsDescription,\n",
        "      experimentsResults = results, \n",
        "      path = pathDesc['func']())\n",
        "  \n",
        "allVisualizations(allResults)"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "lOCzciVysQG3"
      },
      "source": [
        "### Simulation paths visualizations"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "paTc2tXyLZ22"
      },
      "source": [
        "def pathVisualizations(allResults):\n",
        "  for item in allResults:\n",
        "    #path = item['path']\n",
        "    pathDesc = item['pathDesc']\n",
        "    experimentsDescription = item['description']\n",
        "    results = item['results']  \n",
        "    #text = item['texts']\n",
        "    plotSimulationsPaths(\n",
        "      experimentsDescription = experimentsDescription,\n",
        "      experimentsResults = results, \n",
        "      path = pathDesc['func']())\n",
        "    \n",
        "pathVisualizations(allResults)"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "mtaSdzMuQp32"
      },
      "source": [
        "### Simulation robot distance in time visualization"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "9HyEi37dOXL9"
      },
      "source": [
        "def distanceVisualization(allResults):\n",
        "  for item in allResults:\n",
        "    #path = item['path']\n",
        "    #pathDesc = item['pathDesc']\n",
        "    experimentsDescription = item['description']\n",
        "    results = item['results']  \n",
        "    #text = item['texts']\n",
        "    plotValueAndTime(\n",
        "        experimentsDescription = experimentsDescription, \n",
        "        experimentsResults = results)\n",
        "\n",
        "distanceVisualization(allResults)    "
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "KWavESICQ-qe"
      },
      "source": [
        "### Simulation robot energy in time visualization"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "nIBpWY4GPgJu"
      },
      "source": [
        "def energyVisualization(allResults):\n",
        "  for item in allResults:\n",
        "    #path = item['path']\n",
        "    #pathDesc = item['pathDesc']\n",
        "    experimentsDescription = item['description']\n",
        "    results = item['results']  \n",
        "    #text = item['texts']\n",
        "    plotValueAndTime(\n",
        "      experimentsDescription = experimentsDescription, \n",
        "      experimentsResults = results, yIndex = 6, yLabel = '$E[J]$')\n",
        "    \n",
        "energyVisualization(allResults)    "
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "mnqp0djEO5OR"
      },
      "source": [
        "## Interactive Simulation"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "a-WT7LB6O91s",
        "colab": {
          "base_uri": "https://localhost:8080/",
          "height": 583,
          "referenced_widgets": [
            "c835573a8fde448fbf339254ebf948f4",
            "4e36ee2ebf944eeaa806185ed3b5962b",
            "732c7ea62b144cfd917969016e728390",
            "fa29b1673a9d4dafb91a0f768c35165d",
            "679f06be7a3845f0a4ba6de53f062765",
            "464217ad0a6544ff9c72f1f2c57ee044",
            "aa74d0b0cfab47d7abc58f8fe187ec61",
            "b19b510633a74717828b80f40ad78d9e",
            "d6a58da568fa492cb5d3b98eddebf142",
            "6e22c9f4871946c3bd3159b4db148678",
            "5934939b10d0414cb5ac64c739fe1bbe",
            "5b8e5282ae2545c6ba4d88612f8416d1",
            "b88467ce98a1466f959a25211e6fe18a",
            "e557b3e70a5a4ab895b49f53e2e0d27e",
            "6ff6a3aa9cc94f35b04f8d158baefafe",
            "cfb83205e55144758a1c4032d9f4f5fb"
          ]
        },
        "outputId": "94bec83c-e095-4f08-da11-ebe0b631b177"
      },
      "source": [
        "from ipywidgets import interact\n",
        "\n",
        "exTags = []\n",
        "for item in experiments:\n",
        "  for tag in item['tags']:\n",
        "    if not tag in exTags:\n",
        "      exTags.append(tag)\n",
        "\n",
        "pathTags = []\n",
        "for item in paths:\n",
        "  for tag in item['tags']:\n",
        "    if not tag in pathTags:\n",
        "      pathTags.append(tag)\n",
        "\n",
        "visualizations = {\n",
        "    'text': textOutput,\n",
        "    'all': allVisualizations,\n",
        "    'path': pathVisualizations,\n",
        "    'distance': distanceVisualization,\n",
        "    'energy': energyVisualization,\n",
        "}\n",
        "\n",
        "oldpathTag = None\n",
        "oldexperimTag = None\n",
        "\n",
        "def runIt(experimTag, pathTag, visualizations, smallImages):\n",
        "  global oldpathTag\n",
        "  global oldexperimTag\n",
        "  global allResults\n",
        "\n",
        "  if smallImages:\n",
        "    plt.rcParams[\"font.size\"] = \"15\"\n",
        "    plt.rcParams[\"figure.figsize\"] = (22, 22)\n",
        "  else:\n",
        "    plt.rcParams[\"font.size\"] = \"20\"\n",
        "    plt.rcParams[\"figure.figsize\"] = (35, 35)\n",
        "\n",
        "  if not((experimTag == oldexperimTag) & (pathTag == oldpathTag)):\n",
        "    print('Simulating, please be patient')\n",
        "\n",
        "    # select appropriate paths\n",
        "    selectedPaths = list(filter(lambda item: pathTag in item['tags'], paths))\n",
        "    # select appropriate experiments\n",
        "    selectedExperimentsDescription = list(filter(lambda item: experimTag in item['tags'], experiments))\n",
        "\n",
        "    # collect results\n",
        "    allResults = executeSelectedExperiments(\n",
        "        selectedExperimentsDescription, selectedPaths)\n",
        "    \n",
        "  visualizations(allResults)\n",
        "  oldpathTag = pathTag\n",
        "  oldexperimTag = experimTag\n",
        "\n",
        "interact(runIt,  \n",
        "         experimTag=exTags,\n",
        "         pathTag=pathTags,\n",
        "         visualizations=visualizations, smallImages=True)"
      ],
      "execution_count": null,
      "outputs": [
        {
          "output_type": "display_data",
          "data": {
            "text/plain": [
              "interactive(children=(Dropdown(description='experimTag', options=('Main', 'Circle', 'Low Energy', 'Dubins', 'L…"
            ],
            "application/vnd.jupyter.widget-view+json": {
              "version_major": 2,
              "version_minor": 0,
              "model_id": "c835573a8fde448fbf339254ebf948f4"
            }
          },
          "metadata": {}
        },
        {
          "output_type": "execute_result",
          "data": {
            "text/plain": [
              "<function __main__.runIt(experimTag, pathTag, visualizations, smallImages)>"
            ]
          },
          "metadata": {},
          "execution_count": 39
        }
      ]
    }
  ]
}