Python inverse kinematics using IMUS - help needed

[askuric]

Hello!

I'd like to use the IMU cased motion capture in my application with the biorbd and I'd like to do it in python :D

So far I was able to augment my model with the IMUs and I was able to modify you example code for inverse kinematics (marker based) to use IMUs. And it works great!
This is the example code.

import numpy as np
import biorbd

try:
    import bioviz

    biorbd_viz_found = True
except ModuleNotFoundError:
    biorbd_viz_found = False

# Load a predefined model
model = biorbd.Model("pyomeca_models/BrasCompletIMUS.bioMod")
nq = model.nbQ()
nb_mus = model.nbMuscles()

# Generate clapping gesture data
n_frames = 20
qinit = np.array([ 0, -0.3, 0.35, 1.15, -0.35, 1.15, 0])
qmid = np.array([ -1, -0.3, 0.5, 0.8, -1.5, 1.15, 1])
qfinal = np.array([ 0, -0.3, 0.35, 1.15, -0.35, 1.15, 0 ])
target_q = np.concatenate((np.linspace(qinit, qmid, n_frames).T, np.linspace(qmid, qfinal, n_frames).T), axis=1)

# tranfsforming imu to imu data
imusOverFrames = []
for i, q in enumerate(target_q.T):
    imusOverFrames.append([imu for imu in model.IMU(q)])

# Create a Kalman filter structure
freq = 100  # Hz
params = biorbd.KalmanParam(freq)
# kalman = biorbd.KalmanReconsMarkers(model, params)
kalman = biorbd.KalmanReconsIMU(model, params)


# Perform the kalman filter for each frame (the first frame is much longer than the next)
Q = biorbd.GeneralizedCoordinates(qinit)
Qdot = biorbd.GeneralizedVelocity(model)
Qddot = biorbd.GeneralizedAcceleration(model)
q_recons = np.ndarray((model.nbQ(), len(imusOverFrames)))
for i, targetIMUs in enumerate(imusOverFrames):
    kalman.reconstructFrame(model, targetIMUs, 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 = bioviz.Viz(loaded_model=model)
    b.load_movement(q_recons)
    b.exec()

However I'm having issues if I try to use the numpy array data as the target IMU data. The numpy array data will be the type of data that I'll be getting from my sensors. In the example above you can see that I'm providing the Kalman filter the IMU class instances :

# tranfsforming imu to imu data
imusOverFrames = []
for i, q in enumerate(target_q.T):
    imusOverFrames.append([imu for imu in model.IMU(q)])

In your cpp code you've used just simple utils::Vector, for example:

biorbd/binding/c/biorbd_c.cpp

Line 253 in 3a9f4aa

utils::Vector T(3*3*model->nbIMUs()); // Matrice 3*3 * nIMU

So I've tried something similar in python:

imus = np.ndarray((3,3, model.nbIMUs(), 2 * n_frames))
for i, q in enumerate(target_q.T):
    imus[:, :, :,  i] = np.array([imu.to_array()[:3,:3] for imu in model.IMU(q)]).T

# Dispatch imus in biorbd structure so KF can use it
imusOverFrames = []
for i in range(imus.shape[3]):
    imusOverFrames.append([imu for imu in imus[:, :, :, i].T])

But this results in the binding error:

TypeError: Wrong number or type of arguments for overloaded function 'KalmanReconsIMU_reconstructFrame'.
  Possible C/C++ prototypes are:
    BiorbdEigen3::rigidbody::KalmanReconsIMU::reconstructFrame(BiorbdEigen3::Model &,std::vector< BiorbdEigen3::rigidbody::IMU,std::allocator< BiorbdEigen3::rigidbody::IMU > > const &,BiorbdEigen3::rigidbody::GeneralizedCoordinates *,BiorbdEigen3::rigidbody::GeneralizedVelocity *,BiorbdEigen3::rigidbody::GeneralizedAcceleration *)
    BiorbdEigen3::rigidbody::KalmanReconsIMU::reconstructFrame(BiorbdEigen3::Model &,BiorbdEigen3::utils::Vector const &,BiorbdEigen3::rigidbody::GeneralizedCoordinates *,BiorbdEigen3::rigidbody::GeneralizedVelocity *,BiorbdEigen3::rigidbody::GeneralizedAcceleration *)
    BiorbdEigen3::rigidbody::KalmanReconsIMU::reconstructFrame()

I've also tried the strategy to create the IMU instances for each rotation matrix received from the sensor but I've had issues there too. :D
Given a rotation matrix, by trying to create an instance of IMU class, I ended up doing something like this:

import biorbd
import numpy as np

rot = np.eye(3)
imu_i = biorbd.IMU(biorbd.RotoTransNode(biorbd.RotoTrans(rot, np.zeros(3),"xyz")))

But I still got some binding errors:

_biorbd_eigen.RotoTrans_swiginit(self, _biorbd_eigen.new_RotoTrans(*args))
ValueError: Vector must be a numpy vector

So there is definitely something that I'm doing wrong, so I wanted to ask you:

• Is there some binding issues here or am doing something completely wrong? :D
• What would be the best way to use custom IMU data for inverse kinematics using your KalmanReconsIMU class in python

[pariterre]

Hello!

Yep, I definitely should make a better binder for the IMUs... The method expect a vector of 9xnIMU elements (9 being the 3x3 matrix).

I did not test it, but this should work:

[...]
targetIMUs = np.ndarray(9*n_imu)
kalman.reconstructFrame(model, targetIMUs, Q, Qdot, Qddot)

[askuric]

Hey @pariterre,

Thanks for the update.
I was able to use your suggestion and run the code!

At the end I've used a snippet like this to transform the IMU data to the np.array

imusOverFrames = np.ndarray((2 * n_frames, 9* model.nbIMUs()))
for frame, q in enumerate(target_q.T):
    imusOverFrames[frame, :] = np.hstack([imu.to_array()[:3,:3].T.flatten() for imu in model.IMU(q)])

Thanks again!

[pariterre]

Happy Days!

Please do not close the issue, I'll use it as a reminder to add a better interface for specifying the imu real data interface!