added a few example files

Author: atorras1618

notebooks/dtw-matchings/TorusCross-matchings.ipynb

@@ -55,7 +55,7 @@
    "metadata": {},
    "outputs": [],
    "source": [
-    "from perdiver.navground_parser import parser\n",
+    "from perdiver.navground_io import parser\n",
     "\n",
     "args = parser.parse_args([\n",
     "        '--side', '6.5',\n",
@@ -64,7 +64,8 @@
     "        '--time_step', '0.1',\n",
     "        '--num_agents', '10',\n",
     "        '--max_speed', '1.66',\n",
-    "        '--optimal_speed', '1.2',\n",
+    "        '--optimal_speed_min', '1.1',\n",
+    "        '--optimal_speed_min', '1.5',\n",
     "        '--radius', '0.4',\n",
     "        '--safety_margin', '0.1',\n",
     "        '--behavior', 'HL',\n",
@@ -362,6 +363,7 @@
    "metadata": {},
    "outputs": [],
    "source": [
+    "args.optimal_speed=1.2\n",
     "yaml = f\"\"\"\n",
     "runs: {args.num_runs}\n",
     "steps: {args.num_steps}\n",
@@ -387,7 +389,10 @@
     "        max_speed: {args.max_speed}\n",
     "      behavior:\n",
     "        type: {args.behavior}\n",
-    "        optimal_speed: {args.optimal_speed}\n",
+    "        optimal_speed:\n",
+    "            sampler: uniform\n",
+    "            from: {args.optimal_speed_min}\n",
+    "            to: {args.optimal_speed_max}\n",
     "        horizon: 5.0\n",
     "        safety_margin: {args.safety_margin}\n",
     "      state_estimation:\n",

notebooks/examples/Stability.ipynb

@@ -0,0 +1,128 @@
+{
+ "cells": [
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "814e6431-c5d6-42e7-8fbc-1430425b9d10",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "import numpy as np\n",
+    "import matplotlib.pyplot as plt\n",
+    "import matplotlib as mpl\n",
+    "\n",
+    "import os\n",
+    "\n",
+    "from navground import core, sim\n",
+    "from navground.sim.ui.video import display_video_from_run\n",
+    "\n",
+    "import perdiver.perdiver as perdiver\n",
+    "from perdiver.navground_io import parser, run_navground\n",
+    "from perdiver.distances import *\n",
+    "\n",
+    "plots_dir = os.path.join(\"plots\", \"stability\")\n",
+    "os.makedirs(plots_dir, exist_ok=True)"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "09fd3f0a-7243-41ff-b239-9f0d9518cfa6",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "args = parser.parse_args([\n",
+    "        '--scenario', 'Cross',\n",
+    "        '--side', '10',\n",
+    "        '--num_steps', '500',\n",
+    "        '--time_step', '0.1',\n",
+    "        '--num_agents', '12',\n",
+    "        '--max_speed', '1.66',\n",
+    "        '--optimal_speed_min', '0.1',\n",
+    "        '--optimal_speed_min', '0.15',\n",
+    "        '--radius', '0.4',\n",
+    "        '--safety_margin', '0.1',\n",
+    "        '--epsilon', '30',\n",
+    "        '--time_delay', '5',\n",
+    "])\n",
+    "behavior_list = [\"ORCA\"]\n",
+    "runs = {}\n",
+    "for behavior in behavior_list:\n",
+    "    args.behavior = behavior\n",
+    "    runs[behavior] = run_navground(args)"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "578d1674-1e5f-4914-b174-47555adaaeb5",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "from perdiver.perdiver import get_matching_diagram, plot_matching_diagram, plot_timesteps_cross\n",
+    "\n",
+    "args.weight = 1\n",
+    "\n",
+    "run = runs[\"ORCA\"][0]\n",
+    "ps = np.array(run.poses)\n",
+    "twists = np.array(run.twists)\n",
+    "\n",
+    "fig, ax = plt.subplots(figsize=(15,5), ncols=3)\n",
+    "match_diagram_list = []\n",
+    "for i, initial_step in enumerate([150, 153]):\n",
+    "    args.start_step = initial_step\n",
+    "    X = ps[args.start_step]\n",
+    "    Y = ps[args.start_step + args.epsilon]\n",
+    "    vel_X = twists[args.start_step]\n",
+    "    vel_Y = twists[args.start_step + args.epsilon]\n",
+    "    X_len = X.shape[0]-1\n",
+    "    # Plot two timesteps\n",
+    "    plot_timesteps_cross(run, [args.start_step, args.start_step + args.epsilon], args.side, ax[i]) \n",
+    "    # Save figure\n",
+    "    ax[i].set_xticks([])\n",
+    "    ax[i].set_yticks([])\n",
+    "    ax[i].set_title(f\"Start timestep:{initial_step}\", fontsize=20)\n",
+    "    # Plot matching diagram\n",
+    "    Dist_X = distances_2Dtorus_weighted_velocities(X, vel_X, args.weight, args.side)\n",
+    "    Dist_Y = distances_2Dtorus_weighted_velocities(Y, vel_Y, args.weight, args.side)\n",
+    "    match_diagram_list.append(get_matching_diagram(Dist_X, Dist_Y))\n",
+    "# end for    \n",
+    "plot_matching_diagram(match_diagram_list[0], ax[2], color=\"blue\")\n",
+    "match_diagram = match_diagram_list[1]\n",
+    "ax[2].scatter(match_diagram[:,0], match_diagram[:,1], color=\"red\", marker=\"X\")\n",
+    "ax[2].set_title(\"Matching diagrams\", fontsize=20)\n",
+    "# Save figure\n",
+    "plt.savefig(os.path.join(plots_dir, \"stability.png\"))"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "41c8a7d1-ee7c-4e05-9842-00c6d2953890",
+   "metadata": {},
+   "outputs": [],
+   "source": []
+  }
+ ],
+ "metadata": {
+  "kernelspec": {
+   "display_name": "Python 3 (ipykernel)",
+   "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.10.12"
+  }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 5
+}