Skip to content

Commit d93b1f6

Browse files
AlvaroAlvaro
Alvaro
authored and
Alvaro
committed
added experiments perdiver diagram
1 parent d88a171 commit d93b1f6

13 files changed

+4077
-25
lines changed

‎.gitignore

+5-1
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,11 @@ IBloFunMatch
99
build/*
1010
# Jupyter notebook files
1111
.ipynb_checkpoints
12+
*.egg-info
1213
# Output and experiments
1314
output/*
1415
experiments/*
15-
16+
notebooks/experiment_*
17+
notebooks/output
18+
# Python files
19+
__pycache__

‎notebooks/Coordinates.ipynb

+166
Large diffs are not rendered by default.

‎notebooks/Corridor-Trajectories-Matching.ipynb

+266
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,266 @@
1+
{
2+
"cells": [
3+
{
4+
"cell_type": "code",
5+
"execution_count": null,
6+
"id": "f3c351d0-2fb8-4858-be32-35762faf7634",
7+
"metadata": {},
8+
"outputs": [],
9+
"source": [
10+
"import numpy as np\n",
11+
"import matplotlib.pyplot as plt\n",
12+
"import scipy.spatial.distance as dist\n",
13+
"import matplotlib as mpl\n",
14+
"import iblofunmatch.inter as ibfm\n",
15+
"output_dir=\"output\"\n",
16+
"\n",
17+
"from navground import sim, core"
18+
]
19+
},
20+
{
21+
"cell_type": "markdown",
22+
"id": "bfff53f0-e9a4-435d-b5fc-672ce0257431",
23+
"metadata": {},
24+
"source": [
25+
"### Corridor trajectories experiment\n",
26+
"\n",
27+
"Corridor experiment with points converging to horizontal trajectories going left and right."
28+
]
29+
},
30+
{
31+
"cell_type": "code",
32+
"execution_count": null,
33+
"id": "b5e5f0b4-5fc7-4832-84cd-7236a49bd3ad",
34+
"metadata": {},
35+
"outputs": [],
36+
"source": [
37+
"length = 8.0\n",
38+
"num_steps = 5000\n",
39+
"width=1.1\n",
40+
"yaml = f\"\"\"\n",
41+
"steps: 3000\n",
42+
"time_step: 0.1\n",
43+
"save_directory: ''\n",
44+
"record_pose: true\n",
45+
"record_twist: true\n",
46+
"scenario:\n",
47+
" type: Corridor\n",
48+
" length: {length}\n",
49+
" width: {width} \n",
50+
" groups:\n",
51+
" -\n",
52+
" type: thymio\n",
53+
" number: 76\n",
54+
" radius: 0.08\n",
55+
" control_period: 0.1\n",
56+
" speed_tolerance: 0.02\n",
57+
" kinematics:\n",
58+
" type: 2WDiff\n",
59+
" wheel_axis: 0.094\n",
60+
" max_speed: 0.166\n",
61+
" behavior:\n",
62+
" type: HL\n",
63+
" optimal_speed: 0.12\n",
64+
" horizon: 5.0\n",
65+
" safety_margin: 0.02\n",
66+
" state_estimation:\n",
67+
" type: Bounded\n",
68+
" range: 5.0\n",
69+
"\"\"\"\n",
70+
"experiment = sim.load_experiment(yaml)\n",
71+
"experiment.run()"
72+
]
73+
},
74+
{
75+
"cell_type": "code",
76+
"execution_count": null,
77+
"id": "17572183-a835-46f5-a90c-bae332a2a972",
78+
"metadata": {},
79+
"outputs": [],
80+
"source": [
81+
"run = experiment.runs[0]\n",
82+
"ps = run.poses[:,:,[0,1]]\n",
83+
"twists = run.twists[:,:,:2] # ignore angular speeds"
84+
]
85+
},
86+
{
87+
"cell_type": "code",
88+
"execution_count": null,
89+
"id": "4f0f1e07-8467-44ac-9a90-d2ddeb73f617",
90+
"metadata": {},
91+
"outputs": [],
92+
"source": [
93+
"weight = 5"
94+
]
95+
},
96+
{
97+
"cell_type": "code",
98+
"execution_count": null,
99+
"id": "af7fe164-ce18-4b55-a1f1-9ed0b5d278f1",
100+
"metadata": {},
101+
"outputs": [],
102+
"source": [
103+
"def trajectory_corridor_distance_weighted_velocities(positions, velocities, weight, length):\n",
104+
" assert(len(positions)>0)\n",
105+
" assert(len(positions)==len(velocities))\n",
106+
" positions_velocities_list = []\n",
107+
" for idx, points in enumerate(positions):\n",
108+
" positions_velocities_list.append(np.hstack((points, velocities[idx]*weight)))\n",
109+
" distances_list = []\n",
110+
" for idx, points_vel in enumerate(positions_velocities_list):\n",
111+
" # Compare trajectories at same time\n",
112+
" points_vel_compare = positions_velocities_list[idx]\n",
113+
" dist_0 = dist.cdist(points_vel, points_vel_compare, \"minkowski\", p=2)\n",
114+
" shift_points_vel_compare = np.array(points_vel_compare)\n",
115+
" shift_points_vel_compare[shift_points_vel_compare[:,0]<length/2]+=[length, 0, 0, 0]\n",
116+
" shift_points_vel = np.array(points_vel)\n",
117+
" shift_points_vel[shift_points_vel[:,0]<length/2]+=[length, 0, 0, 0]\n",
118+
" dist_1 = dist.cdist(shift_points_vel, shift_points_vel_compare, \"minkowski\", p=2)\n",
119+
" distances_list.append(np.minimum(dist_0, dist_1))\n",
120+
" # end for\n",
121+
" distances_arr = np.array(distances_list)\n",
122+
" return np.min(distances_arr, axis=0)"
123+
]
124+
},
125+
{
126+
"cell_type": "code",
127+
"execution_count": null,
128+
"id": "8cc4d643-b8ee-443e-8dea-f445547f2129",
129+
"metadata": {},
130+
"outputs": [],
131+
"source": [
132+
"def plot_matching_diagram_trajectories(positions, velocities, weight, length, ax, color=\"blue\"):\n",
133+
" half_step = int(len(positions)/2)+1\n",
134+
" # Compute X, Z barcodes and matching\n",
135+
" Dist_X = trajectory_corridor_distance_weighted_velocities(positions[:half_step], velocities[:half_step], weight, length)\n",
136+
" Dist_Z = trajectory_corridor_distance_weighted_velocities(positions, velocities, weight, length)\n",
137+
" idx_S = list(range(Dist_X.shape[0]))\n",
138+
" # Compute matching from X to Z\n",
139+
" ibfm_out = ibfm.get_IBloFunMatch_subset(Dist_X, Dist_Z, idx_S, output_dir, max_rad=-1, num_it=1, store_0_pm=True, points=False, max_dim=1)\n",
140+
" # Plot 0 persistence diagram of matching \n",
141+
" match_diagram = []\n",
142+
" for idx, bar_X in enumerate(ibfm_out[\"S_barcode_0\"]):\n",
143+
" idx_match = ibfm_out[\"induced_matching_0\"][idx]\n",
144+
" bar_Z = ibfm_out[\"X_barcode_0\"][idx_match]\n",
145+
" match_diagram.append([bar_X[1], bar_Z[1]])\n",
146+
" # end for\n",
147+
" match_diagram = np.array(match_diagram)\n",
148+
" # Plot matching diagram\n",
149+
" ax.scatter(match_diagram[:,0], match_diagram[:,1], color=color)\n",
150+
" ax.plot([0,np.max(match_diagram)*1.1], [0,np.max(match_diagram)*1.1], color=\"gray\")"
151+
]
152+
},
153+
{
154+
"cell_type": "code",
155+
"execution_count": null,
156+
"id": "28a92b4f-1e7b-48e2-a63a-24d7f63fd55c",
157+
"metadata": {},
158+
"outputs": [],
159+
"source": [
160+
"fig, ax = plt.subplots(figsize=(6,6))\n",
161+
"steplist = list(range(500,900, 10))\n",
162+
"shift_time = 40\n",
163+
"for idx, start_step in enumerate(steplist):\n",
164+
" plot_matching_diagram_trajectories(ps[start_step:start_step+shift_time], twists[start_step:start_step+shift_time], weight, length, ax, color=mpl.colormaps[\"GnBu\"](idx/len(steplist)))\n"
165+
]
166+
},
167+
{
168+
"cell_type": "code",
169+
"execution_count": null,
170+
"id": "567af0f9-ef1d-4be9-a96b-7086a5887be0",
171+
"metadata": {},
172+
"outputs": [],
173+
"source": [
174+
"def plot_sequence(X_list, ax, mark_points=[]):\n",
175+
" # Plot figure\n",
176+
" X_old = X_list[0]\n",
177+
" ax.scatter(X_old[:,0], X_old[:,1], s=20, marker=\"o\", color=mpl.colormaps[\"RdBu\"](1/(len(X_list)+1)), zorder=2)\n",
178+
" for idx, X in enumerate(X_list[1:]):\n",
179+
" ax.scatter(X[:,0], X[:,1], s=20, marker=\"o\", color=mpl.colormaps[\"RdBu\"]((idx+1)/(len(X_list)+1)), zorder=2, label=\"X\")\n",
180+
" # for edge in zip(X, X_old):\n",
181+
" # edge_pts = np.array(edge)\n",
182+
" # ax.plot(edge_pts[:,0], edge_pts[:,1], color=\"gray\", zorder=1)\n",
183+
" if len(mark_points)>0:\n",
184+
" mark_X = X[mark_points]\n",
185+
" ax.scatter(mark_X[:,0], mark_X[:,1], s=20, marker=\"+\", color=\"red\", zorder=3)\n",
186+
" X_old = X\n",
187+
" #end for "
188+
]
189+
},
190+
{
191+
"cell_type": "code",
192+
"execution_count": null,
193+
"id": "e876c2b1-ac40-43b9-b028-ba74c5fe1d6e",
194+
"metadata": {},
195+
"outputs": [],
196+
"source": [
197+
"start_step=800\n",
198+
"fig, ax = plt.subplots(figsize=(5, 5))\n",
199+
"plot_matching_diagram_trajectories(ps[start_step:start_step+shift_time], twists[start_step:start_step+shift_time], weight, length, ax, color=\"blue\")\n",
200+
"positions = ps[start_step:start_step+shift_time]\n",
201+
"velocities = twists[start_step:start_step+shift_time]\n",
202+
"half_step = int(shift_time/2)+1\n",
203+
"Dist_X = trajectory_corridor_distance_weighted_velocities(positions[:half_step], velocities[:half_step], weight, length)\n",
204+
"Dist_Y = trajectory_corridor_distance_weighted_velocities(positions, velocities, weight, length)\n",
205+
"Dist_Z = np.minimum(Dist_X, Dist_Y)\n",
206+
"idx_S = list(range(Dist_X.shape[0]))\n",
207+
"# Compute matching from X to Z\n",
208+
"ibfm_out = ibfm.get_IBloFunMatch_subset(Dist_X, Dist_Z, idx_S, output_dir, max_rad=-1, num_it=1, store_0_pm=True, points=False, max_dim=1)\n",
209+
"# put persistence pairs together\n",
210+
"match_diagram = []\n",
211+
"for idx, bar_X in enumerate(ibfm_out[\"S_barcode_0\"]):\n",
212+
" idx_match = ibfm_out[\"induced_matching_0\"][idx]\n",
213+
" bar_Z = ibfm_out[\"X_barcode_0\"][idx_match]\n",
214+
" match_diagram.append([bar_X[1], bar_Z[1]])\n",
215+
"# end for\n",
216+
"match_diagram = np.array(match_diagram)\n",
217+
"\n",
218+
"print(np.array(match_diagram))\n",
219+
"print(ibfm_out[\"S_reps_0\"])"
220+
]
221+
},
222+
{
223+
"cell_type": "code",
224+
"execution_count": null,
225+
"id": "0952b84a-7f92-418b-bbb9-b73446c2c652",
226+
"metadata": {},
227+
"outputs": [],
228+
"source": [
229+
"X_seq = ps[list(range(start_step, start_step+shift_time+1, 2))]\n",
230+
"len(X_seq)\n",
231+
"fig, ax = plt.subplots(figsize=(10, 5))\n",
232+
"ax.set_aspect(\"equal\")\n",
233+
"plot_sequence(X_seq, ax, mark_points=[1])"
234+
]
235+
},
236+
{
237+
"cell_type": "code",
238+
"execution_count": null,
239+
"id": "fab597d5-1a55-477a-8307-666542557611",
240+
"metadata": {},
241+
"outputs": [],
242+
"source": []
243+
}
244+
],
245+
"metadata": {
246+
"kernelspec": {
247+
"display_name": "Python 3 (ipykernel)",
248+
"language": "python",
249+
"name": "python3"
250+
},
251+
"language_info": {
252+
"codemirror_mode": {
253+
"name": "ipython",
254+
"version": 3
255+
},
256+
"file_extension": ".py",
257+
"mimetype": "text/x-python",
258+
"name": "python",
259+
"nbconvert_exporter": "python",
260+
"pygments_lexer": "ipython3",
261+
"version": "3.10.12"
262+
}
263+
},
264+
"nbformat": 4,
265+
"nbformat_minor": 5
266+
}

0 commit comments

Comments
 (0)