(ax, traj)
| 230 | |
| 231 | |
| 232 | def plot_trajectory_with_angle(ax, traj): |
| 233 | if traj.shape[-1] > 3: |
| 234 | angle_phase_num = traj.shape[-1] - 2 |
| 235 | phase = 2 * np.pi * np.arange(angle_phase_num) / angle_phase_num |
| 236 | xn = traj[..., -3:] # (N, 3) |
| 237 | angles = -np.arctan2( |
| 238 | np.sum(np.sin(phase) * xn, axis=-1), np.sum(np.cos(phase) * xn, axis=-1) |
| 239 | ) |
| 240 | else: |
| 241 | angles = traj[..., -1] |
| 242 | |
| 243 | ax.plot(traj[:, 0], traj[:, 1], color="black", linewidth=2) |
| 244 | for p, angle in zip(traj, angles): |
| 245 | ax.arrow( |
| 246 | p[0], |
| 247 | p[1], |
| 248 | np.cos(angle) * 0.5, |
| 249 | np.sin(angle) * 0.5, |
| 250 | color="black", |
| 251 | zorder=1, |
| 252 | head_width=0.3, |
| 253 | head_length=0.2, |
| 254 | ) |
| 255 | ax.axis("equal") |
| 256 | |
| 257 | |
| 258 | def plot_crosswalk(ax, edge1, edge2): |
nothing calls this directly
no outgoing calls
no test coverage detected