example-robot-data-loaders 6.0.0__2-py313-none-any.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- cmeel.prefix/lib/python3.13/site-packages/example_robot_data/__init__.py +2 -0
- cmeel.prefix/lib/python3.13/site-packages/example_robot_data/__main__.py +15 -0
- cmeel.prefix/lib/python3.13/site-packages/example_robot_data/human.py +1327 -0
- cmeel.prefix/lib/python3.13/site-packages/example_robot_data/panda.py +70 -0
- cmeel.prefix/lib/python3.13/site-packages/example_robot_data/path.py +2 -0
- cmeel.prefix/lib/python3.13/site-packages/example_robot_data/robots_loader.py +526 -0
- cmeel.prefix/lib/python3.13/site-packages/example_robot_data/talos.py +99 -0
- cmeel.prefix/lib/python3.13/site-packages/example_robot_data/utils.py +174 -0
- cmeel.prefix/share/example-robot-data/hook/python_path.dsv +1 -0
- example_robot_data_loaders-6.0.0.dist-info/METADATA +87 -0
- example_robot_data_loaders-6.0.0.dist-info/RECORD +14 -0
- example_robot_data_loaders-6.0.0.dist-info/WHEEL +6 -0
- example_robot_data_loaders-6.0.0.dist-info/licenses/LICENSE +26 -0
- example_robot_data_loaders-6.0.0.dist-info/top_level.txt +0 -0
|
@@ -0,0 +1,1327 @@
|
|
|
1
|
+
import numpy as np
|
|
2
|
+
import pinocchio as pin
|
|
3
|
+
|
|
4
|
+
from .utils import RobotLoader
|
|
5
|
+
|
|
6
|
+
|
|
7
|
+
class HumanLoader(RobotLoader):
|
|
8
|
+
path = "human_description"
|
|
9
|
+
urdf_filename = "human.urdf"
|
|
10
|
+
free_flyer = True
|
|
11
|
+
ref_posture = "anatomical"
|
|
12
|
+
# Enforced, unchangeable free-flyer orientation (90° about X, and Y↔Z swap)
|
|
13
|
+
freeflyer_ori: np.ndarray = np.array(
|
|
14
|
+
[
|
|
15
|
+
[1, 0, 0],
|
|
16
|
+
[0, 0, -1],
|
|
17
|
+
[0, 1, 0],
|
|
18
|
+
]
|
|
19
|
+
)
|
|
20
|
+
|
|
21
|
+
def __init__(
|
|
22
|
+
self,
|
|
23
|
+
height: float | None = None,
|
|
24
|
+
weight: float | None = None,
|
|
25
|
+
gender: str = "m",
|
|
26
|
+
verbose: bool = False,
|
|
27
|
+
):
|
|
28
|
+
"""
|
|
29
|
+
Initialize the robot loader with optional scaling and configuration parameters. The scaling of the model is based on Dumas 2007, except for the
|
|
30
|
+
abdomen and thorax segments, which are scaled based de Leva 1996.
|
|
31
|
+
Parameters:
|
|
32
|
+
height (float, optional): The height to scale the robot model to. If None, uses the default height in the urdf of 1.80m.
|
|
33
|
+
weight (float, optional): The weight to scale the robot model to. If None, uses the default weight in the urdf 75kg.
|
|
34
|
+
gender (str, optional): The gender specification for scaling, default is 'm'.
|
|
35
|
+
verbose (bool, optional): If True, enables verbose output. Default is False.
|
|
36
|
+
Behavior:
|
|
37
|
+
- Calls the base loader's initializer.
|
|
38
|
+
- If height or weight is provided, scales the robot model accordingly.
|
|
39
|
+
- The free-flyer is present, it enforces its orientation and reapplies joint limits.
|
|
40
|
+
"""
|
|
41
|
+
|
|
42
|
+
# call base loader
|
|
43
|
+
super().__init__(verbose=verbose)
|
|
44
|
+
|
|
45
|
+
if height is not None or weight is not None:
|
|
46
|
+
self._scale_pin_model(
|
|
47
|
+
self.robot.model, self.robot.visual_model, height, weight, gender
|
|
48
|
+
)
|
|
49
|
+
|
|
50
|
+
# automatically apply the enforced free-flyer orientation
|
|
51
|
+
if self.free_flyer:
|
|
52
|
+
# get joint index
|
|
53
|
+
j_id = self.robot.model.getJointId("root_joint")
|
|
54
|
+
# assign enforced rotation
|
|
55
|
+
self.robot.model.jointPlacements[j_id].rotation = self.freeflyer_ori
|
|
56
|
+
# re-apply limits
|
|
57
|
+
self.addFreeFlyerJointLimits()
|
|
58
|
+
|
|
59
|
+
@staticmethod
|
|
60
|
+
def get_dict_inertial_param(
|
|
61
|
+
height: float, weight: float, gender: str = "m"
|
|
62
|
+
) -> dict:
|
|
63
|
+
inertial_segment_names = [
|
|
64
|
+
"middle_pelvis",
|
|
65
|
+
"left_upperleg",
|
|
66
|
+
"left_lowerleg",
|
|
67
|
+
"left_foot",
|
|
68
|
+
"middle_abdomen",
|
|
69
|
+
"middle_thorax",
|
|
70
|
+
"middle_head",
|
|
71
|
+
"left_upperarm",
|
|
72
|
+
"left_lowerarm",
|
|
73
|
+
"left_hand",
|
|
74
|
+
"right_upperarm",
|
|
75
|
+
"right_lowerarm",
|
|
76
|
+
"right_hand",
|
|
77
|
+
"right_upperleg",
|
|
78
|
+
"right_lowerleg",
|
|
79
|
+
"right_foot",
|
|
80
|
+
]
|
|
81
|
+
|
|
82
|
+
sgt_lengths = {}
|
|
83
|
+
|
|
84
|
+
dicts = []
|
|
85
|
+
|
|
86
|
+
# Ratios from Dumas 2007 and De Leva 1996 for abdomen and thorax
|
|
87
|
+
for name in inertial_segment_names:
|
|
88
|
+
dict_sgmt = {}
|
|
89
|
+
if name == "middle_pelvis":
|
|
90
|
+
length = (0.0634 if gender == "f" else 0.0505) * height
|
|
91
|
+
sgt_lengths["middle_pelvis_0"] = length
|
|
92
|
+
# Inter-ASIS distance as a fraction of stature, measured with
|
|
93
|
+
# calipers on 100 adults by Shankar et al.: 0.1368 +/- 0.0117
|
|
94
|
+
# for women and 0.1286 +/- 0.0075 for men. The values used
|
|
95
|
+
# before had the sexes the other way round, which left men with
|
|
96
|
+
# a narrower pelvis than women of the same stature.
|
|
97
|
+
sgt_lengths["middle_pelvis_0_width"] = (
|
|
98
|
+
0.1368 if gender == "f" else 0.1286
|
|
99
|
+
) * height
|
|
100
|
+
dict_sgmt["mass"] = np.round(
|
|
101
|
+
(0.146 if gender == "f" else 0.142) * weight, 2
|
|
102
|
+
)
|
|
103
|
+
dict_sgmt["com"] = np.round(
|
|
104
|
+
(
|
|
105
|
+
np.array([-0.009, -0.232, 0.002])
|
|
106
|
+
if gender == "f"
|
|
107
|
+
else np.array([0.028, -0.28, -0.006])
|
|
108
|
+
)
|
|
109
|
+
* length,
|
|
110
|
+
3,
|
|
111
|
+
)
|
|
112
|
+
dict_sgmt["inertia"] = np.round(
|
|
113
|
+
(
|
|
114
|
+
np.array(
|
|
115
|
+
[
|
|
116
|
+
(length * 0.91) ** 2,
|
|
117
|
+
-((length * 0.34) ** 2),
|
|
118
|
+
-((length * 0.01) ** 2),
|
|
119
|
+
(length * 1) ** 2,
|
|
120
|
+
-((length * 0.01) ** 2),
|
|
121
|
+
(length * 0.79) ** 2,
|
|
122
|
+
]
|
|
123
|
+
)
|
|
124
|
+
if gender == "f"
|
|
125
|
+
else np.array(
|
|
126
|
+
[
|
|
127
|
+
(length * 1.01) ** 2,
|
|
128
|
+
-((length * 0.25) ** 2),
|
|
129
|
+
-((length * 0.12) ** 2),
|
|
130
|
+
(length * 1.06) ** 2,
|
|
131
|
+
-((length * 0.08) ** 2),
|
|
132
|
+
(length * 0.95) ** 2,
|
|
133
|
+
]
|
|
134
|
+
)
|
|
135
|
+
)
|
|
136
|
+
* dict_sgmt["mass"],
|
|
137
|
+
5,
|
|
138
|
+
)
|
|
139
|
+
dicts.append(dict_sgmt)
|
|
140
|
+
elif name == "left_upperleg":
|
|
141
|
+
length = (0.2354 if gender == "f" else 0.2441) * height
|
|
142
|
+
sgt_lengths["left_upperleg_0"] = length
|
|
143
|
+
dict_sgmt["mass"] = np.round(
|
|
144
|
+
(0.146 if gender == "f" else 0.123) * weight, 2
|
|
145
|
+
)
|
|
146
|
+
dict_sgmt["com"] = np.round(
|
|
147
|
+
(
|
|
148
|
+
np.array([-0.077, -0.377, 0.009])
|
|
149
|
+
if gender == "f"
|
|
150
|
+
else np.array([-0.041, -0.429, 0.033])
|
|
151
|
+
)
|
|
152
|
+
* length,
|
|
153
|
+
3,
|
|
154
|
+
)
|
|
155
|
+
dict_sgmt["inertia"] = np.round(
|
|
156
|
+
(
|
|
157
|
+
np.array(
|
|
158
|
+
[
|
|
159
|
+
(length * 0.31) ** 2,
|
|
160
|
+
(length * 0.07) ** 2,
|
|
161
|
+
-((length * 0.02) ** 2),
|
|
162
|
+
(length * 0.19) ** 2,
|
|
163
|
+
-((length * 0.07) ** 2),
|
|
164
|
+
(length * 0.32) ** 2,
|
|
165
|
+
]
|
|
166
|
+
)
|
|
167
|
+
if gender == "f"
|
|
168
|
+
else np.array(
|
|
169
|
+
[
|
|
170
|
+
(length * 0.29) ** 2,
|
|
171
|
+
(length * 0.07) ** 2,
|
|
172
|
+
-((length * 0.02) ** 2),
|
|
173
|
+
(length * 0.15) ** 2,
|
|
174
|
+
-((length * 0.07) ** 2),
|
|
175
|
+
(length * 0.3) ** 2,
|
|
176
|
+
]
|
|
177
|
+
)
|
|
178
|
+
)
|
|
179
|
+
* dict_sgmt["mass"],
|
|
180
|
+
5,
|
|
181
|
+
)
|
|
182
|
+
dicts.append(dict_sgmt)
|
|
183
|
+
elif name == "left_lowerleg":
|
|
184
|
+
length = (0.2410 if gender == "f" else 0.2446) * height
|
|
185
|
+
sgt_lengths["left_lowerleg_0"] = length
|
|
186
|
+
dict_sgmt["mass"] = np.round(
|
|
187
|
+
(0.045 if gender == "f" else 0.048) * weight, 2
|
|
188
|
+
)
|
|
189
|
+
dict_sgmt["com"] = np.round(
|
|
190
|
+
(
|
|
191
|
+
np.array([-0.049, -0.404, 0.031])
|
|
192
|
+
if gender == "f"
|
|
193
|
+
else np.array([-0.048, -0.41, 0.007])
|
|
194
|
+
)
|
|
195
|
+
* length,
|
|
196
|
+
3,
|
|
197
|
+
)
|
|
198
|
+
dict_sgmt["inertia"] = np.round(
|
|
199
|
+
(
|
|
200
|
+
np.array(
|
|
201
|
+
[
|
|
202
|
+
(length * 0.28) ** 2,
|
|
203
|
+
(length * 0.02) ** 2,
|
|
204
|
+
(length * 0.01) ** 2,
|
|
205
|
+
(length * 0.1) ** 2,
|
|
206
|
+
(length * 0.06) ** 2,
|
|
207
|
+
(length * 0.28) ** 2,
|
|
208
|
+
]
|
|
209
|
+
)
|
|
210
|
+
if gender == "f"
|
|
211
|
+
else np.array(
|
|
212
|
+
[
|
|
213
|
+
(length * 0.28) ** 2,
|
|
214
|
+
(length * 0.04) ** 2,
|
|
215
|
+
(length * 0.02) ** 2,
|
|
216
|
+
(length * 0.1) ** 2,
|
|
217
|
+
(length * 0.05) ** 2,
|
|
218
|
+
(length * 0.28) ** 2,
|
|
219
|
+
]
|
|
220
|
+
)
|
|
221
|
+
)
|
|
222
|
+
* dict_sgmt["mass"],
|
|
223
|
+
5,
|
|
224
|
+
)
|
|
225
|
+
dicts.append(dict_sgmt)
|
|
226
|
+
elif name == "left_foot":
|
|
227
|
+
length = (0.1447 if gender == "f" else 0.1497) * height
|
|
228
|
+
sgt_lengths["left_foot_0"] = length
|
|
229
|
+
dict_sgmt["mass"] = np.round(
|
|
230
|
+
(0.01 if gender == "f" else 0.012) * weight, 2
|
|
231
|
+
)
|
|
232
|
+
dict_sgmt["com"] = np.round(
|
|
233
|
+
(
|
|
234
|
+
np.array([0.27, -0.218, 0.039])
|
|
235
|
+
if gender == "f"
|
|
236
|
+
else np.array([0.382, -0.151, 0.026])
|
|
237
|
+
)
|
|
238
|
+
* length,
|
|
239
|
+
3,
|
|
240
|
+
)
|
|
241
|
+
dict_sgmt["inertia"] = np.round(
|
|
242
|
+
(
|
|
243
|
+
np.array(
|
|
244
|
+
[
|
|
245
|
+
(length * 0.17) ** 2,
|
|
246
|
+
-((length * 0.10) ** 2),
|
|
247
|
+
(length * 0.06) ** 2,
|
|
248
|
+
(length * 0.36) ** 2,
|
|
249
|
+
-((length * 0.04) ** 2),
|
|
250
|
+
(length * 0.35) ** 2,
|
|
251
|
+
]
|
|
252
|
+
)
|
|
253
|
+
if gender == "f"
|
|
254
|
+
else np.array(
|
|
255
|
+
[
|
|
256
|
+
(length * 0.17) ** 2,
|
|
257
|
+
(length * 0.13) ** 2,
|
|
258
|
+
-((length * 0.08) ** 2),
|
|
259
|
+
(length * 0.37) ** 2,
|
|
260
|
+
(length * 0) ** 2,
|
|
261
|
+
(length * 0.36) ** 2,
|
|
262
|
+
]
|
|
263
|
+
)
|
|
264
|
+
)
|
|
265
|
+
* dict_sgmt["mass"],
|
|
266
|
+
5,
|
|
267
|
+
)
|
|
268
|
+
dicts.append(dict_sgmt)
|
|
269
|
+
elif name == "middle_abdomen":
|
|
270
|
+
# Abdomen spans L5/S1 to T12/L1 and the thorax T12/L1 to C7/T1,
|
|
271
|
+
# following the intersegmental centres of Dumas et al. The split
|
|
272
|
+
# between them was previously taken from de Leva, whose trunk
|
|
273
|
+
# sub-segments are bounded differently (omphalion, xiphion), so
|
|
274
|
+
# it placed the thoracolumbar junction too high. These ratios
|
|
275
|
+
# match the ones used to scale the same model from motion
|
|
276
|
+
# capture, so a marker-based calibration no longer has to move
|
|
277
|
+
# the joint after the model is built. The trunk total, L5/S1 to
|
|
278
|
+
# C7/T1, is unchanged at 0.2497 H (f) and 0.2627 H (m).
|
|
279
|
+
length = (0.0776 if gender == "f" else 0.0839) * height
|
|
280
|
+
sgt_lengths["middle_abdomen_0"] = length
|
|
281
|
+
dict_sgmt["mass"] = np.round(
|
|
282
|
+
(0.1465 if gender == "f" else 0.1633) * weight, 2
|
|
283
|
+
)
|
|
284
|
+
dict_sgmt["com"] = np.round(
|
|
285
|
+
(
|
|
286
|
+
np.array([-0.016, -0.4512, -0.006])
|
|
287
|
+
if gender == "f"
|
|
288
|
+
else np.array([-0.036, 0.4502, -0.002])
|
|
289
|
+
)
|
|
290
|
+
* length,
|
|
291
|
+
3,
|
|
292
|
+
)
|
|
293
|
+
dict_sgmt["inertia"] = np.round(
|
|
294
|
+
(
|
|
295
|
+
np.array(
|
|
296
|
+
[
|
|
297
|
+
(length * 0.29) ** 2,
|
|
298
|
+
(length * 0.22) ** 2,
|
|
299
|
+
(length * 0.05) ** 2,
|
|
300
|
+
(length * 0.27) ** 2,
|
|
301
|
+
-((length * 0.05) ** 2),
|
|
302
|
+
(length * 0.29) ** 2,
|
|
303
|
+
]
|
|
304
|
+
)
|
|
305
|
+
if gender == "f"
|
|
306
|
+
else np.array(
|
|
307
|
+
[
|
|
308
|
+
(length * 0.27) ** 2,
|
|
309
|
+
(length * 0.18) ** 2,
|
|
310
|
+
-((length * 0.02) ** 2),
|
|
311
|
+
(length * 0.25) ** 2,
|
|
312
|
+
-((length * 0.04) ** 2),
|
|
313
|
+
(length * 0.28) ** 2,
|
|
314
|
+
]
|
|
315
|
+
)
|
|
316
|
+
)
|
|
317
|
+
* dict_sgmt["mass"],
|
|
318
|
+
5,
|
|
319
|
+
)
|
|
320
|
+
dicts.append(dict_sgmt)
|
|
321
|
+
elif name == "middle_thorax":
|
|
322
|
+
# See middle_abdomen: the trunk total is preserved, so what the
|
|
323
|
+
# abdomen loses the thorax gains.
|
|
324
|
+
length = (0.1721 if gender == "f" else 0.1788) * height
|
|
325
|
+
sgt_lengths["middle_thorax_0"] = length
|
|
326
|
+
dict_sgmt["mass"] = np.round(
|
|
327
|
+
(0.1545 if gender == "f" else 0.1596) * weight, 2
|
|
328
|
+
)
|
|
329
|
+
dict_sgmt["com"] = np.round(
|
|
330
|
+
(
|
|
331
|
+
np.array([-0.016, 0.5050, -0.006])
|
|
332
|
+
if gender == "f"
|
|
333
|
+
else np.array([-0.036, 0.5066, -0.002])
|
|
334
|
+
)
|
|
335
|
+
* length,
|
|
336
|
+
3,
|
|
337
|
+
)
|
|
338
|
+
dict_sgmt["inertia"] = np.round(
|
|
339
|
+
(
|
|
340
|
+
np.array(
|
|
341
|
+
[
|
|
342
|
+
(length * 0.29) ** 2,
|
|
343
|
+
(length * 0.22) ** 2,
|
|
344
|
+
(length * 0.05) ** 2,
|
|
345
|
+
(length * 0.27) ** 2,
|
|
346
|
+
-((length * 0.05) ** 2),
|
|
347
|
+
(length * 0.29) ** 2,
|
|
348
|
+
]
|
|
349
|
+
)
|
|
350
|
+
if gender == "f"
|
|
351
|
+
else np.array(
|
|
352
|
+
[
|
|
353
|
+
(length * 0.27) ** 2,
|
|
354
|
+
(length * 0.18) ** 2,
|
|
355
|
+
-((length * 0.02) ** 2),
|
|
356
|
+
(length * 0.25) ** 2,
|
|
357
|
+
-((length * 0.04) ** 2),
|
|
358
|
+
(length * 0.28) ** 2,
|
|
359
|
+
]
|
|
360
|
+
)
|
|
361
|
+
)
|
|
362
|
+
* dict_sgmt["mass"],
|
|
363
|
+
5,
|
|
364
|
+
)
|
|
365
|
+
dicts.append(dict_sgmt)
|
|
366
|
+
elif name == "middle_head":
|
|
367
|
+
length = (0.1308 if gender == "f" else 0.1310) * height
|
|
368
|
+
sgt_lengths["middle_head_0"] = length
|
|
369
|
+
dict_sgmt["mass"] = np.round(
|
|
370
|
+
0.067 * weight, 2
|
|
371
|
+
) # same for male and female
|
|
372
|
+
dict_sgmt["com"] = np.round(
|
|
373
|
+
(
|
|
374
|
+
np.array([-0.07, 0.597, 0])
|
|
375
|
+
if gender == "f"
|
|
376
|
+
else np.array([-0.062, 0.555, 0.001])
|
|
377
|
+
)
|
|
378
|
+
* length,
|
|
379
|
+
3,
|
|
380
|
+
)
|
|
381
|
+
dict_sgmt["inertia"] = np.round(
|
|
382
|
+
(
|
|
383
|
+
np.array(
|
|
384
|
+
[
|
|
385
|
+
(length * 0.32) ** 2,
|
|
386
|
+
-((length * 0.06) ** 2),
|
|
387
|
+
(length * 0.01) ** 2,
|
|
388
|
+
(length * 0.27) ** 2,
|
|
389
|
+
-((length * 0.01) ** 2),
|
|
390
|
+
(length * 0.34) ** 2,
|
|
391
|
+
]
|
|
392
|
+
)
|
|
393
|
+
if gender == "f"
|
|
394
|
+
else np.array(
|
|
395
|
+
[
|
|
396
|
+
(length * 0.31) ** 2,
|
|
397
|
+
-((length * 0.09) ** 2),
|
|
398
|
+
-((length * 0.02) ** 2),
|
|
399
|
+
(length * 0.25) ** 2,
|
|
400
|
+
(length * 0.03) ** 2,
|
|
401
|
+
(length * 0.33) ** 2,
|
|
402
|
+
]
|
|
403
|
+
)
|
|
404
|
+
)
|
|
405
|
+
* dict_sgmt["mass"],
|
|
406
|
+
5,
|
|
407
|
+
)
|
|
408
|
+
dicts.append(dict_sgmt)
|
|
409
|
+
elif name == "left_upperarm":
|
|
410
|
+
length = (0.1510 if gender == "f" else 0.1531) * height
|
|
411
|
+
sgt_lengths["left_upperarm_0"] = length
|
|
412
|
+
dict_sgmt["mass"] = np.round(
|
|
413
|
+
(0.022 if gender == "f" else 0.024) * weight, 2
|
|
414
|
+
)
|
|
415
|
+
dict_sgmt["com"] = np.round(
|
|
416
|
+
(
|
|
417
|
+
np.array([-0.073, -0.454, -0.028])
|
|
418
|
+
if gender == "f"
|
|
419
|
+
else np.array([0.017, -0.452, -0.026])
|
|
420
|
+
)
|
|
421
|
+
* length,
|
|
422
|
+
3,
|
|
423
|
+
)
|
|
424
|
+
dict_sgmt["inertia"] = np.round(
|
|
425
|
+
(
|
|
426
|
+
np.array(
|
|
427
|
+
[
|
|
428
|
+
(length * 0.33) ** 2,
|
|
429
|
+
(length * 0.03) ** 2,
|
|
430
|
+
-((length * 0.05) ** 2),
|
|
431
|
+
(length * 0.17) ** 2,
|
|
432
|
+
-((length * 0.14) ** 2),
|
|
433
|
+
(length * 0.33) ** 2,
|
|
434
|
+
]
|
|
435
|
+
)
|
|
436
|
+
if gender == "f"
|
|
437
|
+
else np.array(
|
|
438
|
+
[
|
|
439
|
+
(length * 0.31) ** 2,
|
|
440
|
+
(length * 0.06) ** 2,
|
|
441
|
+
(length * 0.05) ** 2,
|
|
442
|
+
(length * 0.14) ** 2,
|
|
443
|
+
(length * 0.02) ** 2,
|
|
444
|
+
(length * 0.32) ** 2,
|
|
445
|
+
]
|
|
446
|
+
)
|
|
447
|
+
)
|
|
448
|
+
* dict_sgmt["mass"],
|
|
449
|
+
5,
|
|
450
|
+
)
|
|
451
|
+
dicts.append(dict_sgmt)
|
|
452
|
+
elif name == "left_lowerarm":
|
|
453
|
+
length = (0.1534 if gender == "f" else 0.1593) * height
|
|
454
|
+
sgt_lengths["left_lowerarm_0"] = length
|
|
455
|
+
dict_sgmt["mass"] = np.round(
|
|
456
|
+
(0.013 if gender == "f" else 0.017) * weight, 2
|
|
457
|
+
)
|
|
458
|
+
dict_sgmt["com"] = np.round(
|
|
459
|
+
(
|
|
460
|
+
np.array([0.021, -0.411, 0.019])
|
|
461
|
+
if gender == "f"
|
|
462
|
+
else np.array([0.01, -0.417, 0.014])
|
|
463
|
+
)
|
|
464
|
+
* length,
|
|
465
|
+
3,
|
|
466
|
+
)
|
|
467
|
+
dict_sgmt["inertia"] = np.round(
|
|
468
|
+
(
|
|
469
|
+
np.array(
|
|
470
|
+
[
|
|
471
|
+
(length * 0.26) ** 2,
|
|
472
|
+
(length * 0.1) ** 2,
|
|
473
|
+
(length * 0.04) ** 2,
|
|
474
|
+
(length * 0.14) ** 2,
|
|
475
|
+
-((length * 0.13) ** 2),
|
|
476
|
+
(length * 0.25) ** 2,
|
|
477
|
+
]
|
|
478
|
+
)
|
|
479
|
+
if gender == "f"
|
|
480
|
+
else np.array(
|
|
481
|
+
[
|
|
482
|
+
(length * 0.28) ** 2,
|
|
483
|
+
(length * 0.03) ** 2,
|
|
484
|
+
(length * 0.02) ** 2,
|
|
485
|
+
(length * 0.11) ** 2,
|
|
486
|
+
-((length * 0.08) ** 2),
|
|
487
|
+
(length * 0.27) ** 2,
|
|
488
|
+
]
|
|
489
|
+
)
|
|
490
|
+
)
|
|
491
|
+
* dict_sgmt["mass"],
|
|
492
|
+
5,
|
|
493
|
+
)
|
|
494
|
+
dicts.append(dict_sgmt)
|
|
495
|
+
elif name == "left_hand":
|
|
496
|
+
length = (0.0989 if gender == "f" else 0.1014) * height
|
|
497
|
+
sgt_lengths["left_hand_0"] = length
|
|
498
|
+
dict_sgmt["mass"] = np.round(
|
|
499
|
+
(0.005 if gender == "f" else 0.006) * weight, 2
|
|
500
|
+
)
|
|
501
|
+
dict_sgmt["com"] = np.round(
|
|
502
|
+
(
|
|
503
|
+
np.array([0.077, -0.768, 0.048])
|
|
504
|
+
if gender == "f"
|
|
505
|
+
else np.array([0.082, -0.839, 0.074])
|
|
506
|
+
)
|
|
507
|
+
* length,
|
|
508
|
+
3,
|
|
509
|
+
)
|
|
510
|
+
dict_sgmt["inertia"] = np.round(
|
|
511
|
+
(
|
|
512
|
+
np.array(
|
|
513
|
+
[
|
|
514
|
+
(length * 0.63) ** 2,
|
|
515
|
+
(length * 0.29) ** 2,
|
|
516
|
+
(length * 0.23) ** 2,
|
|
517
|
+
(length * 0.43) ** 2,
|
|
518
|
+
-((length * 0.28) ** 2),
|
|
519
|
+
(length * 0.58) ** 2,
|
|
520
|
+
]
|
|
521
|
+
)
|
|
522
|
+
if gender == "f"
|
|
523
|
+
else np.array(
|
|
524
|
+
[
|
|
525
|
+
(length * 0.61) ** 2,
|
|
526
|
+
(length * 0.22) ** 2,
|
|
527
|
+
(length * 0.15) ** 2,
|
|
528
|
+
(length * 0.38) ** 2,
|
|
529
|
+
-((length * 0.2) ** 2),
|
|
530
|
+
(length * 0.56) ** 2,
|
|
531
|
+
]
|
|
532
|
+
)
|
|
533
|
+
)
|
|
534
|
+
* dict_sgmt["mass"],
|
|
535
|
+
5,
|
|
536
|
+
)
|
|
537
|
+
dicts.append(dict_sgmt)
|
|
538
|
+
elif name == "right_upperarm":
|
|
539
|
+
length = (0.1510 if gender == "f" else 0.1531) * height
|
|
540
|
+
sgt_lengths["right_upperarm_0"] = length
|
|
541
|
+
dict_sgmt["mass"] = np.round(
|
|
542
|
+
(0.022 if gender == "f" else 0.024) * weight, 2
|
|
543
|
+
)
|
|
544
|
+
dict_sgmt["com"] = np.round(
|
|
545
|
+
(
|
|
546
|
+
np.array([-0.073, -0.454, -0.028])
|
|
547
|
+
if gender == "f"
|
|
548
|
+
else np.array([0.017, -0.452, -0.026])
|
|
549
|
+
)
|
|
550
|
+
* length,
|
|
551
|
+
3,
|
|
552
|
+
)
|
|
553
|
+
dict_sgmt["inertia"] = np.round(
|
|
554
|
+
(
|
|
555
|
+
np.array(
|
|
556
|
+
[
|
|
557
|
+
(length * 0.33) ** 2,
|
|
558
|
+
(length * 0.03) ** 2,
|
|
559
|
+
-((length * 0.05) ** 2),
|
|
560
|
+
(length * 0.17) ** 2,
|
|
561
|
+
-((length * 0.14) ** 2),
|
|
562
|
+
(length * 0.33) ** 2,
|
|
563
|
+
]
|
|
564
|
+
)
|
|
565
|
+
if gender == "f"
|
|
566
|
+
else np.array(
|
|
567
|
+
[
|
|
568
|
+
(length * 0.31) ** 2,
|
|
569
|
+
(length * 0.06) ** 2,
|
|
570
|
+
(length * 0.05) ** 2,
|
|
571
|
+
(length * 0.14) ** 2,
|
|
572
|
+
(length * 0.02) ** 2,
|
|
573
|
+
(length * 0.32) ** 2,
|
|
574
|
+
]
|
|
575
|
+
)
|
|
576
|
+
)
|
|
577
|
+
* dict_sgmt["mass"],
|
|
578
|
+
5,
|
|
579
|
+
)
|
|
580
|
+
dicts.append(dict_sgmt)
|
|
581
|
+
elif name == "right_lowerarm":
|
|
582
|
+
length = (0.1534 if gender == "f" else 0.1593) * height
|
|
583
|
+
sgt_lengths["right_lowerarm_0"] = length
|
|
584
|
+
dict_sgmt["mass"] = np.round(
|
|
585
|
+
(0.013 if gender == "f" else 0.017) * weight, 2
|
|
586
|
+
)
|
|
587
|
+
dict_sgmt["com"] = np.round(
|
|
588
|
+
(
|
|
589
|
+
np.array([0.021, -0.411, 0.019])
|
|
590
|
+
if gender == "f"
|
|
591
|
+
else np.array([0.01, -0.417, 0.014])
|
|
592
|
+
)
|
|
593
|
+
* length,
|
|
594
|
+
3,
|
|
595
|
+
)
|
|
596
|
+
dict_sgmt["inertia"] = np.round(
|
|
597
|
+
(
|
|
598
|
+
np.array(
|
|
599
|
+
[
|
|
600
|
+
(length * 0.26) ** 2,
|
|
601
|
+
(length * 0.1) ** 2,
|
|
602
|
+
(length * 0.04) ** 2,
|
|
603
|
+
(length * 0.14) ** 2,
|
|
604
|
+
-((length * 0.13) ** 2),
|
|
605
|
+
(length * 0.25) ** 2,
|
|
606
|
+
]
|
|
607
|
+
)
|
|
608
|
+
if gender == "f"
|
|
609
|
+
else np.array(
|
|
610
|
+
[
|
|
611
|
+
(length * 0.28) ** 2,
|
|
612
|
+
(length * 0.03) ** 2,
|
|
613
|
+
(length * 0.02) ** 2,
|
|
614
|
+
(length * 0.11) ** 2,
|
|
615
|
+
-((length * 0.08) ** 2),
|
|
616
|
+
(length * 0.27) ** 2,
|
|
617
|
+
]
|
|
618
|
+
)
|
|
619
|
+
)
|
|
620
|
+
* dict_sgmt["mass"],
|
|
621
|
+
5,
|
|
622
|
+
)
|
|
623
|
+
dicts.append(dict_sgmt)
|
|
624
|
+
elif name == "right_hand":
|
|
625
|
+
length = (0.0989 if gender == "f" else 0.1014) * height
|
|
626
|
+
sgt_lengths["right_hand_0"] = length
|
|
627
|
+
dict_sgmt["mass"] = np.round(
|
|
628
|
+
(0.005 if gender == "f" else 0.006) * weight, 2
|
|
629
|
+
)
|
|
630
|
+
dict_sgmt["com"] = np.round(
|
|
631
|
+
(
|
|
632
|
+
np.array([0.077, -0.768, 0.048])
|
|
633
|
+
if gender == "f"
|
|
634
|
+
else np.array([0.082, -0.839, 0.074])
|
|
635
|
+
)
|
|
636
|
+
* length,
|
|
637
|
+
3,
|
|
638
|
+
)
|
|
639
|
+
dict_sgmt["inertia"] = np.round(
|
|
640
|
+
(
|
|
641
|
+
np.array(
|
|
642
|
+
[
|
|
643
|
+
(length * 0.63) ** 2,
|
|
644
|
+
(length * 0.29) ** 2,
|
|
645
|
+
(length * 0.23) ** 2,
|
|
646
|
+
(length * 0.43) ** 2,
|
|
647
|
+
-((length * 0.28) ** 2),
|
|
648
|
+
(length * 0.58) ** 2,
|
|
649
|
+
]
|
|
650
|
+
)
|
|
651
|
+
if gender == "f"
|
|
652
|
+
else np.array(
|
|
653
|
+
[
|
|
654
|
+
(length * 0.61) ** 2,
|
|
655
|
+
(length * 0.22) ** 2,
|
|
656
|
+
(length * 0.15) ** 2,
|
|
657
|
+
(length * 0.38) ** 2,
|
|
658
|
+
-((length * 0.2) ** 2),
|
|
659
|
+
(length * 0.56) ** 2,
|
|
660
|
+
]
|
|
661
|
+
)
|
|
662
|
+
)
|
|
663
|
+
* dict_sgmt["mass"],
|
|
664
|
+
5,
|
|
665
|
+
)
|
|
666
|
+
dicts.append(dict_sgmt)
|
|
667
|
+
elif name == "right_upperleg":
|
|
668
|
+
length = (0.2354 if gender == "f" else 0.2441) * height
|
|
669
|
+
sgt_lengths["right_upperleg_0"] = length
|
|
670
|
+
dict_sgmt["mass"] = np.round(
|
|
671
|
+
(0.146 if gender == "f" else 0.123) * weight, 2
|
|
672
|
+
)
|
|
673
|
+
dict_sgmt["com"] = np.round(
|
|
674
|
+
(
|
|
675
|
+
np.array([-0.077, -0.377, 0.009])
|
|
676
|
+
if gender == "f"
|
|
677
|
+
else np.array([-0.041, -0.429, 0.033])
|
|
678
|
+
)
|
|
679
|
+
* length,
|
|
680
|
+
3,
|
|
681
|
+
)
|
|
682
|
+
dict_sgmt["inertia"] = np.round(
|
|
683
|
+
(
|
|
684
|
+
np.array(
|
|
685
|
+
[
|
|
686
|
+
(length * 0.31) ** 2,
|
|
687
|
+
-((length * 0.07) ** 2),
|
|
688
|
+
-((length * 0.02) ** 2),
|
|
689
|
+
(length * 0.19) ** 2,
|
|
690
|
+
-((length * 0.07) ** 2),
|
|
691
|
+
(length * 0.32) ** 2,
|
|
692
|
+
]
|
|
693
|
+
)
|
|
694
|
+
if gender == "f"
|
|
695
|
+
else np.array(
|
|
696
|
+
[
|
|
697
|
+
(length * 0.29) ** 2,
|
|
698
|
+
-((length * 0.07) ** 2),
|
|
699
|
+
-((length * 0.02) ** 2),
|
|
700
|
+
(length * 0.15) ** 2,
|
|
701
|
+
-((length * 0.07) ** 2),
|
|
702
|
+
(length * 0.3) ** 2,
|
|
703
|
+
]
|
|
704
|
+
)
|
|
705
|
+
)
|
|
706
|
+
* dict_sgmt["mass"],
|
|
707
|
+
5,
|
|
708
|
+
)
|
|
709
|
+
dicts.append(dict_sgmt)
|
|
710
|
+
elif name == "right_lowerleg":
|
|
711
|
+
length = (0.2410 if gender == "f" else 0.2446) * height
|
|
712
|
+
sgt_lengths["right_lowerleg_0"] = length
|
|
713
|
+
dict_sgmt["mass"] = np.round(
|
|
714
|
+
(0.045 if gender == "f" else 0.048) * weight, 2
|
|
715
|
+
)
|
|
716
|
+
dict_sgmt["com"] = np.round(
|
|
717
|
+
(
|
|
718
|
+
np.array([-0.049, -0.404, 0.031])
|
|
719
|
+
if gender == "f"
|
|
720
|
+
else np.array([-0.048, -0.41, 0.007])
|
|
721
|
+
)
|
|
722
|
+
* length,
|
|
723
|
+
3,
|
|
724
|
+
)
|
|
725
|
+
dict_sgmt["inertia"] = np.round(
|
|
726
|
+
(
|
|
727
|
+
np.array(
|
|
728
|
+
[
|
|
729
|
+
(length * 0.28) ** 2,
|
|
730
|
+
(length * 0.02) ** 2,
|
|
731
|
+
(length * 0.01) ** 2,
|
|
732
|
+
(length * 0.1) ** 2,
|
|
733
|
+
(length * 0.06) ** 2,
|
|
734
|
+
(length * 0.28) ** 2,
|
|
735
|
+
]
|
|
736
|
+
)
|
|
737
|
+
if gender == "f"
|
|
738
|
+
else np.array(
|
|
739
|
+
[
|
|
740
|
+
(length * 0.28) ** 2,
|
|
741
|
+
(length * 0.04) ** 2,
|
|
742
|
+
(length * 0.02) ** 2,
|
|
743
|
+
(length * 0.1) ** 2,
|
|
744
|
+
(length * 0.05) ** 2,
|
|
745
|
+
(length * 0.28) ** 2,
|
|
746
|
+
]
|
|
747
|
+
)
|
|
748
|
+
)
|
|
749
|
+
* dict_sgmt["mass"],
|
|
750
|
+
5,
|
|
751
|
+
)
|
|
752
|
+
dicts.append(dict_sgmt)
|
|
753
|
+
elif name == "right_foot":
|
|
754
|
+
length = (0.1447 if gender == "f" else 0.1497) * height
|
|
755
|
+
sgt_lengths["right_foot_0"] = length
|
|
756
|
+
dict_sgmt["mass"] = np.round(
|
|
757
|
+
(0.01 if gender == "f" else 0.012) * weight, 2
|
|
758
|
+
)
|
|
759
|
+
dict_sgmt["com"] = np.round(
|
|
760
|
+
(
|
|
761
|
+
np.array([0.27, -0.218, 0.039])
|
|
762
|
+
if gender == "f"
|
|
763
|
+
else np.array([0.382, -0.151, 0.026])
|
|
764
|
+
)
|
|
765
|
+
* length,
|
|
766
|
+
3,
|
|
767
|
+
)
|
|
768
|
+
dict_sgmt["inertia"] = np.round(
|
|
769
|
+
(
|
|
770
|
+
np.array(
|
|
771
|
+
[
|
|
772
|
+
(length * 0.17) ** 2,
|
|
773
|
+
-((length * 0.10) ** 2),
|
|
774
|
+
(length * 0.06) ** 2,
|
|
775
|
+
(length * 0.36) ** 2,
|
|
776
|
+
-((length * 0.04) ** 2),
|
|
777
|
+
(length * 0.35) ** 2,
|
|
778
|
+
]
|
|
779
|
+
)
|
|
780
|
+
if gender == "f"
|
|
781
|
+
else np.array(
|
|
782
|
+
[
|
|
783
|
+
(length * 0.17) ** 2,
|
|
784
|
+
(length * 0.13) ** 2,
|
|
785
|
+
-((length * 0.08) ** 2),
|
|
786
|
+
(length * 0.37) ** 2,
|
|
787
|
+
(length * 0) ** 2,
|
|
788
|
+
(length * 0.36) ** 2,
|
|
789
|
+
]
|
|
790
|
+
)
|
|
791
|
+
)
|
|
792
|
+
* dict_sgmt["mass"],
|
|
793
|
+
5,
|
|
794
|
+
)
|
|
795
|
+
dicts.append(dict_sgmt)
|
|
796
|
+
|
|
797
|
+
return dict(zip(inertial_segment_names, dicts)), sgt_lengths
|
|
798
|
+
|
|
799
|
+
@staticmethod
|
|
800
|
+
def get_dict_joint_placements(height: float, gender: str = "m") -> dict:
|
|
801
|
+
joint_names = [
|
|
802
|
+
"middle_thoracic_Z",
|
|
803
|
+
"middle_cervical_Z",
|
|
804
|
+
"left_clavicle_joint_X",
|
|
805
|
+
"right_clavicle_joint_X",
|
|
806
|
+
"left_shoulder_Z",
|
|
807
|
+
"left_elbow_Z",
|
|
808
|
+
"left_wrist_Z",
|
|
809
|
+
"right_shoulder_Z",
|
|
810
|
+
"right_elbow_Z",
|
|
811
|
+
"right_wrist_Z",
|
|
812
|
+
"left_hip_Z",
|
|
813
|
+
"left_knee_Z",
|
|
814
|
+
"left_ankle_Z",
|
|
815
|
+
"right_hip_Z",
|
|
816
|
+
"right_knee_Z",
|
|
817
|
+
"right_ankle_Z",
|
|
818
|
+
]
|
|
819
|
+
|
|
820
|
+
lengths_names = [
|
|
821
|
+
"L_abdomen",
|
|
822
|
+
"L_thorax_cerv",
|
|
823
|
+
"L_thorax_supr",
|
|
824
|
+
"L_upperarm",
|
|
825
|
+
"L_lowerarm",
|
|
826
|
+
"L_upperleg",
|
|
827
|
+
"L_lowerleg",
|
|
828
|
+
]
|
|
829
|
+
ratios = np.array(
|
|
830
|
+
[
|
|
831
|
+
# The trunk joints of this model are the intersegmental centres
|
|
832
|
+
# L5/S1, T12/L1 and C7/T1 (Dumas et al.), but these two lengths
|
|
833
|
+
# used to come from De Leva's trunk sub-segments, which are
|
|
834
|
+
# bounded by the xiphion and omphalion instead. Those are not
|
|
835
|
+
# the same boundaries, so the thoracolumbar joint sat too high
|
|
836
|
+
# and a marker-based rescaling had to move it back down. These
|
|
837
|
+
# ratios put it where the joint actually is; the trunk total,
|
|
838
|
+
# L5/S1 to C7/T1, is unchanged at 0.2497 H (f) / 0.2627 H (m).
|
|
839
|
+
0.0776 if gender == "f" else 0.0839, # L_abdomen, L5/S1 to T12/L1
|
|
840
|
+
0.1721 if gender == "f" else 0.1788, # L_thorax, T12/L1 to C7/T1
|
|
841
|
+
0.0821 if gender == "f" else 0.0980, # from SUPR to XYPH (De Leva 1996)
|
|
842
|
+
0.1510 if gender == "f" else 0.1531, # L_upperarm (Dumas 2007)
|
|
843
|
+
0.1534 if gender == "f" else 0.1593, # L_lowerarm (Dumas 2007)
|
|
844
|
+
0.2354 if gender == "f" else 0.2441, # L_upperleg (Dumas 2007)
|
|
845
|
+
0.2410 if gender == "f" else 0.2446, # L_lowerleg (Dumas 2007)
|
|
846
|
+
]
|
|
847
|
+
)
|
|
848
|
+
# Ratios from De Leva : HJC from Seidel
|
|
849
|
+
# SJC Schnorenberg-style AC/H = 0.132 (F) or 0.0129 (M)
|
|
850
|
+
SJ_thorax_X_r = 0.0043 if gender == "f" else 0.0046
|
|
851
|
+
SJ_thorax_Y_r = -0.0449 if gender == "f" else -0.0416
|
|
852
|
+
SJ_thorax_Z_r = 0.1108 if gender == "f" else 0.1164
|
|
853
|
+
HJ_pelvis_X_r = 0.0138 if gender == "f" else 0.0126
|
|
854
|
+
HJ_pelvis_Y_r = -0.0570 if gender == "f" else -0.0558
|
|
855
|
+
HJ_pelvis_Z_r = 0.0548 if gender == "f" else 0.0457
|
|
856
|
+
|
|
857
|
+
lengths = np.round(ratios * height, 3) # mm accuracy
|
|
858
|
+
dict_lengths = dict(zip(lengths_names, lengths))
|
|
859
|
+
|
|
860
|
+
joint_placements = []
|
|
861
|
+
for j in joint_names:
|
|
862
|
+
if j == "middle_thoracic_Z":
|
|
863
|
+
joint_placements.append(np.array([0, dict_lengths["L_abdomen"], 0]))
|
|
864
|
+
elif j == "middle_cervical_Z":
|
|
865
|
+
joint_placements.append(np.array([0, dict_lengths["L_thorax_cerv"], 0]))
|
|
866
|
+
elif j == "left_clavicle_joint_X" or j == "right_clavicle_joint_X":
|
|
867
|
+
joint_placements.append(np.array([0, dict_lengths["L_thorax_supr"], 0]))
|
|
868
|
+
elif j == "left_shoulder_Z":
|
|
869
|
+
joint_placements.append(
|
|
870
|
+
np.round(
|
|
871
|
+
height
|
|
872
|
+
* np.array([SJ_thorax_X_r, SJ_thorax_Y_r, -SJ_thorax_Z_r]),
|
|
873
|
+
3,
|
|
874
|
+
)
|
|
875
|
+
)
|
|
876
|
+
elif j == "left_elbow_Z":
|
|
877
|
+
joint_placements.append(np.array([0, -dict_lengths["L_upperarm"], 0]))
|
|
878
|
+
elif j == "left_wrist_Z":
|
|
879
|
+
joint_placements.append(np.array([0, -dict_lengths["L_lowerarm"], 0]))
|
|
880
|
+
elif j == "right_shoulder_Z":
|
|
881
|
+
joint_placements.append(
|
|
882
|
+
np.round(
|
|
883
|
+
height
|
|
884
|
+
* np.array([SJ_thorax_X_r, SJ_thorax_Y_r, SJ_thorax_Z_r]),
|
|
885
|
+
3,
|
|
886
|
+
)
|
|
887
|
+
)
|
|
888
|
+
elif j == "right_elbow_Z":
|
|
889
|
+
joint_placements.append(np.array([0, -dict_lengths["L_upperarm"], 0]))
|
|
890
|
+
elif j == "right_wrist_Z":
|
|
891
|
+
joint_placements.append(np.array([0, -dict_lengths["L_lowerarm"], 0]))
|
|
892
|
+
elif j == "left_hip_Z":
|
|
893
|
+
joint_placements.append(
|
|
894
|
+
np.round(
|
|
895
|
+
height
|
|
896
|
+
* np.array([HJ_pelvis_X_r, HJ_pelvis_Y_r, -HJ_pelvis_Z_r]),
|
|
897
|
+
3,
|
|
898
|
+
)
|
|
899
|
+
)
|
|
900
|
+
elif j == "left_knee_Z":
|
|
901
|
+
joint_placements.append(np.array([0, -dict_lengths["L_upperleg"], 0]))
|
|
902
|
+
elif j == "left_ankle_Z":
|
|
903
|
+
joint_placements.append(np.array([0, -dict_lengths["L_lowerleg"], 0]))
|
|
904
|
+
elif j == "right_hip_Z":
|
|
905
|
+
joint_placements.append(
|
|
906
|
+
np.round(
|
|
907
|
+
height
|
|
908
|
+
* np.array([HJ_pelvis_X_r, HJ_pelvis_Y_r, HJ_pelvis_Z_r]),
|
|
909
|
+
3,
|
|
910
|
+
)
|
|
911
|
+
)
|
|
912
|
+
elif j == "right_knee_Z":
|
|
913
|
+
joint_placements.append(np.array([0, -dict_lengths["L_upperleg"], 0]))
|
|
914
|
+
elif j == "right_ankle_Z":
|
|
915
|
+
joint_placements.append(np.array([0, -dict_lengths["L_lowerleg"], 0]))
|
|
916
|
+
|
|
917
|
+
return dict(zip(joint_names, joint_placements))
|
|
918
|
+
|
|
919
|
+
@staticmethod
|
|
920
|
+
def get_dict_meshes_scale(
|
|
921
|
+
height: float, sgt_lengths: dict, gender: str = "m"
|
|
922
|
+
) -> dict:
|
|
923
|
+
meshes_names = [
|
|
924
|
+
"middle_pelvis_0",
|
|
925
|
+
"left_upperleg_0",
|
|
926
|
+
"left_lowerleg_0",
|
|
927
|
+
"left_lowerleg_1",
|
|
928
|
+
"left_foot_0",
|
|
929
|
+
"middle_abdomen_0",
|
|
930
|
+
"middle_thorax_0",
|
|
931
|
+
"middle_head_0",
|
|
932
|
+
"middle_head_1",
|
|
933
|
+
"left_upperarm_0",
|
|
934
|
+
"left_upperarm_1",
|
|
935
|
+
"left_lowerarm_0",
|
|
936
|
+
"left_lowerarm_1",
|
|
937
|
+
"left_hand_0",
|
|
938
|
+
"right_upperarm_0",
|
|
939
|
+
"right_upperarm_1",
|
|
940
|
+
"right_lowerarm_0",
|
|
941
|
+
"right_lowerarm_1",
|
|
942
|
+
"right_hand_0",
|
|
943
|
+
"right_upperleg_0",
|
|
944
|
+
"right_lowerleg_0",
|
|
945
|
+
"right_lowerleg_1",
|
|
946
|
+
"right_foot_0",
|
|
947
|
+
]
|
|
948
|
+
|
|
949
|
+
scales = []
|
|
950
|
+
|
|
951
|
+
for name in meshes_names:
|
|
952
|
+
if (
|
|
953
|
+
name == "middle_pelvis_0"
|
|
954
|
+
): # pelvis mesh size is : X = 37.050m, Y = 32.551m, Z = 39.252m
|
|
955
|
+
# This width is shared with the torso mesh above, so the two
|
|
956
|
+
# read as one body.
|
|
957
|
+
width = sgt_lengths["middle_pelvis_0_width"]
|
|
958
|
+
scale = np.round(
|
|
959
|
+
np.array([1.09 * width / 39.252, width / 39.252, width / 39.252]),
|
|
960
|
+
6,
|
|
961
|
+
)
|
|
962
|
+
scales.append(scale)
|
|
963
|
+
elif (
|
|
964
|
+
name == "left_upperleg_0"
|
|
965
|
+
): # upperleg mesh size is : X = 20.977m, Y = 68.524m, Z = 20.977m
|
|
966
|
+
scale = np.round(
|
|
967
|
+
np.array(
|
|
968
|
+
[
|
|
969
|
+
0.96 * sgt_lengths["left_upperleg_0"] / 68.524,
|
|
970
|
+
sgt_lengths["left_upperleg_0"] / 68.524,
|
|
971
|
+
1.08 * sgt_lengths["left_upperleg_0"] / 68.524,
|
|
972
|
+
]
|
|
973
|
+
),
|
|
974
|
+
4,
|
|
975
|
+
)
|
|
976
|
+
scales.append(scale)
|
|
977
|
+
elif (
|
|
978
|
+
name == "left_lowerleg_0"
|
|
979
|
+
): # lowerleg mesh size is : X = 17.986m, Y = 67m, Z = 17.977m
|
|
980
|
+
scale = np.round(
|
|
981
|
+
np.array(
|
|
982
|
+
[
|
|
983
|
+
0.96 * sgt_lengths["left_lowerleg_0"] / 67,
|
|
984
|
+
sgt_lengths["left_lowerleg_0"] / 67,
|
|
985
|
+
1.07 * sgt_lengths["left_lowerleg_0"] / 67,
|
|
986
|
+
]
|
|
987
|
+
),
|
|
988
|
+
4,
|
|
989
|
+
)
|
|
990
|
+
scales.append(scale)
|
|
991
|
+
elif name == "left_lowerleg_1": # knee mesh is a sphere of 12.5m
|
|
992
|
+
scale = np.round(
|
|
993
|
+
np.array(
|
|
994
|
+
[
|
|
995
|
+
0.7
|
|
996
|
+
* (0.053 * height if gender == "f" else 0.056 * height)
|
|
997
|
+
/ 12.5,
|
|
998
|
+
0.7
|
|
999
|
+
* (0.053 * height if gender == "f" else 0.056 * height)
|
|
1000
|
+
/ 12.5,
|
|
1001
|
+
0.7
|
|
1002
|
+
* (0.053 * height if gender == "f" else 0.056 * height)
|
|
1003
|
+
/ 12.5,
|
|
1004
|
+
]
|
|
1005
|
+
),
|
|
1006
|
+
4,
|
|
1007
|
+
)
|
|
1008
|
+
scales.append(scale)
|
|
1009
|
+
elif (
|
|
1010
|
+
name == "left_foot_0"
|
|
1011
|
+
): # foot mesh size is : X = 40.5m, Y = 16.5m, Z = 17.999m
|
|
1012
|
+
scale = np.round(
|
|
1013
|
+
np.array(
|
|
1014
|
+
[
|
|
1015
|
+
sgt_lengths["left_foot_0"] / 40.5,
|
|
1016
|
+
sgt_lengths["left_foot_0"] / 40.5,
|
|
1017
|
+
sgt_lengths["left_foot_0"] / 40.5,
|
|
1018
|
+
]
|
|
1019
|
+
),
|
|
1020
|
+
4,
|
|
1021
|
+
)
|
|
1022
|
+
scales.append(scale)
|
|
1023
|
+
elif name == "middle_abdomen_0": # abdomen meshis a sphere of radius 30 m
|
|
1024
|
+
scale = np.round(
|
|
1025
|
+
np.array(
|
|
1026
|
+
[
|
|
1027
|
+
sgt_lengths["middle_abdomen_0"] / 30,
|
|
1028
|
+
sgt_lengths["middle_abdomen_0"] / 30,
|
|
1029
|
+
sgt_lengths["middle_abdomen_0"] / 30,
|
|
1030
|
+
]
|
|
1031
|
+
),
|
|
1032
|
+
4,
|
|
1033
|
+
)
|
|
1034
|
+
scales.append(scale)
|
|
1035
|
+
elif (
|
|
1036
|
+
name == "middle_thorax_0"
|
|
1037
|
+
): # torso mesh size is : X = 36.991m, Y = 52.0m, Z = 40.029m
|
|
1038
|
+
# This mesh stands for the whole trunk, not the thorax segment
|
|
1039
|
+
# alone, so it is scaled to span from the lumbar joint (L5/S1)
|
|
1040
|
+
# up to the cervical joint (C7/T1). Sized to the thorax segment
|
|
1041
|
+
# it stops short of the pelvis.
|
|
1042
|
+
#
|
|
1043
|
+
# The mesh measures 52.0 along Y, not the 35 used before, which
|
|
1044
|
+
# made it about 1.5x too long for whatever it was scaled to.
|
|
1045
|
+
# 6 decimals because this scale is small enough that rounding to
|
|
1046
|
+
# 4 moves the top of the trunk visibly.
|
|
1047
|
+
trunk = sgt_lengths["middle_abdomen_0"] + sgt_lengths["middle_thorax_0"]
|
|
1048
|
+
# Width is taken from the pelvis so the trunk reads as one body;
|
|
1049
|
+
# scaling it uniformly from the length instead leaves the torso
|
|
1050
|
+
# much wider than the pelvis below it.
|
|
1051
|
+
width = sgt_lengths["middle_pelvis_0_width"]
|
|
1052
|
+
scale = np.round(
|
|
1053
|
+
np.array([width / 40.029, trunk / 52.0, width / 40.029]), 6
|
|
1054
|
+
)
|
|
1055
|
+
scales.append(scale)
|
|
1056
|
+
elif (
|
|
1057
|
+
name == "middle_head_0"
|
|
1058
|
+
): # head mesh size is : X = 32.049m, Y = 42m, Z = 32.049m
|
|
1059
|
+
scale = np.round(
|
|
1060
|
+
np.array(
|
|
1061
|
+
[
|
|
1062
|
+
sgt_lengths["middle_head_0"] / 42,
|
|
1063
|
+
sgt_lengths["middle_head_0"] / 42,
|
|
1064
|
+
sgt_lengths["middle_head_0"] / 42,
|
|
1065
|
+
]
|
|
1066
|
+
),
|
|
1067
|
+
4,
|
|
1068
|
+
)
|
|
1069
|
+
scales.append(scale)
|
|
1070
|
+
elif name == "middle_head_1": # neck mesh
|
|
1071
|
+
scale = np.round(
|
|
1072
|
+
np.array(
|
|
1073
|
+
[
|
|
1074
|
+
(0.078 * height if gender == "f" else 0.086 * height)
|
|
1075
|
+
/ 31.0,
|
|
1076
|
+
(0.078 * height if gender == "f" else 0.086 * height)
|
|
1077
|
+
/ 31.0,
|
|
1078
|
+
(0.078 * height if gender == "f" else 0.086 * height)
|
|
1079
|
+
/ 31.0,
|
|
1080
|
+
]
|
|
1081
|
+
),
|
|
1082
|
+
4,
|
|
1083
|
+
)
|
|
1084
|
+
scales.append(scale)
|
|
1085
|
+
elif name == "left_upperarm_0": # shoulder mesh is a shpere of radius 17.5
|
|
1086
|
+
scale = np.round(
|
|
1087
|
+
np.array(
|
|
1088
|
+
[
|
|
1089
|
+
(0.037 * height if gender == "f" else 0.038 * height)
|
|
1090
|
+
/ 17.5,
|
|
1091
|
+
(0.037 * height if gender == "f" else 0.038 * height)
|
|
1092
|
+
/ 17.5,
|
|
1093
|
+
(0.037 * height if gender == "f" else 0.038 * height)
|
|
1094
|
+
/ 17.5,
|
|
1095
|
+
]
|
|
1096
|
+
),
|
|
1097
|
+
4,
|
|
1098
|
+
)
|
|
1099
|
+
scales.append(scale)
|
|
1100
|
+
elif (
|
|
1101
|
+
name == "left_upperarm_1"
|
|
1102
|
+
): # upperarm mesh is of size : X = 17.158m, Y = 46m, Z = 17.183m
|
|
1103
|
+
scale = np.round(
|
|
1104
|
+
np.array(
|
|
1105
|
+
[
|
|
1106
|
+
1.05 * sgt_lengths["left_upperarm_0"] / 46,
|
|
1107
|
+
sgt_lengths["left_upperarm_0"] / 46,
|
|
1108
|
+
1.16 * sgt_lengths["left_upperarm_0"] / 46,
|
|
1109
|
+
]
|
|
1110
|
+
),
|
|
1111
|
+
4,
|
|
1112
|
+
)
|
|
1113
|
+
scales.append(scale)
|
|
1114
|
+
elif (
|
|
1115
|
+
name == "left_lowerarm_0"
|
|
1116
|
+
): # lowerarm mesh is of size : X = 15.547m, Y = 45m, Z = 15.541m
|
|
1117
|
+
scale = np.round(
|
|
1118
|
+
np.array(
|
|
1119
|
+
[
|
|
1120
|
+
sgt_lengths["left_lowerarm_0"] / 45,
|
|
1121
|
+
sgt_lengths["left_lowerarm_0"] / 45,
|
|
1122
|
+
sgt_lengths["left_lowerarm_0"] / 45,
|
|
1123
|
+
]
|
|
1124
|
+
),
|
|
1125
|
+
4,
|
|
1126
|
+
)
|
|
1127
|
+
scales.append(scale)
|
|
1128
|
+
elif name == "left_lowerarm_1": # elbow mesh is a sphere of radius 12.5
|
|
1129
|
+
scale = np.round(
|
|
1130
|
+
np.array(
|
|
1131
|
+
[
|
|
1132
|
+
(0.037 * height if gender == "f" else 0.038 * height)
|
|
1133
|
+
/ 12.5,
|
|
1134
|
+
(0.037 * height if gender == "f" else 0.038 * height)
|
|
1135
|
+
/ 12.5,
|
|
1136
|
+
(0.037 * height if gender == "f" else 0.038 * height)
|
|
1137
|
+
/ 12.5,
|
|
1138
|
+
]
|
|
1139
|
+
),
|
|
1140
|
+
4,
|
|
1141
|
+
)
|
|
1142
|
+
scales.append(scale)
|
|
1143
|
+
elif (
|
|
1144
|
+
name == "left_hand_0"
|
|
1145
|
+
): # hand mesh is of size : X = 12.055m, Y = 30m, Z = 16.646m
|
|
1146
|
+
scale = np.round(
|
|
1147
|
+
np.array(
|
|
1148
|
+
[
|
|
1149
|
+
sgt_lengths["left_hand_0"] / 30,
|
|
1150
|
+
sgt_lengths["left_hand_0"] / 30,
|
|
1151
|
+
sgt_lengths["left_hand_0"] / 30,
|
|
1152
|
+
]
|
|
1153
|
+
),
|
|
1154
|
+
4,
|
|
1155
|
+
)
|
|
1156
|
+
scales.append(scale)
|
|
1157
|
+
elif name == "right_upperarm_0": # shoulder mesh
|
|
1158
|
+
scale = np.round(
|
|
1159
|
+
np.array(
|
|
1160
|
+
[
|
|
1161
|
+
(0.037 * height if gender == "f" else 0.038 * height)
|
|
1162
|
+
/ 17.5,
|
|
1163
|
+
(0.037 * height if gender == "f" else 0.038 * height)
|
|
1164
|
+
/ 17.5,
|
|
1165
|
+
(0.037 * height if gender == "f" else 0.038 * height)
|
|
1166
|
+
/ 17.5,
|
|
1167
|
+
]
|
|
1168
|
+
),
|
|
1169
|
+
4,
|
|
1170
|
+
)
|
|
1171
|
+
scales.append(scale)
|
|
1172
|
+
elif name == "right_upperarm_1": # upperarm mesh
|
|
1173
|
+
scale = np.round(
|
|
1174
|
+
np.array(
|
|
1175
|
+
[
|
|
1176
|
+
1.05 * sgt_lengths["right_upperarm_0"] / 46,
|
|
1177
|
+
sgt_lengths["right_upperarm_0"] / 46,
|
|
1178
|
+
1.16 * sgt_lengths["right_upperarm_0"] / 46,
|
|
1179
|
+
]
|
|
1180
|
+
),
|
|
1181
|
+
4,
|
|
1182
|
+
)
|
|
1183
|
+
scales.append(scale)
|
|
1184
|
+
elif name == "right_lowerarm_0": # lowerarm mesh
|
|
1185
|
+
scale = np.round(
|
|
1186
|
+
np.array(
|
|
1187
|
+
[
|
|
1188
|
+
sgt_lengths["right_lowerarm_0"] / 45,
|
|
1189
|
+
sgt_lengths["right_lowerarm_0"] / 45,
|
|
1190
|
+
sgt_lengths["right_lowerarm_0"] / 45,
|
|
1191
|
+
]
|
|
1192
|
+
),
|
|
1193
|
+
4,
|
|
1194
|
+
)
|
|
1195
|
+
scales.append(scale)
|
|
1196
|
+
elif name == "right_lowerarm_1": # elbow mesh
|
|
1197
|
+
scale = np.round(
|
|
1198
|
+
np.array(
|
|
1199
|
+
[
|
|
1200
|
+
(0.037 * height if gender == "f" else 0.038 * height)
|
|
1201
|
+
/ 12.5,
|
|
1202
|
+
(0.037 * height if gender == "f" else 0.038 * height)
|
|
1203
|
+
/ 12.5,
|
|
1204
|
+
(0.037 * height if gender == "f" else 0.038 * height)
|
|
1205
|
+
/ 12.5,
|
|
1206
|
+
]
|
|
1207
|
+
),
|
|
1208
|
+
4,
|
|
1209
|
+
)
|
|
1210
|
+
scales.append(scale)
|
|
1211
|
+
elif name == "right_hand_0": # hand mesh
|
|
1212
|
+
scale = np.round(
|
|
1213
|
+
np.array(
|
|
1214
|
+
[
|
|
1215
|
+
sgt_lengths["right_hand_0"] / 30,
|
|
1216
|
+
sgt_lengths["right_hand_0"] / 30,
|
|
1217
|
+
sgt_lengths["right_hand_0"] / 30,
|
|
1218
|
+
]
|
|
1219
|
+
),
|
|
1220
|
+
4,
|
|
1221
|
+
)
|
|
1222
|
+
scales.append(scale)
|
|
1223
|
+
elif name == "right_upperleg_0": # upperleg mesh
|
|
1224
|
+
scale = np.round(
|
|
1225
|
+
np.array(
|
|
1226
|
+
[
|
|
1227
|
+
0.96 * sgt_lengths["right_upperleg_0"] / 68.524,
|
|
1228
|
+
sgt_lengths["right_upperleg_0"] / 68.524,
|
|
1229
|
+
1.08 * sgt_lengths["right_upperleg_0"] / 68.524,
|
|
1230
|
+
]
|
|
1231
|
+
),
|
|
1232
|
+
4,
|
|
1233
|
+
)
|
|
1234
|
+
scales.append(scale)
|
|
1235
|
+
elif name == "right_lowerleg_0": # lowerleg mesh
|
|
1236
|
+
scale = np.round(
|
|
1237
|
+
np.array(
|
|
1238
|
+
[
|
|
1239
|
+
0.96 * sgt_lengths["right_lowerleg_0"] / 67,
|
|
1240
|
+
sgt_lengths["right_lowerleg_0"] / 67,
|
|
1241
|
+
1.07 * sgt_lengths["right_lowerleg_0"] / 67,
|
|
1242
|
+
]
|
|
1243
|
+
),
|
|
1244
|
+
4,
|
|
1245
|
+
)
|
|
1246
|
+
scales.append(scale)
|
|
1247
|
+
elif name == "right_lowerleg_1": # knee mesh
|
|
1248
|
+
scale = np.round(
|
|
1249
|
+
np.array(
|
|
1250
|
+
[
|
|
1251
|
+
0.7
|
|
1252
|
+
* (0.053 * height if gender == "f" else 0.056 * height)
|
|
1253
|
+
/ 12.5,
|
|
1254
|
+
0.7
|
|
1255
|
+
* (0.053 * height if gender == "f" else 0.056 * height)
|
|
1256
|
+
/ 12.5,
|
|
1257
|
+
0.7
|
|
1258
|
+
* (0.053 * height if gender == "f" else 0.056 * height)
|
|
1259
|
+
/ 12.5,
|
|
1260
|
+
]
|
|
1261
|
+
),
|
|
1262
|
+
4,
|
|
1263
|
+
)
|
|
1264
|
+
scales.append(scale)
|
|
1265
|
+
elif name == "right_foot_0": # foot mesh
|
|
1266
|
+
scale = np.round(
|
|
1267
|
+
np.array(
|
|
1268
|
+
[
|
|
1269
|
+
sgt_lengths["right_foot_0"] / 40.5,
|
|
1270
|
+
sgt_lengths["right_foot_0"] / 40.5,
|
|
1271
|
+
sgt_lengths["right_foot_0"] / 40.5,
|
|
1272
|
+
]
|
|
1273
|
+
),
|
|
1274
|
+
4,
|
|
1275
|
+
)
|
|
1276
|
+
scales.append(scale)
|
|
1277
|
+
|
|
1278
|
+
return dict(zip(meshes_names, scales))
|
|
1279
|
+
|
|
1280
|
+
def _scale_pin_model(
|
|
1281
|
+
self,
|
|
1282
|
+
model: pin.Model,
|
|
1283
|
+
visual_model: pin.GeometryModel,
|
|
1284
|
+
height: float,
|
|
1285
|
+
weight: float,
|
|
1286
|
+
gender: str = "m",
|
|
1287
|
+
) -> None:
|
|
1288
|
+
"""
|
|
1289
|
+
Overwrite model.inertias, model.jointPlacements, and model.geometryObjects.meshScale
|
|
1290
|
+
based on anthropometry.
|
|
1291
|
+
"""
|
|
1292
|
+
# compute parameters
|
|
1293
|
+
joints = self.get_dict_joint_placements(height, gender)
|
|
1294
|
+
inert, sgt_lengths = self.get_dict_inertial_param(height, weight, gender)
|
|
1295
|
+
scales = self.get_dict_meshes_scale(height, sgt_lengths, gender)
|
|
1296
|
+
|
|
1297
|
+
# apply inertias
|
|
1298
|
+
for seg, P in inert.items():
|
|
1299
|
+
b_id = model.frames[model.getFrameId(seg)].parentJoint
|
|
1300
|
+
|
|
1301
|
+
Ixx, Ixy, Ixz, Iyy, Iyz, Izz = P["inertia"]
|
|
1302
|
+
I_mat = np.array([[Ixx, Ixy, Ixz], [Ixy, Iyy, Iyz], [Ixz, Iyz, Izz]])
|
|
1303
|
+
model.inertias[b_id] = pin.Inertia(P["mass"], P["com"], I_mat)
|
|
1304
|
+
|
|
1305
|
+
# apply joint placements
|
|
1306
|
+
for j, pos in joints.items():
|
|
1307
|
+
j_id = model.getJointId(j)
|
|
1308
|
+
M = model.jointPlacements[j_id]
|
|
1309
|
+
M.translation = pos
|
|
1310
|
+
model.jointPlacements[j_id] = M
|
|
1311
|
+
|
|
1312
|
+
# scale visuals
|
|
1313
|
+
assert len(visual_model.geometryObjects.tolist()) == len(scales)
|
|
1314
|
+
for geom_obj in visual_model.geometryObjects:
|
|
1315
|
+
urdf_scale = np.asarray(geom_obj.meshScale, dtype=float).copy()
|
|
1316
|
+
geom_obj.meshScale = scales[geom_obj.name]
|
|
1317
|
+
|
|
1318
|
+
# A visual offset is a length in the mesh's own frame, so it has to
|
|
1319
|
+
# be rescaled with the mesh. Leaving it fixed, as the URDF value is,
|
|
1320
|
+
# only suits one subject size: it is what put the torso below the
|
|
1321
|
+
# thoracic joint, and further off the taller the subject.
|
|
1322
|
+
if urdf_scale[1]:
|
|
1323
|
+
geom_obj.placement.translation = (
|
|
1324
|
+
np.asarray(geom_obj.placement.translation, dtype=float)
|
|
1325
|
+
* scales[geom_obj.name][1]
|
|
1326
|
+
/ urdf_scale[1]
|
|
1327
|
+
)
|