Merge pull request #157 from pariterre/master

Fixed dimensions of GeneralizedTorque when using quaternions

Author: pariterre

examples/python3/forwardKinematics.py

@@ -1,6 +1,13 @@
 import numpy as np
 import biorbd
 
+try:
+    from BiorbdViz.biorbd_vtk import VtkModel, VtkWindow
+    import pyomeca
+    biorbd_viz_found = True
+except ModuleNotFoundError:
+    biorbd_viz_found = False
+
 #
 # This examples shows how to
 #     1. Load a model
@@ -15,13 +22,32 @@
 # Load a predefined model
 model = biorbd.Model("../pyomecaman.bioMod")
 nq = model.nbQ()
+n_mark = model.nbMarkers()
+n_frames = 20
 
-# Choose a position to get the markers from
-Q = np.zeros((nq,))
+# Generate clapping gesture data
+qinit = np.array([0, 0, -.3, 0.35, 1.15, -0.35, 1.15, 0, 0, 0, 0, 0, 0])
+qmid = np.array([0, 0, -.3, 0.5, 1.15, -0.5, 1.15, 0, 0, 0, 0, 0, 0])
+qfinal = np.array([0, 0, -.3, 0.35, 1.15, -0.35, 1.15, 0, 0, 0, 0, 0, 0])
+q = np.concatenate((np.linspace(qinit, qmid, n_frames).T, np.linspace(qmid, qfinal, n_frames).T), axis=1)
 
 # Proceed with the forward kinematics
-markers = model.markers(Q)
+markers = np.ndarray((3, model.nbMarkers(), 2*n_frames))
+for i, q in enumerate(q.T):
+    markers[:, :, i] = np.array([mark.to_array() for mark in model.markers(q)]).T
+
+# Print the first frame in the console
+print(markers[:, :, 0])
+
+# Animate the markers 
+if biorbd_viz_found:
+    vtkWindow = VtkWindow(background_color=(0.5, 0.5, 0.5))
+    vtkModel = VtkModel(vtkWindow, markers_color=(0, 0, 1), markers_size=0.01)
+    i = 0
+    markers = pyomeca.Markers(markers)
+    while vtkWindow.is_active:
+        # Update the graph
+        vtkModel.update_markers(markers[:, :, [i]])
+        vtkWindow.update_frame()
+        i = (i + 1) % markers.shape[2]
 
-# Print them to the console
-for marker in markers:
-    print(marker.to_array())

examples/python3/inverseKinematicsKalman.py

@@ -1,5 +1,11 @@
 import numpy as np
 import biorbd
+try:
+    import BiorbdViz
+    biorbd_viz_found = True
+except ModuleNotFoundError:
+    biorbd_viz_found = False
+
 
 #
 # This examples shows how to
@@ -17,30 +23,48 @@
 # Load a predefined model
 model = biorbd.Model("../pyomecaman.bioMod")
 nq = model.nbQ()
-# nqdot = model.nbQdot()
-# nqddot = model.nbQddot()
-n_frames = 3
+nb_mus = model.nbMuscles()
+n_frames = 20
+
+# Generate clapping gesture data
+qinit = np.array([0, 0, -.3, 0.35, 1.15, -0.35, 1.15, 0, 0, 0, 0, 0, 0])
+qmid = np.array([0, 0, -.3, 0.5, 1.15, -0.5, 1.15, 0, 0, 0, 0, 0, 0])
+qfinal = np.array([0, 0, -.3, 0.35, 1.15, -0.35, 1.15, 0, 0, 0, 0, 0, 0])
+target_q = np.concatenate((np.linspace(qinit, qmid, n_frames).T, np.linspace(qmid, qfinal, n_frames).T), axis=1)
+markers = np.ndarray((3, model.nbMarkers(), 2*n_frames))
+for i, q in enumerate(target_q.T):
+    markers[:, :, i] = np.array([mark.to_array() for mark in model.markers(q)]).T
 
-# Generate random data (3 frames)
-targetQ = np.random.random((nq,))
-targetMarkers = model.markers(targetQ)
+# If ones was using c3d data opened using ezc3d 
+# import ezc3d
+# c3d = ezc3d.c3d(data_filename)
+# markers = c3d['data']['points'][:3, :, :]  # XYZ1 x markers x time_frame
 
+# Dispatch markers in biorbd structure so EKF can use it
 markersOverFrames = []
-for i in range(n_frames):
-    markersOverFrames.append(targetMarkers)
+for i in range(markers.shape[2]):
+    markersOverFrames.append([biorbd.NodeSegment(m) for m in markers[:, :, i].T])
 
-# Create a Kalman filter
-freq = 100  # 100 Hz
+# 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)
-print(f"Target Q = {targetQ}")
 Q = biorbd.GeneralizedCoordinates(model)
 Qdot = biorbd.GeneralizedVelocity(model)
 Qddot = biorbd.GeneralizedAcceleration(model)
-for targetMarkers in markersOverFrames:
+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(Q.to_array())
+    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 = BiorbdViz.BiorbdViz(loaded_model=model)
+    b.load_movement(q_recons)
+    b.exec()
+

src/RigidBody/Joints.cpp

@@ -83,7 +83,7 @@ void biorbd::rigidbody::Joints::DeepCopy(const biorbd::rigidbody::Joints &other)
 }
 
 unsigned int biorbd::rigidbody::Joints::nbGeneralizedTorque() const {
-    return nbQ();
+    return nbQddot();
 }
 unsigned int biorbd::rigidbody::Joints::nbDof() const {
     return *m_nbDof;

test/models/cube.bioMod

@@ -3,34 +3,34 @@ version 4
 // DEFINITION DU SEGMENTS
 
 // Seg1
-    segment Seg1
+segment Seg1
     translations	xz
     rotations	y
     rangesQ  -0.70 2.30
-            -3 3
-            -pi pi
-	mass 1
-	inertia
-	  1 0 0
-	  0 1 0
-	  0 0 1
-	com 0 0 0
-    mesh 0 -1 -1
-    mesh 0 0 -1
-    mesh 0 0 0
-    mesh 0 -1 0
-    mesh 0 -1 -1
-    mesh 1 -1 -1
-    mesh 1 0 -1
-    mesh 0 0 -1
-    mesh 1 0 -1
-    mesh 1 0 0
-    mesh 0 0 0
-    mesh 1 0 0
-    mesh 1 -1 0
-    mesh 0 -1 0
-    mesh 1 -1 0
-    mesh 1 -1 -1
+             -3    3
+             -pi   pi
+    mass 1
+    inertia
+        1 0 0
+        0 1 0
+        0 0 1
+    com 0 0 0
+        mesh 0 -1 -1
+        mesh 0 0 -1
+        mesh 0 0 0
+        mesh 0 -1 0
+        mesh 0 -1 -1
+        mesh 1 -1 -1
+        mesh 1 0 -1
+        mesh 0 0 -1
+        mesh 1 0 -1
+        mesh 1 0 0
+        mesh 0 0 0
+        mesh 1 0 0
+        mesh 1 -1 0
+        mesh 0 -1 0
+        mesh 1 -1 0
+        mesh 1 -1 -1
 endsegment
 
 // Marker on Seg1

test/test_rigidbody.cpp

@@ -1061,6 +1061,15 @@ TEST(Dynamics, ForwardAccelerationConstraint){
     }
 }
 
+TEST(QuaternionInModel, sizes)
+{
+    biorbd::Model m("models/simple_quat.bioMod");
+    EXPECT_EQ(m.nbQ(), 4);
+    EXPECT_EQ(m.nbQdot(), 3);
+    EXPECT_EQ(m.nbQddot(), 3);
+    EXPECT_EQ(m.nbGeneralizedTorque(), 3);
+}
+
 TEST(Kinematics, computeQdot)
 {
     biorbd::Model m("models/simple_quat.bioMod");