def test_CoM():
m = biorbd.Model("../../models/pyomecaman.bioMod")
q = np.array(
[0.1, 0.1, 0.1, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3])
q_dot = np.array([1, 1, 1, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3])
q_ddot = np.array([10, 10, 10, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30])
expected_CoM = np.array(
[-0.0034679564024098523, 0.15680579877453169, 0.07808112642459612])
expected_CoM_dot = np.array(
[-0.05018973433722229, 1.4166208451420528, 1.4301750486035787])
expected_CoM_ddot = np.array(
[-0.7606169667295027, 11.508107073695976, 16.58853835505851])
if biorbd.currentLinearAlgebraBackend() == 1:
# If CasADi backend is used
from casadi import Function, MX
q_sym = MX.sym("q", m.nbQ(), 1)
q_dot_sym = MX.sym("q_dot", m.nbQdot(), 1)
q_ddot_sym = MX.sym("q_ddot", m.nbQddot(), 1)
CoM_func = Function(
"Compute_CoM",
[q_sym],
[m.CoM(q_sym).to_mx()],
["q"],
["CoM"],
).expand()
CoM_dot_func = Function(
"Compute_CoM_dot",
[q_sym, q_dot_sym],
[m.CoMdot(q_sym, q_dot_sym).to_mx()],
["q", "q_dot"],
["CoM_dot"],
).expand()
CoM_ddot_func = Function(
"Compute_CoM_ddot",
[q_sym, q_dot_sym, q_ddot_sym],
[m.CoMddot(q_sym, q_dot_sym, q_ddot_sym).to_mx()],
["q", "q_dot", "q_ddot"],
["CoM_ddot"],
).expand()
CoM = np.array(CoM_func(q))
CoM_dot = np.array(CoM_dot_func(q, q_dot))
CoM_ddot = np.array(CoM_ddot_func(q, q_dot, q_ddot))
elif not biorbd.currentLinearAlgebraBackend():
# If Eigen backend is used
CoM = m.CoM(q).to_array()
CoM_dot = m.CoMdot(q, q_dot).to_array()
CoM_ddot = m.CoMddot(q, q_dot, q_ddot).to_array()
np.testing.assert_almost_equal(CoM.squeeze(), expected_CoM)
np.testing.assert_almost_equal(CoM_dot.squeeze(), expected_CoM_dot)
np.testing.assert_almost_equal(CoM_ddot.squeeze(), expected_CoM_ddot)
def test_forward_dynamics_with_external_forces():
m = biorbd.Model("../../models/pyomecaman_withActuators.bioMod")
q = np.array([i * 1.1 for i in range(m.nbQ())])
qdot = np.array([i * 1.1 for i in range(m.nbQ())])
tau = np.array([i * 1.1 for i in range(m.nbQ())])
# With external forces
if biorbd.currentLinearAlgebraBackend() == 1:
from casadi import Function, MX
sv1 = MX((11.1, 22.2, 33.3, 44.4, 55.5, 66.6))
sv2 = MX((11.1 * 2, 22.2 * 2, 33.3 * 2, 44.4 * 2, 55.5 * 2, 66.6 * 2))
else:
sv1 = np.array((11.1, 22.2, 33.3, 44.4, 55.5, 66.6))
sv2 = np.array(
(11.1 * 2, 22.2 * 2, 33.3 * 2, 44.4 * 2, 55.5 * 2, 66.6 * 2))
f_ext = biorbd.VecBiorbdSpatialVector([
biorbd.SpatialVector(sv1),
biorbd.SpatialVector(sv2),
])
if biorbd.currentLinearAlgebraBackend() == 1:
q_sym = MX.sym("q", m.nbQ(), 1)
qdot_sym = MX.sym("qdot", m.nbQdot(), 1)
tau_sym = MX.sym("tau", m.nbGeneralizedTorque(), 1)
ForwardDynamics = Function(
"ForwardDynamics",
[q_sym, qdot_sym, tau_sym],
[m.ForwardDynamics(q_sym, qdot_sym, tau_sym, f_ext).to_mx()],
["q_sym", "qdot_sym", "tau_sym"],
["qddot"],
).expand()
qddot = ForwardDynamics(q, qdot, tau)
qddot = np.array(qddot)[:, 0]
elif biorbd.currentLinearAlgebraBackend() == 0:
# if Eigen backend is used
qddot = m.ForwardDynamics(q, qdot, tau, f_ext).to_array()
qddot_expected = np.array([
8.8871711208009998,
-13.647827029817943,
-33.606145294752132,
16.922669487341341,
-21.882821189868423,
41.15364990805439,
68.892537246574463,
-324.59756885799197,
-447.99217990207387,
18884.241415786601,
-331.24622725851572,
1364.7620674666462,
3948.4748602722384,
])
np.testing.assert_almost_equal(qddot, qddot_expected)
def __init__(self, model):
self.m = model
self.data = None
if biorbd.currentLinearAlgebraBackend() == 0:
self._prepare_function_for_eigen()
self.get_data_func = self._get_data_from_eigen
elif biorbd.currentLinearAlgebraBackend() == 1:
self._prepare_function_for_casadi()
self.get_data_func = self._get_data_from_casadi
else:
raise RuntimeError("Unrecognized currentLinearAlgebraBackend")
def test_set_scalar():
def check_value(target):
if biorbd.currentLinearAlgebraBackend() == 1:
assert m.segment(0).characteristics().mass().to_mx() == target
else:
assert m.segment(0).characteristics().mass() == target
m = biorbd.Model("../../models/pyomecaman.bioMod")
m.segment(0).characteristics().setMass(10)
check_value(10)
m.segment(0).characteristics().setMass(11.0)
check_value(11.0)
with pytest.raises(ValueError,
match="Scalar must be a 1x1 array or a float"):
m.segment(0).characteristics().setMass(np.array([]))
m.segment(0).characteristics().setMass(np.array((12, )))
check_value(12.0)
m.segment(0).characteristics().setMass(np.array([[13]]))
check_value(13.0)
with pytest.raises(ValueError,
match="Scalar must be a 1x1 array or a float"):
m.segment(0).characteristics().setMass(np.array([[[14]]]))
if biorbd.currentLinearAlgebraBackend() == 1:
from casadi import MX
m.segment(0).characteristics().setMass(MX(15))
check_value(15.0)
def test_forward_dynamics():
m = biorbd.Model("../../models/pyomecaman_withActuators.bioMod")
q = np.array([i * 1.1 for i in range(m.nbQ())])
qdot = np.array([i * 1.1 for i in range(m.nbQ())])
tau = np.array([i * 1.1 for i in range(m.nbQ())])
if biorbd.currentLinearAlgebraBackend() == 1:
# If CasADi backend is used
from casadi import Function, MX
q_sym = MX.sym("q", m.nbQ(), 1)
qdot_sym = MX.sym("qdot", m.nbQdot(), 1)
tau_sym = MX.sym("tau", m.nbGeneralizedTorque(), 1)
ForwardDynamics = Function(
"ForwardDynamics",
[q_sym, qdot_sym, tau_sym],
[m.ForwardDynamics(q_sym, qdot_sym, tau_sym).to_mx()],
["q_sym", "qdot_sym", "tau_sym"],
["qddot"],
).expand()
qddot = ForwardDynamics(q, qdot, tau)
qddot = np.array(qddot)[:, 0]
elif biorbd.currentLinearAlgebraBackend() == 0:
# if Eigen backend is used
qddot = m.ForwardDynamics(q, qdot, tau).to_array()
qddot_expected = np.array([
20.554883896960259,
-22.317642013324736,
-77.406439058256126,
17.382961188212313,
-63.426361095191858,
93.816468824985876,
106.46105024484631,
95.116641811710167,
-268.1961283528546,
2680.3632159799949,
-183.4582596257801,
755.89411812405604,
163.60239754283589,
])
np.testing.assert_almost_equal(qddot, qddot_expected)
def test_vector3d():
biorbd_model = biorbd.Model()
vec = np.random.rand(3, )
biorbd_model.setGravity(vec)
if biorbd.currentLinearAlgebraBackend() == 1:
from casadi import MX
vec = MX.ones(3, 1)
biorbd_model.setGravity(vec)
def test_set_vector3d():
m = biorbd.Model("../../models/pyomecaman.bioMod")
m.setGravity(np.array((0, 0, -2)))
if biorbd.currentLinearAlgebraBackend() == 1:
from casadi import MX
get_gravity = biorbd.to_casadi_func("Compute_Markers",
m.getGravity)()["o0"]
else:
get_gravity = m.getGravity().to_array()
assert get_gravity[2] == -2
def test_markers():
m = biorbd.Model("../../models/pyomecaman.bioMod")
q = np.array(
[0.1, 0.1, 0.1, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3])
q_dot = np.array([1, 1, 1, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3])
expected_markers_last = np.array([-0.11369, 0.63240501, -0.56253268])
expected_markers_last_dot = np.array([0.0, 4.16996219, 3.99459262])
if biorbd.currentLinearAlgebraBackend() == 1:
# If CasADi backend is used
from casadi import MX
q_sym = MX.sym("q", m.nbQ(), 1)
q_dot_sym = MX.sym("q_dot", m.nbQdot(), 1)
markers_func = biorbd.to_casadi_func("Compute_Markers", m.markers,
q_sym)
markersVelocity_func = biorbd.to_casadi_func("Compute_MarkersVelocity",
m.markersVelocity, q_sym,
q_dot_sym)
markers = np.array(markers_func(q))
markers_dot = np.array(markersVelocity_func(q, q_dot))
elif not biorbd.currentLinearAlgebraBackend():
# If Eigen backend is used
markers = np.array([mark.to_array() for mark in m.markers(q)]).T
markers_dot = np.array(
[mark.to_array() for mark in m.markersVelocity(q, q_dot)]).T
else:
raise NotImplementedError("Backend not implemented in test")
np.testing.assert_almost_equal(markers[:, -1], expected_markers_last)
np.testing.assert_almost_equal(markers_dot[:, -1],
expected_markers_last_dot)
def test_np_mx_to_generalized():
biorbd_model = biorbd.Model("../../models/pyomecaman.bioMod")
q = biorbd.GeneralizedCoordinates(biorbd_model)
qdot = biorbd.GeneralizedVelocity((biorbd_model.nbQdot()))
qddot = biorbd.GeneralizedAcceleration((biorbd_model.nbQddot()))
tau = biorbd_model.InverseDynamics(q, qdot, qddot)
biorbd_model.ForwardDynamics(q, qdot, tau)
if biorbd.currentLinearAlgebraBackend() == 1:
tau = biorbd_model.InverseDynamics(q.to_mx(), qdot.to_mx(),
qddot.to_mx())
biorbd_model.ForwardDynamics(q, qdot, tau.to_mx())
else:
tau = biorbd_model.InverseDynamics(q.to_array(), qdot.to_array(),
qddot.to_array())
biorbd_model.ForwardDynamics(q, qdot, tau.to_array())
def imu_to_array():
m = biorbd.Model("../../models/IMUandCustomRT/pyomecaman_withIMUs.bioMod")
q = np.zeros((m.nbQ(), ))
if biorbd.currentLinearAlgebraBackend() == 1:
from casadi import MX
q_sym = MX.sym("q", m.nbQ(), 1)
imu_func = biorbd.to_casadi_func("imu", m.IMU, q_sym)
imu = imu_func(q)[:, :4]
else:
imu = m.IMU(q)[0].to_array()
np.testing.assert_almost_equal(
imu,
np.array([[0.99003329, -0.09933467, 0.09983342, 0.26719],
[0.10925158, 0.98903828, -0.09933467, 0.04783],
[-0.08887169, 0.10925158, 0.99003329, -0.20946],
[0., 0., 0., 1.]]))
def add_options_panel(self):
# Prepare the sliders
options_layout = QVBoxLayout()
options_layout.addStretch() # Centralize the sliders
sliders_layout = QVBoxLayout()
max_label_width = -1
# Get min and max for all dof
ranges = []
for i in range(self.model.nbSegment()):
seg = self.model.segment(i)
for r in seg.QRanges():
ranges.append([r.min(), r.max()])
for i in range(self.model.nbDof()):
slider_layout = QHBoxLayout()
sliders_layout.addLayout(slider_layout)
# Add a name
name_label = QLabel()
name = f"{self.model.nameDof()[i].to_string()}"
name_label.setText(name)
name_label.setPalette(self.palette_active)
label_width = name_label.fontMetrics().boundingRect(
name_label.text()).width()
if label_width > max_label_width:
max_label_width = label_width
slider_layout.addWidget(name_label)
# Add the slider
slider = QSlider(Qt.Horizontal)
slider.setMinimumSize(100, 0)
slider.setMinimum(ranges[i][0] * self.double_factor)
slider.setMaximum(ranges[i][1] * self.double_factor)
slider.setPageStep(self.double_factor)
slider.setValue(0)
slider.valueChanged.connect(self.__move_avatar_from_sliders)
slider.sliderReleased.connect(
partial(self.__update_muscle_analyses_graphs, False, False,
False, False))
slider_layout.addWidget(slider)
# Add the value
value_label = QLabel()
value_label.setText(f"{0:.2f}")
value_label.setPalette(self.palette_active)
slider_layout.addWidget(value_label)
# Add to the main sliders
self.sliders.append((name_label, slider, value_label))
# Adjust the size of the names
for name_label, _, _ in self.sliders:
name_label.setFixedWidth(max_label_width + 1)
# Put the sliders in a scrollable area
sliders_widget = QWidget()
sliders_widget.setLayout(sliders_layout)
sliders_scroll = QScrollArea()
sliders_scroll.setFrameShape(0)
sliders_scroll.setWidgetResizable(True)
sliders_scroll.setWidget(sliders_widget)
options_layout.addWidget(sliders_scroll)
# Add reset button
button_layout = QHBoxLayout()
options_layout.addLayout(button_layout)
reset_push_button = QPushButton("Reset")
reset_push_button.setPalette(self.palette_active)
reset_push_button.released.connect(self.reset_q)
button_layout.addWidget(reset_push_button)
# Add the radio button for analyses
option_analyses_group = QGroupBox()
option_analyses_layout = QVBoxLayout()
# Add text
analyse_text = QLabel()
analyse_text.setPalette(self.palette_active)
analyse_text.setText("Analyses")
option_analyses_layout.addWidget(analyse_text)
# Add the no analyses
radio_none = QRadioButton()
radio_none.setPalette(self.palette_active)
radio_none.setChecked(True)
radio_none.toggled.connect(
lambda: self.__select_analyses_panel(radio_none, 0))
radio_none.setText("None")
option_analyses_layout.addWidget(radio_none)
# Add the muscles analyses
radio_muscle = QRadioButton()
radio_muscle.setPalette(self.palette_active)
radio_muscle.toggled.connect(
lambda: self.__select_analyses_panel(radio_muscle, 1))
radio_muscle.setText("Muscles")
option_analyses_layout.addWidget(radio_muscle)
# Add the layout to the interface
option_analyses_group.setLayout(option_analyses_layout)
options_layout.addWidget(option_analyses_group)
# Finalize the options panel
options_layout.addStretch() # Centralize the sliders
# Animation panel
animation_layout = QVBoxLayout()
animation_layout.addWidget(self.vtk_window.avatar_widget)
# Add the animation slider
animation_slider_layout = QHBoxLayout()
animation_layout.addLayout(animation_slider_layout)
load_push_button = QPushButton("Load movement")
load_push_button.setPalette(self.palette_active)
load_push_button.released.connect(self.__load_movement_from_button)
animation_slider_layout.addWidget(load_push_button)
# Controllers
self.play_stop_push_button = QPushButton()
self.play_stop_push_button.setIcon(self.start_icon)
self.play_stop_push_button.setPalette(self.palette_active)
self.play_stop_push_button.setEnabled(False)
self.play_stop_push_button.released.connect(
self.__start_stop_animation)
animation_slider_layout.addWidget(self.play_stop_push_button)
slider = QSlider(Qt.Horizontal)
slider.setMinimum(0)
slider.setMaximum(100)
slider.setValue(0)
slider.setEnabled(False)
slider.valueChanged.connect(self.__animate_from_slider)
animation_slider_layout.addWidget(slider)
self.record_push_button = QPushButton()
self.record_push_button.setIcon(self.record_icon)
self.record_push_button.setPalette(self.palette_active)
self.record_push_button.setEnabled(True)
self.record_push_button.released.connect(self.__record)
animation_slider_layout.addWidget(self.record_push_button)
self.stop_record_push_button = QPushButton()
self.stop_record_push_button.setIcon(self.stop_icon)
self.stop_record_push_button.setPalette(self.palette_active)
self.stop_record_push_button.setEnabled(False)
self.stop_record_push_button.released.connect(self.__stop_record, True)
animation_slider_layout.addWidget(self.stop_record_push_button)
# Add the frame count
frame_label = QLabel()
frame_label.setText(f"{0}")
frame_label.setPalette(self.palette_inactive)
animation_slider_layout.addWidget(frame_label)
self.movement_slider = (slider, frame_label)
# Global placement of the window
self.vtk_window.main_layout.addLayout(options_layout, 0, 0)
self.vtk_window.main_layout.addLayout(animation_layout, 0, 1)
self.vtk_window.main_layout.setColumnStretch(0, 1)
self.vtk_window.main_layout.setColumnStretch(1, 2)
# Change the size of the window to account for the new sliders
self.vtk_window.resize(self.vtk_window.size().width() * 2,
self.vtk_window.size().height())
# Prepare all the analyses panel
self.muscle_analyses = MuscleAnalyses(self.analyses_muscle_widget,
self)
if biorbd.currentLinearAlgebraBackend() == 1:
radio_muscle.setEnabled(False)
else:
if self.model.nbMuscles() == 0:
radio_muscle.setEnabled(False)
self.__select_analyses_panel(radio_muscle, 1)
import os
import copy
from functools import partial
import numpy as np
import scipy
import biorbd
if biorbd.currentLinearAlgebraBackend() == 1:
import casadi
from pyomeca import Markers3d
from .biorbd_vtk import VtkModel, VtkWindow, Mesh, MeshCollection, RotoTrans, RotoTransCollection
from PyQt5.QtWidgets import QSlider, QVBoxLayout, QHBoxLayout, QLabel, QPushButton, \
QFileDialog, QScrollArea, QWidget, QMessageBox, QRadioButton, QGroupBox
from PyQt5.QtCore import Qt
from PyQt5.QtGui import QPalette, QColor, QPixmap, QIcon
from .analyses import MuscleAnalyses
class InterfacesCollections:
class BiorbdFunc:
def __init__(self, model):
self.m = model
self.data = None
if biorbd.currentLinearAlgebraBackend() == 0:
self._prepare_function_for_eigen()
self.get_data_func = self._get_data_from_eigen
elif biorbd.currentLinearAlgebraBackend() == 1:
self._prepare_function_for_casadi()
self.get_data_func = self._get_data_from_casadi
def test_forward_dynamics_constraints_direct():
m = biorbd.Model("../../models/pyomecaman.bioMod")
q = np.array([1.0 for i in range(m.nbQ())])
qdot = np.array([1.0 for i in range(m.nbQ())])
tau = np.array([1.0 for i in range(m.nbQ())])
cs = m.getConstraints()
qddot_expected = np.array([
1.9402069774422919,
-9.1992692111538243,
2.9930159570454702,
5.2738378853554133,
8.9387539396273699,
6.0938738229550751,
9.9560407885164217,
38.6297746304162,
-52.159023390563554,
36.702385054876714,
38.629774630416208,
-52.159023390563561,
36.70238505487675,
])
contact_forces_expected = np.array([
-16.344680827308579,
-30.485214214095951,
112.8234134576031,
-16.344680827308611,
-30.485214214095965,
112.82341345760311,
])
np.testing.assert_almost_equal(cs.nbContacts(),
contact_forces_expected.size)
if biorbd.currentLinearAlgebraBackend() == 1:
# If CasADi backend is used
from casadi import Function, MX
q_sym = MX.sym("q", m.nbQ(), 1)
qdot_sym = MX.sym("qdot", m.nbQdot(), 1)
dyn_func = Function(
"Compute_qddot_with_dyn",
[q_sym, qdot_sym],
[
m.ForwardDynamicsConstraintsDirect(q, qdot, tau, cs).to_mx(),
cs.getForce().to_mx()
],
["q", "qdot_sym"],
["qddot", "cs_forces"],
).expand()
qddot, cs_forces = dyn_func(q, qdot)
qddot = np.array(qddot)
cs_forces = np.array(cs_forces)
elif biorbd.currentLinearAlgebraBackend() == 0:
# if Eigen backend is used
qddot = m.ForwardDynamicsConstraintsDirect(q, qdot, tau, cs).to_array()
cs_forces = cs.getForce().to_array()
np.testing.assert_almost_equal(qddot.squeeze(), qddot_expected)
np.testing.assert_almost_equal(cs_forces.squeeze(),
contact_forces_expected)
import biorbd
import numpy as np
if biorbd.currentLinearAlgebraBackend() != biorbd.EIGEN3:
raise RuntimeError("Biorbd backend should be biorbd.EIGEN3")
force_target = biorbd.Vector3d(0, 0, 2)
biorbd_model = biorbd.Model("../../models/BrasViolon.bioMod")
bow_segment_idx = 9
violin_segment_idx = 17
rt_on_string = {"E": 3, "A": 2, "D": 1, "G": 0}
print(f"Converting F={force_target.to_array()} N for each strings")
for string_key in rt_on_string.keys():
b = biorbd.Vector3d(force_target.to_array()[0],
force_target.to_array()[1],
force_target.to_array()[2])
b.applyRT(
biorbd.RotoTrans(
biorbd_model.RT(np.zeros(biorbd_model.nbDof()),
rt_on_string[string_key]).rot()))
print(
f"Moment/Force for {string_key} string = {np.concatenate((np.array([0, 0, 0]), b.to_array()))}"
)
def check_value(target):
if biorbd.currentLinearAlgebraBackend() == 1:
assert m.segment(0).characteristics().mass().to_mx() == target
else:
assert m.segment(0).characteristics().mass() == target
