Merge pull request #209 from pariterre/master

Added a function to get the index of marker and segment from name

Author: pariterre

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})
 

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

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")

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,27 +50,29 @@ 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
 r = biorbd.WrappingCylinder(model.muscle(0).pathModifier().object(0)).radius()
 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

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")