update doc string

ROS_Core/src/Labs/Lab1/scripts/traj_planner.py

@@ -5,23 +5,26 @@
 import numpy as np
 import os
 import time
+import queue
 
-from utils import RealtimeBuffer, get_ros_param, Policy, GeneratePwm, get_obstacle_vertices
-from utils import frs_to_obstacle, frs_to_msg
+from utils import RealtimeBuffer, Policy, GeneratePwm
 from ILQR import RefPath
 from ILQR import ILQR
 
-from racecar_msgs.msg import ServoMsg, OdometryArray
+from racecar_msgs.msg import ServoMsg
 from racecar_planner.cfg import plannerConfig
-from visualization_msgs.msg import MarkerArray
 
 from dynamic_reconfigure.server import Server
 from tf.transformations import euler_from_quaternion
 from nav_msgs.msg import Odometry
 from nav_msgs.msg import Path as PathMsg # used to display the trajectory on RVIZ
 from std_srvs.srv import Empty, EmptyResponse
+
+# You will use those for lab2   
+from racecar_msgs.msg import OdometryArray
+from utils import frs_to_obstacle, frs_to_msg, get_obstacle_vertices, get_ros_param
+from visualization_msgs.msg import MarkerArray
 from racecar_obs_detection.srv import GetFRS, GetFRSResponse
-import queue
 
 class TrajectoryPlanner():
     '''

ROS_Core/src/Labs/Lab2/scripts/frs.py

@@ -14,11 +14,14 @@ def multistep_zonotope_reachset(init_box, a_mat, b_mat, input_box, dt_list, quic
         init_box: bounding box of the initial state, 
             np.ndarray [N,2] for N dimensions state space, 
             each row is [min, max] value of the state space, 
-        a_mat: a matrices, [N,N] np.ndarray
-        b_mat: b matrices, [N,M] np.ndarray for M-dimensional input space
+        a_mat: a matrices for dynamics, [N,N] np.ndarray
+        b_mat: b matrices for dynamics, [N,M] np.ndarray for M-dimensional input space
         input_box: list of input boxes, [M,2] np.ndarray for M-dimensional input space
             each row is [min, max] value of the input space 
-        dt_list: list of time steps
+        dt_list: list of time steps. 
+            Note: i-th dt in this list is the time difference i and i-1 steps of reachable set
+    return: 
+        reachable_set_list: a list of reachable sets, each reachable set is a zonotope
     '''
     reachable_set_list = []
     # Create a zonotope to represent the initial state
@@ -30,8 +33,8 @@ def multistep_zonotope_reachset(init_box, a_mat, b_mat, input_box, dt_list, quic
     # This function have following parameters:
     # Input:
     #     init_z: initial set represented as a zonotope
-    #     a_mat: a matrices, [N,N] np.ndarray for N-dimensional state space
-    #     b_mat: b matrices, [N,M] np.ndarray for M-dimensional input space
+    #     a_mat: a matrices for dynamics, [N,N] np.ndarray for N-dimensional state space
+    #     b_mat: b matricesfor dynamics, [N,M] np.ndarray for M-dimensional input space
     #     input_box: list of input boxes, [M,2] np.ndarray for M-dimensional input space
     #     dt: time step
     #     quick: if True, use the quick method, otherwise use the Kamenev method. Default: False

ROS_Core/src/Labs/Lab2/scripts/task2.ipynb

@@ -0,0 +1,105 @@
+{
+ "cells": [
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "%load_ext autoreload\n",
+    "%autoreload 2"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "import math\n",
+    "import sys\n",
+    "import matplotlib.pyplot as plt\n",
+    "from frs import multistep_zonotope_reachset\n",
+    "import numpy as np\n"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "K_vx = 5\n",
+    "K_y = 5\n",
+    "K_vy = 5\n",
+    "v_ref = 1\n",
+    "dx = 1\n",
+    "dy = 1\n",
+    "# run zonotope example\n",
+    "a_mat = np.array([[0, 0, 1, 0, 0], \n",
+    "        [0, 0, 0, 1, 0],\n",
+    "        [0, 0, 0, 0, 0],\n",
+    "        [0, 0, 0, 0, 0],\n",
+    "        [0, 0, 0, 0, 0]])\n",
+    "b_mat = np.array([[0, 0],\n",
+    "        [0, 0],\n",
+    "        [1,0],\n",
+    "        [0,1],\n",
+    "        [0,0]])\n",
+    "\n",
+    "k_mat = np.array([[0, 0, K_vx, 0, -K_vx],\n",
+    "        [0, K_y, 0, K_vy, 0]])\n",
+    "\n",
+    "\n",
+    "a_hat_mat = a_mat - b_mat@k_mat\n",
+    "\n",
+    "init_box = [[-1e-3, 1e-3], [-1e-3, 1e-3], [1,1], [0,0], [v_ref, v_ref]]\n",
+    "input_box = [[-dx, dx], [-dy, dy]]\n",
+    "\n",
+    "num_steps = 20\n",
+    "dt = 0.1\n",
+    "\n",
+    "dt_list = []\n",
+    "for _ in range(num_steps):\n",
+    "    dt_list.append(dt)\n",
+    "zonotopes = multistep_zonotope_reachset(init_box, a_hat_mat, b_mat, input_box, dt_list)\n",
+    "\n",
+    "# plot first set in red\n",
+    "plt.figure(figsize=(6, 6))\n",
+    "    \n",
+    "zonotopes[0].plot(col='r-o', label='Init')\n",
+    "for i, z in enumerate(zonotopes[1:]):\n",
+    "    label = 'Reach Set' if i == 0 else None\n",
+    "    z.plot(label=label)\n",
+    "\n",
+    "plt.title('Quickzonoreach Output (example_plot.py)')\n",
+    "plt.ylim(-0.5, 0.5)\n",
+    "plt.gca().set_aspect('equal', adjustable='box')\n",
+    "plt.legend()\n",
+    "plt.grid()"
+   ]
+  }
+ ],
+ "metadata": {
+  "kernelspec": {
+   "display_name": "spirit-rl-pybullet",
+   "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.9.15"
+  },
+  "orig_nbformat": 4
+ },
+ "nbformat": 4,
+ "nbformat_minor": 2
+}