def test_model_load():
model_path = "examples/pyomecaman.s2mMod"
# From path
b1 = BiorbdViz(model_path=model_path, show_muscles=False, show_meshes=False)
# From a loaded model
m = biorbd.s2mMusculoSkeletalModel(model_path)
b1 = BiorbdViz(loaded_model=m, show_muscles=False, show_meshes=False)
def animate(self, nb_frames=80, **kwargs):
try:
from BiorbdViz import BiorbdViz
except ModuleNotFoundError:
raise RuntimeError(
"BiorbdViz must be install to animate the model")
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(
BiorbdViz(loaded_model=self.ocp.nlp[idx_phase]["model"],
**kwargs))
all_bioviz[-1].load_movement(
self.ocp.nlp[idx_phase]["q_mapping"].expand.map(data).T)
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:
if b.show_analyses_panel and b.is_animating:
b.movement_slider[0].setValue(
(b.movement_slider[0].value() + 1) %
b.movement_slider[0].maximum())
b.refresh_window()
else:
b_is_visible[i] = False
def animate(self, nb_frames=80, **kwargs):
x = ProblemType.get_q_from_V(self.ocp, self.sol["x"])
t = [
np.array(np.linspace(0, self.ocp.nlp[i]["tf"], self.ocp.nlp[i]["ns"] + 1))
for i in range(self.ocp.nb_phases)
]
if self.ocp.nb_phases == 1:
x = [x]
else:
same_dof = True
for i in range(self.ocp.nb_phases):
for k in range(self.ocp.nlp[0]["model"].nbDof()):
if (
self.ocp.nlp[i]["model"].nameDof()[k].to_string()
!= self.ocp.nlp[i - 1]["model"].nameDof()[k].to_string()
):
same_dof = False
break
if not same_dof:
break
if same_dof:
x_concat = x[0]
t_concat = t[0]
for i in range(1, self.ocp.nb_phases):
x_concat = np.concatenate((x_concat, x[i][:, 1:]), axis=1)
t_concat = np.concatenate((t_concat, t[i][1:] + t_concat[-1]))
x = [x_concat]
t = [t_concat]
try:
from BiorbdViz import BiorbdViz
except ModuleNotFoundError:
print("Install BiorbdViz if you want to have a live view of the optimization")
for i, x_phase in enumerate(x):
x_interpolate = np.ndarray((self.ocp.nlp[i]["nbQ"], nb_frames))
t_interpolate = np.linspace(0, t[i][-1], nb_frames)
for j in range(self.ocp.nlp[i]["nbQ"]):
tck = interpolate.splrep(t[i], x_phase[j], s=0)
x_interpolate[j] = interpolate.splev(t_interpolate, tck)
b = BiorbdViz(loaded_model=self.ocp.nlp[i]["model"], **kwargs)
b.load_movement(x_interpolate.T)
b.exec()
for i in range(m.nbMuscleGroups()):
for j in range(m.muscleGroup(i).nbMuscles()):
plt.subplot(3, 6, cmp + 1)
utils.plot_piecewise_constant(t_final, all_u[cmp, :])
plt.title(m.muscleGroup(i).muscle(j).name().getString())
plt.ylim((0, 1))
cmp += 1
else:
nb_row = np.max(muscle_plot_mapping, axis=0)[3] + 1
nb_col = np.max(muscle_plot_mapping, axis=0)[4] + 1
created_axes = [None] * nb_col * nb_row
for muscle_map in muscle_plot_mapping:
idx_axis = muscle_map[3] * nb_col + muscle_map[4]
if created_axes[idx_axis]:
plt.sca(created_axes[idx_axis])
else:
created_axes[idx_axis] = plt.subplot(nb_row, nb_col,
idx_axis + 1)
utils.plot_piecewise_constant(t_final, all_u[muscle_map[0], :])
# plt.title(m.muscleGroup(map[1]).muscle(map[2]).name().getString())
plt.title(muscle_plot_names[muscle_map[5]])
plt.ylim((0, 1))
plt.tight_layout(w_pad=-1.0, h_pad=-1.0)
plt.show()
# Animate the model
b = BiorbdViz(loaded_model=m)
b.load_movement(q_interp)
b.exec()
for j in range(m.muscleGroup(i).nbMuscles()):
plt.subplot(3, 6, cmp + 1)
utils.plot_piecewise_constant(t_final, all_u[cmp, :])
plt.title(m.muscleGroup(i).muscle(j).name().getString())
plt.ylim((0, 1))
cmp += 1
else:
nb_row = np.max(muscle_plot_mapping, axis=0)[3] + 1
nb_col = np.max(muscle_plot_mapping, axis=0)[4] + 1
created_axes = [None] * nb_col * nb_row
for muscle_map in muscle_plot_mapping:
idx_axis = muscle_map[3] * nb_col + muscle_map[4]
if created_axes[idx_axis]:
plt.sca(created_axes[idx_axis])
else:
created_axes[idx_axis] = plt.subplot(nb_row, nb_col, idx_axis + 1)
utils.plot_piecewise_constant(t_final, all_u[muscle_map[0], :])
# plt.title(m.muscleGroup(map[1]).muscle(map[2]).name().getString())
plt.title(muscle_plot_names[muscle_map[5]])
plt.ylim((0, 1))
plt.tight_layout(w_pad=-1.0, h_pad=-1.0)
plt.show()
# Animate the model
b = BiorbdViz(loaded_model=m, markers_size=0.003)
# b = BiorbdViz(loaded_model=m)
b.load_movement(q_interp)
b.exec()
import time
import pickle
import sys
from BiorbdViz import BiorbdViz
file_path = 0
if len(sys.argv) > 1:
file_path = str(sys.argv[1])
if not isinstance(file_path, str):
t = time.localtime(time.time())
file_path = f"results/{t.tm_year}_{t.tm_mon}_{t.tm_mday}_upDown.bob"
with open(file_path, "rb") as file:
data = pickle.load(file)
data_interpolate, _ = data["data"]
b = BiorbdViz("../models/BrasViolon.bioMod",
markers_size=0.0002,
show_segments_center_of_mass=False,
show_local_ref_frame=False,
show_global_ref_frame=False,
show_muscles=False)
b.load_movement(data_interpolate["q"].T, )
b.exec()
th_opt = sol_opt[2::5]
w_opt = sol_opt[3::5]
u_opt = sol_opt[4::5]
plt.plot(th_opt, label="theta")
plt.plot(w_opt, label="omega")
plt.plot(vel_opt, label="car vel")
plt.plot(pos_opt, label="car pos")
plt.plot(u_opt, label="control")
plt.legend()
plt.show(block=True)
# visualize optim
qs = np.array([pos_opt, th_opt])
np.save("visual", qs.T)
b = BiorbdViz(model_path="../data/inverse_pendulum.bioMod")
b.load_movement(qs.T)
b.exec()
# # simulate control
# m = brbd.Model("/home/fbailly/devel/models/inverse_pendulum.bioMod")
# qs = np.zeros((N,2))
# qdots = np.zeros((N,2))
# qddots = np.zeros((N,2))
# q = np.array([pos_opt[0],th_opt[0]])
# qdot = np.array([vel_opt[0],w_opt[0]])
# dt = T/N
# for t in range(N) :
# tau = np.array([u_opt[t],0])
# qddot = m.ForwardDynamics(q, qdot, tau)
# qdot += qddot.to_array()*dt
# """
# Example script for animating a model
# """
import numpy as np
from BiorbdViz import BiorbdViz
b = BiorbdViz(model_path="pyomecaman.bioMod")
animate_by_hand = 1
if animate_by_hand == 0:
n_frames = 200
Q = np.zeros((n_frames, b.nQ))
Q[:, 4] = np.linspace(0, np.pi / 2, n_frames)
i = 0
while b.vtk_window.is_active:
b.set_q(Q[i, :])
i = (i + 1) % n_frames
elif animate_by_hand == 1:
n_frames = 200
all_q = np.zeros((n_frames, b.nQ))
all_q[:, 4] = np.linspace(0, np.pi / 2, n_frames)
b.load_movement(all_q)
b.exec()
else:
b.exec()
t_interp, q_interp = utils.interpolate_integration(nb_frames=1000, t_int=integrated_tp.t, y_int=integrated_tp.y)
integrated_tp2 = integrate.solve_ivp(fun=lambda t, y: utils.dynamics_from_muscles_and_torques(t, y, biorbd_model, u),
t_span=(0, 1), y0=q_init, method='RK45', atol=1e-8, rtol=1e-6)
t_interp2, q_interp2 = utils.interpolate_integration(nb_frames=1000, t_int=integrated_tp2.t, y_int=integrated_tp2.y)
from matplotlib import pyplot as plt
plt.plot(t_interp, q_interp)
plt.plot(t_interp2, q_interp2, '-.')
plt.show()
bioviz = BiorbdViz(loaded_model=biorbd_model)
bioviz2 = BiorbdViz(loaded_model=biorbd_model)
bioviz.load_movement(q_interp)
bioviz2.load_movement(q_interp2)
bioviz.is_executing = True
bioviz2.is_executing = True
while bioviz.vtk_window.is_active and bioviz2.vtk_window.is_active:
if bioviz.show_options and bioviz.is_animating:
bioviz.movement_slider[0].setValue(
(bioviz.movement_slider[0].value() + 1) % bioviz.movement_slider[0].maximum()
)
if bioviz2.show_options and bioviz2.is_animating:
bioviz2.movement_slider[0].setValue(
(bioviz2.movement_slider[0].value() + 1) % bioviz2.movement_slider[0].maximum()
)
nlp["ns"] + 1] = CS_func(x, u)
plt.plot(contact_forces.T)
plt.show()
try:
from BiorbdViz import BiorbdViz
x, _, _ = ProblemType.get_data_from_V(ocp, sol["x"])
q = np.ndarray((ocp.nlp[0]["model"].nbQ(),
sum([nlp["ns"] for nlp in ocp.nlp]) + 1))
for i in range(len(ocp.nlp)):
if i == 0:
q[:, :ocp.nlp[i]["ns"]] = ocp.nlp[i]["q_mapping"].expand.map(
x[i])[:, :-1]
else:
q[:, ocp.nlp[i - 1]["ns"]:ocp.nlp[i - 1]["ns"] +
ocp.nlp[i]["ns"]] = ocp.nlp[i]["q_mapping"].expand.map(
x[i])[:, :-1]
q[:, -1] = ocp.nlp[-1]["q_mapping"].expand.map(x[-1])[:, -1]
# np.save("results2", q.T)
b = BiorbdViz(loaded_model=ocp.nlp[0]["model"])
b.load_movement(q.T)
b.exec()
except ModuleNotFoundError:
print(
"Install BiorbdViz if you want to have a live view of the optimization"
)
plt.show()