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
commented
1 year ago
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 :