jaxsim 0.5.1.dev133__py3-none-any.whl → 0.5.1.dev143__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/__init__.py +0 -7
- jaxsim/_version.py +2 -2
- jaxsim/api/model.py +5 -7
- jaxsim/integrators/common.py +12 -9
- jaxsim/integrators/fixed_step.py +70 -40
- jaxsim/integrators/variable_step.py +73 -46
- jaxsim/math/adjoint.py +17 -11
- jaxsim/math/transform.py +9 -4
- jaxsim/rbda/contacts/rigid.py +27 -45
- {jaxsim-0.5.1.dev133.dist-info → jaxsim-0.5.1.dev143.dist-info}/METADATA +1 -1
- {jaxsim-0.5.1.dev133.dist-info → jaxsim-0.5.1.dev143.dist-info}/RECORD +14 -14
- {jaxsim-0.5.1.dev133.dist-info → jaxsim-0.5.1.dev143.dist-info}/LICENSE +0 -0
- {jaxsim-0.5.1.dev133.dist-info → jaxsim-0.5.1.dev143.dist-info}/WHEEL +0 -0
- {jaxsim-0.5.1.dev133.dist-info → jaxsim-0.5.1.dev143.dist-info}/top_level.txt +0 -0
jaxsim/__init__.py
CHANGED
@@ -34,13 +34,6 @@ def _jnp_options() -> None:
|
|
34
34
|
logging.info("Enabling JAX to use 64-bit precision")
|
35
35
|
jax.config.update("jax_enable_x64", True)
|
36
36
|
|
37
|
-
import jax.numpy as jnp
|
38
|
-
import numpy as np
|
39
|
-
|
40
|
-
# Verify that 64-bit precision is correctly set.
|
41
|
-
if jnp.empty(0, dtype=float).dtype != jnp.empty(0, dtype=np.float64).dtype:
|
42
|
-
logging.warning("Failed to enable 64-bit precision in JAX")
|
43
|
-
|
44
37
|
# Warn about experimental usage of 32-bit precision.
|
45
38
|
else:
|
46
39
|
logging.warning(
|
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.5.1.
|
16
|
-
__version_tuple__ = version_tuple = (0, 5, 1, '
|
15
|
+
__version__ = version = '0.5.1.dev143'
|
16
|
+
__version_tuple__ = version_tuple = (0, 5, 1, 'dev143')
|
jaxsim/api/model.py
CHANGED
@@ -2295,9 +2295,6 @@ def step(
|
|
2295
2295
|
# Hence, here we need to reset the velocity after each impact to guarantee that
|
2296
2296
|
# the linear velocity of the active collidable points is zero.
|
2297
2297
|
case jaxsim.rbda.contacts.RigidContacts():
|
2298
|
-
assert isinstance(
|
2299
|
-
data_tf.contacts_params, jaxsim.rbda.contacts.RigidContactsParams
|
2300
|
-
)
|
2301
2298
|
|
2302
2299
|
# Raise runtime error for not supported case in which Rigid contacts and
|
2303
2300
|
# Baumgarte stabilization are enabled and used with ForwardEuler integrator.
|
@@ -2331,12 +2328,13 @@ def step(
|
|
2331
2328
|
indices_of_enabled_collidable_points
|
2332
2329
|
]
|
2333
2330
|
M = js.model.free_floating_mass_matrix(model, data_tf)
|
2331
|
+
BW_ν_pre_impact = data_tf.generalized_velocity()
|
2334
2332
|
|
2335
2333
|
# Compute the impact velocity.
|
2336
2334
|
# It may be discontinuous in case new contacts are made.
|
2337
|
-
|
2335
|
+
BW_ν_post_impact = (
|
2338
2336
|
jaxsim.rbda.contacts.RigidContacts.compute_impact_velocity(
|
2339
|
-
|
2337
|
+
generalized_velocity=BW_ν_pre_impact,
|
2340
2338
|
inactive_collidable_points=(δ <= 0),
|
2341
2339
|
M=M,
|
2342
2340
|
J_WC=J_WC,
|
@@ -2344,8 +2342,8 @@ def step(
|
|
2344
2342
|
)
|
2345
2343
|
|
2346
2344
|
# Reset the generalized velocity.
|
2347
|
-
data_tf = data_tf.reset_base_velocity(
|
2348
|
-
data_tf = data_tf.reset_joint_velocities(
|
2345
|
+
data_tf = data_tf.reset_base_velocity(BW_ν_post_impact[0:6])
|
2346
|
+
data_tf = data_tf.reset_joint_velocities(BW_ν_post_impact[6:])
|
2349
2347
|
|
2350
2348
|
# Restore the input velocity representation.
|
2351
2349
|
data_tf = data_tf.replace(
|
jaxsim/integrators/common.py
CHANGED
@@ -170,14 +170,14 @@ class ExplicitRungeKutta(Integrator[PyTreeType, PyTreeType], Generic[PyTreeType]
|
|
170
170
|
"""
|
171
171
|
|
172
172
|
# The Runge-Kutta matrix.
|
173
|
-
A:
|
173
|
+
A: jtp.Matrix
|
174
174
|
|
175
175
|
# The weights coefficients.
|
176
176
|
# Note that in practice we typically use its transpose `b.transpose()`.
|
177
|
-
b:
|
177
|
+
b: jtp.Matrix
|
178
178
|
|
179
179
|
# The nodes coefficients.
|
180
|
-
c:
|
180
|
+
c: jtp.Vector
|
181
181
|
|
182
182
|
# Define the order of the solution.
|
183
183
|
# It should have as many elements as the number of rows of `b.transpose()`.
|
@@ -226,28 +226,31 @@ class ExplicitRungeKutta(Integrator[PyTreeType, PyTreeType], Generic[PyTreeType]
|
|
226
226
|
Returns:
|
227
227
|
The integrator object.
|
228
228
|
"""
|
229
|
+
A = cls.__dataclass_fields__["A"].default_factory()
|
230
|
+
b = cls.__dataclass_fields__["b"].default_factory()
|
231
|
+
c = cls.__dataclass_fields__["c"].default_factory()
|
229
232
|
|
230
233
|
# Check validity of the Butcher tableau.
|
231
|
-
if not ExplicitRungeKutta.butcher_tableau_is_valid(A=
|
234
|
+
if not ExplicitRungeKutta.butcher_tableau_is_valid(A=A, b=b, c=c):
|
232
235
|
raise ValueError("The Butcher tableau of this class is not valid.")
|
233
236
|
|
234
237
|
# Check that b.T has enough rows based on the configured index of the solution.
|
235
|
-
if cls.row_index_of_solution >=
|
238
|
+
if cls.row_index_of_solution >= b.T.shape[0]:
|
236
239
|
msg = "The index of the solution ({}-th row of `b.T`) is out of range ({})."
|
237
|
-
raise ValueError(msg.format(cls.row_index_of_solution,
|
240
|
+
raise ValueError(msg.format(cls.row_index_of_solution, b.T.shape[0]))
|
238
241
|
|
239
242
|
# Check that the tuple containing the order of the b.T rows matches the number
|
240
243
|
# of the b.T rows.
|
241
|
-
if len(cls.order_of_bT_rows) !=
|
244
|
+
if len(cls.order_of_bT_rows) != b.T.shape[0]:
|
242
245
|
msg = "Wrong size of 'order_of_bT_rows' ({}), should be {}."
|
243
|
-
raise ValueError(msg.format(len(cls.order_of_bT_rows),
|
246
|
+
raise ValueError(msg.format(len(cls.order_of_bT_rows), b.T.shape[0]))
|
244
247
|
|
245
248
|
# Check if the Butcher tableau supports FSAL (first-same-as-last).
|
246
249
|
# If it does, store the index of the intermediate derivative to be used as the
|
247
250
|
# first derivative of the next iteration.
|
248
251
|
has_fsal, index_of_fsal = ( # noqa: F841
|
249
252
|
ExplicitRungeKutta.butcher_tableau_supports_fsal(
|
250
|
-
A=
|
253
|
+
A=A, b=b, c=c, index_of_solution=cls.row_index_of_solution
|
251
254
|
)
|
252
255
|
)
|
253
256
|
|
jaxsim/integrators/fixed_step.py
CHANGED
@@ -1,3 +1,4 @@
|
|
1
|
+
import dataclasses
|
1
2
|
from typing import ClassVar, Generic
|
2
3
|
|
3
4
|
import jax.numpy as jnp
|
@@ -21,14 +22,21 @@ class ForwardEuler(ExplicitRungeKutta[PyTreeType], Generic[PyTreeType]):
|
|
21
22
|
Forward Euler integrator.
|
22
23
|
"""
|
23
24
|
|
24
|
-
A:
|
25
|
-
|
26
|
-
|
25
|
+
A: jtp.Matrix = dataclasses.field(
|
26
|
+
default_factory=lambda: jnp.atleast_2d(0).astype(float), compare=False
|
27
|
+
)
|
28
|
+
b: jtp.Matrix = dataclasses.field(
|
29
|
+
default_factory=lambda: jnp.atleast_2d(1).astype(float), compare=False
|
30
|
+
)
|
27
31
|
|
28
|
-
c:
|
32
|
+
c: jtp.Vector = dataclasses.field(
|
33
|
+
default_factory=lambda: jnp.atleast_1d(0).astype(float), compare=False
|
34
|
+
)
|
29
35
|
|
30
|
-
row_index_of_solution:
|
31
|
-
order_of_bT_rows:
|
36
|
+
row_index_of_solution: int = 0
|
37
|
+
order_of_bT_rows: tuple[int, ...] = (1,)
|
38
|
+
index_of_fsal: jtp.IntLike | None = None
|
39
|
+
fsal_enabled_if_supported: bool = False
|
32
40
|
|
33
41
|
|
34
42
|
@jax_dataclasses.pytree_dataclass
|
@@ -37,27 +45,38 @@ class Heun2(ExplicitRungeKutta[PyTreeType], Generic[PyTreeType]):
|
|
37
45
|
Heun's second-order integrator.
|
38
46
|
"""
|
39
47
|
|
40
|
-
A:
|
41
|
-
|
42
|
-
[
|
43
|
-
|
44
|
-
|
45
|
-
|
46
|
-
|
47
|
-
|
48
|
-
jnp.atleast_2d(
|
49
|
-
jnp.array([1 / 2, 1 / 2]),
|
50
|
-
)
|
51
|
-
.astype(float)
|
52
|
-
.transpose()
|
48
|
+
A: jtp.Matrix = dataclasses.field(
|
49
|
+
default_factory=lambda: jnp.array(
|
50
|
+
[
|
51
|
+
[0, 0],
|
52
|
+
[1, 0],
|
53
|
+
]
|
54
|
+
).astype(float),
|
55
|
+
compare=False,
|
53
56
|
)
|
54
57
|
|
55
|
-
|
56
|
-
|
57
|
-
|
58
|
+
b: jtp.Matrix = dataclasses.field(
|
59
|
+
default_factory=lambda: (
|
60
|
+
jnp.atleast_2d(
|
61
|
+
jnp.array([1 / 2, 1 / 2]),
|
62
|
+
)
|
63
|
+
.astype(float)
|
64
|
+
.transpose()
|
65
|
+
),
|
66
|
+
compare=False,
|
67
|
+
)
|
68
|
+
|
69
|
+
c: jtp.Vector = dataclasses.field(
|
70
|
+
default_factory=lambda: jnp.array(
|
71
|
+
[0, 1],
|
72
|
+
).astype(float),
|
73
|
+
compare=False,
|
74
|
+
)
|
58
75
|
|
59
76
|
row_index_of_solution: ClassVar[int] = 0
|
60
77
|
order_of_bT_rows: ClassVar[tuple[int, ...]] = (2,)
|
78
|
+
index_of_fsal: jtp.IntLike | None = None
|
79
|
+
fsal_enabled_if_supported: bool = False
|
61
80
|
|
62
81
|
|
63
82
|
@jax_dataclasses.pytree_dataclass
|
@@ -66,29 +85,40 @@ class RungeKutta4(ExplicitRungeKutta[PyTreeType], Generic[PyTreeType]):
|
|
66
85
|
Fourth-order Runge-Kutta integrator.
|
67
86
|
"""
|
68
87
|
|
69
|
-
A:
|
70
|
-
|
71
|
-
[
|
72
|
-
|
73
|
-
|
74
|
-
|
75
|
-
|
76
|
-
|
77
|
-
|
78
|
-
|
79
|
-
jnp.atleast_2d(
|
80
|
-
jnp.array([1 / 6, 1 / 3, 1 / 3, 1 / 6]),
|
81
|
-
)
|
82
|
-
.astype(float)
|
83
|
-
.transpose()
|
88
|
+
A: jtp.Matrix = dataclasses.field(
|
89
|
+
default_factory=lambda: jnp.array(
|
90
|
+
[
|
91
|
+
[0, 0, 0, 0],
|
92
|
+
[1 / 2, 0, 0, 0],
|
93
|
+
[0, 1 / 2, 0, 0],
|
94
|
+
[0, 0, 1, 0],
|
95
|
+
]
|
96
|
+
).astype(float),
|
97
|
+
compare=False,
|
84
98
|
)
|
85
99
|
|
86
|
-
|
87
|
-
|
88
|
-
|
100
|
+
b: jtp.Matrix = dataclasses.field(
|
101
|
+
default_factory=lambda: (
|
102
|
+
jnp.atleast_2d(
|
103
|
+
jnp.array([1 / 6, 1 / 3, 1 / 3, 1 / 6]),
|
104
|
+
)
|
105
|
+
.astype(float)
|
106
|
+
.transpose()
|
107
|
+
),
|
108
|
+
compare=False,
|
109
|
+
)
|
110
|
+
|
111
|
+
c: jtp.Vector = dataclasses.field(
|
112
|
+
default_factory=lambda: jnp.array(
|
113
|
+
[0, 1 / 2, 1 / 2, 1],
|
114
|
+
).astype(float),
|
115
|
+
compare=False,
|
116
|
+
)
|
89
117
|
|
90
118
|
row_index_of_solution: ClassVar[int] = 0
|
91
119
|
order_of_bT_rows: ClassVar[tuple[int, ...]] = (4,)
|
120
|
+
index_of_fsal: jtp.IntLike | None = None
|
121
|
+
fsal_enabled_if_supported: bool = False
|
92
122
|
|
93
123
|
|
94
124
|
# ===============================================================================
|
@@ -1,3 +1,4 @@
|
|
1
|
+
import dataclasses
|
1
2
|
import functools
|
2
3
|
from typing import Any, ClassVar, Generic
|
3
4
|
|
@@ -254,6 +255,9 @@ class EmbeddedRungeKutta(ExplicitRungeKutta[PyTreeType], Generic[PyTreeType]):
|
|
254
255
|
# Maximum number of rejected steps when the Δt needs to be reduced.
|
255
256
|
max_step_rejections: Static[jtp.IntLike] = MAX_STEP_REJECTIONS_DEFAULT
|
256
257
|
|
258
|
+
index_of_fsal: jtp.IntLike | None = None
|
259
|
+
fsal_enabled_if_supported: bool = False
|
260
|
+
|
257
261
|
def init(
|
258
262
|
self,
|
259
263
|
x0: State,
|
@@ -573,16 +577,18 @@ class EmbeddedRungeKutta(ExplicitRungeKutta[PyTreeType], Generic[PyTreeType]):
|
|
573
577
|
**kwargs: Additional parameters.
|
574
578
|
"""
|
575
579
|
|
580
|
+
b = cls.__dataclass_fields__["b"].default_factory()
|
581
|
+
|
576
582
|
# Check that b.T has enough rows based on the configured index of the
|
577
583
|
# solution estimate. This is necessary for embedded methods.
|
578
584
|
if (
|
579
585
|
cls.row_index_of_solution_estimate is not None
|
580
|
-
and cls.row_index_of_solution_estimate >=
|
586
|
+
and cls.row_index_of_solution_estimate >= b.T.shape[0]
|
581
587
|
):
|
582
588
|
msg = "The index of the solution estimate ({}-th row of `b.T`) "
|
583
589
|
msg += "is out of range ({})."
|
584
590
|
raise ValueError(
|
585
|
-
msg.format(cls.row_index_of_solution_estimate,
|
591
|
+
msg.format(cls.row_index_of_solution_estimate, b.T.shape[0])
|
586
592
|
)
|
587
593
|
|
588
594
|
integrator = super().build(
|
@@ -611,35 +617,47 @@ class HeunEulerSO3(EmbeddedRungeKutta[PyTreeType], ExplicitRungeKuttaSO3Mixin):
|
|
611
617
|
The Heun-Euler integrator for SO(3) dynamics.
|
612
618
|
"""
|
613
619
|
|
614
|
-
A:
|
615
|
-
|
616
|
-
[
|
617
|
-
|
618
|
-
|
619
|
-
|
620
|
-
|
621
|
-
|
622
|
-
jnp.atleast_2d(
|
623
|
-
jnp.array(
|
624
|
-
[
|
625
|
-
[1 / 2, 1 / 2],
|
626
|
-
[1, 0],
|
627
|
-
]
|
628
|
-
),
|
629
|
-
)
|
630
|
-
.astype(float)
|
631
|
-
.transpose()
|
620
|
+
A: jtp.Matrix = dataclasses.field(
|
621
|
+
default_factory=lambda: jnp.array(
|
622
|
+
[
|
623
|
+
[0, 0],
|
624
|
+
[1, 0],
|
625
|
+
]
|
626
|
+
).astype(float),
|
627
|
+
compare=False,
|
632
628
|
)
|
633
629
|
|
634
|
-
|
635
|
-
|
636
|
-
|
630
|
+
b: jtp.Matrix = dataclasses.field(
|
631
|
+
default_factory=lambda: (
|
632
|
+
jnp.atleast_2d(
|
633
|
+
jnp.array(
|
634
|
+
[
|
635
|
+
[1 / 2, 1 / 2],
|
636
|
+
[1, 0],
|
637
|
+
]
|
638
|
+
),
|
639
|
+
)
|
640
|
+
.astype(float)
|
641
|
+
.transpose()
|
642
|
+
),
|
643
|
+
compare=False,
|
644
|
+
)
|
645
|
+
|
646
|
+
c: jtp.Vector = dataclasses.field(
|
647
|
+
default_factory=lambda: jnp.array(
|
648
|
+
[0, 1],
|
649
|
+
).astype(float),
|
650
|
+
compare=False,
|
651
|
+
)
|
637
652
|
|
638
653
|
row_index_of_solution: ClassVar[int] = 0
|
639
654
|
row_index_of_solution_estimate: ClassVar[int | None] = 1
|
640
655
|
|
641
656
|
order_of_bT_rows: ClassVar[tuple[int, ...]] = (2, 1)
|
642
657
|
|
658
|
+
index_of_fsal: jtp.IntLike | None = None
|
659
|
+
fsal_enabled_if_supported: bool = False
|
660
|
+
|
643
661
|
|
644
662
|
@jax_dataclasses.pytree_dataclass
|
645
663
|
class BogackiShampineSO3(EmbeddedRungeKutta[PyTreeType], ExplicitRungeKuttaSO3Mixin):
|
@@ -647,31 +665,40 @@ class BogackiShampineSO3(EmbeddedRungeKutta[PyTreeType], ExplicitRungeKuttaSO3Mi
|
|
647
665
|
The Bogacki-Shampine integrator for SO(3) dynamics.
|
648
666
|
"""
|
649
667
|
|
650
|
-
A:
|
651
|
-
|
652
|
-
[
|
653
|
-
|
654
|
-
|
655
|
-
|
656
|
-
|
657
|
-
|
658
|
-
|
659
|
-
|
660
|
-
jnp.atleast_2d(
|
661
|
-
jnp.array(
|
662
|
-
[
|
663
|
-
[2 / 9, 1 / 3, 4 / 9, 0],
|
664
|
-
[7 / 24, 1 / 4, 1 / 3, 1 / 8],
|
665
|
-
]
|
666
|
-
),
|
667
|
-
)
|
668
|
-
.astype(float)
|
669
|
-
.transpose()
|
668
|
+
A: jtp.Matrix = dataclasses.field(
|
669
|
+
default_factory=lambda: jnp.array(
|
670
|
+
[
|
671
|
+
[0, 0, 0, 0],
|
672
|
+
[1 / 2, 0, 0, 0],
|
673
|
+
[0, 3 / 4, 0, 0],
|
674
|
+
[2 / 9, 1 / 3, 4 / 9, 0],
|
675
|
+
]
|
676
|
+
).astype(float),
|
677
|
+
compare=False,
|
670
678
|
)
|
671
679
|
|
672
|
-
|
673
|
-
|
674
|
-
|
680
|
+
b: jtp.Matrix = dataclasses.field(
|
681
|
+
default_factory=lambda: (
|
682
|
+
jnp.atleast_2d(
|
683
|
+
jnp.array(
|
684
|
+
[
|
685
|
+
[2 / 9, 1 / 3, 4 / 9, 0],
|
686
|
+
[7 / 24, 1 / 4, 1 / 3, 1 / 8],
|
687
|
+
]
|
688
|
+
),
|
689
|
+
)
|
690
|
+
.astype(float)
|
691
|
+
.transpose()
|
692
|
+
),
|
693
|
+
compare=False,
|
694
|
+
)
|
695
|
+
|
696
|
+
c: jtp.Vector = dataclasses.field(
|
697
|
+
default_factory=lambda: jnp.array(
|
698
|
+
[0, 1 / 2, 3 / 4, 1],
|
699
|
+
).astype(float),
|
700
|
+
compare=False,
|
701
|
+
)
|
675
702
|
|
676
703
|
row_index_of_solution: ClassVar[int] = 0
|
677
704
|
row_index_of_solution_estimate: ClassVar[int | None] = 1
|
jaxsim/math/adjoint.py
CHANGED
@@ -13,8 +13,8 @@ class Adjoint:
|
|
13
13
|
|
14
14
|
@staticmethod
|
15
15
|
def from_quaternion_and_translation(
|
16
|
-
quaternion: jtp.Vector
|
17
|
-
translation: jtp.Vector =
|
16
|
+
quaternion: jtp.Vector | None = None,
|
17
|
+
translation: jtp.Vector | None = None,
|
18
18
|
inverse: bool = False,
|
19
19
|
normalize_quaternion: bool = False,
|
20
20
|
) -> jtp.Matrix:
|
@@ -22,14 +22,17 @@ class Adjoint:
|
|
22
22
|
Create an adjoint matrix from a quaternion and a translation.
|
23
23
|
|
24
24
|
Args:
|
25
|
-
quaternion: A quaternion vector (4D) representing orientation.
|
26
|
-
translation: A translation vector (3D).
|
27
|
-
inverse: Whether to compute the inverse adjoint.
|
28
|
-
normalize_quaternion: Whether to normalize the quaternion before creating the adjoint.
|
25
|
+
quaternion (jtp.Vector): A quaternion vector (4D) representing orientation. Default is [1, 0, 0, 0].
|
26
|
+
translation (jtp.Vector): A translation vector (3D). Default is [0, 0, 0].
|
27
|
+
inverse (bool): Whether to compute the inverse adjoint. Default is False.
|
28
|
+
normalize_quaternion (bool): Whether to normalize the quaternion before creating the adjoint.
|
29
|
+
Default is False.
|
29
30
|
|
30
31
|
Returns:
|
31
32
|
jtp.Matrix: The adjoint matrix.
|
32
33
|
"""
|
34
|
+
quaternion = quaternion if quaternion is not None else jnp.array([1.0, 0, 0, 0])
|
35
|
+
translation = translation if translation is not None else jnp.zeros(3)
|
33
36
|
assert quaternion.size == 4
|
34
37
|
assert translation.size == 3
|
35
38
|
|
@@ -64,21 +67,24 @@ class Adjoint:
|
|
64
67
|
|
65
68
|
@staticmethod
|
66
69
|
def from_rotation_and_translation(
|
67
|
-
rotation: jtp.Matrix =
|
68
|
-
translation: jtp.Vector =
|
70
|
+
rotation: jtp.Matrix | None = None,
|
71
|
+
translation: jtp.Vector | None = None,
|
69
72
|
inverse: bool = False,
|
70
73
|
) -> jtp.Matrix:
|
71
74
|
"""
|
72
75
|
Create an adjoint matrix from a rotation matrix and a translation vector.
|
73
76
|
|
74
77
|
Args:
|
75
|
-
rotation: A 3x3 rotation matrix.
|
76
|
-
translation: A translation vector (3D).
|
77
|
-
inverse: Whether to compute the inverse adjoint. Default is False.
|
78
|
+
rotation (jtp.Matrix): A 3x3 rotation matrix. Default is identity.
|
79
|
+
translation (jtp.Vector): A translation vector (3D). Default is [0, 0, 0].
|
80
|
+
inverse (bool): Whether to compute the inverse adjoint. Default is False.
|
78
81
|
|
79
82
|
Returns:
|
80
83
|
jtp.Matrix: The adjoint matrix.
|
81
84
|
"""
|
85
|
+
rotation = rotation if rotation is not None else jnp.eye(3)
|
86
|
+
translation = translation if translation is not None else jnp.zeros(3)
|
87
|
+
|
82
88
|
assert rotation.shape == (3, 3)
|
83
89
|
assert translation.size == 3
|
84
90
|
|
jaxsim/math/transform.py
CHANGED
@@ -11,8 +11,8 @@ class Transform:
|
|
11
11
|
|
12
12
|
@staticmethod
|
13
13
|
def from_quaternion_and_translation(
|
14
|
-
quaternion: jtp.VectorLike
|
15
|
-
translation: jtp.VectorLike =
|
14
|
+
quaternion: jtp.VectorLike | None = None,
|
15
|
+
translation: jtp.VectorLike | None = None,
|
16
16
|
inverse: jtp.BoolLike = False,
|
17
17
|
normalize_quaternion: jtp.BoolLike = False,
|
18
18
|
) -> jtp.Matrix:
|
@@ -30,6 +30,9 @@ class Transform:
|
|
30
30
|
The 4x4 transformation matrix representing the SE(3) transformation.
|
31
31
|
"""
|
32
32
|
|
33
|
+
quaternion = quaternion if quaternion is not None else jnp.array([1.0, 0, 0, 0])
|
34
|
+
translation = translation if translation is not None else jnp.zeros(3)
|
35
|
+
|
33
36
|
W_Q_B = jnp.array(quaternion).astype(float)
|
34
37
|
W_p_B = jnp.array(translation).astype(float)
|
35
38
|
|
@@ -47,8 +50,8 @@ class Transform:
|
|
47
50
|
|
48
51
|
@staticmethod
|
49
52
|
def from_rotation_and_translation(
|
50
|
-
rotation: jtp.MatrixLike =
|
51
|
-
translation: jtp.VectorLike =
|
53
|
+
rotation: jtp.MatrixLike | None = None,
|
54
|
+
translation: jtp.VectorLike | None = None,
|
52
55
|
inverse: jtp.BoolLike = False,
|
53
56
|
) -> jtp.Matrix:
|
54
57
|
"""
|
@@ -62,6 +65,8 @@ class Transform:
|
|
62
65
|
Returns:
|
63
66
|
The 4x4 transformation matrix representing the SE(3) transformation.
|
64
67
|
"""
|
68
|
+
rotation = rotation if rotation is not None else jnp.eye(3)
|
69
|
+
translation = translation if translation is not None else jnp.zeros(3)
|
65
70
|
|
66
71
|
A_R_B = jnp.array(rotation).astype(float)
|
67
72
|
W_p_B = jnp.array(translation).astype(float)
|
jaxsim/rbda/contacts/rigid.py
CHANGED
@@ -173,7 +173,7 @@ class RigidContacts(ContactModel):
|
|
173
173
|
inactive_collidable_points: jtp.ArrayLike,
|
174
174
|
M: jtp.MatrixLike,
|
175
175
|
J_WC: jtp.MatrixLike,
|
176
|
-
|
176
|
+
generalized_velocity: jtp.VectorLike,
|
177
177
|
) -> jtp.Vector:
|
178
178
|
"""
|
179
179
|
Return the new velocity of the system after a potential impact.
|
@@ -182,55 +182,37 @@ class RigidContacts(ContactModel):
|
|
182
182
|
inactive_collidable_points: The activation state of the collidable points.
|
183
183
|
M: The mass matrix of the system (in mixed representation).
|
184
184
|
J_WC: The Jacobian matrix of the collidable points (in mixed representation).
|
185
|
-
|
186
|
-
"""
|
185
|
+
generalized_velocity: The generalized velocity of the system.
|
187
186
|
|
188
|
-
|
189
|
-
|
190
|
-
|
191
|
-
|
192
|
-
J_WC: jtp.MatrixLike,
|
193
|
-
data: js.data.JaxSimModelData,
|
194
|
-
):
|
195
|
-
# Compute system velocity after impact maintaining zero linear velocity of active points
|
196
|
-
with data.switch_velocity_representation(VelRepr.Mixed):
|
197
|
-
sl = jnp.s_[:, 0:3, :]
|
198
|
-
Jl_WC = J_WC[sl]
|
199
|
-
# Zero out the jacobian rows of inactive points
|
200
|
-
Jl_WC = jnp.vstack(
|
201
|
-
jnp.where(
|
202
|
-
inactive_collidable_points[:, jnp.newaxis, jnp.newaxis],
|
203
|
-
jnp.zeros_like(Jl_WC),
|
204
|
-
Jl_WC,
|
205
|
-
)
|
206
|
-
)
|
187
|
+
Note:
|
188
|
+
The mass matrix `M`, the Jacobian `J_WC`, and the generalized velocity `generalized_velocity`
|
189
|
+
must be expressed in the same velocity representation.
|
190
|
+
"""
|
207
191
|
|
208
|
-
|
209
|
-
|
210
|
-
|
211
|
-
|
212
|
-
|
213
|
-
|
214
|
-
|
215
|
-
|
216
|
-
|
217
|
-
|
218
|
-
|
192
|
+
# Compute system velocity after impact maintaining zero linear velocity of active points.
|
193
|
+
sl = jnp.s_[:, 0:3, :]
|
194
|
+
Jl_WC = J_WC[sl]
|
195
|
+
|
196
|
+
# Zero out the jacobian rows of inactive points.
|
197
|
+
Jl_WC = jnp.vstack(
|
198
|
+
jnp.where(
|
199
|
+
inactive_collidable_points[:, jnp.newaxis, jnp.newaxis],
|
200
|
+
jnp.zeros_like(Jl_WC),
|
201
|
+
Jl_WC,
|
202
|
+
)
|
203
|
+
)
|
219
204
|
|
220
|
-
|
205
|
+
A = jnp.vstack(
|
206
|
+
[
|
207
|
+
jnp.hstack([M, -Jl_WC.T]),
|
208
|
+
jnp.hstack([Jl_WC, jnp.zeros((Jl_WC.shape[0], Jl_WC.shape[0]))]),
|
209
|
+
]
|
210
|
+
)
|
211
|
+
b = jnp.hstack([M @ generalized_velocity, jnp.zeros(Jl_WC.shape[0])])
|
221
212
|
|
222
|
-
|
223
|
-
BW_ν_pre_impact = data.generalized_velocity()
|
224
|
-
|
225
|
-
BW_ν_post_impact = impact_velocity(
|
226
|
-
data=data,
|
227
|
-
inactive_collidable_points=inactive_collidable_points,
|
228
|
-
nu_pre=BW_ν_pre_impact,
|
229
|
-
M=M,
|
230
|
-
J_WC=J_WC,
|
231
|
-
)
|
213
|
+
BW_ν_post_impact = jnp.linalg.lstsq(A, b)[0]
|
232
214
|
|
233
|
-
return BW_ν_post_impact
|
215
|
+
return BW_ν_post_impact[0 : M.shape[0]]
|
234
216
|
|
235
217
|
@jax.jit
|
236
218
|
def compute_contact_forces(
|
@@ -1,6 +1,6 @@
|
|
1
1
|
Metadata-Version: 2.1
|
2
2
|
Name: jaxsim
|
3
|
-
Version: 0.5.1.
|
3
|
+
Version: 0.5.1.dev143
|
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>, Filippo Luca Ferretti <filippoluca.ferretti@outlook.com>
|
6
6
|
Maintainer-email: Filippo Luca Ferretti <filippo.ferretti@iit.it>, Alessandro Croci <alessandro.croci@iit.it>
|
@@ -1,5 +1,5 @@
|
|
1
|
-
jaxsim/__init__.py,sha256=
|
2
|
-
jaxsim/_version.py,sha256=
|
1
|
+
jaxsim/__init__.py,sha256=_8rbKOf3bwx-2ChEbspZxs_rZY0RqUcmWAftnEw1bfM,3401
|
2
|
+
jaxsim/_version.py,sha256=C6hy6WCYTd96YQCYmnbJx4KXtfGFrGyu22zvOPSNjVI,428
|
3
3
|
jaxsim/exceptions.py,sha256=qjfTjE9lXvD3-JCPQcxxiX2XSS8QegawzQ6ZuC2tc0Y,2638
|
4
4
|
jaxsim/logging.py,sha256=STI-D_upXZYX-ZezLrlJJ0UlD5YspST0vZ_DcIwkzO4,1553
|
5
5
|
jaxsim/typing.py,sha256=7msl8t5Jt09RNYfKdPJtpjLfWurldcycDappb045Eso,761
|
@@ -12,23 +12,23 @@ jaxsim/api/frame.py,sha256=d6pa6vywGDqfaJU76F_-yjLJs6R3mrjZ6B-KXPu6f3Q,14595
|
|
12
12
|
jaxsim/api/joint.py,sha256=AnqlNWmBOay-gsoo0y4AbfFQ2OCJm-8T1E0IMhZeLoY,7457
|
13
13
|
jaxsim/api/kin_dyn_parameters.py,sha256=BaOp7ICxWosDIdWVjh8-cdlX8mXM9IM8wzy2fHz8Ufc,30444
|
14
14
|
jaxsim/api/link.py,sha256=nHjffhNdi_xGkteMsqdb_hC9mdV9rNw7k3pl89Uhw_8,12798
|
15
|
-
jaxsim/api/model.py,sha256=
|
15
|
+
jaxsim/api/model.py,sha256=LSdPk1NrBHEP4avKh3K0ANIXkPHVF1AyzZj8TfDiQ00,80162
|
16
16
|
jaxsim/api/ode.py,sha256=XFi3gGRU2s-hqOpZEAuk7o4cxEa871V1LmGcvT5wf10,16056
|
17
17
|
jaxsim/api/ode_data.py,sha256=ggF1AVaLW5QuXrfpNsFs-voVcW6gZkxK2Xe9GiDmou0,13755
|
18
18
|
jaxsim/api/references.py,sha256=YkdZhRv8NoBC94qvpwn1w9_alVuxrfiZV5w5NHQIt-g,20737
|
19
19
|
jaxsim/integrators/__init__.py,sha256=hxvOD-VK_mmd6v31wtC-nb28AYve1gLuZCNLV9wS-Kg,103
|
20
|
-
jaxsim/integrators/common.py,sha256=
|
21
|
-
jaxsim/integrators/fixed_step.py,sha256=
|
22
|
-
jaxsim/integrators/variable_step.py,sha256=
|
20
|
+
jaxsim/integrators/common.py,sha256=sXc0HLmqiGluU-ffsNcAxVO-UA4QzS96pjef5N-5rHI,20174
|
21
|
+
jaxsim/integrators/fixed_step.py,sha256=roiRR1evnQMPmdvY5D_iRHTYlbOvkG4CM6JDSqlmTAU,4034
|
22
|
+
jaxsim/integrators/variable_step.py,sha256=pMYiMbaqQTlq2VF4Ca78ovxU06lMF1mrnrfzAahtvcg,24577
|
23
23
|
jaxsim/math/__init__.py,sha256=2T1WUU_chNBCvyvkKSdiesPlckbo-gXVbCZEGoF-W0I,381
|
24
|
-
jaxsim/math/adjoint.py,sha256=
|
24
|
+
jaxsim/math/adjoint.py,sha256=AT1iDFnryGxpulSZUpzl0kmm85fYo_3fN_smK_29mSc,4808
|
25
25
|
jaxsim/math/cross.py,sha256=ihL1Ss2XCqf6IiaRFfu5IvAVE4txrNZt0CZEYdf_UvM,1378
|
26
26
|
jaxsim/math/inertia.py,sha256=Bh92FlJvJMMZg8825mzEzV3sHAAufJOSaQST1ZnzgSQ,1631
|
27
27
|
jaxsim/math/joint_model.py,sha256=EzAveaG5B6ZnCFNUzN30KEQUVesd83lfWXJarYR-kUw,9989
|
28
28
|
jaxsim/math/quaternion.py,sha256=fO3VNrIoZrcchCXCv_Zn2Ad6-rcgrNzysRrn5raQWJE,4595
|
29
29
|
jaxsim/math/rotation.py,sha256=bl9WCbYyLKg6RyRkMaEBBTmARBs8pB-FGR0JVbfbaNE,2187
|
30
30
|
jaxsim/math/skew.py,sha256=FeyKPMxrGzf6c4fohLR-24deYutetT1jw2r43q2yJEo,1151
|
31
|
-
jaxsim/math/transform.py,sha256=
|
31
|
+
jaxsim/math/transform.py,sha256=ZIhcHa9KCxBWyDj_UTiy8qOfhavOtIaDK6aAofyx28M,3267
|
32
32
|
jaxsim/math/utils.py,sha256=2id1F6QOvkHkIF3Nuxuj_tz_kI0IYlrlgVQrETmXFfI,1058
|
33
33
|
jaxsim/mujoco/__init__.py,sha256=fZyRWre49pIhOrYdf6yJk_hOax8qWGe8OCmoq-dMVq8,201
|
34
34
|
jaxsim/mujoco/__main__.py,sha256=GBmB7J-zj75ZnFyuAAmpSOpbxi_HhHhWJeot3ljGDJY,5291
|
@@ -58,7 +58,7 @@ jaxsim/rbda/utils.py,sha256=GLt7XIl1ROkx0_fnBCKUHYdB9_IBF3Yi4OnkHSX3gxA,5365
|
|
58
58
|
jaxsim/rbda/contacts/__init__.py,sha256=L5MM-2pv76YPGzxExdz2EErgGBATuAjYnNHlq5QOySs,503
|
59
59
|
jaxsim/rbda/contacts/common.py,sha256=7ZveKD4ddhUaW_-7mU315zyFdCBgzo60TRK74SOdFpY,10574
|
60
60
|
jaxsim/rbda/contacts/relaxed_rigid.py,sha256=kwk1PoGtqE9e4n1ySNCIOoyZBSUkBj3BSKlgXFPZR6k,20563
|
61
|
-
jaxsim/rbda/contacts/rigid.py,sha256=
|
61
|
+
jaxsim/rbda/contacts/rigid.py,sha256=ISaUwE5vk4TlcDTYWd9q3SXT215_YrG5QgFf-SRBY6A,15469
|
62
62
|
jaxsim/rbda/contacts/soft.py,sha256=XDzHVNrw0gbX8e-3uHVEOAK2OQiwlrnCur5HXsnPitc,16960
|
63
63
|
jaxsim/rbda/contacts/visco_elastic.py,sha256=3CVnZpTZPjyaVO2O5-CiFeNvK3c8Gcw304EVXCcpUvA,39935
|
64
64
|
jaxsim/terrain/__init__.py,sha256=f7lVX-iNpH_wkkjef9Qpjh19TTAUOQw76EiLYJDVizc,78
|
@@ -67,8 +67,8 @@ jaxsim/utils/__init__.py,sha256=Y5zyoRevl3EMVQadhZ4EtSwTEkDt2vcnFoRhPJjKTZ0,215
|
|
67
67
|
jaxsim/utils/jaxsim_dataclass.py,sha256=Fxa555u14VUsVlKU1rBQFurrVzBp7BNsIaVoNko0lrI,11261
|
68
68
|
jaxsim/utils/tracing.py,sha256=Btwxdfhb7fJLk3r5PlQkGYj60Y2KbFT1gANGIA697FU,530
|
69
69
|
jaxsim/utils/wrappers.py,sha256=3IMwydqFgmSPqeuUQ3PRmdhDc1IoT6XC23jPC_LjWXs,4175
|
70
|
-
jaxsim-0.5.1.
|
71
|
-
jaxsim-0.5.1.
|
72
|
-
jaxsim-0.5.1.
|
73
|
-
jaxsim-0.5.1.
|
74
|
-
jaxsim-0.5.1.
|
70
|
+
jaxsim-0.5.1.dev143.dist-info/LICENSE,sha256=eaYdFmdeMbiIoIiPzEK0MjP1S9wtFXjXNR5er49uLR0,1546
|
71
|
+
jaxsim-0.5.1.dev143.dist-info/METADATA,sha256=CMgyPcqS76DRPvmIo9U3olZDYG3awyQQe79c-lJ4yVI,19484
|
72
|
+
jaxsim-0.5.1.dev143.dist-info/WHEEL,sha256=A3WOREP4zgxI0fKrHUG8DC8013e3dK3n7a6HDbcEIwE,91
|
73
|
+
jaxsim-0.5.1.dev143.dist-info/top_level.txt,sha256=LxGMA8FLtXjQ6oI7N5gd_R_oSUHxpXxUEOfT1xS_ni0,7
|
74
|
+
jaxsim-0.5.1.dev143.dist-info/RECORD,,
|
File without changes
|
File without changes
|
File without changes
|