def plot_joint_disp(smal_npz, j=0, mocap_src=None, **kwargs):
J = get_smal_data(smal_npz)
freq = 30
fig, ax = plt.subplots()
j = 3
t = np.arange(J.shape[0]) / freq
dims = "z"
for d in dims:
n = dims.index(d)
x = J[:, j, n]
x_smoothed = signal.savgol_filter(x, window_length=15, polyorder=3)
ax.plot(t, x, alpha=.1, label=d)
ax.plot(t, x, label=d)
if mocap_src is not None:
## Add plot of correct mocap joint - for now always left front bottom
mocap_data = C3DData(ax=None, src=mocap_src, interpolate=True)
joint = "left rear paw"
mocap_j_data = mocap_data.get_marker_data_by_names(joint)
mocap_z_data = mocap_j_data[:, 0, 2]
t0 = 6.55
mocap_t = np.arange(4, 4 + mocap_data.n_frames / mocap_data.freq,
1 / mocap_data.freq)
ax2 = ax.twinx()
ax2.plot(mocap_t, mocap_z_data)
plt.legend()
plt.show()
def load_kin_data(kin_src, clip_length, mocap=True, resample_freq=100): if mocap: joint_data = C3DData(ax=None, src=kin_src, interpolate=True, crop=clip_length, fix_rotations="3 kph" in kin_src) else: joint_data = SMALData(kin_src, freq=30, norm=True, crop=clip_length, smooth=True) joint_data.resample_at(resample_freq) ### TRY RESAMPLING DATA TO 100 Hz target_bones, body_joints, no_torque_joints, ls_joints = joint_data.generate_skeleton_mapping() # Normalise data based on z data, so that the dog is roughly 0.5m high. Also smooth data kin_data = np.array(joint_data.all_data) kin_data = norm_kin_data(kin_data) paw_data = kin_data[:, no_torque_joints, 2] if not mocap: paw_data = signal.savgol_filter(paw_data, window_length=51, polyorder=2, axis=0) return paw_data
def load_solver(kin_src, clip_length, mocap=True, resample_freq=100):
if mocap:
joint_data = C3DData(
ax=None,
src=kin_src,
interpolate=True,
crop=clip_length,
fix_rotations="3 kph"
in kin_src) # only fix rotations for 3 kph for now
else:
joint_data = SMALData(kin_src,
freq=30,
norm=True,
crop=clip_length,
smooth=True)
joint_data.resample_at(resample_freq) ### TRY RESAMPLING DATA TO 100 Hz
target_bones, body_joints, no_torque_joints, leg_spring_joints = joint_data.generate_skeleton_mapping(
)
# Normalise data based on z data, so that the dog is roughly 0.5m high. Also smooth data
kin_data = np.array(joint_data.all_data)
kin_data = norm_kin_data(kin_data, targ_markers=leg_spring_joints)
solver_kwargs = dict(target_bones=target_bones,
body_joints=body_joints,
no_torque_joints=no_torque_joints,
foot_joints=no_torque_joints,
leg_spring_joints=leg_spring_joints,
freq=joint_data.freq,
name=kin_src_to_solver_name(kin_src))
solver = InverseDynamicsSolver(joint_data=kin_data,
**solver_kwargs,
is_mocap=mocap)
print(f"Solver loaded. Mass = {solver.total_mass:.1f} kg.")
return solver
def optimise_paws_to_c3d(mocap_src, dynamic_src=r"set_2 -- 3kph run3"):
"""Given a .c3d file, and the resultant GRFs, optimise the paws"""
freq_forceplate = 100
clip_length = 10 # seconds
# LOAD MOCAP DATA
mocap_data = C3DData(ax=None,
src=mocap_src,
interpolate=True,
crop=clip_length)
mocap_data.resample_at(100) ### TRY RESAMPLING DATA TO 100 Hz
target_bones, body_joints, no_torque_joints = mocap_data.generate_skeleton_mapping(
)
# Normalise data based on z data, so that the dog is roughly 0.5m high. Also smooth data
kin_data = np.array(mocap_data.all_data)
kin_data = 0.635 * kin_data / np.amax(kin_data[:, :, 2])
solver_kwargs = dict(target_bones=target_bones,
body_joints=body_joints,
no_torque_joints=no_torque_joints,
foot_joints=no_torque_joints,
freq=mocap_data.freq)
# LOAD FORCEPLATE DATA
force_plate_data, force_plate_tdelay = load_force_plate_data(dynamic_src)
dyn_data = force_plate_data
mass = 25.7 # normalise dynamic data by mass of dog
dyn_data *= 1 / (mass * g)
frame_delay = 10 # number of frames dynamic data is *ahead* of kinematic daya
# NEED TO CROP FOOTPLATE DATA TO CORRECT SIZE CLIP
freq_mocap = mocap_data.freq
frame_delay = int(freq_forceplate * force_plate_tdelay)
n_frames_forceplate = int(
mocap_data.n_frames * freq_forceplate / freq_mocap
) # number of frames for forceplate to be same time length as mocap
dyn_data = dyn_data[
frame_delay:frame_delay +
n_frames_forceplate] # crop forceplate data to match mocap/SMAL data
optimiser = ModelOptimiser(solver_kwargs,
kin_data,
dyn_data,
param_selection="paws",
dyn_format="grfs")
# PLOTTING FOR TESTING
solver = InverseDynamicsSolver(joint_data=kin_data,
**solver_kwargs,
model=optimiser)
# solver.view_ground_displacements(deriv = 0)
# solver.view_com_displacements(deriv=0)
# save dyn data and kin
src = r"C:\Users\Ollie\Dropbox\Ollie\University\IIB\Project\Figures\inverse_dynamics\l0_characterisation"
np.save(path_join(src, "disps.npy"),
np.swapaxes(solver.joint_pos[:, no_torque_joints, 2], 0, 1))
np.save(path_join(src, "unsmoothed_disps.npy"),
np.swapaxes(solver.unsmoothed_data[:, no_torque_joints, 2], 0, 1))
np.save(path_join(src, "grfs.npy"), np.swapaxes(dyn_data, 0, 1))
print("saved")
efd = 10
forces, torques = solver.solve_forces(save=False,
end_frames_disregarded=efd,
report_equations=False)
paw_joints = solver_kwargs["foot_joints"]
pred = forces[:, paw_joints, 2] / (solver.total_mass * g)
# plotting
fig, axes = plt.subplots(nrows=3, ncols=2, sharex="all", sharey="all")
[ax.set_xlabel("Time (s)") for ax in axes[2]]
[ax.set_ylabel("Norm Force \\ $F/mg$ ") for ax in axes[:, 0]]
[
ax.xaxis.set_major_formatter(
mpl.ticker.FuncFormatter(lambda x, p: x / solver.freq))
for ax in np.ravel(axes)
]
for i in range(4):
ax = axes[i // 2, i % 2]
ax.set_title(foot_joint_labels[i].title())
ax.plot(pred[:, i], label="Predicted")
ax.plot(np.roll(dyn_data[:, i], 0 * frame_delay), label="Measured")
ax.legend()
ax = axes[2, 0]
ax.set_title("Overall")
ax.plot(pred[:].sum(axis=1))
ax.plot(dyn_data[:].sum(axis=1))
axes[2, 1].axis("off")
fig.subplots_adjust(wspace=.08,
hspace=.29,
left=.09,
bottom=.09,
right=.98,
top=.95)
# PLOT GRF AS A FRAC OF STANCE, AND SPECTRAL DENSITY
fig, axes = plt.subplots(nrows=2, ncols=2, sharex="all", sharey="all")
fig, axes_sd = plt.subplots(nrows=2, ncols=2, sharex="all", sharey="all")
[ax.set_xlabel("\% Stance") for ax in axes[1]]
[ax.set_ylabel("Normalised Force $F/mg$ ") for ax in axes[:, 0]]
[
ax.xaxis.set_major_formatter(
mpl.ticker.FuncFormatter(lambda x, p: int(100 * x)))
for ax in np.ravel(axes)
]
for i in range(4):
ax = axes[i // 2, i % 2]
ax.set_title(foot_joint_labels[i].title())
# from pred grf, split by non-zero. Then, for each, define x stance %
pred_grf = pred[:, i]
for n, data in enumerate([pred_grf, dyn_data[:, i]]):
col = ["blue", "orange"][n]
# Plot pred stance graphs
footfalls = consecutive(np.where(
data > 1e-5)[0]) # list of indices for individual footfalls
trange = np.arange(0, clip_length, 1 / solver.freq)
all_x, all_y = [], []
for footfall in footfalls:
t = trange[footfall]
if t.min() == t.max(): continue # skip blank footfalls
x = (t - t.min()) / (t.max() - t.min()
) # format to % of stance
all_x += list(x)
all_y += list(data[footfall])
all_x = np.array(all_x)
all_y = np.array(all_y)
idx = all_x.argsort()
all_x, all_y = all_x[idx], all_y[idx]
all_y = np.array([y for _, y in sorted(zip(all_x, all_y))
]) # sort y by x
all_x = np.sort(all_x)
# NEW METHOD: FIT SINES TO CURVE. First mode is sin(pi x), as is 0 at x=0, x=1
n_freq = 5
sine_curve = lambda x, *coeff: sum(
[c * np.sin(np.pi * x * f) for f, c in enumerate(coeff)])
coeff, cov = optimize.curve_fit(sine_curve,
all_x,
all_y,
p0=[.6] + [0] * (n_freq - 1))
xrange = np.arange(0, 1, .01)
y_fit = sine_curve(xrange, *coeff)
ax.plot(xrange,
y_fit,
lw=3,
alpha=1.0,
color=col,
label=["Predicted", "Actual"][n])
ax_sd = axes_sd[i // 2, i % 2]
ax_sd.bar(list(range(1, n_freq + 1)), coeff, alpha=.6)
err = np.zeros_like(
xrange
) # error as a function of stance, based on time average window
window = .2
for j, h in enumerate(xrange):
err[j] = np.std(all_y[(h - window / 2 < all_x)
& (all_x <= h + window / 2)] - y_fit[j])
ax.fill_between(xrange,
y_fit + err,
y_fit - err,
color=col,
alpha=.6)
fig.subplots_adjust(wspace=.08,
hspace=.29,
left=.09,
bottom=.09,
right=.98,
top=.95)
plt.legend()
plt.show()
def animateC3D(src=None):
"""Plots .c3D data in 3D, front and side views."""
# Set up plots
fig = plt.figure()
gs = gridspec.GridSpec(2, 2, height_ratios=[1, 2])
ax_3d = fig.add_subplot(gs[1, :], projection='3d')
ax_front, ax_side = fig.add_subplot(gs[0, 0]), fig.add_subplot(gs[0, 1])
ax_front.set_title("Front")
ax_side.set_title("Side")
ax_3d.set_title("3D", pad=-10)
ax_3d.axis("off")
ax_3d.patch.set_edgecolor("black")
ax_3d.patch.set_linewidth('1')
for ax in [ax_side, ax_front]:
ax.tick_params(axis="x", which="both", bottom=False, labelbottom=False)
ax.tick_params(axis="y", which="both", left=False, labelleft=False)
mocap_data = C3DData(ax=ax_3d,
src=src,
alt_views={
"xz": ax_front,
"yz": ax_side
}) # Load MoCap data
fig.suptitle(mocap_data.name)
frames = mocap_data.n_frames
fps = mocap_data.frame_rate
buffer = lambda low, high, val=0: (low - val, high + val)
x_bounds, y_bounds, z_bounds = [
buffer(*getattr(mocap_data, f"{dim}_bounds"), val=50) for dim in "xyz"
] # Get all dimensions with a buffer of 10
# Set bounds and add axes arrows
ax_3d.set_xlim(*x_bounds)
ax_3d.set_ylim(*y_bounds)
ax_3d.set_zlim(*z_bounds)
ax_front.axis("equal")
ax_side.axis("equal")
ax_front.set_xlim(*x_bounds)
ax_front.set_ylim(*z_bounds)
ax_side.set_xlim(*y_bounds)
ax_side.set_ylim(*z_bounds)
scale = ((mocap_data.x_bounds[1] - mocap_data.x_bounds[0]) +
(mocap_data.y_bounds[1] - mocap_data.y_bounds[0]) +
((mocap_data.z_bounds[1] - mocap_data.z_bounds[0]))) / 3
arrow_args = dict(arrowstyle="-|>",
color="red",
mutation_scale=scale / 150,
lw=scale / 400)
arrow_length = min([
mocap_data.z_bounds[1], mocap_data.x_bounds[1], mocap_data.y_bounds[1]
]) * 0.3
a = Arrow3D((0, 0), (0, 0), (0, arrow_length), **arrow_args)
b = Arrow3D((0, arrow_length), (0, 0), (0, 0), **arrow_args)
c = Arrow3D((0, 0), (0, arrow_length), (0, 0), **arrow_args)
[ax_3d.add_artist(i) for i in [a, b, c]]
ax_3d.legend()
progress = tqdm(total=frames)
def animate(i):
try:
next(mocap_data)
progress.update(1)
except StopIteration:
pass
save_animation(animate, fig, frames, fps=fps, title=mocap_data.name)
def anim_smal_and_mocap(mocap_src, smal_src):
"""View animation of smal alongside mocap"""
freq = 60
mocap_data = C3DData(ax=None, src = mocap_src, crop = clip_length)
smal_data = SMALData(smal_src, freq = 30) # full length clip
for data in [mocap_data, smal_data]:
data.resample_at(freq)
delay = get_delay_between(mocap_data.name, "smal", "mocap")
paw_labels = ["left front", "right front", "left rear", "right rear"]
mocap_paws = mocap_data.get_marker_indices(*[p + " paw" for p in paw_labels])
smal_paws = smal_data.get_marker_indices(*[p + " paw" for p in paw_labels])
# quickly plot 3d of each
# mocap_all_data = norm_kin_data()
mocap_all = norm_kin_data(mocap_data.all_data)[:, mocap_paws, 2]
smal_all = norm_kin_data(smal_data.all_data)[:, smal_paws, 2]
# get frame matching delay through estimation
timed_fdelay = int(delay * freq) # delay before frame matching shift
test_fds = []
frame_match_range = np.arange(-50, 50, 1)
for i in frame_match_range:
# roll smal data, and crop to desired time range, before element wise product with mocap data
l = (smal_all[timed_fdelay+i:timed_fdelay+i+len(mocap_all), 0] * mocap_all[:, 0]).sum()
test_fds.append(l)
frame_delay = frame_match_range[np.argmax(test_fds)]
print(f"Selected Frame Delay: {frame_delay}")
# plt.plot(frame_match_range, test_fds)
# plt.show()
# TODO REVIEW SCALING HERE - I THINK THERE'S A BIG DISCREPANCY BETWEEN HOW MOCAP AND SMAL ARE SCALED.
# NEEDS TO BE BASED ON SAME JOINTS (MAYBE CHANGE FROM SHOULDERS TO UPPER?)
f = 20
fig = plt.figure()
ax = fig.add_subplot(111, projection = "3d")
b_map_mocap, body_joints, *_ = mocap_data.generate_skeleton_mapping()
mocap_3d = norm_kin_data(mocap_data.all_data, targ_markers=np.ravel(body_joints))
mocap_scat = ax.scatter(*zip(*mocap_3d[f]), color="green")
mocap_plots = []
for b, (j1, j2) in b_map_mocap.items():
mocap_plots.append(plt.plot(*zip(*mocap_3d[f, [j1, j2]]), color="green")[0])
b_map_smal, body_joints, *_ = smal_data.generate_skeleton_mapping()
smal_3d = norm_kin_data(smal_data.all_data, targ_markers=np.ravel(body_joints))[timed_fdelay+frame_delay:timed_fdelay+frame_delay+len(mocap_all)]
smal_scat = ax.scatter(*zip(*smal_3d[f]), color="red")
smal_plots = []
for b, (j1, j2) in b_map_smal.items():
smal_plots.append(plt.plot(*zip(*smal_3d[f, [j1, j2]]), color="red")[0])
def anim(i):
for data, scat, plots, b_map in [[mocap_3d, mocap_scat, mocap_plots, b_map_mocap], [smal_3d, smal_scat, smal_plots, b_map_smal]]:
scat._offsets3d = np.swapaxes(data[i], 0, 1)
for n, (j1, j2) in enumerate(b_map.values()):
plot = plots[n]
for dim, method in enumerate([plot.set_xdata, plot.set_ydata, plot.set_3d_properties]):
method(data[i, [j1,j2], dim])
X, Y, Z = np.swapaxes(mocap_3d, 0, 2)
bbox = BBox(np.ravel(X), np.ravel(Y), np.ravel(Z))
bbox.equal_3d_axes(ax, zoom = 1.5)
func = FuncAnimation(fig, anim, frames=100)
plt.show()
def compare_smal_to_mocap(mocap_src, smal_src, print_vals = False):
"""Produces graph of z position for all paw joints, for mocap and smal src"""
freq = 60
mocap_data = C3DData(ax=None, src = mocap_src, crop = clip_length, fix_rotations = True)
smal_data = SMALData(smal_src, freq = 30, smooth=True) # full length clip
# for data in [mocap_data, smal_data]:
# data.resample_at(freq)
delay = get_delay_between(mocap_data.name, "smal", "mocap")
paw_labels = ["left front", "right front", "left rear", "right rear"]
colours = ["green", "red"]
fig, axes = plt.subplots(nrows=2, ncols=2, sharex="all", sharey="all",
figsize = (8, 4))
ymax = 0 # running y max
mocap_paws = mocap_data.get_marker_indices(*[p + " paw" for p in paw_labels])
smal_paws = smal_data.get_marker_indices(*[p + " paw" for p in paw_labels])
# smal_paws[1] = mocap_data.get_marker_indices("right front ankle")[0]
# quickly plot 3d of each
# mocap_all_data = norm_kin_data()
mocap_all = norm_kin_data(mocap_data.all_data)[:, mocap_paws, 2]
smal_all = norm_kin_data(smal_data.all_data)[:, smal_paws, 2]
# get frame matching delay through estimation
timed_fdelay = int(delay * smal_data.freq) # delay before frame matching shift
test_fds = []
frame_match_range = np.arange(-50, 50, 1)
for i in frame_match_range:
# roll smal data, and crop to desired time range, before element wise product with mocap data
l = (smal_all[timed_fdelay-i:timed_fdelay-i+len(mocap_all), 1] * mocap_all[:,1]).sum()
test_fds.append(l)
frame_delay = frame_match_range[np.argmax(test_fds)]
print(f"Selected Frame Delay: {frame_delay}")
print_data = np.zeros((int(mocap_data.clip_length * mocap_data.freq), 10)) # t_mocap, t_smal, 4*mocap, 4*smal
plots = []
for n, data in enumerate([mocap_data, smal_data]):
b_mapping, body_joints, *_ = data.generate_skeleton_mapping()
all_data = norm_kin_data(data.all_data, targ_markers=np.ravel(body_joints))
## crop to clip
for j in range(4):
ax = axes[j // 2, j % 2]
paw_idx = [mocap_paws, smal_paws][n][j]
joint_data = all_data[:, paw_idx, 2]
t = np.arange(0, data.clip_length, 1/data.freq)
if n ==0:
t_mocap = t.copy()
if n ==0: mocap_joint_data = joint_data # store for correcting
if n == 1:
t -= (delay - frame_delay / freq) # temporal shift to match data
# crop to same time region as mocap
f_start, f_end = int(delay*data.freq), int((delay+clip_length)*data.freq)
joint_data = joint_data[f_start: f_end]
t = t[f_start: f_end]
t_smal = t.copy()
# Correcting factor, subtract off height difference
# joint_data -= (np.percentile(joint_data, 10) - np.percentile(mocap_joint_data,10))
# ax = ax.twinx()
plots.append(
ax.plot(t, joint_data, color = colours[n], label = ["Mocap", "KDN"][n])
[0])
print_data[:len(joint_data), 2 + (4*n) + j] = joint_data
# only consider mocap in ymax for now
if n == 0: ymax = max(ymax, joint_data.max())
# ax.set_xlim(t.min(), t.max()) # set xrange based off of mocap data ( smaller range )
print_data[:len(t_mocap), 0] = t_mocap
print_data[:len(t_smal), 1] = t_smal
if print_vals:
# convert to mm
print_data[:, 2:] *= 1000
print("Tmocap mocap1 mocap2 mocap3 mocap4")
idxs = [0, 2, 3, 4, 5]
for i in range(len(print_data)):
print(" ".join([f"{v:.2f}" for v in print_data[i, idxs]]))
print("--------")
print("Tsmal smal1 smal2 smal3 smal4")
idxs = [1, 6, 7, 8, 9]
for i in range(len(print_data)):
print(" ".join([f"{v:.2f}" for v in print_data[i, idxs]]))
xtitle="Time (s)"
ytitle = "Height (m)"
[axes[j//2, j%2].set_title(paw_labels[j].title()) for j in range(4)]
[ax.set_xlabel(xtitle) for ax in axes[-1, :]]
[ax.set_ylabel(ytitle) for ax in axes[:, 0]]
ax.set_ylim(0, ymax)
fig.legend(["Motion capture", "KDN"], handles=[plots[0], plots[5]], ncol=2, loc = 8)
plt.subplots_adjust(left=.1, right=.92, top=.93, bottom=.2, wspace = .19, hspace = .25)
# plt.savefig(r"C:\Users\Ollie\Dropbox\Ollie\University\IIB\Project\Figures\image_processing\smal_results_displacement\set_2_3r4.png",
# dpi = 300)
plt.show()