-
Notifications
You must be signed in to change notification settings - Fork 0
/
main2.py
283 lines (235 loc) · 13.5 KB
/
main2.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
"""
Title: Jerk Decomposition Analysis
Description:
This program performs jerk decomposition analysis based on the methodology presented in the Neuromorph Movement Control course
It loads data containing joint angles, filters and calculates derivatives, computes Jacobian matrices, defines jerk components, and plots the results.
The goal is to analyze the acceleration components contributing to jerk in a motion capture scenario.
Author: Alexis GIBERT
Date: 22/05/2024
Requirements:
- Python 3.x
- NumPy
- Matplotlib
- SciPy
Usage:
Run the program and ensure the data file 'data4.txt' is present in the same directory. Modify the file path if necessary.
"""
import numpy as np
import matplotlib.pyplot as plt
from scipy.signal import savgol_filter
def main():
time_ms, markers, _ = import_data('data4.txt')
thetas = convert_markers_to_angles(markers)
seg_len = [300,300,300]
thetas_d, thetas_dd, thetas_ddd = savitzky_golay_filter(thetas, time_ms)
J, DJdt, D2Jdt2 = calculate_jacobians(seg_len, thetas, thetas_d, thetas_dd)
all_norms = calculate_jerk_components(thetas, thetas_d, thetas_dd, thetas_ddd, J, DJdt, D2Jdt2)
def import_data(filename):
"""
Import data from a text file.
Parameters:
filename (str): The path to the text file containing the data.
Returns:
tuple: A tuple containing time in milliseconds, marker positions, and muscle voltages.
"""
header_lines = 13
data = np.loadtxt(filename, skiprows=header_lines)
# Delete NaN lines
mask = ~np.any(np.isnan(data), axis=1)
data = data[mask]
time_ms = data[:, 0]
marker_1 = data[:, 1:4]
marker_2 = data[:, 4:7]
marker_3 = data[:, 7:10]
marker_4 = data[:, 10:13]
biceps_mv = data[:, 13]
triceps_mv = data[:, 14]
flexor_digitorum_mv = data[:, 15]
return time_ms, [marker_1, marker_2, marker_3, marker_4], [biceps_mv, triceps_mv, flexor_digitorum_mv]
def convert_markers_to_angles(markers, plot=True):
"""
Convert marker positions to joint angles.
Parameters:
markers (list): List of marker positions.
plot (bool): Whether to plot the angles or not.
Returns:
numpy.ndarray: Array of joint angles.
"""
thetas = []
v0 = np.zeros(3) # Define the fictitious marker 0
for i in range(len(markers[0])):
marker_angles = []
for j in range(len(markers) - 1):
# Calculate vectors between consecutive markers
v1 = markers[j][i] - v0 if j == 0 else markers[j][i] - markers[j - 1][i]
# Calculate the vector for the next marker
v2 = markers[j + 1][i] - markers[j][i]
# Calculate the cosine of the angle between the vectors
cos_angle = np.dot(v1, v2) / (np.linalg.norm(v1) * np.linalg.norm(v2))
# Calculate the angle and append to the list
angle = np.arccos(cos_angle)
marker_angles.append(angle)
thetas.append(marker_angles)
if plot==True:
# Plot the angles
plt.figure()
plt.plot(thetas)
plt.title('Angles between segments')
plt.xlabel('Time (ms)')
plt.ylabel('Angle (randian)')
plt.legend()
plt.show()
return np.array(thetas).T
def savitzky_golay_filter(thetas, time_ms):
"""
Apply Savitzky-Golay filter to smooth angle data and calculate derivatives.
Parameters:
thetas (numpy.ndarray): Array of joint angles.
time_ms (numpy.ndarray): Array of time in milliseconds.
Returns:
tuple: Tuple containing first, second, and third derivatives of angles.
"""
# Extract columns
theta1 = thetas[0, :]
theta2 = thetas[1, :]
theta3 = thetas[2, :]
# Calculate derivatives and filter with Savitzky-Golay filter
theta1dot = savgol_filter(x=np.gradient(theta1) / (time_ms[1] - time_ms[0]), window_length=8, polyorder=7)
theta2dot = savgol_filter(x=np.gradient(theta2) / (time_ms[1] - time_ms[0]), window_length=8, polyorder=7)
theta3dot = savgol_filter(x=np.gradient(theta3) / (time_ms[1] - time_ms[0]), window_length=8, polyorder=7)
theta1dotdot = savgol_filter(x=np.gradient(theta1dot) / (time_ms[1] - time_ms[0]), window_length=8, polyorder=7)
theta2dotdot = savgol_filter(x=np.gradient(theta2dot) / (time_ms[1] - time_ms[0]), window_length=8, polyorder=7)
theta3dotdot = savgol_filter(x=np.gradient(theta3dot) / (time_ms[1] - time_ms[0]), window_length=8, polyorder=7)
theta1dotdotdot = savgol_filter(x=np.gradient(theta1dotdot) / (time_ms[1] - time_ms[0]), window_length=8, polyorder=7)
theta2dotdotdot = savgol_filter(x=np.gradient(theta2dotdot) / (time_ms[1] - time_ms[0]), window_length=8, polyorder=7)
theta3dotdotdot = savgol_filter(x=np.gradient(theta3dotdot) / (time_ms[1] - time_ms[0]), window_length=8, polyorder=7)
thetas = [theta1, theta2, theta3]
thetas_d = [theta1dot, theta2dot, theta3dot]
thetas_dd = [theta1dotdot, theta2dotdot, theta3dotdot]
thetas_ddd = [theta1dotdotdot, theta2dotdotdot, theta3dotdotdot]
# Print Thetas
print("\n\nAngular configuration ...")
print("\nTheta\n", np.array(thetas))
print("\nTheta dot\n", np.array(thetas_d))
print("\nTheta dot dot\n", np.array(thetas_dd))
print("\nTheta dot dot dot\n", np.array(thetas_ddd))
return thetas_d, thetas_dd, thetas_ddd
def calculate_jacobians(seg_len, thetas, thetas_d, thetas_dd):
"""
Calculate Jacobian matrices and their derivatives.
Parameters:
seg_len (list): List of segment lengths.
thetas (numpy.ndarray): Array of joint angles.
thetas_d (numpy.ndarray): Array of first derivatives of joint angles.
thetas_dd (numpy.ndarray): Array of second derivatives of joint angles.
Returns:
tuple: Tuple containing Jacobians, first derivatives, and second derivatives.
"""
theta1, theta2, theta3 = thetas
theta1dot, theta2dot, theta3dot = thetas_d
theta1dotdot, theta2dotdot, theta3dotdot = thetas_dd
# Initialize matrices
num_samples = len(theta1)
J = np.zeros((2, 3, num_samples))
DJdt = np.zeros((2, 3, num_samples))
D2Jdt2 = np.zeros((2, 3, num_samples))
# Jacobian matrix + derivatives
for i in range(num_samples):
J[:, :, i] = np.array([
[-seg_len[0] * np.sin(theta1[i]) - seg_len[1] * np.sin(theta1[i] + theta2[i]) - seg_len[2] * np.sin(theta1[i] + theta2[i] + theta3[i]),
-seg_len[1] * np.sin(theta1[i] + theta2[i]) - seg_len[2] * np.sin(theta1[i] + theta2[i] + theta3[i]),
-seg_len[2] * np.sin(theta1[i] + theta2[i] + theta3[i])],
[seg_len[0] * np.cos(theta1[i]) + seg_len[1] * np.cos(theta1[i] + theta2[i]) + seg_len[2] * np.cos(theta1[i] + theta2[i] + theta3[i]),
seg_len[1] * np.cos(theta1[i] + theta2[i]) + seg_len[2] * np.cos(theta1[i] + theta2[i] + theta3[i]),
seg_len[2] * np.cos(theta1[i] + theta2[i] + theta3[i])]
])
DJdt[:, :, i] = np.array([
[-seg_len[0] * np.cos(theta1[i]) * theta1dot[i] - seg_len[1] * np.cos(theta1[i] + theta2[i]) * (theta1dot[i] + theta2dot[i]) - seg_len[2] * np.cos(theta1[i] + theta2[i] + theta3[i]) * (theta1dot[i] + theta2dot[i] + theta3dot[i]),
-seg_len[1] * np.cos(theta1[i] + theta2[i]) * (theta1dot[i] + theta2dot[i]) - seg_len[2] * np.cos(theta1[i] + theta2[i] + theta3[i]) * (theta1dot[i] + theta2dot[i] + theta3dot[i]),
-seg_len[2] * np.cos(theta1[i] + theta2[i] + theta3[i]) * (theta1dot[i] + theta2dot[i] + theta3dot[i])],
[-seg_len[0] * np.sin(theta1[i]) * theta1dot[i] - seg_len[1] * np.sin(theta1[i] + theta2[i]) * (theta1dot[i] + theta2dot[i]) - seg_len[2] * np.sin(theta1[i] + theta2[i] + theta3[i]) * (theta1dot[i] + theta2dot[i] + theta3dot[i]),
-seg_len[1] * np.sin(theta1[i] + theta2[i]) * (theta1dot[i] + theta2dot[i]) - seg_len[2] * np.sin(theta1[i] + theta2[i] + theta3[i]) * (theta1dot[i] + theta2dot[i] + theta3dot[i]),
-seg_len[2] * np.sin(theta1[i] + theta2[i] + theta3[i]) * (theta1dot[i] + theta2dot[i] + theta3dot[i])]
])
D2Jdt2[:, :, i] = np.array([
[-seg_len[0] * np.cos(theta1[i]) * theta1dotdot[i] - seg_len[1] * np.cos(theta1[i] + theta2[i]) * (theta1dotdot[i] + theta2dotdot[i]) - seg_len[2] * np.cos(theta1[i] + theta2[i] + theta3[i]) * (theta1dotdot[i] + theta2dotdot[i] + theta3dotdot[i]) + seg_len[0] * np.sin(theta1[i]) * theta1dot[i]**2 + seg_len[1] * np.sin(theta1[i] + theta2[i]) * (theta1dot[i] + theta2dot[i])**2 + seg_len[2] * np.sin(theta1[i] + theta2[i] + theta3[i]) * (theta1dot[i] + theta2dot[i] + theta3dot[i])**2,
-seg_len[1] * np.cos(theta1[i] + theta2[i]) * (theta1dotdot[i] + theta2dotdot[i]) - seg_len[2] * np.cos(theta1[i] + theta2[i] + theta3[i]) * (theta1dotdot[i] + theta2dotdot[i] + theta3dotdot[i]) + seg_len[1] * np.sin(theta1[i] + theta2[i]) * (theta1dot[i] + theta2dot[i])**2 + seg_len[2] * np.sin(theta1[i] + theta2[i] + theta3[i]) * (theta1dot[i] + theta2dot[i] + theta3dot[i])**2,
-seg_len[2] * np.cos(theta1[i] + theta2[i] + theta3[i]) * (theta1dotdot[i] + theta2dotdot[i] + theta3dotdot[i]) + seg_len[2] * np.sin(theta1[i] + theta2[i] + theta3[i]) * (theta1dot[i] + theta2dot[i] + theta3dot[i])**2],
[-seg_len[0] * np.sin(theta1[i]) * theta1dotdot[i] - seg_len[1] * np.sin(theta1[i] + theta2[i]) * (theta1dotdot[i] + theta2dotdot[i]) - seg_len[2] * np.sin(theta1[i] + theta2[i] + theta3[i]) * (theta1dotdot[i] + theta2dotdot[i] + theta3dotdot[i]) - seg_len[0] * np.cos(theta1[i]) * theta1dot[i]**2 - seg_len[1] * np.cos(theta1[i] + theta2[i]) * (theta1dot[i] + theta2dot[i])**2 - seg_len[2] * np.cos(theta1[i] + theta2[i] + theta3[i]) * (theta1dot[i] + theta2dot[i] + theta3dot[i])**2,
-seg_len[1] * np.sin(theta1[i] + theta2[i]) * (theta1dotdot[i] + theta2dotdot[i]) - seg_len[2] * np.sin(theta1[i] + theta2[i] + theta3[i]) * (theta1dotdot[i] + theta2dotdot[i] + theta3dotdot[i]) - seg_len[1] * np.cos(theta1[i] + theta2[i]) * (theta1dot[i] + theta2dot[i])**2 - seg_len[2] * np.cos(theta1[i] + theta2[i] + theta3[i]) * (theta1dot[i] + theta2dot[i] + theta3dot[i])**2,
-seg_len[2] * np.sin(theta1[i] + theta2[i] + theta3[i]) * (theta1dotdot[i] + theta2dotdot[i] + theta3dotdot[i]) - seg_len[2] * np.cos(theta1[i] + theta2[i] + theta3[i]) * (theta1dot[i] + theta2dot[i] + theta3dot[i])**2]
])
# Print Jacobians
print("\n\nJacobians + derivatives ...")
print("\nJacobian\n", J)
print("\nJacobian first derivative\n", DJdt)
print("\nJacobian second derivative\n", D2Jdt2)
return J, DJdt, D2Jdt2
def calculate_jerk_components(thetas, thetas_d, thetas_dd, thetas_ddd, J, DJdt, D2Jdt2, plot=True):
"""
Calculate jerk acceleration components and their norms.
Parameters:
thetas (numpy.ndarray): Array of joint angles.
thetas_d (numpy.ndarray): Array of first derivatives of joint angles.
thetas_dd (numpy.ndarray): Array of second derivatives of joint angles.
thetas_ddd (numpy.ndarray): Array of third derivatives of joint angles.
J (numpy.ndarray): Jacobian matrices.
DJdt (numpy.ndarray): First derivatives of Jacobians.
D2Jdt2 (numpy.ndarray): Second derivatives of Jacobians.
plot (bool): Whether to plot the jerk components or not.
Returns:
list: List containing the norms of jerk components.
"""
theta1, _, _ = thetas
theta1dot, theta2dot, theta3dot = thetas_d
theta1dotdot, theta2dotdot, theta3dotdot = thetas_dd
theta1dotdotdot, theta2dotdotdot, theta3dotdotdot = thetas_ddd
num_samples = len(theta1)
# Jerk acceleration components
G3 = np.zeros((2, num_samples))
G2 = np.zeros((2, num_samples))
G1 = np.zeros((2, num_samples))
GT = np.zeros((2, num_samples))
GTsq = np.zeros(num_samples)
G1sq = np.zeros(num_samples)
G2sq = np.zeros(num_samples)
G3sq = np.zeros(num_samples)
G4sq = np.zeros(num_samples)
for i in range(num_samples):
G3[:, i] = J[:, :, i] @ np.array([theta1dotdotdot[i], theta2dotdotdot[i], theta3dotdotdot[i]])
G2[:, i] = 2 * DJdt[:, :, i] @ np.array([theta1dotdot[i], theta2dotdot[i], theta3dotdot[i]])
G1[:, i] = D2Jdt2[:, :, i] @ np.array([theta1dot[i], theta2dot[i], theta3dot[i]])
GT[:, i] = G1[:, i] + G2[:, i] + G3[:, i]
GTsq[i] = GT[:, i].T @ GT[:, i]
G1sq[i] = G1[:, i].T @ G1[:, i]
G2sq[i] = G2[:, i].T @ G2[:, i]
G3sq[i] = G3[:, i].T @ G3[:, i]
G4sq[i] = 2 * (G1[:, i].T @ G2[:, i] + G1[:, i].T @ G3[:, i] + G2[:, i].T @ G3[:, i])
# Compute the percentage of each jerk component
IntG1 = 100 * np.sum(G1sq[100:-100]) / np.sum(GTsq[100:-100])
IntG2 = 100 * np.sum(G2sq[100:-100]) / np.sum(GTsq[100:-100])
IntG3 = 100 * np.sum(G3sq[100:-100]) / np.sum(GTsq[100:-100])
IntG4 = 100 * np.sum(G4sq[100:-100]) / np.sum(GTsq[100:-100])
AllNorms = [IntG1, IntG2, IntG3, IntG4]
# Print Jacobians
print("\n\nPercentage of each jerk component...")
print("G1 percentage:\t", IntG1)
print("G2 percentage:\t", IntG2)
print("G3 percentage:\t", IntG3)
print("G4 percentage:\t", IntG4)
# Plot
if plot == True:
plt.figure()
plt.plot(G1[0, :], 'b', label='G1-x')
plt.plot(G1[1, :], 'b--', label='G1-y')
plt.plot(G2[0, :], 'r', label='G2-x')
plt.plot(G2[1, :], 'r--', label='G2-y')
plt.plot(G3[0, :], 'k', label='G3-x')
plt.plot(G3[1, :], 'k--', label='G3-y')
plt.xlabel('Time Step')
plt.ylabel('Acceleration')
plt.title('Jerk Components')
plt.legend()
plt.show()
return AllNorms
main()