Creating a ExternalForce object from c3d force plateform

[ANaaim]

Just to be sure, is the following code the intended way of creating an ExternalForces from a c3d files and generating a list of forces for the inverse dynamics to be performed :

    Qopt_sqp = ik_solver.solve(method="sqpmethod")
    # ---------Dynamics----------------
    # Extract the forces plateforms
    c3d = ezc3d.c3d(filename_dynamic, extract_forceplat_data=True)

    # Calculation of the qdot and qddot
    freq = c3d["parameters"]["POINT"]["RATE"]["value"][0]  # frequency
    nb_frame = c3d["parameters"]["POINT"]["FRAMES"]["value"][0]

    cut_freq = 6

    w = cut_freq / (freq / 2)  # Normalize the frequency
    b, a = signal.butter(4, w, "low")

    Qdopt_sqp = np.gradient(Qopt_sqp, 1 / freq, axis=1)
    Qdopt_sqp_filt = signal.filtfilt(b, a, Qdopt_sqp, axis=1)
    Qddopt_sqp = np.gradient(Qdopt_sqp_filt, 1 / freq, axis=1)
    Qddopt_sqp_filt = signal.filtfilt(b, a, Qddopt_sqp, axis=1)

    PF1_force, PF1_moment, PF1_origin, PF1_freq = PF_proc.extract_data(c3d, 0)
    PF2_force, PF2_moment, PF2_origin, PF2_freq = PF_proc.extract_data(c3d, 1)

    # # resampling
    PF1_force, PF1_moment = PF_proc.resample_data(PF1_force, PF1_moment, freq, PF1_freq, nb_frame)
    PF2_force, PF2_moment = PF_proc.resample_data(PF2_force, PF2_moment, freq, PF2_freq, nb_frame)

    # compute moments at the origin of the global reference frame
    PF1_moment0 = PF_proc.moment_at_origin(PF1_force, PF1_moment, PF1_origin)
    PF2_moment0 = PF_proc.moment_at_origin(PF2_force, PF2_moment, PF2_origin)

    # compute CoP (for the visualisation)
    PF1_CoP = PF_proc.compute_CoP(PF1_force, PF1_moment0)
    PF2_CoP = PF_proc.compute_CoP(PF2_force, PF2_moment0)

    # So now I have my forces express in the global frame :
    # How to express it in the segment frame
    center_of_plateform = np.repeat(PF1_origin[:, np.newaxis], Qopt_sqp.shape[1], axis=1)
    # Calculation of point - rp
    u_foot = Qopt_sqp[36:39, :]
    rp_foot = Qopt_sqp[39:42, :]
    rd_foot = Qopt_sqp[42:45, :]
    w_foot = Qopt_sqp[45:48, :]

    from bionc.bionc_numpy.cartesian_vector import vector_projection_in_non_orthogonal_basis

    point = vector_projection_in_non_orthogonal_basis(center_of_plateform - rp_foot, u_foot, rp_foot - rd_foot, w_foot)

    # Projection of the force on the segment frame
    position_point_application = rp_foot - np.repeat(PF1_origin[:, np.newaxis], Qopt_sqp.shape[1], axis=1)

    GRF = ExternalForce.from_components(
        application_point_in_local=position_point_application, force=PF1_force, torque=PF1_moment0
    )
    list_external_force = ExternalForceList.empty_from_nb_segment(model.nb_segments)
    # Integration of the force in the list
    list_external_force.add_external_force(3, GRF)
    # How to do a list of forces :
    # Should it be expressed in the global reference frame or in the segment frame ?
    for ind_frame in range(0, Qopt_sqp.shape[1]):
        tuple_of_Q = [
            SegmentNaturalCoordinates.from_components(
                u=Qopt_sqp[i * 12 : i * 12 + 3, ind_frame],
                rp=Qopt_sqp[i * 12 + 3 : i * 12 + 6, ind_frame],
                rd=Qopt_sqp[i * 12 + 6 : i * 12 + 9, ind_frame],
                w=Qopt_sqp[i * 12 + 9 : i * 12 + 12, ind_frame],
            )
            for i in range(0, model.nb_segments)
        ]
        tuple_of_Qddot = [
            SegmentNaturalAccelerations.from_components(
                uddot=Qddopt_sqp_filt[i * 12 : i * 12 + 3, ind_frame],
                rpddot=Qddopt_sqp_filt[i * 12 + 3 : i * 12 + 6, ind_frame],
                rdddot=Qddopt_sqp_filt[i * 12 + 6 : i * 12 + 9, ind_frame],
                wddot=Qddopt_sqp_filt[i * 12 + 9 : i * 12 + 12, ind_frame],
            )
            for i in range(0, model.nb_segments)
        ]
        list_temp_force = ExternalForceList.empty_from_nb_segment(model.nb_segments)
        for ind_segment in range(0, model.nb_segments):
            if len(list_external_force.segment_external_forces(ind_segment)) > 0:
                point_temp = list_external_force.segment_external_forces(3)[0].application_point_in_local[:, ind_frame]
                force_temp = list_external_force.segment_external_forces(3)[0].force[:, ind_frame]
                torque_temp = list_external_force.segment_external_forces(3)[0].torque[:, ind_frame]
                list_temp_force.add_external_force(
                    ind_segment,
                    ExternalForce.from_components(
                        application_point_in_local=point_temp, force=force_temp, torque=torque_temp
                    ),
                )

        Q = NaturalCoordinates.from_qi(tuple(tuple_of_Q))
        Qddot = NaturalAccelerations.from_qddoti(tuple(tuple_of_Qddot))
        torques, forces, lambdas = model.inverse_dynamics(Q=Q, Qddot=Qddot, external_forces=list_temp_force)

[Ipuch]

This looks "pas pire". 👍