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())
# 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(markers.shape[2]):
markersOverFrames.append(
[biorbd.NodeSegment(m) for m in markers[:, :, i].T])
# 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)
Q = biorbd.GeneralizedCoordinates(model)
Qdot = biorbd.GeneralizedVelocity(model)
Qddot = biorbd.GeneralizedAcceleration(model)
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(
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)