Model tracking by measured marker data

[Purin-musi]

Hello. I'm currently trying to run an example using model arm26.bioMod, such as "muscle_excitations_tracker.py" provided as an example, with data measured from actual 3D motion capture, but it doesn't work.

I may have made a big mistake, so I would like to ask you about the prerequisites. In the above example, is it possible to run the program by replacing the marker data passed to OCP with the measured marker data? The markers are arranged according to the arm26 code, but no other special processing is added. If I have to do something with the marker data to perform the marker tracking, or if the marker scale is limited, please let me know.

I'm sorry to take the time to ask a rudimentary question, but I would appreciate any advice.

[pariterre]

Hi @Purin-musi

Thanks for trying bioptim! It is indeed possible to change the values of a target. Could you post a copy-paste of the code you are running, or at least the line you changed? That would greatly help us to help you :)

Also, could you explain what you mean by "it doesn't work", does it fail to optimize? Does it crash? Does it optimize but return an unexpected answer? etc.

We will be pleased to help you after you provide us with more details!

[Purin-musi]

Hello pariterre. Thank you for your reply.

I'm sorry. The first question was lacking in conditions. I'm using Windows. The code I ran is a slight modification of "muscle_excitations_tracker.py". Marker data was taken at 50Hz for 10 seconds, so I changed it accordingly.

def main():

    # Define the problem
    biorbd_model = biorbd.Model("arm26.bioMod")
    final_time = 10            #0.5
    n_shooting_points = 499     #30
    use_residual_torque = True

    # Generate random data to fit
    t, markers_ref, x_ref, muscle_excitations_ref = generate_data(biorbd_model, final_time, n_shooting_points)

    def make_1D(data_1D):
        return [float(s) for s in data_1D.split()][0:]
    def make_2D(data_2D):
        return [make_1D(s) for s in data_2D]

    with open('test2.txt') as f:
        data = f.readlines()
    marker_list_3D = []

    for i  in range(500): # max 500
           marker_list_3D.append(np.array(make_2D(data[(i*6):(i*6)+6]))) 

    markers_ref = np.stack(marker_list_3D).T

    # Track these data
    biorbd_model = biorbd.Model("arm26.bioMod")  # To allow for non free variable, the model must be reloaded
    ocp = prepare_ocp(
        biorbd_model,
        final_time,
        n_shooting_points,
        markers_ref,
        muscle_excitations_ref,
        x_ref[: biorbd_model.nbQ(), :],
        use_residual_torque=use_residual_torque,
        kin_data_to_track="markers",
    )

    # --- Solve the program --- #
    sol = ocp.solve()
    sol.animate()

if __name__ == "__main__":
    main()

The array that finally goes into marker_ref is as in the attached file. array.txt

When I do this, there are two unclear points. The first is that iter exceeds 1000 and execution ends. This may be something wrong with the settings. Can you suggest something?

The second is that the motion output by sol.animate () is different from what was actually measured. Actually, I captured the bending motion of the arm twice in 10 seconds, but the output result is a completely different motion.

I would be grateful if you could give me some advice.

[pariterre]

Hi again!

The piece of code you provided is not a minimum viable as it does not run when we run it. I assume the "prepare_ocp" and "generate_data" are the one provided in the example, which is a bit odd since "generate_data" should be fully replaced by actual data (therefore should not be called at all but replaced by the reading of the file), which you seem to do, but I am not sure. Also, the file read in the code is test2.txt but you provided me array.txt and if I just swap the name, the function make_1D fails as the array is not of the expected formatting.

I could spend time to debug the code, but unfortunately I hope you understand that I cannot afford to spend that time on other's code (as I have my own thesis to write haha). Sorry to be a bit nitpicky..

If you can provide an actual minimum viable, I'll be happy to help :)

[Purin-musi]

Hello

I apologize for sending you an odd code. I was unfamiliar with this and was not thoughtful. This time I think you can run it right away with the attached files and code. In addition, I removed the part that remained as described in the example and changed the code a little.

from scipy.integrate import solve_ivp
import numpy as np
import biorbd_casadi as biorbd
from casadi import MX, vertcat
from matplotlib import pyplot as plt
from bioptim import (
    OptimalControlProgram,
    NonLinearProgram,
    BiMapping,
    DynamicsList,
    DynamicsFcn,
    DynamicsFunctions,
    ObjectiveList,
    ObjectiveFcn,
    BoundsList,
    Bounds,
    QAndQDotBounds,
    InitialGuessList,
    OdeSolver,
    Node,
    CostType,
)

def prepare_ocp(
    biorbd_model: biorbd.Model,
    final_time: float,
    n_shooting: int,
    markers_ref: np.ndarray,
    excitations_ref: np.ndarray,
    use_residual_torque: bool,
    kin_data_to_track: str = "markers",
    ode_solver: OdeSolver = OdeSolver.COLLOCATION(),
) -> OptimalControlProgram:

    # Add objective functions
    objective_functions = ObjectiveList()
    objective_functions.add(ObjectiveFcn.Lagrange.TRACK_CONTROL, key="muscles", target=excitations_ref)
    if use_residual_torque:
        objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau")
    if kin_data_to_track == "markers":
        objective_functions.add(ObjectiveFcn.Lagrange.TRACK_MARKERS, node=Node.ALL, weight=100, target=markers_ref)
    else:
        raise RuntimeError("Wrong choice of kin_data_to_track")

    # Dynamics
    dynamics = DynamicsList()
    dynamics.add(DynamicsFcn.MUSCLE_DRIVEN, with_excitations=True, with_torque=use_residual_torque)

    # Path constraint
    x_bounds = BoundsList()
    x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
    # Due to unpredictable movement of the forward dynamics that generated the movement, the bound must be larger
    x_bounds[0].min[[0, 1], :] = -2 * np.pi
    x_bounds[0].max[[0, 1], :] = 2 * np.pi

    # Add muscle to the bounds
    activation_min, activation_max, activation_init = 0, 1, 0.5
    x_bounds[0].concatenate(
        Bounds([activation_min] * biorbd_model.nbMuscles(), [activation_max] * biorbd_model.nbMuscles())
    )
    x_bounds[0][(biorbd_model.nbQ() + biorbd_model.nbQdot()) :, 0] = excitations_ref[:, 0]

    # Initial guess
    x_init = InitialGuessList()
    x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()) + [0] * biorbd_model.nbMuscles())

    # Define control path constraint
    excitation_min, excitation_max, excitation_init = 0, 1, 0.5
    u_bounds = BoundsList()
    u_init = InitialGuessList()
    if use_residual_torque:
        tau_min, tau_max, tau_init = -100, 100, 0
        u_bounds.add(
            [tau_min] * biorbd_model.nbGeneralizedTorque() + [excitation_min] * biorbd_model.nbMuscles(),
            [tau_max] * biorbd_model.nbGeneralizedTorque() + [excitation_max] * biorbd_model.nbMuscles(),
        )
        u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() + [excitation_init] * biorbd_model.nbMuscles())
    else:
        u_bounds.add([excitation_min] * biorbd_model.nbMuscles(), [excitation_max] * biorbd_model.nbMuscles())
        u_init.add([excitation_init] * biorbd_model.nbMuscles())
    # ------------- #

    return OptimalControlProgram(
        biorbd_model,
        dynamics,
        n_shooting,
        final_time,
        x_init,
        u_init,
        x_bounds,
        u_bounds,
        objective_functions,
        ode_solver=ode_solver,
    )

def main():

    # Define the problem
    biorbd_model = biorbd.Model("arm26.bioMod")
    ### If the processing is heavy, you can change it to 2, 99, 100 and process for 2 seconds.###
    final_time = 10             # or 2
    n_shooting_points = 499     # or 99
    use_residual_torque = True

    # Generate random EMG
    n_mus = biorbd_model.nbMuscleTotal()
    muscle_excitations_ref = np.random.rand(n_shooting_points, n_mus).T

    # Measured marker data to array
    def make_1D(data_1D):
        return [float(s) for s in data_1D.split()][0:]
    def make_2D(data_2D):
        return [make_1D(s) for s in data_2D]

    with open('test2.txt') as f:
        data = f.readlines()

    marker_list_3D = []
    for i  in range(500): # or 100
           marker_list_3D.append(np.array(make_2D(data[(i*6):(i*6)+6]))) #マーカの数分指定
    markers_ref = np.stack(marker_list_3D).T

    ## Marker data check
    # np.set_printoptions(threshold=np.inf)
    # with open('array.txt', 'w') as f:
    #     print(markers_ref, file=f)

    # Track these data
    biorbd_model = biorbd.Model("arm26.bioMod")  # To allow for non free variable, the model must be reloaded
    ocp = prepare_ocp(
        biorbd_model,
        final_time,
        n_shooting_points,
        markers_ref, # meaeured real data
        muscle_excitations_ref, # generated random data
        use_residual_torque=use_residual_torque,
        kin_data_to_track="markers",
    )

    # --- Solve the program --- #
    sol = ocp.solve()

    # --- Plot --- #
    #plt.show()
    sol.animate()

if __name__ == "__main__":
    main()

Then we are currently using measurements only for marker data and EMG data using generated random data. I couldn't measure EMG right away, so I thought I'd start the experiment after confirming the operation with the real marker. I think this would hurt the correctness of the analysis, but I didn't think it would have a catastrophic effect on the animation output. Perhaps this outlook was lenient ... I'm sorry if it was because I didn't measure the EMG data.

test2.txt is the marker data output from motion capture (actually, a little processing was added). array.txt is an array of test2.txt data with lines 111-122 of code. This value is stored in marker_ref. (When processing all 500 frames) arm26.bioMod is the arm model included in the example. I haven't changed anything.

When I ran this code, I expected sol.animate () to output the action of raising and lowering the arm twice in 10 seconds actually measured, but it didn't work. The animation is output, but it only moves slightly with the arms raised greatly. Also, I am worried that the iter at the time of processing exceeds 1000 and the execution is completed. Is this normal behavior?

I don't want to burden the developers, but I would appreciate any advice. Thank you

test2.txt array.txt

[pariterre]

Hi!

Yep I could run the example now :)

Firstly, welcome to the optimal control community! So what you are experiencing is a classical: I expected a bit too much haha (no worry, we all go through this process :P)

Optimal control is a very hard program to solve, for instance, your program has almost 30 000 variables to solve under 25 000 constraints, and this is not counting the objectives that it has to optimize for. What that means is that a 10 seconds movement is unrealistic to solve under these conditions.

As your intuition went, since the solver went to the maximum number of iterations, it means that the solution is not optimal. Depending on the inf_du value, it may or may not respect the constraints (typically a value smaller than 10^-6 means that the constraints are respected). The main constraint of the program is the dynamics constraints, that is the continuity of the states. If it is not respected, the movement will not even look something realistic. The other reason it may not solve is the solver is stuck to a particular point. If this point happens to respect the constraints, then the movement will be realistic, but I may or may not be the expected movement.

So the rule of thumb, if the solver does not tell you "Optimal solution found" you should discard the results as there is a huge chance of there results to be compromised anyway.

My suggestions:

• You should start with a much shorter movement (1 second is a good start) with at least 30 shooting nodes per second. I tested it with final_time=1 and n_shooting_points = 49 (and adjusting the size of the matrix of the markers accordingly) and I could solve the problem. Please note that I had to remove your hard constraint on the starting point for the muscle activation (x_bounds[0][(biorbd_model.nbQ() + biorbd_model.nbQdot()) :, 0] = excitations_ref[:, 0]) again because the EMG and kinematics are not sound, and therefore you cannot force the solver to follow them. I'll post the working example in the next post
• I strongly suggest against optimizing using the muscle dynamics with with_excitation=True. This introduces a lot of nonlinearity to the program, which makes it even harder for long optimization. At least for your first test. Put yourself in winning condition!
• I prefer using RK instead of COLLOCATION, as I think it is more stable, but it is a matter of opinion I think
• Activate the show_online_optim to have an idea what goes wrong while it optimizes. As a bonus, you can add the Constraints and Objective to show (or any other graphs)
• Also, please be aware that these kind of problems (with muscles and tracking) cab be really sensitive. So providing a set of muscle activations to track while providing a kinematics which is unrelated to these activations is hard for the solver, making it much more prone to fail.
• Finally, your program is well suited for moving horizon estimator, or nonlinear model predictive control (MHE, or NMPC) which is a module of bioptim. In short, you solve multiple programs in a sliding window fashion. I strongly suggest against testing this before have a solid grasp of what optimal control is, and the pit falls of numerically solving them. But when you fell that you understand well, this will help you a lot!

[pariterre]

This examples solves the program, you can start from there :)

from scipy.integrate import solve_ivp
import numpy as np
import biorbd_casadi as biorbd
from casadi import MX, vertcat
from matplotlib import pyplot as plt
from bioptim import (
    OptimalControlProgram,
    NonLinearProgram,
    BiMapping,
    DynamicsList,
    DynamicsFcn,
    DynamicsFunctions,
    ObjectiveList,
    ObjectiveFcn,
    BoundsList,
    Bounds,
    QAndQDotBounds,
    InitialGuessList,
    OdeSolver,
    Node,
    CostType,
)

def prepare_ocp(
    biorbd_model: biorbd.Model,
    final_time: float,
    n_shooting: int,
    markers_ref: np.ndarray,
    excitations_ref: np.ndarray,
    use_residual_torque: bool,
    kin_data_to_track: str = "markers",
    ode_solver: OdeSolver = OdeSolver.RK4(),
) -> OptimalControlProgram:

    # Add objective functions
    objective_functions = ObjectiveList()
    # objective_functions.add(ObjectiveFcn.Lagrange.TRACK_CONTROL, key="muscles", target=excitations_ref)
    if use_residual_torque:
        objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau")
    if kin_data_to_track == "markers":
        objective_functions.add(ObjectiveFcn.Lagrange.TRACK_MARKERS, node=Node.ALL, weight=100, target=markers_ref[:, :, :n_shooting + 1])
    else:
        raise RuntimeError("Wrong choice of kin_data_to_track")

    # Dynamics
    dynamics = DynamicsList()
    dynamics.add(DynamicsFcn.MUSCLE_DRIVEN, with_excitations=False, with_torque=use_residual_torque)

    # Path constraint
    x_bounds = BoundsList()
    x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
    # Due to unpredictable movement of the forward dynamics that generated the movement, the bound must be larger
    x_bounds[0].min[[0, 1], :] = -2 * np.pi
    x_bounds[0].max[[0, 1], :] = 2 * np.pi

    # # Add muscle to the bounds
    # activation_min, activation_max, activation_init = 0, 1, 0.5
    # x_bounds[0].concatenate(
    #     Bounds([activation_min] * biorbd_model.nbMuscles(), [activation_max] * biorbd_model.nbMuscles())
    # )
    # # x_bounds[0][(biorbd_model.nbQ() + biorbd_model.nbQdot()) :, 0] = excitations_ref[:, 0]

    # Initial guess
    x_init = InitialGuessList()
    x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))  # + [0] * biorbd_model.nbMuscles())

    # Define control path constraint
    excitation_min, excitation_max, excitation_init = 0, 1, 0.5
    u_bounds = BoundsList()
    u_init = InitialGuessList()
    if use_residual_torque:
        tau_min, tau_max, tau_init = -100, 100, 0
        u_bounds.add(
            [tau_min] * biorbd_model.nbGeneralizedTorque() + [excitation_min] * biorbd_model.nbMuscles(),
            [tau_max] * biorbd_model.nbGeneralizedTorque() + [excitation_max] * biorbd_model.nbMuscles(),
        )
        u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() + [excitation_init] * biorbd_model.nbMuscles())
    else:
        u_bounds.add([excitation_min] * biorbd_model.nbMuscles(), [excitation_max] * biorbd_model.nbMuscles())
        u_init.add([excitation_init] * biorbd_model.nbMuscles())
    # ------------- #

    return OptimalControlProgram(
        biorbd_model,
        dynamics,
        n_shooting,
        final_time,
        x_init,
        u_init,
        x_bounds,
        u_bounds,
        objective_functions,
        ode_solver=ode_solver,
    )

def main():

    # Define the problem
    biorbd_model = biorbd.Model("arm26.bioMod")
    ### If the processing is heavy, you can change it to 2, 99, 100 and process for 2 seconds.###
    final_time = 1             # or 2
    n_shooting_points = 49     # or 99
    use_residual_torque = True

    # Generate random EMG
    n_mus = biorbd_model.nbMuscleTotal()
    muscle_excitations_ref = np.random.rand(n_shooting_points, n_mus).T

    # Measured marker data to array
    def make_1D(data_1D):
        return [float(s) for s in data_1D.split()][0:]
    def make_2D(data_2D):
        return [make_1D(s) for s in data_2D]

    with open('test2.txt') as f:
        data = f.readlines()

    marker_list_3D = []
    for i  in range(500): # or 100
           marker_list_3D.append(np.array(make_2D(data[(i*6):(i*6)+6]))) #マーカの数分指定
    markers_ref = np.stack(marker_list_3D).T

    ## Marker data check
    # np.set_printoptions(threshold=np.inf)
    # with open('array.txt', 'w') as f:
    #     print(markers_ref, file=f)

    # Track these data
    biorbd_model = biorbd.Model("arm26.bioMod")  # To allow for non free variable, the model must be reloaded
    ocp = prepare_ocp(
        biorbd_model,
        final_time,
        n_shooting_points,
        markers_ref, # meaeured real data
        muscle_excitations_ref, # generated random data
        use_residual_torque=use_residual_torque,
        kin_data_to_track="markers",
    )

    # --- Solve the program --- #
    sol = ocp.solve(show_online_optim=True)

    # --- Plot --- #
    #plt.show()
    sol.animate()

if __name__ == "__main__":
    main()

[Purin-musi]

Hello.

Thank you very much for your kindness before. I thought I understood it to some extent, but I didn't study much about optimization calculations and forward dynamics. It's still not enough, but I'm deeply grateful to you.

In parallel with that study, I am trying to create a tracking program from real data by MHE based on your advice, but I am stuck again this time, and I would be grateful if you could give me some advice.

I measured the markers and EMG for a total of 4 seconds with 30 shots per second and entered them into the program. However, as far as sol.animate () is seen, the output is different from the actual motion shot. Actually, it takes 4 seconds to measure the movement of bending and stretching the elbow very slowly and only once. EMG is absolute valued in Python, and the amount amplified by the amplifier is restored by simple division. No particular processing has been added to the marker data. I think there is something wrong with the input of measurement data, but I didn't understand. If you have any ideas, I would be grateful if you could give me some advice.

Also, the method proposed last time with_excitations = False, but when I execute it with this code, an error occurs. I'm sorry, but I would appreciate it if you could tell me this solution again.

Below is the minimum executable code and measurement data. Originally, I made something that receives the output from the measuring instrument in real time and processes it while updating the data, so there may be strange unnatural parts, but I deleted it because it is long as it is. the behavior in question has been reproduced.

test3_30Hz.txt

from copy import copy
import os
import matplotlib.pyplot as plt
from scipy.integrate import solve_ivp
import casadi as cas
from casadi import MX, vertcat
import numpy as np
import biorbd_casadi as biorbd
import biorbd as bd
from bioptim import (
    MovingHorizonEstimator,
    OptimalControlProgram,
    NonLinearProgram,
    BiMapping,
    DynamicsFunctions,
    Dynamics,
    DynamicsFcn,
    DynamicsList,
    OdeSolver,
    Objective,
    ObjectiveFcn,
    ObjectiveList,
    Bounds,
    BoundsList,
    QAndQDotBounds,
    InitialGuess,
    InitialGuessList,
    InterpolationType,
    Solver,
    Node,
)

def make_1D(data_1D):
    return [float(s) for s in data_1D.split()][1:]
def make_2D(data_2D):
    return [make_1D(s) for s in data_2D]

def prepare_mhe(
biorbd_model,
window_len,
window_duration,
max_torque,
x_init,
u_init,
excitations_ref: np.ndarray,
use_residual_torque: bool,
ode_solver: OdeSolver = OdeSolver.RK4(),    #常微分方程式ソルバーの指定
):
    # Dynamics
    dynamics = DynamicsList()
    dynamics.add(DynamicsFcn.MUSCLE_DRIVEN, with_excitations=True, with_torque=use_residual_torque)

    # Path constraint
    x_bounds = BoundsList()
    x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
    # Due to unpredictable movement of the forward dynamics that generated the movement, the bound must be larger
    x_bounds[0].min[[0, 1], :] = -2 * np.pi
    x_bounds[0].max[[0, 1], :] = 2 * np.pi

    # Add muscle to the bounds
    activation_min, activation_max, activation_init = 0, 1, 0.5
    x_bounds[0].concatenate(
        Bounds([activation_min] * biorbd_model.nbMuscles(), [activation_max] * biorbd_model.nbMuscles())
    )
    x_bounds[0][(biorbd_model.nbQ() + biorbd_model.nbQdot()) :, 0] = excitations_ref[:, 0]

    # Initial guess
    x_init = InitialGuessList()
    x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()) + [0] * biorbd_model.nbMuscles())

    # Define control path constraint
    excitation_min, excitation_max, excitation_init = 0, 1, 0.5
    u_bounds = BoundsList()
    u_init = InitialGuessList()
    if use_residual_torque:
        tau_min, tau_max, tau_init = -100, 100, 0
        u_bounds.add(
            [tau_min] * biorbd_model.nbGeneralizedTorque() + [excitation_min] * biorbd_model.nbMuscles(),
            [tau_max] * biorbd_model.nbGeneralizedTorque() + [excitation_max] * biorbd_model.nbMuscles(),
        )
        u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() + [excitation_init] * biorbd_model.nbMuscles())
    else:
        u_bounds.add([excitation_min] * biorbd_model.nbMuscles(), [excitation_max] * biorbd_model.nbMuscles())
        u_init.add([excitation_init] * biorbd_model.nbMuscles())

    return MovingHorizonEstimator(
        biorbd_model,
        dynamics,
        window_len,
        window_duration,

        x_init=x_init,
        u_init=u_init,
        x_bounds=x_bounds,
        u_bounds=u_bounds,
        ode_solver=ode_solver,

        )

def get_solver_options(solver):
    mhe_dict = {"solver": None, "solver_options": None, "solver_options_first_iter": None}
    if solver == Solver.ACADOS:
        mhe_dict["solver"] = Solver.ACADOS
        mhe_dict["solver_options"] = {
            "nlp_solver_max_iter": 1000,
            "integrator_type": "ERK",
            "print_level": 0,
        }
    elif solver == Solver.IPOPT:
        mhe_dict["solver"] = Solver.IPOPT
        mhe_dict["solver_options"] = {
            "hessian_approximation": "limited-memory",
            "limited_memory_max_history": 50,
            "max_iter": 5,
            "print_level": 0,
            "tol": 1e-1,
            "bound_frac": 1e-10,
            "bound_push": 1e-10,
        }
        mhe_dict["solver_options_first_iter"] = copy(mhe_dict["solver_options"])
        mhe_dict["solver_options_first_iter"]["max_iter"] = 50
        mhe_dict["solver_options_first_iter"]["tol"] = 1e-6
    else:
        raise NotImplementedError("Solver not recognized")

    return mhe_dict

def main():
    # Define the problem
    biorbd_model_path = "./arm26.bioMod"
    biorbd_model = biorbd.Model(biorbd_model_path)
    use_residual_torque = True
    kin_data_to_track = "markers" # q or markers

    solver = Solver.IPOPT  # or Solver.ACADOS
    final_time = 4
    n_shooting_points = 30  # Number of points measured per second
    window_len = 3          # Number of points to process at one time
    window_duration = 1 / n_shooting_points * window_len
    n_frames_total = final_time * n_shooting_points - window_len - 1

    # Input measured data　
    with open('test3_30Hz.txt') as f:
        data = f.readlines()
    try:
        data_old
    except NameError:
        data_old = ["test", "test"]
        marker_list_3D = []
        EMG_list_1D = []

    for i  in range(len(data_old), len(data)):
        if data[i] == ' nMarkers=6\n':
            marker_list_3D.append(np.array(make_2D(data[i+2:i+8])))

    for k  in range(len(data_old), len(data)):
        if data[k] == 'nAnalogSamples=1\n':
            EMG_list_1D.append(np.array(make_1D(data[k+2])))

    #data_old = data #in preparation
    markers_ref = np.stack(marker_list_3D).T
    muscle_excitations_ref = np.abs(np.stack(EMG_list_1D).T / 10000)

    # # data check
    # np.set_printoptions(threshold=np.inf)
    # print(markers_ref)
    # print("##########################################################")
    # print(muscle_excitations_ref)

    x_init = np.zeros((biorbd_model.nbQ() * 2, window_len + 1))
    u_init = np.zeros((biorbd_model.nbQ(), window_len))
    torque_max = 5  # Give a bit of slack on the max torque

    biorbd_model = biorbd.Model("arm26.bioMod")  # To allow for non free variable, the model must be reloaded
    mhe = prepare_mhe(
        biorbd_model,
        window_len=window_len,
        window_duration=window_duration,
        max_torque=torque_max,
        x_init=x_init,
        u_init=u_init,
        excitations_ref=muscle_excitations_ref,
        use_residual_torque=use_residual_torque,
    )

    model = bd.Model("arm26.bioMod")  #bd is biorbd

    def update_function(mhe, t, sol):

        def new_objective(i: int):
            objective_functions = ObjectiveList()
            objective_functions.add(ObjectiveFcn.Lagrange.TRACK_CONTROL, key="muscles", target=muscle_excitations_ref[:, i : i + window_len])
            if use_residual_torque:
                objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau")
            if kin_data_to_track == "markers":
                objective_functions.add(ObjectiveFcn.Lagrange.TRACK_MARKERS, node=Node.ALL, weight=100, target=markers_ref[:, :, i : i + window_len + 1])
            elif kin_data_to_track == "q":
                objective_functions.add( ObjectiveFcn.Lagrange.TRACK_STATE, key="q", weight=100, node=Node.ALL, target=x_ref[: biorbd_model.nbQ(), :][:, i : i + window_len + 1], index=range(biorbd_model.nbQ()) )
            else:
                raise RuntimeError("Wrong choice of kin_data_to_track")
            return objective_functions

        mhe.update_objectives(new_objective_function = new_objective(t), )

        return t < n_frames_total  # True if there are still some frames to reconstruct

    # Solve the program
    sol = mhe.solve(update_function, **get_solver_options(solver))

    print(f"Window size of MHE : {window_duration} s.")
    print(f"New measurement every : {1/n_shooting_points} s.")
    print(f"Average time per iteration of MHE : {sol.time_to_optimize/(n_frames_total - 1)} s.")
    print(f"Average real time per iteration of MHE : {sol.real_time_to_optimize/(n_frames_total - 1)} s.")

    #plt.show()
    #sol.graphs()
    sol.animate()

if __name__ == "__main__":
    main()