From 1bfb283ecfbd30e98b19b6fd11de6b12c2cb593d Mon Sep 17 00:00:00 2001 From: Pariterre Date: Tue, 11 May 2021 18:22:08 -0400 Subject: [PATCH] Added a function to get the index of marker and segment from name --- CMakeLists.txt | 2 +- binding/python3/__init__.py | 3 +- binding/python3/rigid_body.py | 12 ++++ examples/Test_longueur_wrapping.py | 55 +++++++++++-------- .../Python3/test_binder_python_rigidbody.py | 28 ++++++++-- 5 files changed, 70 insertions(+), 30 deletions(-) create mode 100644 binding/python3/rigid_body.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 85b22cde..627d1764 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ if (${CMAKE_VERSION} VERSION_GREATER_EQUAL "3.0") cmake_policy(SET CMP0042 NEW) endif() -project(biorbd VERSION 1.5.2) +project(biorbd VERSION 1.5.3) set (CMAKE_CXX_STANDARD 11) set (BIORBD_NAME ${PROJECT_NAME}) diff --git a/binding/python3/__init__.py b/binding/python3/__init__.py index 559d6f93..733c9c38 100644 --- a/binding/python3/__init__.py +++ b/binding/python3/__init__.py @@ -1,6 +1,7 @@ from .biorbd import * from ._version import __version__ -from .surface_max_torque_actuator import surface_max_torque_actuator +from .surface_max_torque_actuator import * +from .rigid_body import * if biorbd.currentLinearAlgebraBackend() == 1: from casadi import Function, MX, horzcat diff --git a/binding/python3/rigid_body.py b/binding/python3/rigid_body.py new file mode 100644 index 00000000..fdf0acfc --- /dev/null +++ b/binding/python3/rigid_body.py @@ -0,0 +1,12 @@ +def marker_index(model, marker_name): + try: + return [n.to_string() for n in model.markerNames()].index(marker_name) + except ValueError: + raise ValueError(f"{marker_name} is not in the biorbd model") + + +def segment_index(model, segment_name): + try: + return [model.segment(i).name().to_string() for i in range(model.nbSegment())].index(segment_name) + except ValueError: + raise ValueError(f"{segment_name} is not in the biorbd model") diff --git a/examples/Test_longueur_wrapping.py b/examples/Test_longueur_wrapping.py index e04a9434..ef5b1646 100644 --- a/examples/Test_longueur_wrapping.py +++ b/examples/Test_longueur_wrapping.py @@ -7,7 +7,7 @@ # functions for cylinder rotation def cyl2cart(r, theta, z): - return (r*np.cos(theta), r*np.sin(theta), z) + return (r * np.cos(theta), r * np.sin(theta), z) def roll(R, zi, zf, RT): @@ -20,23 +20,26 @@ def roll(R, zi, zf, RT): x, y, z = cyl2cart(r, t, z) # Euler rotation - rot = np.dot( - RT.rot().to_array(), - np.array([x.ravel(), y.ravel(), z.ravel()]) - ) + rot = np.dot(RT.rot().to_array(), np.array([x.ravel(), y.ravel(), z.ravel()])) x_rt = rot[0, :].reshape(x.shape) + RTw.trans().to_array()[0] y_rt = rot[1, :].reshape(y.shape) + RTw.trans().to_array()[1] z_rt = rot[2, :].reshape(z.shape) + RTw.trans().to_array()[2] - return x_rt, y_rt, z_rt, + return ( + x_rt, + y_rt, + z_rt, + ) + # model model = biorbd.Model("WrappingObjectExample.bioMod") -# wrapping RT matrix -RTw = biorbd.WrappingCylinder( - model.muscle(0).pathModifier().object(0)).RT(model, np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1])) +# wrapping RT matrix +RTw = biorbd.WrappingCylinder(model.muscle(0).pathModifier().object(0)).RT( + model, np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) +) RTw_trans = RTw.transpose() # Point in global base @@ -47,19 +50,19 @@ def roll(R, zi, zf, RT): # Plot the surface fig = plt.figure() -ax = fig.add_subplot(111, projection='3d') +ax = fig.add_subplot(111, projection="3d") x, y, z = roll(0.05, -0.2, 0.2, RTw) ax.plot_surface(x, y, z) -plt.plot((Po[0], Po_wrap[0]), (Po[1], Po_wrap[1]), (Po[2], Po_wrap[2]), marker='o') -plt.plot((Pi[0], Pi_wrap[0]), (Pi[1], Pi_wrap[1]), (Pi[2], Pi_wrap[2]), marker='o') +plt.plot((Po[0], Po_wrap[0]), (Po[1], Po_wrap[1]), (Po[2], Po_wrap[2]), marker="o") +plt.plot((Pi[0], Pi_wrap[0]), (Pi[1], Pi_wrap[1]), (Pi[2], Pi_wrap[2]), marker="o") # To set Axe3D to (almost) equal -max_range = np.array([x.max()-x.min(), y.max()-y.min(), z.max()-z.min()]).max() -Xb = 0.5*max_range*np.mgrid[-1:2:2, -1:2:2, -1:2:2][0].flatten() + 0.5*(x.max()+x.min()) -Yb = 0.5*max_range*np.mgrid[-1:2:2, -1:2:2, -1:2:2][1].flatten() + 0.5*(y.max()+y.min()) -Zb = 0.5*max_range*np.mgrid[-1:2:2, -1:2:2, -1:2:2][2].flatten() + 0.5*(z.max()+z.min()) +max_range = np.array([x.max() - x.min(), y.max() - y.min(), z.max() - z.min()]).max() +Xb = 0.5 * max_range * np.mgrid[-1:2:2, -1:2:2, -1:2:2][0].flatten() + 0.5 * (x.max() + x.min()) +Yb = 0.5 * max_range * np.mgrid[-1:2:2, -1:2:2, -1:2:2][1].flatten() + 0.5 * (y.max() + y.min()) +Zb = 0.5 * max_range * np.mgrid[-1:2:2, -1:2:2, -1:2:2][2].flatten() + 0.5 * (z.max() + z.min()) for xb, yb, zb in zip(Xb, Yb, Zb): - ax.plot([xb], [yb], [zb], 'w') + ax.plot([xb], [yb], [zb], "w") plt.show() # Compute muscle length @@ -67,7 +70,9 @@ def roll(R, zi, zf, RT): l_muscle_bd = model.muscle(0).musculoTendonLength(model, np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1])) -l_wt_arc = np.sqrt((Po_wrap[0]-Po[0])**2 + (Po_wrap[1]-Po[1])**2 + (Po_wrap[2]-Po[2])**2) + np.sqrt((Pi_wrap[0]-Pi[0])**2 + (Pi_wrap[1]-Pi[1])**2 + (Pi_wrap[2]-Pi[2])**2) +l_wt_arc = np.sqrt((Po_wrap[0] - Po[0]) ** 2 + (Po_wrap[1] - Po[1]) ** 2 + (Po_wrap[2] - Po[2]) ** 2) + np.sqrt( + (Pi_wrap[0] - Pi[0]) ** 2 + (Pi_wrap[1] - Pi[1]) ** 2 + (Pi_wrap[2] - Pi[2]) ** 2 +) # Pi_wrap and Po_wrap provide in wrapping base to compute arc length Pi_wrap = biorbd.Vector3d(0.28997869688863276, 0.34739632020407846, 0.6636920365713757) @@ -78,10 +83,14 @@ def roll(R, zi, zf, RT): Po_wrap.applyRT(RTw_trans) Po_wrap = Po_wrap.to_array() -arc = math.acos( - ((Po_wrap[0] * Pi_wrap[0]) + (Pi_wrap[1] * Po_wrap[1])) / ( - math.sqrt(((Pi_wrap[1]**2)+(Pi_wrap[0]**2)) * ((Po_wrap[1]**2) + (Po_wrap[0]**2))))) * r +arc = ( + math.acos( + ((Po_wrap[0] * Pi_wrap[0]) + (Pi_wrap[1] * Po_wrap[1])) + / (math.sqrt(((Pi_wrap[1] ** 2) + (Pi_wrap[0] ** 2)) * ((Po_wrap[1] ** 2) + (Po_wrap[0] ** 2)))) + ) + * r +) -l_arc = math.sqrt(arc**2 + (Po_wrap[2]-Pi_wrap[2])**2) +l_arc = math.sqrt(arc ** 2 + (Po_wrap[2] - Pi_wrap[2]) ** 2) -l_m = l_wt_arc + l_arc # l_m = 0.582246096069153 +l_m = l_wt_arc + l_arc # l_m = 0.582246096069153 diff --git a/test/binding/Python3/test_binder_python_rigidbody.py b/test/binding/Python3/test_binder_python_rigidbody.py index b5a3e36f..9837b647 100644 --- a/test/binding/Python3/test_binder_python_rigidbody.py +++ b/test/binding/Python3/test_binder_python_rigidbody.py @@ -218,6 +218,7 @@ def test_set_vector3d(): 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() @@ -238,21 +239,22 @@ def check_value(target): 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) @@ -358,3 +360,19 @@ def test_forward_dynamics_constraints_direct(): np.testing.assert_almost_equal(qddot.squeeze(), qddot_expected) np.testing.assert_almost_equal(cs_forces.squeeze(), contact_forces_expected) + + +def test_name_to_index(): + m = biorbd.Model("../../models/pyomecaman.bioMod") + + # Index of a segment + np.testing.assert_equal(biorbd.segment_index(m, "Pelvis"), 0) + np.testing.assert_equal(biorbd.segment_index(m, "PiedG"), 10) + with pytest.raises(ValueError, match="dummy is not in the biorbd model"): + biorbd.segment_index(m, "dummy") + + # Index of a marker + np.testing.assert_equal(biorbd.marker_index(m, "pelv1"), 0) + np.testing.assert_equal(biorbd.marker_index(m, "piedg6"), 96) + with pytest.raises(ValueError, match="dummy is not in the biorbd model"): + biorbd.marker_index(m, "dummy")