diff --git a/ROS_Core/src/Labs/Lab1/scripts/traj_planner.py b/ROS_Core/src/Labs/Lab1/scripts/traj_planner.py index d8d5d02..cf14c83 100644 --- a/ROS_Core/src/Labs/Lab1/scripts/traj_planner.py +++ b/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(): ''' diff --git a/ROS_Core/src/Labs/Lab2/scripts/frs.py b/ROS_Core/src/Labs/Lab2/scripts/frs.py index ca9869d..a6d02f2 100644 --- a/ROS_Core/src/Labs/Lab2/scripts/frs.py +++ b/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 diff --git a/ROS_Core/src/Labs/Lab2/scripts/task2.ipynb b/ROS_Core/src/Labs/Lab2/scripts/task2.ipynb new file mode 100644 index 0000000..70bf008 --- /dev/null +++ b/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 +}