generate_simulink_blocks.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import math\n",
    "import json\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "\n",
    "import openrtdynamics2.lang as dy\n",
    "import openrtdynamics2.py_execute as dyexe\n",
    "import openrtdynamics2.targets as tg\n",
    "from openrtdynamics2.ORTDtoNumpy import ORTDtoNumpy\n",
    "from openrtdynamics2.manifest_import import show_inputs, show_outputs\n",
    "\n",
    "from vehicle_lib import *\n",
    "from vehicle_lib.motion_primitives import *\n",
    "from vehicle_lib.path_transformations import compile_lateral_path_transformer\n",
    "from vehicle_lib.path_tracking import compile_path_tracker\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Generate simulink s-functions\n",
    "\n",
    "Using this notebook, a subset of the algorithms implemented in the library are exported to Simulink blocks. Therefore, they can be used in Simulink diagrams to build larger control systems or simulations. The generated code is written into the folder sfunctions/ ."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Path tracking and sampling"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "compiling system Sys1000_store_input_data (level 1)... \n",
      "compiling system Sys1003_optim_loop (level 3)... \n",
      "compiling system Sys1002_tracker (level 2)... \n",
      "compiling system Sys1004_controller (level 2)... \n",
      "compiling system Sys1001_process_data (level 1)... \n",
      "compiling system simulation (level 0)... \n",
      "Generated code will be written to sfunctions/ .\n",
      "writing file sfunctions/simulation_manifest.json\n",
      "writing file sfunctions/path_tracker.cpp\n",
      "writing file sfunctions/path_tracker_block_mask_commands.m\n"
     ]
    }
   ],
   "source": [
    "code_gen_results, compiled_system = compile_path_tracker(\n",
    "    target_template = tg.TargetCppSimulinkSFunction(sfun_name='path_tracker'),\n",
    "    folder          = 'sfunctions/', # the folder to which the generated files are written\n",
    "    samples_in_buffer = 10000        # the size of the buffer storing the samples for the input path \n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Show a summary of the blocks connectors."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "+-------+------------------------+---------+--------+-------+----------------+-----------------------------------------+\n",
      "| #port | input signal,  to -->  | outputs | update | reset | datatype (c++) | description                             |\n",
      "+-------+------------------------+---------+--------+-------+----------------+-----------------------------------------+\n",
      "|   0   |           Ts           |         |   X    |       |     double     | sampling time [s]                       |\n",
      "|   1   |       velocity_        |    X    |        |       |     double     | vehicle velocity [m/s]                  |\n",
      "|   2   |      velocity_dot      |         |        |       |     double     | vehicle acceleration [m/s^2] (optional) |\n",
      "|   3   |           x            |    X    |   X    |       |     double     | state x [m]                             |\n",
      "|   4   |           y            |    X    |   X    |       |     double     | state y [m]                             |\n",
      "|   5   |         x_dot          |         |        |       |     double     | state d/dt x [m/s] (optional)           |\n",
      "|   6   |         y_dot          |         |        |       |     double     | state d/dt y [m/s] (optional)           |\n",
      "|   7   |        Delta_u         |    X    |        |       |     double     | Delta_u [rad] (optional)                |\n",
      "|   8   |      Delta_u_dot       |         |        |       |     double     | Delta_u_dot[rad/s] (optional)           |\n",
      "|   9   | async_input_data_valid |         |   X    |       |      bool      |                                         |\n",
      "|   10  |   input_sample_valid   |    X    |   X    |       |      bool      |                                         |\n",
      "|   11  |        d_sample        |         |   X    |       |     double     |                                         |\n",
      "|   12  |        x_sample        |         |   X    |       |     double     |                                         |\n",
      "|   13  |        y_sample        |         |   X    |       |     double     |                                         |\n",
      "|   14  |       psi_sample       |         |   X    |       |     double     |                                         |\n",
      "|   15  |        K_sample        |         |   X    |       |     double     |                                         |\n",
      "+-------+------------------------+---------+--------+-------+----------------+-----------------------------------------+\n"
     ]
    }
   ],
   "source": [
    "show_inputs( code_gen_results['manifest'] )"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "+-------+--------------------------------+----------------+\n",
      "| #port |          input signal          | datatype (c++) |\n",
      "+-------+--------------------------------+----------------+\n",
      "|   0   |          output_valid          |      bool      |\n",
      "|   1   |   need_more_path_input_data    |      bool      |\n",
      "|   2   | distance_at_the_end_of_horizon |     double     |\n",
      "|   3   |         distance_ahead         |     double     |\n",
      "|   4   |           head_index           |    int32_t     |\n",
      "|   5   |         read_position          |    int32_t     |\n",
      "|   6   |     elements_free_to_write     |    int32_t     |\n",
      "|   7   |         tracked_index          |    int32_t     |\n",
      "|   8   |             d_star             |     double     |\n",
      "|   9   |             v_star             |     double     |\n",
      "|   10  |              x_r               |     double     |\n",
      "|   11  |              y_r               |     double     |\n",
      "|   12  |             psi_r              |     double     |\n",
      "|   13  |              K_r               |     double     |\n",
      "|   14  |           psi_r_dot            |     double     |\n",
      "|   15  |            Delta_l             |     double     |\n",
      "|   16  |          Delta_l_dot           |     double     |\n",
      "+-------+--------------------------------+----------------+\n"
     ]
    }
   ],
   "source": [
    "show_outputs(code_gen_results['manifest'] )"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Lateral path manipulation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "compiling system Sys1000_store_input_data (level 1)... \n",
      "compiling system Sys1003_optim_loop (level 3)... \n",
      "compiling system Sys1002_tracker (level 2)... \n",
      "compiling system Sys1004_controller (level 2)... \n",
      "compiling system Sys1005_simulation_model (level 2)... \n",
      "compiling system Sys1001_process_data (level 1)... \n",
      "compiling system simulation (level 0)... \n",
      "Generated code will be written to sfunctions/ .\n",
      "writing file sfunctions/simulation_manifest.json\n",
      "writing file sfunctions/lateral_path_transformer.cpp\n",
      "writing file sfunctions/lateral_path_transformer_block_mask_commands.m\n"
     ]
    }
   ],
   "source": [
    "code_gen_results, compiled_system = compile_lateral_path_transformer(\n",
    "    target_template = tg.TargetCppSimulinkSFunction(sfun_name='lateral_path_transformer'),\n",
    "    folder          = 'sfunctions/', # the folder to which the generated files are written\n",
    "    samples_in_buffer = 10000        # the size of the buffer storing the samples for the input path \n",
    ")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Show a summary of the blocks connectors."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "+-------+------------------------+---------+--------+-------+----------------+--------------------------------------------------------------------+\n",
      "| #port | input signal,  to -->  | outputs | update | reset | datatype (c++) | description                                                        |\n",
      "+-------+------------------------+---------+--------+-------+----------------+--------------------------------------------------------------------+\n",
      "|   0   |           Ts           |         |   X    |       |     double     | sampling time [s]                                                  |\n",
      "|   1   |       wheelbase        |         |   X    |       |     double     | wheelbase (l_r) [m]                                                |\n",
      "|   2   |       velocity_        |    X    |   X    |       |     double     | vehicle velocity [m/s]                                             |\n",
      "|   3   |       Delta_l_r        |    X    |        |       |     double     | lateral deviation to the path [m]                                  |\n",
      "|   4   |     Delta_l_r_dot      |    X    |        |       |     double     | 1st-order time derivative of lateral deviation to the path [m/s]   |\n",
      "|   5   |    Delta_l_r_dotdot    |    X    |        |       |     double     | 2nd-order time derivative of lateral deviation to the path [m/s^2] |\n",
      "|   6   |           d0           |    X    |        |       |     double     | initial state d0 [m]                                               |\n",
      "|   7   |           x0           |    X    |        |       |     double     | initial state x0 [m]                                               |\n",
      "|   8   |           y0           |    X    |        |       |     double     | initial state y0 [m]                                               |\n",
      "|   9   |          psi0          |    X    |        |       |     double     | initial state psi0 [rad]                                           |\n",
      "|   10  |         delta0         |         |        |       |     double     | initial state delta0 [rad]                                         |\n",
      "|   11  |       delta_dot0       |         |        |       |     double     | initial state delta_dot0 [rad/s]                                   |\n",
      "|   12  | async_input_data_valid |         |   X    |       |      bool      |                                                                    |\n",
      "|   13  |   input_sample_valid   |    X    |   X    |       |      bool      |                                                                    |\n",
      "|   14  |        d_sample        |         |   X    |       |     double     |                                                                    |\n",
      "|   15  |        x_sample        |         |   X    |       |     double     |                                                                    |\n",
      "|   16  |        y_sample        |         |   X    |       |     double     |                                                                    |\n",
      "|   17  |       psi_sample       |         |   X    |       |     double     |                                                                    |\n",
      "|   18  |        K_sample        |         |   X    |       |     double     |                                                                    |\n",
      "+-------+------------------------+---------+--------+-------+----------------+--------------------------------------------------------------------+\n"
     ]
    }
   ],
   "source": [
    "show_inputs( code_gen_results['manifest'] )"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "+-------+--------------------------------+----------------+\n",
      "| #port |          input signal          | datatype (c++) |\n",
      "+-------+--------------------------------+----------------+\n",
      "|   0   |          output_valid          |      bool      |\n",
      "|   1   |   need_more_path_input_data    |      bool      |\n",
      "|   2   | distance_at_the_end_of_horizon |     double     |\n",
      "|   3   |         distance_ahead         |     double     |\n",
      "|   4   |           head_index           |    int32_t     |\n",
      "|   5   |         read_position          |    int32_t     |\n",
      "|   6   |     elements_free_to_write     |    int32_t     |\n",
      "|   7   |         tracked_index          |    int32_t     |\n",
      "|   8   |          path_d_star           |     double     |\n",
      "|   9   |             path_d             |     double     |\n",
      "|   10  |             path_x             |     double     |\n",
      "|   11  |             path_y             |     double     |\n",
      "|   12  |            path_psi            |     double     |\n",
      "|   13  |             path_K             |     double     |\n",
      "|   14  |         vehicle_delta          |     double     |\n",
      "|   15  |       vehicle_delta_dot        |     double     |\n",
      "|   16  |          vehicle_psi           |     double     |\n",
      "|   17  |        vehicle_psi_dot         |     double     |\n",
      "|   18  |            velocity            |     double     |\n",
      "+-------+--------------------------------+----------------+\n"
     ]
    }
   ],
   "source": [
    "show_outputs(code_gen_results['manifest'] )"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/json": {
       "api_functions": {
        "calculate_output": "calcResults_1",
        "reset": "resetStates",
        "state_update": "updateStates"
       },
       "api_name": "simulation",
       "io": {
        "all_inputs_by_port_number": [
         {
          "cpptype": "double",
          "name": "Ts",
          "printf_patterns": "%f",
          "properties": {
           "default_value": 0.01,
           "title": "sampling time [s]"
          }
         },
         {
          "cpptype": "double",
          "name": "wheelbase",
          "printf_patterns": "%f",
          "properties": {
           "default_value": 3,
           "title": "wheelbase (l_r) [m]"
          }
         },
         {
          "cpptype": "double",
          "name": "velocity_",
          "printf_patterns": "%f",
          "properties": {
           "default_value": 1,
           "range": [
            0,
            25
           ],
           "title": "vehicle velocity [m/s]"
          }
         },
         {
          "cpptype": "double",
          "name": "Delta_l_r",
          "printf_patterns": "%f",
          "properties": {
           "default_value": 0,
           "range": [
            -10,
            10
           ],
           "title": "lateral deviation to the path [m]"
          }
         },
         {
          "cpptype": "double",
          "name": "Delta_l_r_dot",
          "printf_patterns": "%f",
          "properties": {
           "default_value": 0,
           "range": [
            -10,
            10
           ],
           "title": "1st-order time derivative of lateral deviation to the path [m/s]"
          }
         },
         {
          "cpptype": "double",
          "name": "Delta_l_r_dotdot",
          "printf_patterns": "%f",
          "properties": {
           "default_value": 0,
           "range": [
            -10,
            10
           ],
           "title": "2nd-order time derivative of lateral deviation to the path [m/s^2]"
          }
         },
         {
          "cpptype": "double",
          "name": "d0",
          "printf_patterns": "%f",
          "properties": {
           "default_value": 0,
           "title": "initial state d0 [m]"
          }
         },
         {
          "cpptype": "double",
          "name": "x0",
          "printf_patterns": "%f",
          "properties": {
           "default_value": 0,
           "title": "initial state x0 [m]"
          }
         },
         {
          "cpptype": "double",
          "name": "y0",
          "printf_patterns": "%f",
          "properties": {
           "default_value": 0,
           "title": "initial state y0 [m]"
          }
         },
         {
          "cpptype": "double",
          "name": "psi0",
          "printf_patterns": "%f",
          "properties": {
           "default_value": 0,
           "title": "initial state psi0 [rad]"
          }
         },
         {
          "cpptype": "double",
          "name": "delta0",
          "printf_patterns": "%f",
          "properties": {
           "default_value": 0,
           "title": "initial state delta0 [rad]"
          }
         },
         {
          "cpptype": "double",
          "name": "delta_dot0",
          "printf_patterns": "%f",
          "properties": {
           "default_value": 0,
           "title": "initial state delta_dot0 [rad/s]"
          }
         },
         {
          "cpptype": "bool",
          "name": "async_input_data_valid",
          "printf_patterns": "%d",
          "properties": {
           "title": ""
          }
         },
         {
          "cpptype": "bool",
          "name": "input_sample_valid",
          "printf_patterns": "%d",
          "properties": {
           "title": ""
          }
         },
         {
          "cpptype": "double",
          "name": "d_sample",
          "printf_patterns": "%f",
          "properties": {
           "title": ""
          }
         },
         {
          "cpptype": "double",
          "name": "x_sample",
          "printf_patterns": "%f",
          "properties": {
           "title": ""
          }
         },
         {
          "cpptype": "double",
          "name": "y_sample",
          "printf_patterns": "%f",
          "properties": {
           "title": ""
          }
         },
         {
          "cpptype": "double",
          "name": "psi_sample",
          "printf_patterns": "%f",
          "properties": {
           "title": ""
          }
         },
         {
          "cpptype": "double",
          "name": "K_sample",
          "printf_patterns": "%f",
          "properties": {
           "title": ""
          }
         }
        ],
        "inputs": {
         "calculate_output": {
          "cpptypes": [
           "double",
           "double",
           "double",
           "bool",
           "double",
           "double",
           "double",
           "double",
           "double"
          ],
          "names": [
           "d0",
           "Delta_l_r",
           "Delta_l_r_dot",
           "input_sample_valid",
           "psi0",
           "x0",
           "y0",
           "Delta_l_r_dotdot",
           "velocity_"
          ],
          "port_numbers": [
           6,
           3,
           4,
           13,
           9,
           7,
           8,
           5,
           2
          ],
          "printf_patterns": [
           "%f",
           "%f",
           "%f",
           "%d",
           "%f",
           "%f",
           "%f",
           "%f",
           "%f"
          ],
          "properties": [
           {
            "default_value": 0,
            "title": "initial state d0 [m]"
           },
           {
            "default_value": 0,
            "range": [
             -10,
             10
            ],
            "title": "lateral deviation to the path [m]"
           },
           {
            "default_value": 0,
            "range": [
             -10,
             10
            ],
            "title": "1st-order time derivative of lateral deviation to the path [m/s]"
           },
           {
            "title": ""
           },
           {
            "default_value": 0,
            "title": "initial state psi0 [rad]"
           },
           {
            "default_value": 0,
            "title": "initial state x0 [m]"
           },
           {
            "default_value": 0,
            "title": "initial state y0 [m]"
           },
           {
            "default_value": 0,
            "range": [
             -10,
             10
            ],
            "title": "2nd-order time derivative of lateral deviation to the path [m/s^2]"
           },
           {
            "default_value": 1,
            "range": [
             0,
             25
            ],
            "title": "vehicle velocity [m/s]"
           }
          ]
         },
         "reset": {
          "cpptypes": [],
          "names": [],
          "port_numbers": [],
          "printf_patterns": [],
          "properties": []
         },
         "state_update": {
          "cpptypes": [
           "double",
           "bool",
           "double",
           "double",
           "double",
           "double",
           "bool",
           "double",
           "double",
           "double"
          ],
          "names": [
           "psi_sample",
           "input_sample_valid",
           "wheelbase",
           "x_sample",
           "K_sample",
           "Ts",
           "async_input_data_valid",
           "y_sample",
           "d_sample",
           "velocity_"
          ],
          "port_numbers": [
           17,
           13,
           1,
           15,
           18,
           0,
           12,
           16,
           14,
           2
          ],
          "printf_patterns": [
           "%f",
           "%d",
           "%f",
           "%f",
           "%f",
           "%f",
           "%d",
           "%f",
           "%f",
           "%f"
          ],
          "properties": [
           {
            "title": ""
           },
           {
            "title": ""
           },
           {
            "default_value": 3,
            "title": "wheelbase (l_r) [m]"
           },
           {
            "title": ""
           },
           {
            "title": ""
           },
           {
            "default_value": 0.01,
            "title": "sampling time [s]"
           },
           {
            "title": ""
           },
           {
            "title": ""
           },
           {
            "title": ""
           },
           {
            "default_value": 1,
            "range": [
             0,
             25
            ],
            "title": "vehicle velocity [m/s]"
           }
          ]
         }
        },
        "outputs": {
         "calculate_output": {
          "cpptypes": [
           "bool",
           "bool",
           "double",
           "double",
           "int32_t",
           "int32_t",
           "int32_t",
           "int32_t",
           "double",
           "double",
           "double",
           "double",
           "double",
           "double",
           "double",
           "double",
           "double",
           "double",
           "double"
          ],
          "names": [
           "output_valid",
           "need_more_path_input_data",
           "distance_at_the_end_of_horizon",
           "distance_ahead",
           "head_index",
           "read_position",
           "elements_free_to_write",
           "tracked_index",
           "path_d_star",
           "path_d",
           "path_x",
           "path_y",
           "path_psi",
           "path_K",
           "vehicle_delta",
           "vehicle_delta_dot",
           "vehicle_psi",
           "vehicle_psi_dot",
           "velocity"
          ],
          "port_numbers": [
           null,
           null,
           null,
           null,
           null,
           null,
           null,
           null,
           null,
           null,
           null,
           null,
           null,
           null,
           null,
           null,
           null,
           null,
           null
          ],
          "printf_patterns": [
           "%d",
           "%d",
           "%f",
           "%f",
           "%d",
           "%d",
           "%d",
           "%d",
           "%f",
           "%f",
           "%f",
           "%f",
           "%f",
           "%f",
           "%f",
           "%f",
           "%f",
           "%f",
           "%f"
          ],
          "properties": [
           {},
           {},
           {},
           {},
           {},
           {},
           {},
           {},
           {},
           {},
           {},
           {},
           {},
           {},
           {},
           {},
           {},
           {},
           {}
          ]
         },
         "reset": {
          "cpptypes": [],
          "names": [],
          "port_numbers": [],
          "printf_patterns": [],
          "properties": []
         },
         "state_update": {
          "cpptypes": [],
          "names": [],
          "port_numbers": [],
          "printf_patterns": [],
          "properties": []
         }
        }
       }
      },
      "text/plain": [
       "<IPython.core.display.JSON object>"
      ]
     },
     "execution_count": 8,
     "metadata": {
      "application/json": {
       "expanded": false,
       "root": "root"
      }
     },
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# show the manifest\n",
    "\n",
    "from IPython.display import JSON, Code\n",
    "JSON(code_gen_results['manifest'], expanded=False)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "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.8.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}