jaxsim 0.4.3.dev181__py3-none-any.whl → 0.4.3.dev200__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/contact.py +58 -19
- jaxsim/api/data.py +3 -2
- jaxsim/api/model.py +8 -12
- jaxsim/mujoco/__init__.py +1 -0
- jaxsim/mujoco/loaders.py +1 -1
- jaxsim/mujoco/model.py +4 -3
- jaxsim/mujoco/utils.py +101 -0
- jaxsim/mujoco/visualizer.py +1 -1
- jaxsim/rbda/contacts/__init__.py +7 -0
- jaxsim/rbda/contacts/common.py +49 -3
- jaxsim/rbda/contacts/relaxed_rigid.py +133 -72
- jaxsim/rbda/contacts/rigid.py +94 -115
- jaxsim/rbda/contacts/soft.py +21 -37
- jaxsim/rbda/contacts/visco_elastic.py +3 -3
- {jaxsim-0.4.3.dev181.dist-info → jaxsim-0.4.3.dev200.dist-info}/METADATA +1 -1
- {jaxsim-0.4.3.dev181.dist-info → jaxsim-0.4.3.dev200.dist-info}/RECORD +20 -19
- {jaxsim-0.4.3.dev181.dist-info → jaxsim-0.4.3.dev200.dist-info}/WHEEL +1 -1
- {jaxsim-0.4.3.dev181.dist-info → jaxsim-0.4.3.dev200.dist-info}/LICENSE +0 -0
- {jaxsim-0.4.3.dev181.dist-info → jaxsim-0.4.3.dev200.dist-info}/top_level.txt +0 -0
jaxsim/rbda/contacts/rigid.py
CHANGED
@@ -13,6 +13,7 @@ from jaxsim import logging
|
|
13
13
|
from jaxsim.api.common import ModelDataWithVelocityRepresentation, VelRepr
|
14
14
|
from jaxsim.terrain import FlatTerrain, Terrain
|
15
15
|
|
16
|
+
from . import common
|
16
17
|
from .common import ContactModel, ContactsParams
|
17
18
|
|
18
19
|
try:
|
@@ -170,46 +171,6 @@ class RigidContacts(ContactModel):
|
|
170
171
|
_solver_options_values=tuple(solver_options.values()),
|
171
172
|
)
|
172
173
|
|
173
|
-
@staticmethod
|
174
|
-
def detect_contacts(
|
175
|
-
W_p_C: jtp.ArrayLike,
|
176
|
-
terrain_height: jtp.ArrayLike,
|
177
|
-
) -> tuple[jtp.Vector, jtp.Vector]:
|
178
|
-
"""
|
179
|
-
Detect contacts between the collidable points and the terrain.
|
180
|
-
|
181
|
-
Args:
|
182
|
-
W_p_C: The position of the collidable points.
|
183
|
-
terrain_height: The height of the terrain at the collidable point position.
|
184
|
-
|
185
|
-
Returns:
|
186
|
-
A tuple containing the activation state of the collidable points
|
187
|
-
and the contact penetration depth h.
|
188
|
-
"""
|
189
|
-
|
190
|
-
# TODO: reduce code duplication with js.contact.in_contact
|
191
|
-
def detect_contact(
|
192
|
-
W_p_C: jtp.ArrayLike,
|
193
|
-
terrain_height: jtp.FloatLike,
|
194
|
-
) -> tuple[jtp.Bool, jtp.Float]:
|
195
|
-
"""
|
196
|
-
Detect contacts between the collidable points and the terrain.
|
197
|
-
"""
|
198
|
-
|
199
|
-
# Unpack the position of the collidable point.
|
200
|
-
_, _, pz = W_p_C.squeeze()
|
201
|
-
|
202
|
-
inactive = pz > terrain_height
|
203
|
-
|
204
|
-
# Compute contact penetration depth
|
205
|
-
h = jnp.maximum(0.0, terrain_height - pz)
|
206
|
-
|
207
|
-
return inactive, h
|
208
|
-
|
209
|
-
inactive_collidable_points, h = jax.vmap(detect_contact)(W_p_C, terrain_height)
|
210
|
-
|
211
|
-
return inactive_collidable_points, h
|
212
|
-
|
213
174
|
@staticmethod
|
214
175
|
def compute_impact_velocity(
|
215
176
|
inactive_collidable_points: jtp.ArrayLike,
|
@@ -281,7 +242,7 @@ class RigidContacts(ContactModel):
|
|
281
242
|
*,
|
282
243
|
link_forces: jtp.MatrixLike | None = None,
|
283
244
|
joint_force_references: jtp.VectorLike | None = None,
|
284
|
-
) -> tuple[jtp.
|
245
|
+
) -> tuple[jtp.Matrix, tuple]:
|
285
246
|
"""
|
286
247
|
Compute the contact forces.
|
287
248
|
|
@@ -295,36 +256,41 @@ class RigidContacts(ContactModel):
|
|
295
256
|
Optional `(n_joints,)` vector of joint forces.
|
296
257
|
|
297
258
|
Returns:
|
298
|
-
A tuple containing the contact forces.
|
259
|
+
A tuple containing as first element the computed contact forces.
|
299
260
|
"""
|
300
261
|
|
301
262
|
# Initialize the model and data this contact model is operating on.
|
302
263
|
# This will raise an exception if either the contact model or the
|
303
264
|
# contact parameters are not compatible.
|
304
265
|
model, data = self.initialize_model_and_data(model=model, data=data)
|
266
|
+
assert isinstance(data.contacts_params, RigidContactsParams)
|
305
267
|
|
306
|
-
# Import qpax just in this method
|
268
|
+
# Import qpax privately just in this method.
|
307
269
|
import qpax
|
308
270
|
|
309
|
-
link_forces = (
|
310
|
-
link_forces
|
271
|
+
link_forces = jnp.atleast_2d(
|
272
|
+
jnp.array(link_forces, dtype=float).squeeze()
|
311
273
|
if link_forces is not None
|
312
274
|
else jnp.zeros((model.number_of_links(), 6))
|
313
275
|
)
|
314
276
|
|
315
|
-
joint_force_references = (
|
316
|
-
joint_force_references
|
277
|
+
joint_force_references = jnp.atleast_1d(
|
278
|
+
jnp.array(joint_force_references, dtype=float).squeeze()
|
317
279
|
if joint_force_references is not None
|
318
280
|
else jnp.zeros((model.number_of_joints(),))
|
319
281
|
)
|
320
282
|
|
321
|
-
# Compute kin-dyn quantities used in the contact model
|
283
|
+
# Compute kin-dyn quantities used in the contact model.
|
322
284
|
with data.switch_velocity_representation(VelRepr.Mixed):
|
285
|
+
|
286
|
+
BW_ν = data.generalized_velocity()
|
287
|
+
|
323
288
|
M = js.model.free_floating_mass_matrix(model=model, data=data)
|
289
|
+
|
324
290
|
J_WC = js.contact.jacobian(model=model, data=data)
|
291
|
+
J̇_WC = js.contact.jacobian_derivative(model=model, data=data)
|
292
|
+
|
325
293
|
W_H_C = js.contact.transforms(model=model, data=data)
|
326
|
-
J̇_WC_BW = js.contact.jacobian_derivative(model=model, data=data)
|
327
|
-
BW_ν = data.generalized_velocity()
|
328
294
|
|
329
295
|
# Compute the position and linear velocities (mixed representation) of
|
330
296
|
# all collidable points belonging to the robot.
|
@@ -332,23 +298,16 @@ class RigidContacts(ContactModel):
|
|
332
298
|
model=model, data=data
|
333
299
|
)
|
334
300
|
|
335
|
-
|
336
|
-
n_collidable_points = model.kin_dyn_parameters.contact_parameters.
|
301
|
+
# Get the number of collidable points.
|
302
|
+
n_collidable_points = len(model.kin_dyn_parameters.contact_parameters.body)
|
337
303
|
|
338
|
-
# Compute the
|
339
|
-
|
340
|
-
|
341
|
-
|
342
|
-
)
|
343
|
-
|
344
|
-
# Compute the Delassus matrix.
|
345
|
-
delassus_matrix = RigidContacts._delassus_matrix(M=M, J_WC=J_WC)
|
346
|
-
|
347
|
-
# Add regularization for better numerical conditioning.
|
348
|
-
delassus_matrix = delassus_matrix + self.regularization_delassus * jnp.eye(
|
349
|
-
delassus_matrix.shape[0]
|
304
|
+
# Compute the penetration depth and velocity of the collidable points.
|
305
|
+
# Note that this function considers the penetration in the normal direction.
|
306
|
+
δ, δ_dot, n̂ = jax.vmap(common.compute_penetration_data, in_axes=(0, 0, None))(
|
307
|
+
position, velocity, model.terrain
|
350
308
|
)
|
351
309
|
|
310
|
+
# Build a references object to simplify converting link forces.
|
352
311
|
references = js.references.JaxSimModelReferences.build(
|
353
312
|
model=model,
|
354
313
|
data=data,
|
@@ -357,10 +316,12 @@ class RigidContacts(ContactModel):
|
|
357
316
|
joint_force_references=joint_force_references,
|
358
317
|
)
|
359
318
|
|
319
|
+
# Compute the generalized free acceleration.
|
360
320
|
with (
|
361
321
|
references.switch_velocity_representation(VelRepr.Mixed),
|
362
322
|
data.switch_velocity_representation(VelRepr.Mixed),
|
363
323
|
):
|
324
|
+
|
364
325
|
BW_ν̇_free = jnp.hstack(
|
365
326
|
js.ode.system_acceleration(
|
366
327
|
model=model,
|
@@ -372,64 +333,74 @@ class RigidContacts(ContactModel):
|
|
372
333
|
)
|
373
334
|
)
|
374
335
|
|
336
|
+
# Compute the free linear acceleration of the collidable points.
|
337
|
+
# Since we use doubly-mixed jacobian, this corresponds to W_p̈_C.
|
375
338
|
free_contact_acc = RigidContacts._linear_acceleration_of_collidable_points(
|
376
339
|
BW_nu=BW_ν,
|
377
340
|
BW_nu_dot=BW_ν̇_free,
|
378
341
|
CW_J_WC_BW=J_WC,
|
379
|
-
CW_J_dot_WC_BW=J̇
|
342
|
+
CW_J_dot_WC_BW=J̇_WC,
|
380
343
|
).flatten()
|
381
344
|
|
382
|
-
# Compute stabilization term
|
383
|
-
ḣ = velocity[:, 2].squeeze()
|
345
|
+
# Compute stabilization term.
|
384
346
|
baumgarte_term = RigidContacts._compute_baumgarte_stabilization_term(
|
385
|
-
inactive_collidable_points=
|
386
|
-
|
387
|
-
|
388
|
-
|
389
|
-
|
347
|
+
inactive_collidable_points=(δ <= 0),
|
348
|
+
δ=δ,
|
349
|
+
δ_dot=δ_dot,
|
350
|
+
n=n̂,
|
351
|
+
K=data.contacts_params.K,
|
352
|
+
D=data.contacts_params.D,
|
390
353
|
).flatten()
|
391
354
|
|
392
|
-
|
355
|
+
# Compute the Delassus matrix.
|
356
|
+
delassus_matrix = RigidContacts._delassus_matrix(M=M, J_WC=J_WC)
|
357
|
+
|
358
|
+
# Initialize regularization term of the Delassus matrix for
|
359
|
+
# better numerical conditioning.
|
360
|
+
Iε = self.regularization_delassus * jnp.eye(delassus_matrix.shape[0])
|
361
|
+
|
362
|
+
# Construct the quadratic cost function.
|
363
|
+
Q = delassus_matrix + Iε
|
364
|
+
q = free_contact_acc - baumgarte_term
|
393
365
|
|
394
|
-
#
|
395
|
-
Q = delassus_matrix
|
396
|
-
q = free_contact_acc
|
366
|
+
# Construct the inequality constraints.
|
397
367
|
G = RigidContacts._compute_ineq_constraint_matrix(
|
398
|
-
inactive_collidable_points=
|
368
|
+
inactive_collidable_points=(δ <= 0), mu=data.contacts_params.mu
|
399
369
|
)
|
400
370
|
h_bounds = RigidContacts._compute_ineq_bounds(
|
401
371
|
n_collidable_points=n_collidable_points
|
402
372
|
)
|
373
|
+
|
374
|
+
# Construct the equality constraints.
|
403
375
|
A = jnp.zeros((0, 3 * n_collidable_points))
|
404
376
|
b = jnp.zeros((0,))
|
405
377
|
|
406
|
-
# Solve the optimization problem
|
407
|
-
|
378
|
+
# Solve the following optimization problem with qpax:
|
379
|
+
#
|
380
|
+
# min_{x} 0.5 x⊤ Q x + q⊤ x
|
381
|
+
#
|
382
|
+
# s.t. A x = b
|
383
|
+
# G x ≤ h
|
384
|
+
#
|
385
|
+
# TODO: add possibility to notify if the QP problem did not converge.
|
386
|
+
solution, _, _, _, converged, _ = qpax.solve_qp( # noqa: F841
|
408
387
|
Q=Q, q=q, A=A, b=b, G=G, h=h_bounds, **self.solver_options
|
409
388
|
)
|
410
389
|
|
411
|
-
|
412
|
-
|
413
|
-
# Transform linear contact forces to 6D
|
414
|
-
CW_f_C = jnp.hstack(
|
415
|
-
(
|
416
|
-
f_C_lin,
|
417
|
-
jnp.zeros((f_C_lin.shape[0], 3)),
|
418
|
-
)
|
419
|
-
)
|
390
|
+
# Reshape the optimized solution to be a matrix of 3D contact forces.
|
391
|
+
CW_fl_C = solution.reshape(-1, 3)
|
420
392
|
|
421
|
-
#
|
393
|
+
# Convert the contact forces from mixed to inertial-fixed representation.
|
422
394
|
W_f_C = jax.vmap(
|
423
|
-
lambda
|
424
|
-
|
425
|
-
|
426
|
-
|
427
|
-
|
395
|
+
lambda CW_fl_C, W_H_C: (
|
396
|
+
ModelDataWithVelocityRepresentation.other_representation_to_inertial(
|
397
|
+
array=jnp.zeros(6).at[0:3].set(CW_fl_C),
|
398
|
+
transform=W_H_C,
|
399
|
+
other_representation=VelRepr.Mixed,
|
400
|
+
is_force=True,
|
401
|
+
)
|
428
402
|
),
|
429
|
-
)(
|
430
|
-
CW_f_C,
|
431
|
-
W_H_C,
|
432
|
-
)
|
403
|
+
)(CW_fl_C, W_H_C)
|
433
404
|
|
434
405
|
return W_f_C, ()
|
435
406
|
|
@@ -438,6 +409,7 @@ class RigidContacts(ContactModel):
|
|
438
409
|
M: jtp.MatrixLike,
|
439
410
|
J_WC: jtp.MatrixLike,
|
440
411
|
) -> jtp.Matrix:
|
412
|
+
|
441
413
|
sl = jnp.s_[:, 0:3, :]
|
442
414
|
J_WC_lin = jnp.vstack(J_WC[sl])
|
443
415
|
|
@@ -448,6 +420,7 @@ class RigidContacts(ContactModel):
|
|
448
420
|
def _compute_ineq_constraint_matrix(
|
449
421
|
inactive_collidable_points: jtp.Vector, mu: jtp.FloatLike
|
450
422
|
) -> jtp.Matrix:
|
423
|
+
|
451
424
|
def compute_G_single_point(mu: float, c: float) -> jtp.Matrix:
|
452
425
|
"""
|
453
426
|
Compute the inequality constraint matrix for a single collidable point
|
@@ -475,6 +448,7 @@ class RigidContacts(ContactModel):
|
|
475
448
|
|
476
449
|
@staticmethod
|
477
450
|
def _compute_ineq_bounds(n_collidable_points: jtp.FloatLike) -> jtp.Vector:
|
451
|
+
|
478
452
|
n_constraints = 6 * n_collidable_points
|
479
453
|
return jnp.zeros(shape=(n_constraints,))
|
480
454
|
|
@@ -485,45 +459,50 @@ class RigidContacts(ContactModel):
|
|
485
459
|
CW_J_WC_BW: jtp.MatrixLike,
|
486
460
|
CW_J_dot_WC_BW: jtp.MatrixLike,
|
487
461
|
) -> jtp.Matrix:
|
488
|
-
|
462
|
+
|
489
463
|
BW_ν = BW_nu
|
490
464
|
BW_ν̇ = BW_nu_dot
|
465
|
+
CW_J̇_WC_BW = CW_J_dot_WC_BW
|
491
466
|
|
467
|
+
# Compute the linear acceleration of the collidable points.
|
468
|
+
# Since we use doubly-mixed jacobians, this corresponds to W_p̈_C.
|
492
469
|
CW_a_WC = jnp.vstack(CW_J̇_WC_BW) @ BW_ν + jnp.vstack(CW_J_WC_BW) @ BW_ν̇
|
493
|
-
CW_a_WC = CW_a_WC.reshape(-1, 6)
|
494
470
|
|
471
|
+
CW_a_WC = CW_a_WC.reshape(-1, 6)
|
495
472
|
return CW_a_WC[:, 0:3].squeeze()
|
496
473
|
|
497
474
|
@staticmethod
|
498
475
|
def _compute_baumgarte_stabilization_term(
|
499
476
|
inactive_collidable_points: jtp.ArrayLike,
|
500
|
-
|
501
|
-
|
477
|
+
δ: jtp.ArrayLike,
|
478
|
+
δ_dot: jtp.ArrayLike,
|
479
|
+
n: jtp.ArrayLike,
|
502
480
|
K: jtp.FloatLike,
|
503
481
|
D: jtp.FloatLike,
|
504
482
|
) -> jtp.Array:
|
505
|
-
|
483
|
+
|
484
|
+
def baumgarte_stabilization_of_single_point(
|
506
485
|
inactive: jtp.BoolLike,
|
507
|
-
|
508
|
-
|
486
|
+
δ: jtp.FloatLike,
|
487
|
+
δ_dot: jtp.FloatLike,
|
488
|
+
n: jtp.ArrayLike,
|
509
489
|
k_baumgarte: jtp.FloatLike,
|
510
490
|
d_baumgarte: jtp.FloatLike,
|
511
491
|
) -> jtp.Array:
|
492
|
+
|
512
493
|
baumgarte_term = jax.lax.cond(
|
513
494
|
inactive,
|
514
|
-
lambda
|
515
|
-
|
516
|
-
|
517
|
-
|
518
|
-
|
519
|
-
k_baumgarte,
|
520
|
-
d_baumgarte,
|
521
|
-
),
|
495
|
+
lambda δ, δ_dot, n, K, D: jnp.zeros(3),
|
496
|
+
# This is equivalent to: K*(pT - p)⋅n̂ + D*(0 - v)⋅n̂,
|
497
|
+
# where pT is the point on the terrain surface vertical to p.
|
498
|
+
lambda δ, δ_dot, n, K, D: (K * δ + D * δ_dot) * n,
|
499
|
+
*(δ, δ_dot, n, k_baumgarte, d_baumgarte),
|
522
500
|
)
|
501
|
+
|
523
502
|
return baumgarte_term
|
524
503
|
|
525
504
|
baumgarte_term = jax.vmap(
|
526
|
-
|
527
|
-
)(inactive_collidable_points,
|
505
|
+
baumgarte_stabilization_of_single_point, in_axes=(0, 0, 0, 0, None, None)
|
506
|
+
)(inactive_collidable_points, δ, δ_dot, n, K, D)
|
528
507
|
|
529
508
|
return baumgarte_term
|
jaxsim/rbda/contacts/soft.py
CHANGED
@@ -14,7 +14,7 @@ from jaxsim import logging
|
|
14
14
|
from jaxsim.math import StandardGravity
|
15
15
|
from jaxsim.terrain import FlatTerrain, Terrain
|
16
16
|
|
17
|
-
from .
|
17
|
+
from . import common
|
18
18
|
|
19
19
|
try:
|
20
20
|
from typing import Self
|
@@ -23,7 +23,7 @@ except ImportError:
|
|
23
23
|
|
24
24
|
|
25
25
|
@jax_dataclasses.pytree_dataclass
|
26
|
-
class SoftContactsParams(ContactsParams):
|
26
|
+
class SoftContactsParams(common.ContactsParams):
|
27
27
|
"""Parameters of the soft contacts model."""
|
28
28
|
|
29
29
|
K: jtp.Float = dataclasses.field(
|
@@ -161,7 +161,9 @@ class SoftContactsParams(ContactsParams):
|
|
161
161
|
f_average = m * g / number_of_active_collidable_points_steady_state
|
162
162
|
|
163
163
|
# Compute the stiffness to get the desired steady-state penetration.
|
164
|
-
|
164
|
+
# Note that this is dependent on the non-linear exponent used in
|
165
|
+
# the damping term of the Hunt/Crossley model.
|
166
|
+
K = f_average / jnp.power(δ_max, 1 + p)
|
165
167
|
|
166
168
|
# Compute the damping using the damping ratio.
|
167
169
|
critical_damping = 2 * jnp.sqrt(K * m)
|
@@ -189,7 +191,7 @@ class SoftContactsParams(ContactsParams):
|
|
189
191
|
|
190
192
|
|
191
193
|
@jax_dataclasses.pytree_dataclass
|
192
|
-
class SoftContacts(ContactModel):
|
194
|
+
class SoftContacts(common.ContactModel):
|
193
195
|
"""Soft contacts model."""
|
194
196
|
|
195
197
|
parameters: SoftContactsParams = dataclasses.field(
|
@@ -277,9 +279,7 @@ class SoftContacts(ContactModel):
|
|
277
279
|
μ = mu
|
278
280
|
|
279
281
|
# Compute the penetration depth, its rate, and the considered terrain normal.
|
280
|
-
δ, δ̇, n̂ =
|
281
|
-
p=W_p_C, v=W_ṗ_C, terrain=terrain
|
282
|
-
)
|
282
|
+
δ, δ̇, n̂ = common.compute_penetration_data(p=W_p_C, v=W_ṗ_C, terrain=terrain)
|
283
283
|
|
284
284
|
# There are few operations like computing the norm of a vector with zero length
|
285
285
|
# or computing the square root of zero that are problematic in an AD context.
|
@@ -423,7 +423,18 @@ class SoftContacts(ContactModel):
|
|
423
423
|
self,
|
424
424
|
model: js.model.JaxSimModel,
|
425
425
|
data: js.data.JaxSimModelData,
|
426
|
-
) -> tuple[jtp.
|
426
|
+
) -> tuple[jtp.Matrix, tuple[jtp.Matrix]]:
|
427
|
+
"""
|
428
|
+
Compute the contact forces.
|
429
|
+
|
430
|
+
Args:
|
431
|
+
model: The model to consider.
|
432
|
+
data: The data of the considered model.
|
433
|
+
|
434
|
+
Returns:
|
435
|
+
A tuple containing as first element the computed contact forces, and as
|
436
|
+
second element the derivative of the material deformation.
|
437
|
+
"""
|
427
438
|
|
428
439
|
# Initialize the model and data this contact model is operating on.
|
429
440
|
# This will raise an exception if either the contact model or the
|
@@ -444,36 +455,9 @@ class SoftContacts(ContactModel):
|
|
444
455
|
position=p,
|
445
456
|
velocity=v,
|
446
457
|
tangential_deformation=m,
|
447
|
-
parameters=
|
448
|
-
terrain=
|
458
|
+
parameters=data.contacts_params,
|
459
|
+
terrain=model.terrain,
|
449
460
|
)
|
450
461
|
)(W_p_C, W_ṗ_C, m)
|
451
462
|
|
452
463
|
return W_f, (ṁ,)
|
453
|
-
|
454
|
-
@staticmethod
|
455
|
-
@jax.jit
|
456
|
-
def compute_penetration_data(
|
457
|
-
p: jtp.VectorLike,
|
458
|
-
v: jtp.VectorLike,
|
459
|
-
terrain: jaxsim.terrain.Terrain,
|
460
|
-
) -> tuple[jtp.Float, jtp.Float, jtp.Vector]:
|
461
|
-
|
462
|
-
# Pre-process the position and the linear velocity of the collidable point.
|
463
|
-
W_ṗ_C = jnp.array(v).squeeze()
|
464
|
-
px, py, pz = jnp.array(p).squeeze()
|
465
|
-
|
466
|
-
# Compute the terrain normal and the contact depth.
|
467
|
-
n̂ = terrain.normal(x=px, y=py).squeeze()
|
468
|
-
h = jnp.array([0, 0, terrain.height(x=px, y=py) - pz])
|
469
|
-
|
470
|
-
# Compute the penetration depth normal to the terrain.
|
471
|
-
δ = jnp.maximum(0.0, jnp.dot(h, n̂))
|
472
|
-
|
473
|
-
# Compute the penetration normal velocity.
|
474
|
-
δ̇ = -jnp.dot(W_ṗ_C, n̂)
|
475
|
-
|
476
|
-
# Enforce the penetration rate to be zero when the penetration depth is zero.
|
477
|
-
δ̇ = jnp.where(δ > 0, δ̇, 0.0)
|
478
|
-
|
479
|
-
return δ, δ̇, n̂
|
@@ -195,7 +195,7 @@ class ViscoElasticContacts(common.ContactModel):
|
|
195
195
|
default_factory=FlatTerrain
|
196
196
|
)
|
197
197
|
|
198
|
-
max_squarings: jax_dataclasses.Static[int] = 25
|
198
|
+
max_squarings: jax_dataclasses.Static[int] = dataclasses.field(default=25)
|
199
199
|
|
200
200
|
@classmethod
|
201
201
|
def build(
|
@@ -239,7 +239,7 @@ class ViscoElasticContacts(common.ContactModel):
|
|
239
239
|
parameters=parameters,
|
240
240
|
terrain=terrain or cls.__dataclass_fields__["terrain"].default_factory(),
|
241
241
|
max_squarings=int(
|
242
|
-
max_squarings or cls.__dataclass_fields__["max_squarings"].default
|
242
|
+
max_squarings or cls.__dataclass_fields__["max_squarings"].default
|
243
243
|
),
|
244
244
|
)
|
245
245
|
|
@@ -266,7 +266,7 @@ class ViscoElasticContacts(common.ContactModel):
|
|
266
266
|
dt: jtp.FloatLike | None = None,
|
267
267
|
link_forces: jtp.MatrixLike | None = None,
|
268
268
|
joint_force_references: jtp.VectorLike | None = None,
|
269
|
-
) -> tuple[jtp.
|
269
|
+
) -> tuple[jtp.Matrix, tuple[jtp.Matrix, jtp.Matrix]]:
|
270
270
|
"""
|
271
271
|
Compute the contact forces.
|
272
272
|
|
@@ -1,6 +1,6 @@
|
|
1
1
|
Metadata-Version: 2.1
|
2
2
|
Name: jaxsim
|
3
|
-
Version: 0.4.3.
|
3
|
+
Version: 0.4.3.dev200
|
4
4
|
Summary: A differentiable physics engine and multibody dynamics library for control and robot learning.
|
5
5
|
Author-email: Diego Ferigo <dgferigo@gmail.com>
|
6
6
|
Maintainer-email: Diego Ferigo <dgferigo@gmail.com>, Filippo Luca Ferretti <filippo.ferretti@iit.it>
|
@@ -1,18 +1,18 @@
|
|
1
1
|
jaxsim/__init__.py,sha256=opgtbhhd1kDsHI4H1vOd3loMPDRi884yQ3tohfFGfNc,3382
|
2
|
-
jaxsim/_version.py,sha256=
|
2
|
+
jaxsim/_version.py,sha256=WDziMJEeSmuE81cozOtxmazlb4qAX6VPTrKOR0f3akg,428
|
3
3
|
jaxsim/exceptions.py,sha256=vSoScaRD4nvh6jltgK9Ry5pKnE0O5hb4_yI_pk_fvR8,2175
|
4
4
|
jaxsim/logging.py,sha256=STI-D_upXZYX-ZezLrlJJ0UlD5YspST0vZ_DcIwkzO4,1553
|
5
5
|
jaxsim/typing.py,sha256=2HXy9hgazPXjofi1vLQ09ZubPtgVmg80U9NKmZ6NYiI,761
|
6
6
|
jaxsim/api/__init__.py,sha256=8eV22t2S3UwNyCg8karPetG1dmX1VDBXkyv28_FwNQA,210
|
7
7
|
jaxsim/api/com.py,sha256=m-p3EJDhpnMTlXKplfbZE_aH9NqX_VyLlAE3vUhc6l4,13642
|
8
8
|
jaxsim/api/common.py,sha256=SNgxq42r6eF_-aPszvOjUYkGwXOzz4hKmhDwEUkscFQ,6650
|
9
|
-
jaxsim/api/contact.py,sha256=
|
10
|
-
jaxsim/api/data.py,sha256=
|
9
|
+
jaxsim/api/contact.py,sha256=Egc62310ljn5goXlswwJYSB-LyW6M5gmPoT_a3mkd7U,25812
|
10
|
+
jaxsim/api/data.py,sha256=gQX6hfEaw0ooJYvpr5f8UvEJwqhtflEK_NHWn9XgTZY,28935
|
11
11
|
jaxsim/api/frame.py,sha256=KS8A5wRfjxhe9NgcVo2QA516iP5zky7UVnWxG7nTa7c,12911
|
12
12
|
jaxsim/api/joint.py,sha256=lksT1Doxz2jknHyhb4ls20z6f6dofpZSzBJtVacZXAE,7129
|
13
13
|
jaxsim/api/kin_dyn_parameters.py,sha256=thJbz9XhpXgom23S6MXX2ugxGoAD-k947ZMAHDisy2w,29620
|
14
14
|
jaxsim/api/link.py,sha256=LAA6ZMQXkWomXeptURBtc7z3_xDZ2BBnBMhVrohh0bE,18621
|
15
|
-
jaxsim/api/model.py,sha256
|
15
|
+
jaxsim/api/model.py,sha256=s2i4obxMjZ_XntJgT0dEV57LCo0GIC7VppUnxsqC1fc,69704
|
16
16
|
jaxsim/api/ode.py,sha256=J_WuaoPl3ZY-yvTrCQun-rQoIAv_duynSXAGxqx93sg,14211
|
17
17
|
jaxsim/api/ode_data.py,sha256=1SD-x-lYk_YSEnVpxTLd69uOKC0mFUj44ZqpSmEDOxw,20190
|
18
18
|
jaxsim/api/references.py,sha256=fW77LitZ8DYgT6ZmUInJfm5luBV1mTcqcNRiC_i79og,20862
|
@@ -29,11 +29,12 @@ jaxsim/math/quaternion.py,sha256=_WA7W3iv7px83sWO1V1n0-J78hqAlO4SL1-jofE-UZ4,475
|
|
29
29
|
jaxsim/math/rotation.py,sha256=k-nwT79zmWrys3NNAB-lGWxat7Kqm_6JnFRoimJ8rBg,2156
|
30
30
|
jaxsim/math/skew.py,sha256=oOGSSR8PUGROl6IJFlrmu6K3gPH-u16hUPfKIkcVv9o,1177
|
31
31
|
jaxsim/math/transform.py,sha256=KXzQgOnCfAtbXCwxhplpJ3F0JT3oEyeLVby1_uRAryQ,2892
|
32
|
-
jaxsim/mujoco/__init__.py,sha256=
|
32
|
+
jaxsim/mujoco/__init__.py,sha256=fZyRWre49pIhOrYdf6yJk_hOax8qWGe8OCmoq-dMVq8,201
|
33
33
|
jaxsim/mujoco/__main__.py,sha256=GBmB7J-zj75ZnFyuAAmpSOpbxi_HhHhWJeot3ljGDJY,5291
|
34
|
-
jaxsim/mujoco/loaders.py,sha256=
|
35
|
-
jaxsim/mujoco/model.py,sha256=
|
36
|
-
jaxsim/mujoco/
|
34
|
+
jaxsim/mujoco/loaders.py,sha256=CkFGydgOku5P_Pz7wdWlM2SCJRs71ePF-vsY9i90-I0,25350
|
35
|
+
jaxsim/mujoco/model.py,sha256=5_7rWk_WBkNKDHqeewIFj0t2ZGqJpE6RDXHSbRvw4e4,16493
|
36
|
+
jaxsim/mujoco/utils.py,sha256=bGbLMSzcdqbinIwHHJHt8ZN1uup_6DLdB2dWqKiXwO4,3955
|
37
|
+
jaxsim/mujoco/visualizer.py,sha256=nD6SNWmn-nxjjjIY9oPAHvL2j8q93DJDjZeepzke_DQ,6988
|
37
38
|
jaxsim/parsers/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
|
38
39
|
jaxsim/parsers/kinematic_graph.py,sha256=wT2bgaCS8VQJTHy2H9sENkVPDOiMkRikxEF1t_WaahQ,34748
|
39
40
|
jaxsim/parsers/descriptions/__init__.py,sha256=PbIlunVfb59pB5jSX97YVpMAANRZPRkJ0X-hS14rzv4,221
|
@@ -52,20 +53,20 @@ jaxsim/rbda/forward_kinematics.py,sha256=2GmEoWsrioVl_SAbKRKfhOLz57pY4aR81PKRdul
|
|
52
53
|
jaxsim/rbda/jacobian.py,sha256=p0EV_8cLzLVV-93VKznT7VPuRj8W7h7rQWkPlWJXfCA,11023
|
53
54
|
jaxsim/rbda/rnea.py,sha256=CLfqs9XFVaD-hvkLABshDAfdw5bm_AMV3UVAQ_IvURQ,7542
|
54
55
|
jaxsim/rbda/utils.py,sha256=eeT21Y4DiiyhrdF0lUE_VvRuwru5-rR7yOlOlWzCCWE,5381
|
55
|
-
jaxsim/rbda/contacts/__init__.py,sha256=
|
56
|
-
jaxsim/rbda/contacts/common.py,sha256=
|
57
|
-
jaxsim/rbda/contacts/relaxed_rigid.py,sha256=
|
58
|
-
jaxsim/rbda/contacts/rigid.py,sha256=
|
59
|
-
jaxsim/rbda/contacts/soft.py,sha256=
|
60
|
-
jaxsim/rbda/contacts/visco_elastic.py,sha256=
|
56
|
+
jaxsim/rbda/contacts/__init__.py,sha256=L5MM-2pv76YPGzxExdz2EErgGBATuAjYnNHlq5QOySs,503
|
57
|
+
jaxsim/rbda/contacts/common.py,sha256=iywCQtesrnrwywRQv8cjyot2bG11dT_iONyF8OJztIA,5798
|
58
|
+
jaxsim/rbda/contacts/relaxed_rigid.py,sha256=TR81tJ4ipcpvPnwlfkpyNDhvWizpEG542SFVu_CwHRU,19614
|
59
|
+
jaxsim/rbda/contacts/rigid.py,sha256=3aDPFrIm2_QpKKRpTqJJk8qBK-W63gq7Arc8WDVAcHc,17382
|
60
|
+
jaxsim/rbda/contacts/soft.py,sha256=6eFgV2hJK793RZfoY8oSqw-zC1UqFldaE0hfGHELnmU,16325
|
61
|
+
jaxsim/rbda/contacts/visco_elastic.py,sha256=wATvBhLrV-7IyVLJhW7OaMg_HDAmczl_8MnYm3wuqSc,39819
|
61
62
|
jaxsim/terrain/__init__.py,sha256=f7lVX-iNpH_wkkjef9Qpjh19TTAUOQw76EiLYJDVizc,78
|
62
63
|
jaxsim/terrain/terrain.py,sha256=K91HEzPqTSyNrc_j1KfAAEF_5oDeuk_-jnnZGrcMEcY,5015
|
63
64
|
jaxsim/utils/__init__.py,sha256=Y5zyoRevl3EMVQadhZ4EtSwTEkDt2vcnFoRhPJjKTZ0,215
|
64
65
|
jaxsim/utils/jaxsim_dataclass.py,sha256=TGmTQV2Lq7Q-2nLoAEaeNtkPa_qj0IKkdBm4COj46Os,11312
|
65
66
|
jaxsim/utils/tracing.py,sha256=KDMoyVPlu2NJvFkhtZwq5AkqMMgajt3munvJom-vEjQ,650
|
66
67
|
jaxsim/utils/wrappers.py,sha256=Fh82ZcaFi5fUnByyFLnmumaobsu1hJIvFdopUVzJ1ps,4052
|
67
|
-
jaxsim-0.4.3.
|
68
|
-
jaxsim-0.4.3.
|
69
|
-
jaxsim-0.4.3.
|
70
|
-
jaxsim-0.4.3.
|
71
|
-
jaxsim-0.4.3.
|
68
|
+
jaxsim-0.4.3.dev200.dist-info/LICENSE,sha256=eaYdFmdeMbiIoIiPzEK0MjP1S9wtFXjXNR5er49uLR0,1546
|
69
|
+
jaxsim-0.4.3.dev200.dist-info/METADATA,sha256=NUJ6GXIFFK-y9-p-M2OTTdI3g7utYHa3Lsg2VXSXtoI,17276
|
70
|
+
jaxsim-0.4.3.dev200.dist-info/WHEEL,sha256=OVMc5UfuAQiSplgO0_WdW7vXVGAt9Hdd6qtN4HotdyA,91
|
71
|
+
jaxsim-0.4.3.dev200.dist-info/top_level.txt,sha256=LxGMA8FLtXjQ6oI7N5gd_R_oSUHxpXxUEOfT1xS_ni0,7
|
72
|
+
jaxsim-0.4.3.dev200.dist-info/RECORD,,
|
File without changes
|
File without changes
|