def main():
sx, sy, syaw = 10.0, 1.0, np.deg2rad(180.0)
gx, gy, gyaw = 0.0, -3.0, np.deg2rad(-45.0)
offset = 3.0
path, control_points = calc_4points_bezier_path(sx, sy, syaw, gx, gy, gyaw,
offset)
t = 0.8 # Number in [0, 1]
x_target, y_target = bezier(t, control_points)
derivatives_cp = bezier_derivatives_control_points(control_points, 2)
point = bezier(t, control_points)
dt = bezier(t, derivatives_cp[1])
ddt = bezier(t, derivatives_cp[2])
# Radius of curv
radius = 1 / curvature(dt[0], dt[1], ddt[0], ddt[1])
# Normalize derivative
dt /= np.linalg.norm(dt, 2)
tangent = np.array([point, point + dt])
normal = np.array([point, point + [-dt[1], dt[0]]])
curvature_center = point + np.array([-dt[1], dt[0]]) * radius
circle = plt.Circle(tuple(curvature_center),
radius,
color=(0, 0.8, 0.8),
fill=False,
linewidth=1)
assert path.T[0][0] == sx, "path is invalid"
assert path.T[1][0] == sy, "path is invalid"
assert path.T[0][-1] == gx, "path is invalid"
assert path.T[1][-1] == gy, "path is invalid"
fig, ax = plt.subplots()
ax.plot(path.T[0], path.T[1], label="Bezier Path")
ax.plot(control_points.T[0],
control_points.T[1],
'--o',
label="Control Points")
ax.plot(x_target, y_target)
ax.plot(tangent[:, 0], tangent[:, 1], label="Tangent")
ax.plot(normal[:, 0], normal[:, 1], label="Normal")
ax.add_artist(circle)
draw.Arrow(sx, sy, syaw, 1, "darkorange")
draw.Arrow(gx, gy, gyaw, 1, "darkorange")
plt.grid(True)
plt.title("Bezier Path: from Atsushi's work")
ax.axis("equal")
plt.show()
def main():
sx, sy, syaw = 10.0, 1.0, np.deg2rad(180.0)
gx, gy, gyaw = 0.0, -3.0, np.deg2rad(-45.0)
offset = 3.0
dist = np.hypot(sx - gx, sy - gy) / offset
control_points = np.array(
[[sx, sy], [sx + dist * np.cos(syaw), sy + dist * np.sin(syaw)],
[gx - dist * np.cos(gyaw), gy - dist * np.sin(gyaw)], [gx, gy]])
bz = BezierCurve(control_points)
path = bz.calc_path(100)
t = 0.8
point = bz.calc_deriv(t, 0)
pdt = bz.calc_deriv(t, 1)
pddt = bz.calc_deriv(t, 2)
# Radius of curv
radius = 1 / curvature(pdt[0], pdt[1], pddt[0], pddt[1])
# Normalize derivative
pdt /= np.linalg.norm(pdt, 2)
tangent = np.array([point, point + pdt])
normal = np.array([point, point + [-pdt[1], pdt[0]]])
curvature_center = point + np.array([-pdt[1], pdt[0]]) * radius
assert path[0, 0] == sx and path[0, 1] == sy, "path is invalid"
assert path[-1, 0] == gx and path[-1, 1] == gy, "path is invalid"
fig, ax = plt.subplots()
yaw = np.linspace(-np.pi, np.pi)
circle = curvature_center + radius * np.array([np.cos(yaw), np.sin(yaw)]).T
ax.plot(control_points.T[0],
control_points.T[1],
'--o',
label="Control Points")
plt.plot(path[:, 0], path[:, 1], label='Bezier Curve')
ax.plot(tangent[:, 0], tangent[:, 1], label="Tangent")
ax.plot(normal[:, 0], normal[:, 1], label="Normal")
ax.plot(circle[:, 0], circle[:, 1], c='cyan', label='Curvature Circle')
draw.Arrow(sx, sy, syaw, 1, "darkorange")
draw.Arrow(gx, gy, gyaw, 1, "darkorange")
plt.grid(True)
plt.title("Bezier Path")
ax.axis("equal")
plt.legend()
plt.show()
def main():
# choose states pairs: (s, y, yaw)
# simulation-1
# states = [(0, 0, 0), (10, 10, -90), (20, 5, 60), (30, 10, 120),
# (35, -5, 30), (25, -10, -120), (15, -15, 100), (0, -10, -90)]
# simulation-2
states = [(-3, 3, 120), (10, -7, 30), (10, 13, 30), (20, 5, -25),
(35, 10, 180), (32, -10, 180), (5, -12, 90)]
max_c = 0.1 # max curvature
path_x, path_y, yaw = [], [], []
for i in range(len(states) - 1):
s_x = states[i][0]
s_y = states[i][1]
s_yaw = np.deg2rad(states[i][2])
g_x = states[i + 1][0]
g_y = states[i + 1][1]
g_yaw = np.deg2rad(states[i + 1][2])
path_i = calc_optimal_path(s_x, s_y, s_yaw,
g_x, g_y, g_yaw, max_c)
path_x += path_i.x
path_y += path_i.y
yaw += path_i.yaw
# animation
plt.ion()
plt.figure(1)
for i in range(len(path_x)):
plt.clf()
plt.plot(path_x, path_y, linewidth=1, color='gray')
for x, y, theta in states:
draw.Arrow(x, y, np.deg2rad(theta), 2, 'blueviolet')
draw.Car(path_x[i], path_y[i], yaw[i], 1.5, 3)
plt.axis("equal")
plt.title("Simulation of Reeds-Shepp Curves")
plt.axis([-10, 42, -20, 20])
plt.draw()
plt.pause(0.001)
plt.pause(1)
def main():
PATH = [(0, 0, 0), (10, 10, -90), (20, 5, 60), (30, 10, 120), (35, -5, 30),
(25, -10, -120), (15, -15, 100), (0, -10, -90)]
for i in range(len(PATH) - 1):
sx, sy, syaw = PATH[i]
gx, gy, gyaw = PATH[i + 1]
syaw = math.radians(syaw)
gyaw = math.radians(gyaw)
rs = ReedsShepp(sx, sy, syaw, gx, gy, gyaw, 1.0 / 4, 0.1)
paths = rs.get_all_paths()
# for path in paths:
# plt.plot(path[:, 0], path[:, 1])
opt_path = rs.get_optimal_path()
plt.plot(opt_path[:, 0], opt_path[:, 1], linewidth=1, color='gray')
for x, y, theta in PATH:
draw.Arrow(x, y, np.deg2rad(theta), 2, 'blueviolet')
plt.xlabel('X [m]')
plt.ylabel('Y [m]')
plt.title('Reeds Shepp curve')
plt.axis("equal")
plt.show()
from myterial import pink
import draw
f, ax = plt.subplots(figsize=(7, 10))
# load and draw tracking data
from fcutils.path import files
for fp in files(
"/Users/federicoclaudi/Dropbox (UCL)/Rotation_vte/Locomotion/analysis/control/",
"*.h5",
):
tracking = pd.read_hdf(fp, key="hdf")
tracking.x = 20 - tracking.x + 20
# draw.Tracking.scatter(tracking.x, tracking.y, c=tracking.theta, vmin=0, vmax=360, cmap='bwr', lw=1, ec='k')
draw.Tracking(tracking.x, tracking.y, alpha=0.7)
# draw hairpin arena
draw.Hairpin(ax)
# draw waypoints
wps = Waypoints()
for wp in wps:
draw.Arrow(wp.x, wp.y, wp.theta, 2, width=4, color=pink, outline=True)
# fit dubin path
dubin = DubinPath(wps).fit()
draw.Tracking(dubin.x, dubin.y, lw=2, color="k")
plt.show()
def animate_one(ROI: str, crossing_id: int, FPS: int):
scale = 1 / 30 # to scale velocity vectors
save_folder = (paths.analysis_folder / "behavior" /
"roi_crossings_animations")
# ----------------------------------- prep ----------------------------------- #
# load tracking
bouts = pd.read_hdf(paths.analysis_folder / "behavior" / "roi_crossings" /
f"{ROI}_crossings.h5")
bout = bouts.sort_values("duration").iloc[crossing_id]
logger.info(f"Animating bout {crossing_id} - {round(bout.duration, 2)}s")
# tracking: dict = ROICrossing.get_crossing_tracking(bout.crossing_id)
# generate a path from tracking
path = Path(bout.x, bout.y)
# create fig and start camera
if ROI == "T4":
f = plt.figure(figsize=(12, 12))
elif "S" in ROI:
f = plt.figure(figsize=(8, 14))
else:
f = plt.figure(figsize=(5, 14))
axes = f.subplot_mosaic("""
AA
AA
BB
""")
camera = Camera(f)
# ----------------------------------- plot ----------------------------------- #
n_interpolated_frames = int(60 / FPS)
n_frames = len(bout.x) - 1
trajectory = GrowingPath()
for frame in track(range(n_frames), total=n_frames):
# repeat each frame N times interpolating between frames
for interpol in np.linspace(0, 1, n_interpolated_frames):
# get interpolated quantities
x = interpolate_at_frame(path.x, frame, interpol)
y = interpolate_at_frame(path.y, frame, interpol)
speed = interpolate_at_frame(path.speed, frame, interpol)
trajectory.update(x, y, speed=speed)
# time elapsed
_time = (np.arange(len(trajectory.speed)) / n_interpolated_frames /
60)
# plot the arena
draw.ROI(ROI, set_ax=True, shade=False, ax=axes["A"])
# plot tracking so far
draw.Tracking(trajectory.x, trajectory.y, lw=3, ax=axes["A"])
draw.Dot(x, y, s=50, ax=axes["A"])
# plot speed and velocity vectors
draw.Arrow(
x,
y,
interpolate_at_frame(path.velocity.angle,
frame,
interpol,
method="step"),
L=scale *
interpolate_at_frame(path.velocity.magnitude, frame, interpol),
color=colors.velocity,
outline=True,
width=2,
ax=axes["A"],
)
draw.Arrow(
x,
y,
path.acceleration.angle[frame],
L=4 * scale * path.acceleration.magnitude[frame],
color=colors.acceleration,
outline=True,
width=2,
ax=axes["A"],
zorder=200,
)
# plot speed trace
axes["B"].fill_between(_time,
0,
trajectory.speed,
alpha=0.5,
color=colors.speed)
axes["B"].plot(_time, trajectory.speed, lw=4, color=colors.speed)
axes["A"].set(title=f"{ROI} - crossing: {crossing_id}")
axes["B"].set(xlabel="time (s)", ylabel="speed (cm/s)")
camera.snap()
# ------------------------------- video creation ------------------------------- #
save_path = save_folder / f"{ROI}_{bout.crossing_id}.mp4"
logger.info(f"Saving animation @: '{save_path}'")
animation = camera.animate(interval=1000 / FPS)
animation.save(save_path, fps=FPS)
logger.info("Done!")
plt.cla()
plt.close(f)
del camera
del animation
# ----------------------------------- plot ----------------------------------- #
f = plot_roi_crossing(bout, step=4)
recorder.add_figures(svg=False)
plt.close(f)
tracking = pd.read_hdf(fp, key="hdf")
tracking.x = 20 - tracking.x + 20
# draw.Tracking.scatter(tracking.x, tracking.y, c=tracking.speed, vmin=0, vmax=100, cmap='bwr', lw=1, ec='k')
draw.Tracking(tracking.x, tracking.y, alpha=0.7)
if n == 5:
wps = Waypoints.from_tracking(
tracking.x, tracking.y, tracking.speed, tracking.theta,
)
# draw hairpin arena
draw.Hairpin(ax)
# draw waypoints
for wp in wps:
draw.Arrow(wp.x, wp.y, wp.theta, 2, width=4, color="g")
# fit and animate quintic polinomial
# wps = [
# Waypoint(20, 40, 270, 1, .2, 5),
# Waypoint(20, 30, 270, 3, 0, 5),
# Waypoint(15, 5, 180, 3, 0, 5),
# ]
# wps = [
# Waypoint(x=20.682933975843298, y=36.4433300139396, theta=241.34371213247948, speed=3.4675332536458368, accel=3.4675332536458368, segment_duration=1),
# Waypoint(x=20.09675309568283, y=21.35347306956288, theta=274.07352782608245, speed=62.565652699469716, accel=0.7206048508470744, segment_duration=1)
# ]
qp = QuinticPolinomial(wps).fit()
# draw path and iitial/final positoin
draw.Tracking(qp.x, qp.y, alpha=1, lw=1, color="k")
for (x, y), vecs in xy_vectors.items():
x = x + 0.5
y = y + 0.5
vectors = Vector.from_list(vecs)
mean_vector = vectors_mean(*vecs)
ax.scatter(x, y, s=10, color="k", zorder=500)
# draw.Arrows(x, y, vectors.angle, L=0.5, width=0.75, alpha=0.5, color='k', ax=ax)
draw.Arrow(
x,
y,
mean_vector.angle,
L=mean_vector.magnitude * scale_factor[vec_name],
width=2,
ax=ax,
color=colors.variables[vec_name],
zorder=1000,
alpha=1,
)
ax.set(title=vec_name)
# %%
f = plt.figure(figsize=(16, 6))
ax = f.add_subplot(111, projection="polar")
cross = bouts.iloc[7]
path = Path(cross.x, cross.y)
MODEL.vehicle.x,
MODEL.vehicle.y,
MODEL.vehicle.theta,
-MODEL.vehicle.steer,
)
# draw car velocity and acceleration vectors
if len(MODEL.trajectory.x) > 5:
velocity, _, _, acceleration, _, _ = va.compute_vectors(
MODEL.trajectory.x[:-3], MODEL.trajectory.y[:-3]
)
draw.Arrow(
MODEL.vehicle.x,
MODEL.vehicle.y,
velocity.angle[-1],
L=10 * velocity.magnitude[-1],
color=indigo_dark,
)
draw.Arrow(
MODEL.vehicle.x,
MODEL.vehicle.y,
acceleration.angle[-1],
L=15 * acceleration.magnitude[-1] * 10,
color=purple,
)
# draw currently considered trajectory:
draw.Tracking(
path.x[
MODEL.target_trajectory.ind_old : MODEL.target_trajectory.horizon
