Python
1import os 2import numpy as np 3from scipy.spatial.transform import Rotation as R 4import pandas as pd 5 6# データの読み込み7R_dir = r'XXXXXX\\'8RF = R_dir + 'XXXXXXXX.trc'9O_data = np.loadtxt(RF, dtype=str, delimiter='\t', skiprows=5)10O_data = pd.DataFrame(O_data)11 12# 空の文字列をNaNに変換し、補完13O_data.replace('', np.nan, inplace=True)14O_data.fillna(method='ffill', inplace=True)15O_data.fillna(method='bfill', inplace=True)16O_data = O_data.astype(float)17 18# 各変数の取得19R_mp1 = O_data.iloc[:, 2:5].to_numpy()20R_mp5 = O_data.iloc[:, 5:8].to_numpy()21Rm_ankle = O_data.iloc[:, 8:11].to_numpy()22Rl_ankle = O_data.iloc[:, 11:14].to_numpy()23R_heel = O_data.iloc[:, 14:17].to_numpy()24Rm_knee = O_data.iloc[:, 17:20].to_numpy()25Rl_knee = O_data.iloc[:, 20:23].to_numpy()26R_tor = O_data.iloc[:, 23:26].to_numpy()27 28# キックサイクルの検出（上昇後下降に切り替わる点を見つける）29right_contact = [i for i in range(1, len(R_heel) - 1) if R_heel[i - 1, 1] < R_heel[i, 1] and R_heel[i, 1] > R_heel[i + 1, 1]]30 31# R_mp1の最小値から+15される時点を見つける32additional_splits = []33for i in range(len(right_contact) - 1):34 segment = R_mp1[right_contact[i]:right_contact[i + 1], 2]35 min_index = np.argmin(segment)36 additional_split = right_contact[i] + min_index + 15037 if additional_split < right_contact[i + 1]: # 範囲内か確認38 additional_splits.append(additional_split)39 40# 分割位置を統合41all_splits = sorted(set(right_contact + additional_splits))42 43# 各変数を新しい分割位置で再分割44segments = {45 "R_mp1": R_mp1,46 "R_mp5": R_mp5,47 "Rl_ankle": Rl_ankle,48 "Rm_ankle": Rm_ankle,49 "R_heel": R_heel,50 "Rl_knee": Rl_knee,51 "Rm_knee": Rm_knee,52 "R_tor": R_tor 53}54 55split_segments = {key: [] for key in segments.keys()}56 57for key, data in segments.items():58 for i in range(len(all_splits) - 1):59 split_segments[key].append(data[all_splits[i]:all_splits[i + 1], :])60 61#########################ここから計算##########################62# 各分割されたセグメントのベクトルを計算_3つのセグメント（foot, shank, thighを作ります）63def calculate_vectors():64 vectors = []65 for i in range(len(split_segments["R_heel"])):66 mid_foot = (split_segments["R_mp1"][i] + split_segments["R_mp5"][i]) / 267 mid_ankle = (split_segments["Rl_ankle"][i] + split_segments["Rm_ankle"][i]) / 268 mid_knee = (split_segments["Rl_knee"][i] + split_segments["Rm_knee"][i]) / 269 70 foot_vector = split_segments["R_heel"][i] - mid_foot 71 shank_vector = mid_knee - mid_ankle 72 thigh_vector = split_segments["R_tor"][i] - mid_knee 73 74 vectors.append({75 "foot": foot_vector,76 "shank": shank_vector,77 "thigh": thigh_vector,78 "mid_foot": mid_foot,79 "mid_ankle": mid_ankle,80 "mid_knee": mid_knee 81 })82 return vectors 83 84vectors = calculate_vectors()85 86# 各セグメントの回転行列とオイラー角を計算87def calculate_rotation_matrices_and_euler_angles(vectors):88 foot_shank_angles = []89 shank_thigh_angles = []90 91 for i in range(len(vectors)):92 foot_segment = []93 shank_segment = []94 95 for k in range(len(vectors[i]["foot"])):96 #footの計算97 #z軸を定義します_foot98 z_axis_ft = vectors[i]["foot"][k] / np.linalg.norm(vectors[i]["foot"][k])99 z_axis_ft /= np.linalg.norm(z_axis_ft) # 正規化100 #y軸を定義します_foot101 y_axis_ft = np.cross(z_axis_ft, split_segments["R_mp1"][i][k] - split_segments["R_mp5"][i][k])102 y_axis_ft /= np.linalg.norm(y_axis_ft) # 正規化103 104 #x軸はzとy軸から構成される平面に直行する軸として定義します_foot105 x_axis_ft = np.cross(y_axis_ft, z_axis_ft)106 x_axis_ft /= np.linalg.norm(x_axis_ft) # 正規化107 108 #y軸を再計算_shank109 y_axis_corrected_ft = np.cross(x_axis_ft, z_axis_ft)110 y_axis_corrected_ft /= np.linalg.norm(y_axis_corrected_ft) # 正規化111 112 #回転行列の計算_foot113 rotation_matrix_ft = np.stack([x_axis_ft, y_axis_corrected_ft, z_axis_ft], axis=-1)114 r_ft = R.from_matrix(rotation_matrix_ft)115 116 #shankの計算117 #z軸を定義します_shank118 z_axis_sh = vectors[i]["shank"][k] / np.linalg.norm(vectors[i]["shank"][k])119 z_axis_sh /= np.linalg.norm(z_axis_sh) # 正規化120 121 #y軸を定義します_shank122 y_axis_sh = np.cross(z_axis_sh, split_segments["Rm_ankle"][i][k] - split_segments["Rl_ankle"][i][k])123 y_axis_sh /= np.linalg.norm(y_axis_sh) # 正規化124 125 #x軸を計算します_shank126 x_axis_sh = np.cross(y_axis_sh, z_axis_sh)127 x_axis_sh /= np.linalg.norm(x_axis_sh) # 正規化128 129 #y軸を再計算_shank130 y_axis_corrected_sh = np.cross(x_axis_sh, z_axis_sh)131 y_axis_corrected_sh /= np.linalg.norm(y_axis_corrected_sh) # 正規化132 133 #回転行列の計算_shank134 rotation_matrix_sh = np.stack([x_axis_sh, y_axis_corrected_sh, z_axis_sh], axis=-1)135 r_sh = R.from_matrix(rotation_matrix_sh)136 137 #thighの計算138 #z軸を定義します_thigh139 z_axis_th = vectors[i]["thigh"][k] / np.linalg.norm(vectors[i]["thigh"][k])140 z_axis_th /= np.linalg.norm(z_axis_th) # 正規化141 142 #y軸を定義します_thigh143 y_axis_th = np.cross(z_axis_th, split_segments["Rm_knee"][i][k] - split_segments["Rl_knee"][i][k])144 y_axis_th /= np.linalg.norm(y_axis_th) # 正規化145 146 #x軸を計算します_thigh147 x_axis_th = np.cross(y_axis_th, z_axis_th)148 x_axis_th /= np.linalg.norm(x_axis_th) # 正規化149 150 #回転行列の計算_thigh151 rotation_matrix_th = np.stack([x_axis_th, y_axis_th, z_axis_th], axis=-1)152 r_th = R.from_matrix(rotation_matrix_th)153 154 #相対角度の計算155 foot_shank_rel_rot = r_ft * r_sh.inv()156 shank_thigh_rel_rot = r_sh * r_th.inv()157 158 foot_shank_rel_euler = foot_shank_rel_rot.as_euler('xyz', degrees=True)159 shank_thigh_rel_euler = shank_thigh_rel_rot.as_euler('xyz', degrees=True)160 161 foot_segment.append(foot_shank_rel_euler)162 shank_segment.append(shank_thigh_rel_euler)163 164 foot_shank_angles.append(foot_segment)165 shank_thigh_angles.append(shank_segment)166 167 return foot_shank_angles, shank_thigh_angles 168 169foot_shank_angles, shank_thigh_angles = calculate_rotation_matrices_and_euler_angles(vectors)
0 コメント