jaxsim 0.2.1.dev69__py3-none-any.whl → 0.2.1.dev74__py3-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.
- jaxsim/_version.py +2 -2
- jaxsim/api/__init__.py +11 -1
- jaxsim/api/com.py +176 -0
- jaxsim/api/frame.py +221 -0
- jaxsim/api/model.py +10 -0
- jaxsim/parsers/descriptions/link.py +37 -29
- {jaxsim-0.2.1.dev69.dist-info → jaxsim-0.2.1.dev74.dist-info}/METADATA +1 -1
- {jaxsim-0.2.1.dev69.dist-info → jaxsim-0.2.1.dev74.dist-info}/RECORD +11 -10
- {jaxsim-0.2.1.dev69.dist-info → jaxsim-0.2.1.dev74.dist-info}/LICENSE +0 -0
- {jaxsim-0.2.1.dev69.dist-info → jaxsim-0.2.1.dev74.dist-info}/WHEEL +0 -0
- {jaxsim-0.2.1.dev69.dist-info → jaxsim-0.2.1.dev74.dist-info}/top_level.txt +0 -0
jaxsim/_version.py
CHANGED
@@ -12,5 +12,5 @@ __version__: str
|
|
12
12
|
__version_tuple__: VERSION_TUPLE
|
13
13
|
version_tuple: VERSION_TUPLE
|
14
14
|
|
15
|
-
__version__ = version = '0.2.1.
|
16
|
-
__version_tuple__ = version_tuple = (0, 2, 1, '
|
15
|
+
__version__ = version = '0.2.1.dev74'
|
16
|
+
__version_tuple__ = version_tuple = (0, 2, 1, 'dev74')
|
jaxsim/api/__init__.py
CHANGED
@@ -1,3 +1,13 @@
|
|
1
1
|
from . import common # isort:skip
|
2
2
|
from . import model, data # isort:skip
|
3
|
-
from . import
|
3
|
+
from . import (
|
4
|
+
com,
|
5
|
+
contact,
|
6
|
+
frame,
|
7
|
+
joint,
|
8
|
+
kin_dyn_parameters,
|
9
|
+
link,
|
10
|
+
ode,
|
11
|
+
ode_data,
|
12
|
+
references,
|
13
|
+
)
|
jaxsim/api/com.py
CHANGED
@@ -238,3 +238,179 @@ def average_centroidal_velocity_jacobian(
|
|
238
238
|
G_Mbb = locked_centroidal_spatial_inertia(model=model, data=data)
|
239
239
|
|
240
240
|
return jnp.linalg.inv(G_Mbb) @ G_J
|
241
|
+
|
242
|
+
|
243
|
+
@jax.jit
|
244
|
+
def bias_acceleration(
|
245
|
+
model: js.model.JaxSimModel, data: js.data.JaxSimModelData
|
246
|
+
) -> jtp.Vector:
|
247
|
+
r"""
|
248
|
+
Compute the bias linear acceleration of the center of mass.
|
249
|
+
|
250
|
+
Args:
|
251
|
+
model: The model to consider.
|
252
|
+
data: The data of the considered model.
|
253
|
+
|
254
|
+
Returns:
|
255
|
+
The bias linear acceleration of the center of mass in the active representation.
|
256
|
+
|
257
|
+
Note:
|
258
|
+
The bias acceleration is expressed in the mixed frame
|
259
|
+
:math:`G = ({}^W \mathbf{p}_{\text{CoM}}, [C])`, where :math:`[C] = [W]` if the
|
260
|
+
active velocity representation is either inertial-fixed or mixed,
|
261
|
+
and :math:`[C] = [B]` if the active velocity representation is body-fixed.
|
262
|
+
"""
|
263
|
+
|
264
|
+
# Compute the pose of all links with forward kinematics.
|
265
|
+
W_H_L = js.model.forward_kinematics(model=model, data=data)
|
266
|
+
|
267
|
+
# Compute the bias acceleration of all links by zeroing the generalized velocity
|
268
|
+
# in the active representation.
|
269
|
+
v̇_bias_WL = js.model.link_bias_accelerations(model=model, data=data)
|
270
|
+
|
271
|
+
def other_representation_to_body(
|
272
|
+
C_v̇_WL: jtp.Vector, C_v_WC: jtp.Vector, L_H_C: jtp.Matrix, L_v_LC: jtp.Vector
|
273
|
+
) -> jtp.Vector:
|
274
|
+
"""
|
275
|
+
Helper to convert the body-fixed representation of the link bias acceleration
|
276
|
+
C_v̇_WL expressed in a generic frame C to the body-fixed representation L_v̇_WL.
|
277
|
+
"""
|
278
|
+
|
279
|
+
L_X_C = jaxsim.math.Adjoint.from_transform(transform=L_H_C)
|
280
|
+
C_X_L = jaxsim.math.Adjoint.inverse(L_X_C)
|
281
|
+
|
282
|
+
L_v̇_WL = L_X_C @ (C_v̇_WL + jaxsim.math.Cross.vx(C_X_L @ L_v_LC) @ C_v_WC)
|
283
|
+
return L_v̇_WL
|
284
|
+
|
285
|
+
# We need here to get the body-fixed bias acceleration of the links.
|
286
|
+
# Since it's computed in the active representation, we need to convert it to body.
|
287
|
+
match data.velocity_representation:
|
288
|
+
|
289
|
+
case VelRepr.Body:
|
290
|
+
L_a_bias_WL = v̇_bias_WL
|
291
|
+
|
292
|
+
case VelRepr.Inertial:
|
293
|
+
|
294
|
+
C_v̇_WL = W_v̇_bias_WL = v̇_bias_WL
|
295
|
+
C_v_WC = W_v_WW = jnp.zeros(6)
|
296
|
+
|
297
|
+
L_H_C = L_H_W = jax.vmap(
|
298
|
+
lambda W_H_L: jaxsim.math.Transform.inverse(W_H_L)
|
299
|
+
)(W_H_L)
|
300
|
+
|
301
|
+
L_v_LC = L_v_LW = jax.vmap(
|
302
|
+
lambda i: -js.link.velocity(
|
303
|
+
model=model, data=data, link_index=i, output_vel_repr=VelRepr.Body
|
304
|
+
)
|
305
|
+
)(jnp.arange(model.number_of_links()))
|
306
|
+
|
307
|
+
L_a_bias_WL = jax.vmap(
|
308
|
+
lambda i: other_representation_to_body(
|
309
|
+
C_v̇_WL=C_v̇_WL[i],
|
310
|
+
C_v_WC=C_v_WC,
|
311
|
+
L_H_C=L_H_C[i],
|
312
|
+
L_v_LC=L_v_LC[i],
|
313
|
+
)
|
314
|
+
)(jnp.arange(model.number_of_links()))
|
315
|
+
|
316
|
+
case VelRepr.Mixed:
|
317
|
+
|
318
|
+
C_v̇_WL = LW_v̇_bias_WL = v̇_bias_WL
|
319
|
+
|
320
|
+
C_v_WC = LW_v_W_LW = jax.vmap(
|
321
|
+
lambda i: js.link.velocity(
|
322
|
+
model=model, data=data, link_index=i, output_vel_repr=VelRepr.Mixed
|
323
|
+
)
|
324
|
+
.at[3:6]
|
325
|
+
.set(jnp.zeros(3))
|
326
|
+
)(jnp.arange(model.number_of_links()))
|
327
|
+
|
328
|
+
L_H_C = L_H_LW = jax.vmap(
|
329
|
+
lambda W_H_L: jaxsim.math.Transform.inverse(
|
330
|
+
W_H_L.at[0:3, 3].set(jnp.zeros(3))
|
331
|
+
)
|
332
|
+
)(W_H_L)
|
333
|
+
|
334
|
+
L_v_LC = L_v_L_LW = jax.vmap(
|
335
|
+
lambda i: -js.link.velocity(
|
336
|
+
model=model, data=data, link_index=i, output_vel_repr=VelRepr.Body
|
337
|
+
)
|
338
|
+
.at[0:3]
|
339
|
+
.set(jnp.zeros(3))
|
340
|
+
)(jnp.arange(model.number_of_links()))
|
341
|
+
|
342
|
+
L_a_bias_WL = jax.vmap(
|
343
|
+
lambda i: other_representation_to_body(
|
344
|
+
C_v̇_WL=C_v̇_WL[i],
|
345
|
+
C_v_WC=C_v_WC[i],
|
346
|
+
L_H_C=L_H_C[i],
|
347
|
+
L_v_LC=L_v_LC[i],
|
348
|
+
)
|
349
|
+
)(jnp.arange(model.number_of_links()))
|
350
|
+
|
351
|
+
case _:
|
352
|
+
raise ValueError(data.velocity_representation)
|
353
|
+
|
354
|
+
# Compute the bias of the 6D momentum derivative.
|
355
|
+
def bias_momentum_derivative_term(
|
356
|
+
link_index: jtp.Int, L_a_bias_WL: jtp.Vector
|
357
|
+
) -> jtp.Vector:
|
358
|
+
|
359
|
+
# Get the body-fixed 6D inertia matrix.
|
360
|
+
L_M_L = js.link.spatial_inertia(model=model, link_index=link_index)
|
361
|
+
|
362
|
+
# Compute the body-fixed 6D velocity.
|
363
|
+
L_v_WL = js.link.velocity(
|
364
|
+
model=model, data=data, link_index=link_index, output_vel_repr=VelRepr.Body
|
365
|
+
)
|
366
|
+
|
367
|
+
# Compute the world-to-link transformations for 6D forces.
|
368
|
+
W_Xf_L = jaxsim.math.Adjoint.from_transform(
|
369
|
+
transform=W_H_L[link_index], inverse=True
|
370
|
+
).T
|
371
|
+
|
372
|
+
# Compute the contribution of the link to the bias acceleration of the CoM.
|
373
|
+
W_ḣ_bias_link_contribution = W_Xf_L @ (
|
374
|
+
L_M_L @ L_a_bias_WL + jaxsim.math.Cross.vx_star(L_v_WL) @ L_M_L @ L_v_WL
|
375
|
+
)
|
376
|
+
|
377
|
+
return W_ḣ_bias_link_contribution
|
378
|
+
|
379
|
+
# Sum the contributions of all links to the bias acceleration of the CoM.
|
380
|
+
W_ḣ_bias = jax.vmap(bias_momentum_derivative_term)(
|
381
|
+
jnp.arange(model.number_of_links()), L_a_bias_WL
|
382
|
+
).sum(axis=0)
|
383
|
+
|
384
|
+
# Compute the total mass of the model.
|
385
|
+
m = js.model.total_mass(model=model)
|
386
|
+
|
387
|
+
# Compute the position of the CoM.
|
388
|
+
W_p_CoM = com_position(model=model, data=data)
|
389
|
+
|
390
|
+
match data.velocity_representation:
|
391
|
+
|
392
|
+
# G := G[W] = (W_p_CoM, [W])
|
393
|
+
case VelRepr.Inertial | VelRepr.Mixed:
|
394
|
+
|
395
|
+
W_H_GW = jnp.eye(4).at[0:3, 3].set(W_p_CoM)
|
396
|
+
GW_Xf_W = jaxsim.math.Adjoint.from_transform(W_H_GW).T
|
397
|
+
|
398
|
+
GW_ḣ_bias = GW_Xf_W @ W_ḣ_bias
|
399
|
+
GW_v̇l_com_bias = GW_ḣ_bias[0:3] / m
|
400
|
+
|
401
|
+
return GW_v̇l_com_bias
|
402
|
+
|
403
|
+
# G := G[B] = (W_p_CoM, [B])
|
404
|
+
case VelRepr.Body:
|
405
|
+
|
406
|
+
GB_Xf_W = jaxsim.math.Adjoint.from_transform(
|
407
|
+
transform=data.base_transform().at[0:3].set(W_p_CoM)
|
408
|
+
).T
|
409
|
+
|
410
|
+
GB_ḣ_bias = GB_Xf_W @ W_ḣ_bias
|
411
|
+
GB_v̇l_com_bias = GB_ḣ_bias[0:3] / m
|
412
|
+
|
413
|
+
return GB_v̇l_com_bias
|
414
|
+
|
415
|
+
case _:
|
416
|
+
raise ValueError(data.velocity_representation)
|
jaxsim/api/frame.py
ADDED
@@ -0,0 +1,221 @@
|
|
1
|
+
import functools
|
2
|
+
from typing import Sequence
|
3
|
+
|
4
|
+
import jax
|
5
|
+
import jax.numpy as jnp
|
6
|
+
import jaxlie
|
7
|
+
import numpy as np
|
8
|
+
|
9
|
+
import jaxsim.api as js
|
10
|
+
import jaxsim.math
|
11
|
+
import jaxsim.typing as jtp
|
12
|
+
|
13
|
+
from .common import VelRepr
|
14
|
+
|
15
|
+
# =======================
|
16
|
+
# Index-related functions
|
17
|
+
# =======================
|
18
|
+
|
19
|
+
|
20
|
+
def idx_of_parent_link(model: js.model.JaxSimModel, *, frame_idx: jtp.IntLike) -> int:
|
21
|
+
"""
|
22
|
+
Get the index of the link to which the frame is rigidly attached.
|
23
|
+
|
24
|
+
Args:
|
25
|
+
model: The model to consider.
|
26
|
+
frame_idx: The index of the frame.
|
27
|
+
|
28
|
+
Returns:
|
29
|
+
The index of the frame's parent link.
|
30
|
+
"""
|
31
|
+
|
32
|
+
# Get the intermediate representation parsed from the model description.
|
33
|
+
ir = model.description.get()
|
34
|
+
|
35
|
+
# Extract the indices of the frame and the link it is attached to.
|
36
|
+
F = ir.frames[frame_idx - model.number_of_links()]
|
37
|
+
L = ir.links_dict[F.parent.name].index
|
38
|
+
|
39
|
+
return int(L)
|
40
|
+
|
41
|
+
|
42
|
+
def name_to_idx(model: js.model.JaxSimModel, *, frame_name: str) -> int:
|
43
|
+
"""
|
44
|
+
Convert the name of a frame to its index.
|
45
|
+
|
46
|
+
Args:
|
47
|
+
model: The model to consider.
|
48
|
+
frame_name: The name of the frame.
|
49
|
+
|
50
|
+
Returns:
|
51
|
+
The index of the frame.
|
52
|
+
"""
|
53
|
+
|
54
|
+
frame_names = np.array([frame.name for frame in model.description.get().frames])
|
55
|
+
|
56
|
+
if frame_name in frame_names:
|
57
|
+
idx_in_list = np.argwhere(frame_names == frame_name)
|
58
|
+
return int(idx_in_list.squeeze().tolist()) + model.number_of_links()
|
59
|
+
|
60
|
+
return -1
|
61
|
+
|
62
|
+
|
63
|
+
def idx_to_name(model: js.model.JaxSimModel, *, frame_index: jtp.IntLike) -> str:
|
64
|
+
"""
|
65
|
+
Convert the index of a frame to its name.
|
66
|
+
|
67
|
+
Args:
|
68
|
+
model: The model to consider.
|
69
|
+
frame_index: The index of the frame.
|
70
|
+
|
71
|
+
Returns:
|
72
|
+
The name of the frame.
|
73
|
+
"""
|
74
|
+
|
75
|
+
return model.description.get().frames[frame_index - model.number_of_links()].name
|
76
|
+
|
77
|
+
|
78
|
+
@functools.partial(jax.jit, static_argnames=["frame_names"])
|
79
|
+
def names_to_idxs(
|
80
|
+
model: js.model.JaxSimModel, *, frame_names: Sequence[str]
|
81
|
+
) -> jax.Array:
|
82
|
+
"""
|
83
|
+
Convert a sequence of frame names to their corresponding indices.
|
84
|
+
|
85
|
+
Args:
|
86
|
+
model: The model to consider.
|
87
|
+
frame_names: The names of the frames.
|
88
|
+
|
89
|
+
Returns:
|
90
|
+
The indices of the frames.
|
91
|
+
"""
|
92
|
+
|
93
|
+
return jnp.array(
|
94
|
+
[name_to_idx(model=model, frame_name=frame_name) for frame_name in frame_names]
|
95
|
+
).astype(int)
|
96
|
+
|
97
|
+
|
98
|
+
def idxs_to_names(
|
99
|
+
model: js.model.JaxSimModel, *, frame_indices: Sequence[jtp.IntLike]
|
100
|
+
) -> tuple[str, ...]:
|
101
|
+
"""
|
102
|
+
Convert a sequence of frame indices to their corresponding names.
|
103
|
+
|
104
|
+
Args:
|
105
|
+
model: The model to consider.
|
106
|
+
frame_indices: The indices of the frames.
|
107
|
+
|
108
|
+
Returns:
|
109
|
+
The names of the frames.
|
110
|
+
"""
|
111
|
+
|
112
|
+
return tuple(
|
113
|
+
idx_to_name(model=model, frame_index=frame_index)
|
114
|
+
for frame_index in frame_indices
|
115
|
+
)
|
116
|
+
|
117
|
+
|
118
|
+
# ==========
|
119
|
+
# Frame APIs
|
120
|
+
# ==========
|
121
|
+
|
122
|
+
|
123
|
+
@functools.partial(jax.jit, static_argnames=["frame_index"])
|
124
|
+
def transform(
|
125
|
+
model: js.model.JaxSimModel,
|
126
|
+
data: js.data.JaxSimModelData,
|
127
|
+
*,
|
128
|
+
frame_index: jtp.IntLike,
|
129
|
+
) -> jtp.Matrix:
|
130
|
+
"""
|
131
|
+
Compute the SE(3) transform from the world frame to the specified frame.
|
132
|
+
|
133
|
+
Args:
|
134
|
+
model: The model to consider.
|
135
|
+
data: The data of the considered model.
|
136
|
+
frame_index: The index of the frame for which the transform is requested.
|
137
|
+
|
138
|
+
Returns:
|
139
|
+
The 4x4 matrix representing the transform.
|
140
|
+
"""
|
141
|
+
|
142
|
+
# Compute the necessary transforms.
|
143
|
+
L = idx_of_parent_link(model=model, frame_idx=frame_index)
|
144
|
+
W_H_L = js.link.transform(model=model, data=data, link_index=L)
|
145
|
+
|
146
|
+
# Get the static frame pose wrt the parent link.
|
147
|
+
frame = model.description.get().frames[frame_index - model.number_of_links()]
|
148
|
+
L_H_F = frame.pose
|
149
|
+
|
150
|
+
# Combine the transforms computing the frame pose.
|
151
|
+
return W_H_L @ L_H_F
|
152
|
+
|
153
|
+
|
154
|
+
@functools.partial(jax.jit, static_argnames=["frame_index", "output_vel_repr"])
|
155
|
+
def jacobian(
|
156
|
+
model: js.model.JaxSimModel,
|
157
|
+
data: js.data.JaxSimModelData,
|
158
|
+
*,
|
159
|
+
frame_index: jtp.IntLike,
|
160
|
+
output_vel_repr: VelRepr | None = None,
|
161
|
+
) -> jtp.Matrix:
|
162
|
+
"""
|
163
|
+
Compute the free-floating jacobian of the frame.
|
164
|
+
|
165
|
+
Args:
|
166
|
+
model: The model to consider.
|
167
|
+
data: The data of the considered model.
|
168
|
+
frame_index: The index of the frame.
|
169
|
+
output_vel_repr:
|
170
|
+
The output velocity representation of the free-floating jacobian.
|
171
|
+
|
172
|
+
Returns:
|
173
|
+
The 6×(6+n) free-floating jacobian of the frame.
|
174
|
+
|
175
|
+
Note:
|
176
|
+
The input representation of the free-floating jacobian is the active
|
177
|
+
velocity representation.
|
178
|
+
"""
|
179
|
+
|
180
|
+
output_vel_repr = (
|
181
|
+
output_vel_repr if output_vel_repr is not None else data.velocity_representation
|
182
|
+
)
|
183
|
+
|
184
|
+
# Get the index of the parent link.
|
185
|
+
L = idx_of_parent_link(model=model, frame_idx=frame_index)
|
186
|
+
|
187
|
+
# Compute the Jacobian of the parent link using body-fixed output representation.
|
188
|
+
L_J_WL = js.link.jacobian(
|
189
|
+
model=model, data=data, link_index=L, output_vel_repr=VelRepr.Body
|
190
|
+
)
|
191
|
+
|
192
|
+
# Adjust the output representation
|
193
|
+
match output_vel_repr:
|
194
|
+
case VelRepr.Inertial:
|
195
|
+
W_H_L = js.link.transform(model=model, data=data, link_index=L)
|
196
|
+
W_X_L = jaxlie.SE3.from_matrix(W_H_L).adjoint()
|
197
|
+
W_J_WL = W_X_L @ L_J_WL
|
198
|
+
O_J_WL_I = W_J_WL
|
199
|
+
|
200
|
+
case VelRepr.Body:
|
201
|
+
W_H_L = js.link.transform(model=model, data=data, link_index=L)
|
202
|
+
W_H_F = transform(model=model, data=data, frame_index=frame_index)
|
203
|
+
F_H_L = jaxsim.math.Transform.inverse(W_H_F) @ W_H_L
|
204
|
+
F_X_L = jaxlie.SE3.from_matrix(F_H_L).adjoint()
|
205
|
+
F_J_WL = F_X_L @ L_J_WL
|
206
|
+
O_J_WL_I = F_J_WL
|
207
|
+
|
208
|
+
case VelRepr.Mixed:
|
209
|
+
W_H_L = js.link.transform(model=model, data=data, link_index=L)
|
210
|
+
W_H_F = transform(model=model, data=data, frame_index=frame_index)
|
211
|
+
F_H_L = jaxsim.math.Transform.inverse(W_H_F) @ W_H_L
|
212
|
+
FW_H_F = W_H_F.at[0:3, 3].set(jnp.zeros(3))
|
213
|
+
FW_H_L = FW_H_F @ F_H_L
|
214
|
+
FW_X_L = jaxlie.SE3.from_matrix(FW_H_L).adjoint()
|
215
|
+
FW_J_WL = FW_X_L @ L_J_WL
|
216
|
+
O_J_WL_I = FW_J_WL
|
217
|
+
|
218
|
+
case _:
|
219
|
+
raise ValueError(output_vel_repr)
|
220
|
+
|
221
|
+
return O_J_WL_I
|
jaxsim/api/model.py
CHANGED
@@ -252,6 +252,16 @@ class JaxSimModel(JaxsimDataclass):
|
|
252
252
|
|
253
253
|
return self.kin_dyn_parameters.link_names
|
254
254
|
|
255
|
+
def frame_names(self) -> tuple[str, ...]:
|
256
|
+
"""
|
257
|
+
Return the names of the links in the model.
|
258
|
+
|
259
|
+
Returns:
|
260
|
+
The names of the links in the model.
|
261
|
+
"""
|
262
|
+
|
263
|
+
return tuple([frame.name for frame in self.description.get().frames])
|
264
|
+
|
255
265
|
|
256
266
|
# =====================
|
257
267
|
# Model post-processing
|
@@ -1,5 +1,6 @@
|
|
1
|
+
from __future__ import annotations
|
2
|
+
|
1
3
|
import dataclasses
|
2
|
-
from typing import List
|
3
4
|
|
4
5
|
import jax.numpy as jnp
|
5
6
|
import jax_dataclasses
|
@@ -16,39 +17,46 @@ class LinkDescription(JaxsimDataclass):
|
|
16
17
|
In-memory description of a robot link.
|
17
18
|
|
18
19
|
Attributes:
|
19
|
-
name
|
20
|
-
mass
|
21
|
-
inertia
|
22
|
-
index
|
23
|
-
parent
|
24
|
-
pose
|
25
|
-
children
|
20
|
+
name: The name of the link.
|
21
|
+
mass: The mass of the link.
|
22
|
+
inertia: The inertia tensor of the link.
|
23
|
+
index: An optional index for the link (it gets automatically assigned).
|
24
|
+
parent: The parent link of this link.
|
25
|
+
pose: The pose transformation matrix of the link.
|
26
|
+
children: List of child links.
|
26
27
|
"""
|
27
28
|
|
28
29
|
name: Static[str]
|
29
|
-
mass: float
|
30
|
-
inertia: jtp.Matrix
|
30
|
+
mass: float = dataclasses.field(repr=False)
|
31
|
+
inertia: jtp.Matrix = dataclasses.field(repr=False)
|
31
32
|
index: int | None = None
|
32
|
-
parent:
|
33
|
+
parent: LinkDescription = dataclasses.field(default=None, repr=False)
|
33
34
|
pose: jtp.Matrix = dataclasses.field(default_factory=lambda: jnp.eye(4), repr=False)
|
34
|
-
|
35
|
+
|
36
|
+
children: Static[list[LinkDescription]] = dataclasses.field(
|
35
37
|
default_factory=list, repr=False
|
36
38
|
)
|
37
39
|
|
38
40
|
def __hash__(self) -> int:
|
39
|
-
|
40
|
-
|
41
|
-
|
42
|
-
|
43
|
-
|
44
|
-
|
45
|
-
|
46
|
-
|
47
|
-
|
48
|
-
|
49
|
-
and self.children == other.children
|
41
|
+
|
42
|
+
return hash(
|
43
|
+
(
|
44
|
+
hash(self.name),
|
45
|
+
hash(float(self.mass)),
|
46
|
+
hash(tuple(self.inertia.flatten().tolist())),
|
47
|
+
hash(int(self.index)),
|
48
|
+
hash(self.parent),
|
49
|
+
hash(tuple(hash(c) for c in self.children)),
|
50
|
+
)
|
50
51
|
)
|
51
52
|
|
53
|
+
def __eq__(self, other: LinkDescription) -> bool:
|
54
|
+
|
55
|
+
if not isinstance(other, LinkDescription):
|
56
|
+
return False
|
57
|
+
|
58
|
+
return hash(self) == hash(other)
|
59
|
+
|
52
60
|
@property
|
53
61
|
def name_and_index(self) -> str:
|
54
62
|
"""
|
@@ -61,19 +69,19 @@ class LinkDescription(JaxsimDataclass):
|
|
61
69
|
return f"#{self.index}_<{self.name}>"
|
62
70
|
|
63
71
|
def lump_with(
|
64
|
-
self, link:
|
65
|
-
) ->
|
72
|
+
self, link: LinkDescription, lumped_H_removed: jtp.Matrix
|
73
|
+
) -> LinkDescription:
|
66
74
|
"""
|
67
75
|
Combine the current link with another link, preserving mass and inertia.
|
68
76
|
|
69
77
|
Args:
|
70
|
-
link
|
71
|
-
lumped_H_removed
|
78
|
+
link: The link to combine with.
|
79
|
+
lumped_H_removed: The transformation matrix between the two links.
|
72
80
|
|
73
81
|
Returns:
|
74
|
-
|
75
|
-
|
82
|
+
The combined link.
|
76
83
|
"""
|
84
|
+
|
77
85
|
# Get the 6D inertia of the link to remove
|
78
86
|
I_removed = link.inertia
|
79
87
|
|
@@ -1,16 +1,17 @@
|
|
1
1
|
jaxsim/__init__.py,sha256=OcrfoYS1DGcmAGqu2AqlCTiUVxcpi-IsVwcr_16x74Q,1789
|
2
|
-
jaxsim/_version.py,sha256=
|
2
|
+
jaxsim/_version.py,sha256=4Oh93XR_460KA2Rq1fEo8S7jdrJW940Uh6izGU9tdL0,426
|
3
3
|
jaxsim/logging.py,sha256=c4zhwBKf9eAYAHVp62kTEllqdsZgh0K-kPKVy8L3elU,1584
|
4
4
|
jaxsim/typing.py,sha256=MeuOCQtLAr-sPkvB_sU8FtwGNRirz1auCwIgRC-QZl8,646
|
5
|
-
jaxsim/api/__init__.py,sha256=
|
6
|
-
jaxsim/api/com.py,sha256=
|
5
|
+
jaxsim/api/__init__.py,sha256=8eV22t2S3UwNyCg8karPetG1dmX1VDBXkyv28_FwNQA,210
|
6
|
+
jaxsim/api/com.py,sha256=Yof6otFi-mLWAs1rqjmeNJTOWIH9gn7BdU5EIjiL6Ts,13481
|
7
7
|
jaxsim/api/common.py,sha256=DV-WZG28sikXopNv458aYvpLjmiAtFr5LRscOwXusuk,6640
|
8
8
|
jaxsim/api/contact.py,sha256=Ve4ZOWkLEBRgK3KhtICxKY7YzsxYvc3lO-pPRBjqSnY,8659
|
9
9
|
jaxsim/api/data.py,sha256=GXSCfq4_PsRZuUwsmcl18e2Ppdtu4ykYs0oWih8J2ZI,26775
|
10
|
+
jaxsim/api/frame.py,sha256=68liCmwFDikuP0GQOpo-nF7aVeHJ173e1USSOwskTP0,6246
|
10
11
|
jaxsim/api/joint.py,sha256=-5DogPg4g4mmLckyVIVNjwv-Rxz0IWS7_md9nDlhPWA,4581
|
11
12
|
jaxsim/api/kin_dyn_parameters.py,sha256=9xdHS3lDWSEdV0qAqCVoS6cSMUw17ebPJdbBfq4vjVU,25959
|
12
13
|
jaxsim/api/link.py,sha256=rypTwkMf9HJ5UuAtHRJh0LqqdJWcLKTtTjWcjduEsF0,9842
|
13
|
-
jaxsim/api/model.py,sha256=
|
14
|
+
jaxsim/api/model.py,sha256=7qBsneoc3dWNIsd6INzItGaIWSTgO98TQUZaeyVRrdk,53809
|
14
15
|
jaxsim/api/ode.py,sha256=6l-6i2YHagsQvR8Ac-_fmO6P0hBVT6NkHhwXnrdITEg,9785
|
15
16
|
jaxsim/api/ode_data.py,sha256=dwRFVPJ30XMmdUbPXEu7YxsQ97jZP4L4fd5ZzhrO5Ys,22184
|
16
17
|
jaxsim/api/references.py,sha256=Lvskf17r619KKxwCJP7hAAty2kaXgDXJX1uKqoDIDgo,15483
|
@@ -37,7 +38,7 @@ jaxsim/parsers/kinematic_graph.py,sha256=zFt7x7pPGJar36Azukdi1eI_sa1kMWD3B8kZqcH
|
|
37
38
|
jaxsim/parsers/descriptions/__init__.py,sha256=PbIlunVfb59pB5jSX97YVpMAANRZPRkJ0X-hS14rzv4,221
|
38
39
|
jaxsim/parsers/descriptions/collision.py,sha256=HUWwuRgI9KznY29FFw1_zU3bGigDEezrcPOJSxSJGNU,3382
|
39
40
|
jaxsim/parsers/descriptions/joint.py,sha256=r7teUWJyw62WS7qNCB_9FGQD1hvGHBepk-2LCSlmcSc,3052
|
40
|
-
jaxsim/parsers/descriptions/link.py,sha256=
|
41
|
+
jaxsim/parsers/descriptions/link.py,sha256=GC-6ZgRZuRVpcRo1sY6YaR8lkCHkR4DvHNs2Ydw_tn4,2887
|
41
42
|
jaxsim/parsers/descriptions/model.py,sha256=uO5xOJtViihVPnSSsmfQJvCh45ANyi9KYAzLOhH0R8g,8993
|
42
43
|
jaxsim/parsers/rod/__init__.py,sha256=G2vqlLajBLUc4gyzXwsEI2Wsi4TMOIF9bLDFeT6KrGU,92
|
43
44
|
jaxsim/parsers/rod/parser.py,sha256=Q13TOkmpU0SHpgSV8WRYWb290aPNNLsaz4eMlD4Mq5w,13525
|
@@ -57,8 +58,8 @@ jaxsim/utils/__init__.py,sha256=tnQq1_CavdfeKaLYt3pmO7Jk4MU2RwwQU_qICkjyoTY,197
|
|
57
58
|
jaxsim/utils/hashless.py,sha256=bFIwKeo9KiWwsY8QM55duEGGQOyyJ4jQyPcuqTLEp5k,297
|
58
59
|
jaxsim/utils/jaxsim_dataclass.py,sha256=h26timZ_XrBL_Q_oymv-DkQd-EcUiHn8QexAaZXBY9c,11396
|
59
60
|
jaxsim/utils/tracing.py,sha256=KDMoyVPlu2NJvFkhtZwq5AkqMMgajt3munvJom-vEjQ,650
|
60
|
-
jaxsim-0.2.1.
|
61
|
-
jaxsim-0.2.1.
|
62
|
-
jaxsim-0.2.1.
|
63
|
-
jaxsim-0.2.1.
|
64
|
-
jaxsim-0.2.1.
|
61
|
+
jaxsim-0.2.1.dev74.dist-info/LICENSE,sha256=eaYdFmdeMbiIoIiPzEK0MjP1S9wtFXjXNR5er49uLR0,1546
|
62
|
+
jaxsim-0.2.1.dev74.dist-info/METADATA,sha256=rj10ROK1KcakStKl-rEOQcaxVHOJfga3OZPiSdOhiNo,9744
|
63
|
+
jaxsim-0.2.1.dev74.dist-info/WHEEL,sha256=GJ7t_kWBFywbagK5eo9IoUwLW6oyOeTKmQ-9iHFVNxQ,92
|
64
|
+
jaxsim-0.2.1.dev74.dist-info/top_level.txt,sha256=LxGMA8FLtXjQ6oI7N5gd_R_oSUHxpXxUEOfT1xS_ni0,7
|
65
|
+
jaxsim-0.2.1.dev74.dist-info/RECORD,,
|
File without changes
|
File without changes
|
File without changes
|