path_following_control.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import json\n",
    "import math\n",
    "import numpy as np\n",
    "import openrtdynamics2.lang as dy\n",
    "import openrtdynamics2.targets as tg\n",
    "\n",
    "from vehicle_lib.vehicle_lib import *"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "# load track data\n",
    "with open(\"track_data/simple_track.json\", \"r\") as read_file:\n",
    "    track_data = json.load(read_file)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "compiling system Sys1000_optim_loop (level 1)... \n",
      "compiling system simulation (level 0)... \n",
      "Generated code will be written to generated/path_following_control .\n",
      "writing file generated/path_following_control/simulation_manifest.json\n",
      "writing file generated/path_following_control/main.cpp\n",
      "Running compiler: emcc --bind -s MODULARIZE=1 -s EXPORT_NAME=\"ORTD_simulator\" generated/path_following_control/main.cpp -O2 -s -o generated/path_following_control/main.js\n",
      "Compilation result:  0\n"
     ]
    }
   ],
   "source": [
    "#\n",
    "# Demo: a vehicle controlled to follow a given path\n",
    "#\n",
    "#       Implemented using the code generator openrtdynamics 2 - https://pypi.org/project/openrtdynamics2/ .\n",
    "#       This generates c++ code for Web Assembly to be run within the browser.\n",
    "#\n",
    "\n",
    "system = dy.enter_system()\n",
    "\n",
    "velocity               = dy.system_input( dy.DataTypeFloat64(1), name='velocity',              default_value=23.75,  value_range=[0, 25],   title=\"vehicle velocity\")\n",
    "k_p                    = dy.system_input( dy.DataTypeFloat64(1), name='k_p',                   default_value=2.0,    value_range=[0, 10.0], title=\"controller gain\")\n",
    "disturbance_amplitude  = dy.system_input( dy.DataTypeFloat64(1), name='disturbance_amplitude', default_value=20.0,   value_range=[-45, 45], title=\"disturbance amplitude\") * dy.float64(math.pi / 180.0)\n",
    "sample_disturbance     = dy.system_input( dy.DataTypeInt32(1),   name='sample_disturbance',    default_value=50,     value_range=[0, 300],  title=\"disturbance position\")\n",
    "\n",
    "# parameters\n",
    "wheelbase = 3.0\n",
    "\n",
    "# sampling time\n",
    "Ts = 0.01\n",
    "\n",
    "# create storage for the reference path:\n",
    "path = import_path_data(track_data)\n",
    "\n",
    "# create placeholders for the plant output signals\n",
    "x   = dy.signal()\n",
    "y   = dy.signal()\n",
    "psi = dy.signal()\n",
    "\n",
    "# track the evolution of the closest point on the path to the vehicles position\n",
    "projection = track_projection_on_path(path, x, y)\n",
    "\n",
    "d_star  = projection['d_star']  # the distance parameter of the path describing the closest point to the vehicle\n",
    "x_r     = projection['x_r']     # (x_r, y_r) the projected vehicle position on the path\n",
    "y_r     = projection['y_r']\n",
    "psi_r   = projection['psi_r']   # the orientation angle (tangent of the path)\n",
    "K_r     = projection['K_r']     # the curvature of the path\n",
    "Delta_l = projection['Delta_l'] # the lateral distance between vehicle and path \n",
    "\n",
    "tracked_index  = projection['tracked_index'] # the index describing the closest sample of the input path\n",
    "\n",
    "# reference for the lateral distance\n",
    "Delta_l_r = dy.float64(0.0) # zero in this example\n",
    "\n",
    "dy.append_output(Delta_l_r, 'Delta_l_r')\n",
    "\n",
    "# feedback control\n",
    "u = dy.PID_controller(r=Delta_l_r, y=Delta_l, Ts=0.01, kp=k_p)\n",
    "\n",
    "# path tracking\n",
    "# resulting lateral model u --> Delta_l : 1/s\n",
    "Delta_u = dy.asin( dy.saturate(u / velocity, -0.99, 0.99) )\n",
    "delta_star = psi_r - psi\n",
    "steering =  delta_star + Delta_u\n",
    "steering = dy.unwrap_angle(angle=steering, normalize_around_zero = True)\n",
    "\n",
    "dy.append_output(Delta_u, 'Delta_u')\n",
    "dy.append_output(delta_star, 'delta_star')\n",
    "\n",
    "\n",
    "#\n",
    "# The model of the vehicle including a disturbance\n",
    "#\n",
    "\n",
    "# model the disturbance\n",
    "disturbance_transient = np.concatenate(( cosra(50, 0, 1.0), co(10, 1.0), cosra(50, 1.0, 0) ))\n",
    "steering_disturbance, i = dy.play(disturbance_transient, start_trigger=dy.counter() == sample_disturbance, auto_start=False)\n",
    "\n",
    "# apply disturbance to the steering input\n",
    "disturbed_steering = steering + steering_disturbance * disturbance_amplitude\n",
    "\n",
    "# steering angle limit\n",
    "disturbed_steering = dy.saturate(u=disturbed_steering, lower_limit=-math.pi/2.0, upper_limit=math.pi/2.0)\n",
    "\n",
    "# the model of the vehicle\n",
    "x_, y_, psi_, x_dot, y_dot, psi_dot = discrete_time_bicycle_model(disturbed_steering, velocity, Ts, wheelbase)\n",
    "\n",
    "# close the feedback loops\n",
    "x   << x_\n",
    "y   << y_\n",
    "psi << psi_\n",
    "\n",
    "\n",
    "\n",
    "#\n",
    "# outputs: these are available for visualization in the html set-up\n",
    "#\n",
    "\n",
    "dy.append_output(x, 'x')\n",
    "dy.append_output(y, 'y')\n",
    "dy.append_output(psi, 'psi')\n",
    "\n",
    "dy.append_output(steering, 'steering')\n",
    "\n",
    "dy.append_output(x_r, 'x_r')\n",
    "dy.append_output(y_r, 'y_r')\n",
    "dy.append_output(psi_r, 'psi_r')\n",
    "\n",
    "dy.append_output(Delta_l, 'Delta_l')\n",
    "\n",
    "dy.append_output(steering_disturbance, 'steering_disturbance')\n",
    "dy.append_output(disturbed_steering, 'disturbed_steering')\n",
    "\n",
    "dy.append_output(tracked_index, 'tracked_index')\n",
    "\n",
    "\n",
    "# generate code for Web Assembly (wasm), requires emcc (emscripten) to build\n",
    "code_gen_results = dy.generate_code(template=tg.TargetCppWASM(), folder=\"generated/path_following_control\", build=True)\n",
    "\n",
    "#\n",
    "dy.clear()\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "        <iframe\n",
       "            width=\"100%\"\n",
       "            height=\"1000\"\n",
       "            src=\"../vehicle_control_tutorial/path_following_control.html\"\n",
       "            frameborder=\"0\"\n",
       "            allowfullscreen\n",
       "        ></iframe>\n",
       "        "
      ],
      "text/plain": [
       "<IPython.lib.display.IFrame at 0x7fc79e48f460>"
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "import IPython\n",
    "IPython.display.IFrame(src='../vehicle_control_tutorial/path_following_control.html', width='100%', height=1000)"
   ]
  },
  {
   "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
}