def animate(self, nb_frames=80, show_now=True, **kwargs):
"""
Animate solution with bioviz
:param nb_frames: Number of frames in the animation. (integer)
:param show_now: If updates must be automatically done (True) or not (False)
"""
try:
import bioviz
except ModuleNotFoundError:
raise RuntimeError("bioviz must be install to animate the model")
check_version(bioviz, "2.0.1", "2.1.0")
data_interpolate, data_control = Data.get_data(
self.ocp, self.sol["x"], integrate=False, interpolate_nb_frames=nb_frames
)
if not isinstance(data_interpolate["q"], (list, tuple)):
data_interpolate["q"] = [data_interpolate["q"]]
all_bioviz = []
for idx_phase, data in enumerate(data_interpolate["q"]):
all_bioviz.append(bioviz.Viz(loaded_model=self.ocp.nlp[idx_phase].model, **kwargs))
all_bioviz[-1].load_movement(self.ocp.nlp[idx_phase].mapping["q"].expand.map(data))
if show_now:
b_is_visible = [True] * len(all_bioviz)
while sum(b_is_visible):
for i, b in enumerate(all_bioviz):
if b.vtk_window.is_active:
b.update()
else:
b_is_visible[i] = False
else:
return all_bioviz
plt.figure("Q_dot")
for i in range(q_ac.shape[0]):
plt.subplot(2, 3, i + 1)
plt.plot(t, qdot_ac[i, :])
plt.plot(t, qdot_ip[i, :])
plt.plot(t, q_dot_ref[i, :])
plt.title(q_name[i])
plt.legend(labels=['Acados', 'Ipopt', 'EKF'],
bbox_to_anchor=(1.05, 1),
loc='upper left',
borderaxespad=0.)
# plt.show()
plt.figure("Muscles activations")
for i in range(u_ac.shape[0]):
plt.subplot(4, 5, i + 1)
plt.step(t, u_ac[i, :])
plt.step(t, u_ip[i, :])
plt.step(t, emg_treat[i, :], color='r')
plt.title(biorbd_model.muscleNames()[i].to_string())
plt.legend(labels=['Acados', 'Ipopt', 'Target'],
bbox_to_anchor=(1.05, 1),
loc='upper left',
borderaxespad=0.)
plt.show()
import bioviz
b = bioviz.Viz(model_path=model_path + model)
b.load_movement(q_ac)
b.exec()
def animate(self,
n_frames: int = 0,
shooting_type: Shooting = None,
show_now: bool = True,
**kwargs: Any) -> Union[None, list]:
"""
Animate the simulation
Parameters
----------
n_frames: int
The number of frames to interpolate to. If the value is 0, the data are merged to a one phase if possible.
If the value is -1, the data is not merge in one phase
shooting_type: Shooting
The Shooting type to animate
show_now: bool
If the bioviz exec() function should be called automatically. This is blocking method
kwargs: Any
Any parameters to pass to bioviz
Returns
-------
A list of bioviz structures (one for each phase). So one can call exec() by hand
"""
try:
import bioviz
except ModuleNotFoundError:
raise RuntimeError("bioviz must be install to animate the model")
check_version(bioviz, "2.1.1", "2.2.0")
data_to_animate = self.integrate(
shooting_type=shooting_type) if shooting_type else self.copy()
if n_frames == 0:
try:
data_to_animate = data_to_animate.interpolate(sum(self.ns))
except RuntimeError:
pass
elif n_frames > 0:
data_to_animate = data_to_animate.interpolate(n_frames)
states = data_to_animate.states
if not isinstance(states, (list, tuple)):
states = [states]
all_bioviz = []
for idx_phase, data in enumerate(states):
# Convert parameters to actual values
nlp = self.ocp.nlp[idx_phase]
for param in nlp.parameters:
if param.function:
param.function(nlp.model, self.parameters[param.name],
**param.params)
all_bioviz.append(
bioviz.Viz(
self.ocp.nlp[idx_phase].model.path().absolutePath().
to_string(), **kwargs))
all_bioviz[-1].load_movement(
self.ocp.nlp[idx_phase].variable_mappings["q"].to_second.map(
data["q"]))
if show_now:
b_is_visible = [True] * len(all_bioviz)
while sum(b_is_visible):
for i, b in enumerate(all_bioviz):
if b.vtk_window.is_active:
b.update()
else:
b_is_visible[i] = False
return None
else:
return all_bioviz
# -- Plot Q -- #
q_new = np.ndarray((biorbd_model.nbQ(), len(markersOverFrames)))
qdot_new = np.ndarray((biorbd_model.nbQ(), len(markersOverFrames)))
for k in range(q_recons.shape[0]):
b = np.poly1d(np.polyfit(t, q_recons[k, :], 4))
c = np.poly1d(np.polyfit(t, q_dot_recons[k, :], 4))
q_new[k, :] = b(t)
qdot_new[k, :] = c(t)
for k in range(q_recons.shape[0]):
plt.subplot(5, 4, k+1)
plt.plot(t, q_recons[k, :])
# plt.plot(t, q_dot_recons[k, :])
# plt.plot(t, qdot_new[k, :])
plt.plot(t, q_new[k, :])
plt.show()
if bioviz_found:
b = bioviz.Viz(loaded_model=biorbd_model)
b.load_movement(q_new)
b.exec()
# Save initial states in .mat file
emg_norm = reduce(marker_treat.shape[2], emg_norm)[0]
# x_init = np.concatenate((q_recons, q_dot_recons, emg_norm))
x_init = np.concatenate((q_new, qdot_new, emg_norm))
dic[f'x_init_{tries}'] = x_init
dic[f't_final_{tries}'] = t_final
sio.savemat(data_path + f"sujet_5/states_init_{data}.mat", dic)
sol = ocp.solve(
solver=Solver.
ACADOS, # FULL_CONDENSING_QPOASES, "PARTIAL_CONDENSING_HPIPM"
solver_options={
"qp_solver": "PARTIAL_CONDENSING_HPIPM",
"integrator_type": "IRK",
"nlp_solver_max_iter": 50,
"sim_method_num_steps": 2,
"nlp_solver_tol_ineq": float("1e%d" % -i),
"nlp_solver_tol_stat": float("1e%d" % -i),
"nlp_solver_tol_comp": float("1e%d" % -i),
"nlp_solver_tol_eq": float("1e%d" % -i)
})
# --- Get the results --- #
states, controls, params = Data.get_data(ocp, sol, get_parameters=True)
# --- show reults --- #
# result = ShowResult(ocp, sol)
# result.graphs()
# result.animate()
q = sol['qqdot'][1:3, :]
length = params["gravity_z"][0, 0]
print(length)
import bioviz
b = bioviz.Viz(
model_path=
"/home/amedeo/Documents/programmation/marker_emg_tracking/models/pendulum.bioMod"
)
b.load_movement(q)
b.exec()
pn, pn, idx_segment_bow_hair, rt_on_string)
custom_rt = Function("custom_rt", [pn.nlp.q], [pn.val]).expand()
ObjectiveFcn.Mayer.SUPERIMPOSE_MARKERS.value[0](pn, pn, tag_bow_contact,
tag_violin)
superimpose = Function("superimpose", [pn.nlp.q], [pn.val]).expand()
def objective_function(x, *args, **kwargs):
out = np.ndarray((6, ))
out[:3] = np.array(custom_rt(x))[:, 0]
out[3:] = np.array(superimpose(x))[:, 0]
return out
b = bioviz.Viz(loaded_model=m,
markers_size=0.003,
show_markers=True,
show_meshes=False)
x0 = np.zeros(m.nbDof(), )
if bow_place == "frog":
bounds[0][-1] = -0.0701
bounds[1][-1] = -0.0699
else:
bounds[0][-1] = -0.551
bounds[1][-1] = -0.549
x0 = np.mean(bounds, axis=0)
pos = optimize.least_squares(objective_function, x0=x0, bounds=bounds)
print(
f"Optimal Q for the bow at {bow_place} on {string_to_test} string is:\n{pos.x}\n"
f"with cost function = {objective_function(pos.x)}")
b.set_q(pos.x)
b.exec()
plt.subplot(2, 3, i + 1)
plt.plot(t, qdot[i, :], c='purple')
plt.title(q_name[i])
# plt.figure("Tau")
# for i in range(q.shape[0]):
# plt.subplot(2, 3, i + 1)
# plt.plot(t, tau[i, :], c='orange')
# plt.title(biorbd_model.muscleNames()[i].to_string())
plt.figure("Muscles controls")
for i in range(u.shape[0]):
plt.subplot(4, 5, i + 1)
plt.step(t, u[i, :], c='orange')
plt.plot(t, a[i, :], c='purple')
plt.title(biorbd_model.muscleNames()[i].to_string())
plt.legend(labels=['excitations', "activations"],
bbox_to_anchor=(1.05, 1),
loc='upper left',
borderaxespad=0.)
plt.show()
b = bioviz.Viz(model_path="arm_wt_rot_scap.bioMod")
b.load_movement(q)
b.exec()
# --- Show results --- #
result = ShowResult(ocp, sol)
result.graphs()
# result.animate()
# Dispatch markers in biorbd structure so EKF can use it
markersOverFrames = []
for i in range(markers.shape[2]):
markersOverFrames.append(
[biorbd.NodeSegment(m) for m in markers[:, :, i].T])
# Create a Kalman filter structure
freq = 100 # Hz
params = biorbd.KalmanParam(freq)
kalman = biorbd.KalmanReconsMarkers(model, params)
# Perform the kalman filter for each frame (the first frame is much longer than the next)
Q = biorbd.GeneralizedCoordinates(model)
Qdot = biorbd.GeneralizedVelocity(model)
Qddot = biorbd.GeneralizedAcceleration(model)
q_recons = np.ndarray((model.nbQ(), len(markersOverFrames)))
for i, targetMarkers in enumerate(markersOverFrames):
kalman.reconstructFrame(model, targetMarkers, Q, Qdot, Qddot)
q_recons[:, i] = Q.to_array()
# Print the kinematics to the console
print(
f"Frame {i}\nExpected Q = {target_q[:, i]}\nCurrent Q = {q_recons[:, i]}\n"
)
# Animate the results if biorbd viz is installed
if biorbd_viz_found:
b = bioviz.Viz(loaded_model=model)
b.load_movement(q_recons)
b.exec()
"-",
color="#1f77b4",
label="Mass Force")
axs_2.axvline(x=5, color="k", linewidth=0.8, alpha=0.6, linestyle=(0, (5, 5)))
axs_2.set_xlabel("Time [s]").set_fontsize(16)
axs_2.set_ylabel("Mass force actuation\n[N]", color="#1f77b4").set_fontsize(16)
axs_2.tick_params(axis="y", labelcolor="#1f77b4")
ax2 = axs_2.twinx()
ax2.plot(0, 0, "-", color="#1f77b4", label="Mass Force")
ax2.step(time_vector[:-1],
-10 * q[0, :-1],
"-",
color="#2ca02c",
label="Spring Force")
ax2.set_ylabel("Spring external force\n[N]", color="#2ca02c").set_fontsize(16)
ax2.tick_params(axis="y", labelcolor="#2ca02c")
try:
os.mkdir(root_path + "figure")
except FileExistsError:
pass
plt.savefig("figure/Mass_Pendulum_Fext.eps", format="eps")
plt.show()
print("RMS q_m - q*_m : ", np.std(q[0, 51:] - 0.5))
b = bioviz.Viz(model_path)
b.load_movement(q)
b.exec()
def show(self, q):
import bioviz
b = bioviz.Viz(self.models[0].path().absolutePath().to_string())
b.set_q(q if len(q.shape) == 1 else q[:, 0])
b.exec()
import scipy.io as sio
import seaborn
from utils import *
import bioviz
import matplotlib.pyplot as plt
biorbd_model = biorbd.Model("arm_wt_rot_scap.bioMod")
# Define folder and status file
fold_w_emg = f"solutions/w_track_emg_rt_exc/"
# Get data for optimal (track EMG) movement
mat_content = sio.loadmat(f"{fold_w_emg}track_mhe_w_EMG_excitation_driven_co_lvl0_noise_lvl_0_0.mat")
X_est = mat_content["X_est"]
q_est = np.mean(X_est[:, : biorbd_model.nbQ(), :], axis=0)
b = bioviz.Viz("arm_wt_rot_scap.bioMod")
b.load_movement(q_est)
b.vtk_window.change_background_color((1, 1, 1))
b.exec()
dic = {
"x_est": x_est,
"u_est": u_est,
"x_ref": x_ref,
"x_init": x_wt_noise,
"u_ref": u_ref,
"time_per_mhe": toc / ceil(ns / rt_ratio - ns_mhe),
"time_tot": toc,
"q_noise": q_noise,
"N_mhe": ns_mhe,
"N_tot": ns,
"rt_ratio": rt_ratio,
"f_est": force_est,
"f_ref": force_ref,
}
sio.savemat(f"data/MHE_results.mat", dic)
duration = 1
t = 8
ns = 800
print("*********************************************")
print(f"Problem solved with Acados")
print(f"Solving time : {dic['time_tot']}s")
print(f"Solving frequency : {1/dic['time_per_mhe']}s")
# ------ Animate ------ #
b = bioviz.Viz(model)
b.load_movement(x_est[:biorbd_model.nbQ(), :])
b.exec()
