jaxsim 0.6.2.dev109__py3-none-any.whl → 0.6.2.dev156__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 +9 -4
- jaxsim/api/integrators.py +116 -35
- jaxsim/api/kin_dyn_parameters.py +3 -3
- jaxsim/api/model.py +40 -32
- jaxsim/api/ode.py +5 -9
- jaxsim/mujoco/visualizer.py +23 -5
- jaxsim/parsers/descriptions/link.py +3 -8
- jaxsim/parsers/descriptions/model.py +4 -4
- jaxsim/parsers/kinematic_graph.py +17 -20
- jaxsim/parsers/rod/parser.py +1 -1
- jaxsim/parsers/rod/utils.py +9 -2
- jaxsim/rbda/utils.py +6 -0
- {jaxsim-0.6.2.dev109.dist-info → jaxsim-0.6.2.dev156.dist-info}/METADATA +48 -25
- {jaxsim-0.6.2.dev109.dist-info → jaxsim-0.6.2.dev156.dist-info}/RECORD +17 -17
- {jaxsim-0.6.2.dev109.dist-info → jaxsim-0.6.2.dev156.dist-info}/WHEEL +1 -1
- {jaxsim-0.6.2.dev109.dist-info → jaxsim-0.6.2.dev156.dist-info}/LICENSE +0 -0
- {jaxsim-0.6.2.dev109.dist-info → jaxsim-0.6.2.dev156.dist-info}/top_level.txt +0 -0
jaxsim/_version.py
CHANGED
@@ -1,8 +1,13 @@
|
|
1
|
-
# file generated by
|
1
|
+
# file generated by setuptools-scm
|
2
2
|
# don't change, don't track in version control
|
3
|
+
|
4
|
+
__all__ = ["__version__", "__version_tuple__", "version", "version_tuple"]
|
5
|
+
|
3
6
|
TYPE_CHECKING = False
|
4
7
|
if TYPE_CHECKING:
|
5
|
-
from typing import Tuple
|
8
|
+
from typing import Tuple
|
9
|
+
from typing import Union
|
10
|
+
|
6
11
|
VERSION_TUPLE = Tuple[Union[int, str], ...]
|
7
12
|
else:
|
8
13
|
VERSION_TUPLE = object
|
@@ -12,5 +17,5 @@ __version__: str
|
|
12
17
|
__version_tuple__: VERSION_TUPLE
|
13
18
|
version_tuple: VERSION_TUPLE
|
14
19
|
|
15
|
-
__version__ = version = '0.6.2.
|
16
|
-
__version_tuple__ = version_tuple = (0, 6, 2, '
|
20
|
+
__version__ = version = '0.6.2.dev156'
|
21
|
+
__version_tuple__ = version_tuple = (0, 6, 2, 'dev156')
|
jaxsim/api/integrators.py
CHANGED
@@ -1,76 +1,157 @@
|
|
1
1
|
import dataclasses
|
2
|
+
from collections.abc import Callable
|
2
3
|
|
4
|
+
import jax
|
3
5
|
import jax.numpy as jnp
|
4
6
|
|
5
7
|
import jaxsim
|
6
8
|
import jaxsim.api as js
|
7
9
|
import jaxsim.typing as jtp
|
8
10
|
from jaxsim.api.data import JaxSimModelData
|
9
|
-
from jaxsim.math import
|
11
|
+
from jaxsim.math import Skew
|
10
12
|
|
11
13
|
|
12
14
|
def semi_implicit_euler_integration(
|
13
15
|
model: js.model.JaxSimModel,
|
14
16
|
data: js.data.JaxSimModelData,
|
15
|
-
|
16
|
-
|
17
|
+
link_forces: jtp.Vector,
|
18
|
+
joint_torques: jtp.Vector,
|
17
19
|
) -> JaxSimModelData:
|
18
20
|
"""Integrate the system state using the semi-implicit Euler method."""
|
19
|
-
|
21
|
+
|
20
22
|
with data.switch_velocity_representation(jaxsim.VelRepr.Inertial):
|
21
23
|
|
22
|
-
|
23
|
-
W_v̇_WB =
|
24
|
-
|
24
|
+
# Compute the system acceleration
|
25
|
+
W_v̇_WB, s̈ = js.ode.system_acceleration(
|
26
|
+
model=model,
|
27
|
+
data=data,
|
28
|
+
link_forces=link_forces,
|
29
|
+
joint_torques=joint_torques,
|
30
|
+
)
|
25
31
|
|
26
|
-
|
27
|
-
BW_X_W = Adjoint.from_transform(B_H_W)
|
32
|
+
dt = model.time_step
|
28
33
|
|
34
|
+
# Compute the new generalized velocity.
|
29
35
|
new_generalized_acceleration = jnp.hstack([W_v̇_WB, s̈])
|
30
|
-
|
31
36
|
new_generalized_velocity = (
|
32
37
|
data.generalized_velocity + dt * new_generalized_acceleration
|
33
38
|
)
|
34
39
|
|
35
|
-
|
36
|
-
|
40
|
+
# Extract the new base and joint velocities.
|
41
|
+
W_v_B = new_generalized_velocity[0:6]
|
42
|
+
ṡ = new_generalized_velocity[6:]
|
37
43
|
|
38
|
-
|
44
|
+
# Compute the new base position and orientation.
|
45
|
+
W_ω_WB = new_generalized_velocity[3:6]
|
39
46
|
|
40
|
-
|
41
|
-
|
42
|
-
|
47
|
+
# To obtain the derivative of the base position, we need to subtract
|
48
|
+
# the skew-symmetric matrix of the base angular velocity times the base position.
|
49
|
+
# See: S. Traversaro and A. Saccon, “Multibody Dynamics Notation (Version 2), pg.9
|
50
|
+
W_ṗ_B = new_generalized_velocity[0:3] + Skew.wedge(W_ω_WB) @ data.base_position
|
43
51
|
|
44
|
-
|
52
|
+
W_Q̇_B = jaxsim.math.Quaternion.derivative(
|
45
53
|
quaternion=data.base_orientation,
|
46
|
-
omega=
|
54
|
+
omega=W_ω_WB,
|
47
55
|
omega_in_body_fixed=False,
|
48
56
|
).squeeze()
|
49
57
|
|
50
|
-
|
51
|
-
|
58
|
+
W_p_B = data.base_position + dt * W_ṗ_B
|
59
|
+
W_Q_B = data.base_orientation + dt * W_Q̇_B
|
52
60
|
|
53
|
-
base_quaternion_norm = jaxsim.math.safe_norm(
|
61
|
+
base_quaternion_norm = jaxsim.math.safe_norm(W_Q_B)
|
54
62
|
|
55
|
-
|
56
|
-
base_quaternion_norm == 0, 1.0, base_quaternion_norm
|
57
|
-
)
|
63
|
+
W_Q_B = W_Q_B / jnp.where(base_quaternion_norm == 0, 1.0, base_quaternion_norm)
|
58
64
|
|
59
|
-
|
65
|
+
s = data.joint_positions + dt * ṡ
|
60
66
|
|
61
67
|
# TODO: Avoid double replace, e.g. by computing cached value here
|
62
68
|
data = dataclasses.replace(
|
63
69
|
data,
|
64
|
-
_base_quaternion=
|
65
|
-
_base_position=
|
66
|
-
_joint_positions=
|
67
|
-
_joint_velocities=
|
68
|
-
_base_linear_velocity=
|
69
|
-
|
70
|
-
# it's equivalent to the one in inertial representation
|
71
|
-
# See: S. Traversaro and A. Saccon, “Multibody Dynamics Notation (Version 2), pg.9
|
72
|
-
_base_angular_velocity=base_ang_velocity_mixed,
|
70
|
+
_base_quaternion=W_Q_B,
|
71
|
+
_base_position=W_p_B,
|
72
|
+
_joint_positions=s,
|
73
|
+
_joint_velocities=ṡ,
|
74
|
+
_base_linear_velocity=W_v_B[0:3],
|
75
|
+
_base_angular_velocity=W_ω_WB,
|
73
76
|
)
|
74
|
-
|
77
|
+
|
78
|
+
# Update the cached computations.
|
79
|
+
data = data.replace(model=model)
|
75
80
|
|
76
81
|
return data
|
82
|
+
|
83
|
+
|
84
|
+
def rk4_integration(
|
85
|
+
model: js.model.JaxSimModel,
|
86
|
+
data: JaxSimModelData,
|
87
|
+
link_forces: jtp.Vector,
|
88
|
+
joint_torques: jtp.Vector,
|
89
|
+
) -> JaxSimModelData:
|
90
|
+
"""Integrate the system state using the Runge-Kutta 4 method."""
|
91
|
+
|
92
|
+
dt = model.time_step
|
93
|
+
|
94
|
+
def f(x) -> dict[str, jtp.Matrix]:
|
95
|
+
|
96
|
+
with data.switch_velocity_representation(jaxsim.VelRepr.Inertial):
|
97
|
+
|
98
|
+
data_ti = data.replace(model=model, **x)
|
99
|
+
|
100
|
+
return js.ode.system_dynamics(
|
101
|
+
model=model,
|
102
|
+
data=data_ti,
|
103
|
+
link_forces=link_forces,
|
104
|
+
joint_torques=joint_torques,
|
105
|
+
)
|
106
|
+
|
107
|
+
base_quaternion_norm = jaxsim.math.safe_norm(data._base_quaternion)
|
108
|
+
base_quaternion = data._base_quaternion / jnp.where(
|
109
|
+
base_quaternion_norm == 0, 1.0, base_quaternion_norm
|
110
|
+
)
|
111
|
+
|
112
|
+
x_t0 = dict(
|
113
|
+
base_position=data._base_position,
|
114
|
+
base_quaternion=base_quaternion,
|
115
|
+
joint_positions=data._joint_positions,
|
116
|
+
base_linear_velocity=data._base_linear_velocity,
|
117
|
+
base_angular_velocity=data._base_angular_velocity,
|
118
|
+
joint_velocities=data._joint_velocities,
|
119
|
+
)
|
120
|
+
|
121
|
+
euler_mid = lambda x, dxdt: x + (0.5 * dt) * dxdt
|
122
|
+
euler_fin = lambda x, dxdt: x + dt * dxdt
|
123
|
+
|
124
|
+
k1 = f(x_t0)
|
125
|
+
k2 = f(jax.tree.map(euler_mid, x_t0, k1))
|
126
|
+
k3 = f(jax.tree.map(euler_mid, x_t0, k2))
|
127
|
+
k4 = f(jax.tree.map(euler_fin, x_t0, k3))
|
128
|
+
|
129
|
+
# Average the slopes and compute the RK4 state derivative.
|
130
|
+
average = lambda k1, k2, k3, k4: (k1 + 2 * k2 + 2 * k3 + k4) / 6
|
131
|
+
|
132
|
+
dxdt = jax.tree_util.tree_map(average, k1, k2, k3, k4)
|
133
|
+
|
134
|
+
# Integrate the dynamics
|
135
|
+
x_tf = jax.tree_util.tree_map(euler_fin, x_t0, dxdt)
|
136
|
+
|
137
|
+
data_tf = dataclasses.replace(
|
138
|
+
data,
|
139
|
+
**{
|
140
|
+
"_base_position": x_tf["base_position"],
|
141
|
+
"_base_quaternion": x_tf["base_quaternion"],
|
142
|
+
"_joint_positions": x_tf["joint_positions"],
|
143
|
+
"_base_linear_velocity": x_tf["base_linear_velocity"],
|
144
|
+
"_base_angular_velocity": x_tf["base_angular_velocity"],
|
145
|
+
"_joint_velocities": x_tf["joint_velocities"],
|
146
|
+
},
|
147
|
+
)
|
148
|
+
|
149
|
+
return data_tf.replace(model=model)
|
150
|
+
|
151
|
+
|
152
|
+
_INTEGRATORS_MAP: dict[
|
153
|
+
js.model.IntegratorType, Callable[..., js.data.JaxSimModelData]
|
154
|
+
] = {
|
155
|
+
js.model.IntegratorType.SemiImplicitEuler: semi_implicit_euler_integration,
|
156
|
+
js.model.IntegratorType.RungeKutta4: rk4_integration,
|
157
|
+
}
|
jaxsim/api/kin_dyn_parameters.py
CHANGED
@@ -177,9 +177,9 @@ class KinDynParameters(JaxsimDataclass):
|
|
177
177
|
# Build the parent array λ(i) of the model.
|
178
178
|
# Note: the parent of the base link is not set since it's not defined.
|
179
179
|
parent_array_dict = {
|
180
|
-
link.index: link.
|
180
|
+
link.index: model_description.links_dict[link.parent_name].index
|
181
181
|
for link in ordered_links
|
182
|
-
if link.
|
182
|
+
if link.parent_name is not None
|
183
183
|
}
|
184
184
|
parent_array = jnp.array([-1, *list(parent_array_dict.values())], dtype=int)
|
185
185
|
|
@@ -862,7 +862,7 @@ class FrameParameters(JaxsimDataclass):
|
|
862
862
|
|
863
863
|
# For each frame, extract the index of the link to which it is attached to.
|
864
864
|
parent_link_index_of_frames = tuple(
|
865
|
-
model_description.links_dict[frame.
|
865
|
+
model_description.links_dict[frame.parent_name].index
|
866
866
|
for frame in model_description.frames
|
867
867
|
)
|
868
868
|
|
jaxsim/api/model.py
CHANGED
@@ -2,6 +2,7 @@ from __future__ import annotations
|
|
2
2
|
|
3
3
|
import copy
|
4
4
|
import dataclasses
|
5
|
+
import enum
|
5
6
|
import functools
|
6
7
|
import pathlib
|
7
8
|
from collections.abc import Sequence
|
@@ -23,14 +24,19 @@ from jaxsim.utils import JaxsimDataclass, Mutability, wrappers
|
|
23
24
|
from .common import VelRepr
|
24
25
|
|
25
26
|
|
27
|
+
class IntegratorType(enum.IntEnum):
|
28
|
+
"""The integrators available for the simulation."""
|
29
|
+
|
30
|
+
SemiImplicitEuler = enum.auto()
|
31
|
+
RungeKutta4 = enum.auto()
|
32
|
+
|
33
|
+
|
26
34
|
@jax_dataclasses.pytree_dataclass(eq=False, unsafe_hash=False)
|
27
35
|
class JaxSimModel(JaxsimDataclass):
|
28
36
|
"""
|
29
37
|
The JaxSim model defining the kinematics and dynamics of a robot.
|
30
38
|
"""
|
31
39
|
|
32
|
-
# link_spatial_inertial_matrices, motion_subspaces
|
33
|
-
|
34
40
|
model_name: Static[str]
|
35
41
|
|
36
42
|
time_step: float = dataclasses.field(
|
@@ -55,6 +61,10 @@ class JaxSimModel(JaxsimDataclass):
|
|
55
61
|
dataclasses.field(default=None, repr=False)
|
56
62
|
)
|
57
63
|
|
64
|
+
integrator: Static[IntegratorType] = dataclasses.field(
|
65
|
+
default=IntegratorType.SemiImplicitEuler, repr=False
|
66
|
+
)
|
67
|
+
|
58
68
|
built_from: Static[str | pathlib.Path | rod.Model | None] = dataclasses.field(
|
59
69
|
default=None, repr=False
|
60
70
|
)
|
@@ -111,6 +121,7 @@ class JaxSimModel(JaxsimDataclass):
|
|
111
121
|
terrain: jaxsim.terrain.Terrain | None = None,
|
112
122
|
contact_model: jaxsim.rbda.contacts.ContactModel | None = None,
|
113
123
|
contact_params: jaxsim.rbda.contacts.ContactsParams | None = None,
|
124
|
+
integrator: IntegratorType | None = None,
|
114
125
|
is_urdf: bool | None = None,
|
115
126
|
considered_joints: Sequence[str] | None = None,
|
116
127
|
) -> JaxSimModel:
|
@@ -131,6 +142,7 @@ class JaxSimModel(JaxsimDataclass):
|
|
131
142
|
The contact model to consider.
|
132
143
|
If not specified, a soft contacts model is used.
|
133
144
|
contact_params: The parameters of the contact model.
|
145
|
+
integrator: The integrator to use for the simulation.
|
134
146
|
is_urdf:
|
135
147
|
The optional flag to force the model description to be parsed as a URDF.
|
136
148
|
This is usually automatically inferred.
|
@@ -164,6 +176,7 @@ class JaxSimModel(JaxsimDataclass):
|
|
164
176
|
terrain=terrain,
|
165
177
|
contact_model=contact_model,
|
166
178
|
contacts_params=contact_params,
|
179
|
+
integrator=integrator,
|
167
180
|
)
|
168
181
|
|
169
182
|
# Store the origin of the model, in case downstream logic needs it.
|
@@ -182,6 +195,7 @@ class JaxSimModel(JaxsimDataclass):
|
|
182
195
|
terrain: jaxsim.terrain.Terrain | None = None,
|
183
196
|
contact_model: jaxsim.rbda.contacts.ContactModel | None = None,
|
184
197
|
contacts_params: jaxsim.rbda.contacts.ContactsParams | None = None,
|
198
|
+
integrator: IntegratorType | None = None,
|
185
199
|
gravity: jtp.FloatLike = jaxsim.math.STANDARD_GRAVITY,
|
186
200
|
) -> JaxSimModel:
|
187
201
|
"""
|
@@ -202,6 +216,7 @@ class JaxSimModel(JaxsimDataclass):
|
|
202
216
|
The contact model to consider.
|
203
217
|
If not specified, a soft contacts model is used.
|
204
218
|
contacts_params: The parameters of the soft contacts.
|
219
|
+
integrator: The integrator to use for the simulation.
|
205
220
|
gravity: The gravity constant.
|
206
221
|
|
207
222
|
Returns:
|
@@ -237,6 +252,13 @@ class JaxSimModel(JaxsimDataclass):
|
|
237
252
|
if contacts_params is None:
|
238
253
|
contacts_params = contact_model._parameters_class()
|
239
254
|
|
255
|
+
# Consider the default integrator if not specified.
|
256
|
+
integrator = (
|
257
|
+
integrator
|
258
|
+
if integrator is not None
|
259
|
+
else JaxSimModel.__dataclass_fields__["integrator"].default
|
260
|
+
)
|
261
|
+
|
240
262
|
# Build the model.
|
241
263
|
model = cls(
|
242
264
|
model_name=model_name,
|
@@ -247,6 +269,7 @@ class JaxSimModel(JaxsimDataclass):
|
|
247
269
|
terrain=terrain,
|
248
270
|
contact_model=contact_model,
|
249
271
|
contacts_params=contacts_params,
|
272
|
+
integrator=integrator,
|
250
273
|
gravity=gravity,
|
251
274
|
# The following is wrapped as hashless since it's a static argument, and we
|
252
275
|
# don't want to trigger recompilation if it changes. All relevant parameters
|
@@ -447,6 +470,9 @@ def reduce(
|
|
447
470
|
time_step=model.time_step,
|
448
471
|
terrain=model.terrain,
|
449
472
|
contact_model=model.contact_model,
|
473
|
+
contacts_params=model.contacts_params,
|
474
|
+
gravity=model.gravity,
|
475
|
+
integrator=model.integrator,
|
450
476
|
)
|
451
477
|
|
452
478
|
# Store the origin of the model, in case downstream logic needs it.
|
@@ -1420,14 +1446,10 @@ def free_floating_gravity_forces(
|
|
1420
1446
|
The free-floating gravity forces of the model.
|
1421
1447
|
"""
|
1422
1448
|
|
1423
|
-
# Build a zeroed
|
1424
|
-
data_rnea = js.data.JaxSimModelData.
|
1425
|
-
model=model, velocity_representation=data.velocity_representation
|
1426
|
-
)
|
1427
|
-
|
1428
|
-
# Set just the generalized position.
|
1429
|
-
data_rnea = data_rnea.replace(
|
1449
|
+
# Build a new state with zeroed velocities.
|
1450
|
+
data_rnea = js.data.JaxSimModelData.build(
|
1430
1451
|
model=model,
|
1452
|
+
velocity_representation=data.velocity_representation,
|
1431
1453
|
base_position=data.base_position,
|
1432
1454
|
base_quaternion=data.base_quaternion,
|
1433
1455
|
joint_positions=data.joint_positions,
|
@@ -1462,19 +1484,16 @@ def free_floating_bias_forces(
|
|
1462
1484
|
The free-floating bias forces of the model.
|
1463
1485
|
"""
|
1464
1486
|
|
1465
|
-
# Build a zeroed state.
|
1466
|
-
data_rnea = js.data.JaxSimModelData.zero(
|
1467
|
-
model=model, velocity_representation=data.velocity_representation
|
1468
|
-
)
|
1469
|
-
|
1470
1487
|
# Set the generalized position and generalized velocity.
|
1471
1488
|
base_linear_velocity, base_angular_velocity = None, None
|
1472
1489
|
if model.floating_base():
|
1473
1490
|
base_velocity = data.base_velocity
|
1474
1491
|
base_linear_velocity = base_velocity[:3]
|
1475
1492
|
base_angular_velocity = base_velocity[3:]
|
1476
|
-
|
1493
|
+
|
1494
|
+
data_rnea = js.data.JaxSimModelData.build(
|
1477
1495
|
model=model,
|
1496
|
+
velocity_representation=data.velocity_representation,
|
1478
1497
|
base_position=data.base_position,
|
1479
1498
|
base_quaternion=data.base_quaternion,
|
1480
1499
|
joint_positions=data.joint_positions,
|
@@ -2065,27 +2084,16 @@ def step(
|
|
2065
2084
|
|
2066
2085
|
W_f_L_total = W_f_L_external + W_f_L_terrain
|
2067
2086
|
|
2068
|
-
# ===============================
|
2069
|
-
# Compute the system acceleration
|
2070
|
-
# ===============================
|
2071
|
-
|
2072
|
-
with data.switch_velocity_representation(jaxsim.VelRepr.Inertial):
|
2073
|
-
W_v̇_WB, s̈ = js.ode.system_acceleration(
|
2074
|
-
model=model,
|
2075
|
-
data=data,
|
2076
|
-
link_forces=W_f_L_total,
|
2077
|
-
joint_torques=τ_total,
|
2078
|
-
)
|
2079
|
-
|
2080
2087
|
# =============================
|
2081
2088
|
# Advance the simulation state
|
2082
2089
|
# =============================
|
2083
2090
|
|
2084
|
-
|
2085
|
-
|
2086
|
-
|
2087
|
-
|
2088
|
-
|
2091
|
+
from .integrators import _INTEGRATORS_MAP
|
2092
|
+
|
2093
|
+
integrator_fn = _INTEGRATORS_MAP[model.integrator]
|
2094
|
+
|
2095
|
+
data_tf = integrator_fn(
|
2096
|
+
model=model, data=data, link_forces=W_f_L_total, joint_torques=τ_total
|
2089
2097
|
)
|
2090
2098
|
|
2091
2099
|
return data_tf
|
jaxsim/api/ode.py
CHANGED
@@ -3,7 +3,6 @@ import jax.numpy as jnp
|
|
3
3
|
|
4
4
|
import jaxsim.api as js
|
5
5
|
import jaxsim.typing as jtp
|
6
|
-
from jaxsim.api.data import JaxSimModelData
|
7
6
|
from jaxsim.math import Quaternion, Skew
|
8
7
|
|
9
8
|
from .common import VelRepr
|
@@ -123,7 +122,7 @@ def system_dynamics(
|
|
123
122
|
link_forces: jtp.Vector | None = None,
|
124
123
|
joint_torques: jtp.Vector | None = None,
|
125
124
|
baumgarte_quaternion_regularization: jtp.FloatLike = 1.0,
|
126
|
-
) ->
|
125
|
+
) -> dict[str, jtp.Vector]:
|
127
126
|
"""
|
128
127
|
Compute the dynamics of the system.
|
129
128
|
|
@@ -139,9 +138,9 @@ def system_dynamics(
|
|
139
138
|
quaternion (only used in integrators not operating on the SO(3) manifold).
|
140
139
|
|
141
140
|
Returns:
|
142
|
-
A
|
143
|
-
|
144
|
-
|
141
|
+
A dictionary containing the derivatives of the base position, the base quaternion,
|
142
|
+
the joint positions, the base linear velocity, the base angular velocity, and the
|
143
|
+
joint velocities.
|
145
144
|
"""
|
146
145
|
|
147
146
|
with data.switch_velocity_representation(velocity_representation=VelRepr.Inertial):
|
@@ -158,8 +157,7 @@ def system_dynamics(
|
|
158
157
|
baumgarte_quaternion_regularization=baumgarte_quaternion_regularization,
|
159
158
|
)
|
160
159
|
|
161
|
-
|
162
|
-
model=model,
|
160
|
+
return dict(
|
163
161
|
base_position=W_ṗ_B,
|
164
162
|
base_quaternion=W_Q̇_B,
|
165
163
|
joint_positions=ṡ,
|
@@ -167,5 +165,3 @@ def system_dynamics(
|
|
167
165
|
base_angular_velocity=W_v̇_WB[3:6],
|
168
166
|
joint_velocities=s̈,
|
169
167
|
)
|
170
|
-
|
171
|
-
return ode_state_derivative
|
jaxsim/mujoco/visualizer.py
CHANGED
@@ -46,7 +46,7 @@ class MujocoVideoRecorder:
|
|
46
46
|
|
47
47
|
self.fps = fps
|
48
48
|
self.frames: list[npt.NDArray] = []
|
49
|
-
self.data: mujoco.MjData | None = None
|
49
|
+
self.data: list[mujoco.MjData] | mujoco.MjData | None = None
|
50
50
|
self.model: mujoco.MjModel | None = None
|
51
51
|
self.reset(model=model, data=data)
|
52
52
|
|
@@ -56,20 +56,38 @@ class MujocoVideoRecorder:
|
|
56
56
|
)
|
57
57
|
|
58
58
|
def reset(
|
59
|
-
self,
|
59
|
+
self,
|
60
|
+
model: mj.MjModel | None = None,
|
61
|
+
data: list[mujoco.MjData] | mujoco.MjData | None = None,
|
60
62
|
) -> None:
|
61
63
|
"""Reset the model and data."""
|
62
64
|
|
63
65
|
self.frames = []
|
64
66
|
|
65
|
-
self.data = data if data is not None else self.data
|
67
|
+
self.data = [data] if data is not None else self.data
|
68
|
+
self.data = self.data if isinstance(self.data, list) else [self.data]
|
69
|
+
|
66
70
|
self.model = model if model is not None else self.model
|
67
71
|
|
68
72
|
def render_frame(self, camera_name: str = "track") -> npt.NDArray:
|
69
73
|
"""Render a frame."""
|
70
74
|
|
71
|
-
|
72
|
-
|
75
|
+
for idx, data in enumerate(self.data):
|
76
|
+
|
77
|
+
mujoco.mj_forward(self.model, data)
|
78
|
+
|
79
|
+
if idx == 0:
|
80
|
+
self.renderer.update_scene(data=data, camera=camera_name)
|
81
|
+
continue
|
82
|
+
|
83
|
+
mujoco.mjv_addGeoms(
|
84
|
+
m=self.model,
|
85
|
+
d=data,
|
86
|
+
opt=mujoco.MjvOption(),
|
87
|
+
pert=mujoco.MjvPerturb(),
|
88
|
+
catmask=mujoco.mjtCatBit.mjCAT_DYNAMIC,
|
89
|
+
scn=self.renderer.scene,
|
90
|
+
)
|
73
91
|
|
74
92
|
return self.renderer.render()
|
75
93
|
|
@@ -31,7 +31,7 @@ class LinkDescription(JaxsimDataclass):
|
|
31
31
|
mass: float = dataclasses.field(repr=False)
|
32
32
|
inertia: jtp.Matrix = dataclasses.field(repr=False)
|
33
33
|
index: int | None = None
|
34
|
-
|
34
|
+
parent_name: Static[str | None] = dataclasses.field(default=None, repr=False)
|
35
35
|
pose: jtp.Matrix = dataclasses.field(default_factory=lambda: jnp.eye(4), repr=False)
|
36
36
|
|
37
37
|
children: Static[tuple[LinkDescription]] = dataclasses.field(
|
@@ -50,8 +50,7 @@ class LinkDescription(JaxsimDataclass):
|
|
50
50
|
hash(int(self.index)) if self.index is not None else 0,
|
51
51
|
HashedNumpyArray.hash_of_array(self.pose),
|
52
52
|
hash(tuple(self.children)),
|
53
|
-
|
54
|
-
hash(self.parent.name) if self.parent is not None else 0,
|
53
|
+
hash(self.parent_name) if self.parent_name is not None else 0,
|
55
54
|
)
|
56
55
|
)
|
57
56
|
|
@@ -67,11 +66,7 @@ class LinkDescription(JaxsimDataclass):
|
|
67
66
|
and self.index == other.index
|
68
67
|
and np.allclose(self.pose, other.pose)
|
69
68
|
and self.children == other.children
|
70
|
-
and
|
71
|
-
(self.parent is not None and self.parent.name == other.parent.name)
|
72
|
-
if self.parent is not None
|
73
|
-
else other.parent is None
|
74
|
-
),
|
69
|
+
and self.parent_name == other.parent_name
|
75
70
|
):
|
76
71
|
return False
|
77
72
|
|
@@ -119,16 +119,16 @@ class ModelDescription(KinematicGraph):
|
|
119
119
|
for cp in collision_shape.collidable_points:
|
120
120
|
# Find the link that is part of the (reduced) model in which the
|
121
121
|
# collision shape's parent was lumped into
|
122
|
-
|
122
|
+
real_parent_link_name = kinematic_graph.frames_dict[
|
123
123
|
parent_link_of_shape.name
|
124
|
-
].
|
124
|
+
].parent_name
|
125
125
|
|
126
126
|
# Change the link associated to the collidable point, updating their
|
127
127
|
# relative pose
|
128
128
|
moved_cp = cp.change_link(
|
129
|
-
new_link=
|
129
|
+
new_link=kinematic_graph.links_dict[real_parent_link_name],
|
130
130
|
new_H_old=fk.relative_transform(
|
131
|
-
relative_to=
|
131
|
+
relative_to=real_parent_link_name,
|
132
132
|
name=cp.parent_link.name,
|
133
133
|
),
|
134
134
|
)
|
@@ -274,10 +274,10 @@ class KinematicGraph(Sequence[LinkDescription]):
|
|
274
274
|
|
275
275
|
# Check that our parser correctly resolved the frame's parent to be a link.
|
276
276
|
for frame in frames:
|
277
|
-
assert frame.
|
278
|
-
assert frame.
|
279
|
-
assert frame.
|
280
|
-
assert frame.
|
277
|
+
assert frame.parent_name != "", frame
|
278
|
+
assert frame.parent_name is not None, frame
|
279
|
+
assert frame.parent_name != "__model__", frame
|
280
|
+
assert frame.parent_name not in frames_dict, frame
|
281
281
|
|
282
282
|
# ===========================================================
|
283
283
|
# Populate the kinematic graph with links, joints, and frames
|
@@ -302,7 +302,7 @@ class KinematicGraph(Sequence[LinkDescription]):
|
|
302
302
|
assert parent_link.name == joint.parent.name
|
303
303
|
|
304
304
|
# Assign link's parent.
|
305
|
-
child_link.
|
305
|
+
child_link.parent_name = parent_link.name
|
306
306
|
|
307
307
|
# Assign link's children and make sure they are unique.
|
308
308
|
if child_link.name not in {l.name for l in parent_link.children}:
|
@@ -331,7 +331,7 @@ class KinematicGraph(Sequence[LinkDescription]):
|
|
331
331
|
# Collect all the frames of the kinematic graph.
|
332
332
|
# Note: our parser ensures that the parent of a frame is not another frame.
|
333
333
|
all_frames_in_graph = [
|
334
|
-
frame for frame in frames if frame.
|
334
|
+
frame for frame in frames if frame.parent_name in all_link_names_in_graph
|
335
335
|
]
|
336
336
|
|
337
337
|
# Get the names of all frames in the kinematic graph.
|
@@ -450,7 +450,7 @@ class KinematicGraph(Sequence[LinkDescription]):
|
|
450
450
|
|
451
451
|
# Get the link to remove and its parent, i.e. the lumped link
|
452
452
|
link_to_remove = links_dict[link.name]
|
453
|
-
parent_of_link_to_remove = links_dict[link.
|
453
|
+
parent_of_link_to_remove = links_dict[link.parent_name]
|
454
454
|
|
455
455
|
msg = "Lumping chain: {}->({})->{}"
|
456
456
|
logging.info(
|
@@ -586,7 +586,7 @@ class KinematicGraph(Sequence[LinkDescription]):
|
|
586
586
|
assert name_of_new_parent_link in reduced_graph, name_of_new_parent_link
|
587
587
|
|
588
588
|
# Notify the user if the parent link has changed.
|
589
|
-
if name_of_new_parent_link != frame.
|
589
|
+
if name_of_new_parent_link != frame.parent_name:
|
590
590
|
msg = "New parent of frame '{}' is '{}'"
|
591
591
|
logging.debug(msg=msg.format(frame.name, name_of_new_parent_link))
|
592
592
|
|
@@ -601,7 +601,7 @@ class KinematicGraph(Sequence[LinkDescription]):
|
|
601
601
|
)
|
602
602
|
|
603
603
|
# Update the parent link such that the pose is expressed in its frame.
|
604
|
-
frame.
|
604
|
+
frame.parent_name = name_of_new_parent_link
|
605
605
|
|
606
606
|
# Update dynamic parameters of the frame.
|
607
607
|
frame.mass = 0.0
|
@@ -880,7 +880,7 @@ class KinematicGraphTransforms:
|
|
880
880
|
|
881
881
|
# Get the joint between the link and its parent.
|
882
882
|
parent_joint = self.graph.joints_connection_dict[
|
883
|
-
link.
|
883
|
+
link.parent_name, link.name
|
884
884
|
]
|
885
885
|
|
886
886
|
# Get the transform of the parent joint.
|
@@ -901,7 +901,7 @@ class KinematicGraphTransforms:
|
|
901
901
|
frame = self.graph.frames_dict[name]
|
902
902
|
|
903
903
|
# Get the transform of the parent link.
|
904
|
-
M_H_L = self.transform(name=frame.
|
904
|
+
M_H_L = self.transform(name=frame.parent_name)
|
905
905
|
|
906
906
|
# Rename the pose of the frame w.r.t. its parent link.
|
907
907
|
L_H_F = frame.pose
|
@@ -971,13 +971,10 @@ class KinematicGraphTransforms:
|
|
971
971
|
except KeyError as e:
|
972
972
|
raise ValueError(f"Frame '{name}' not found in the kinematic graph") from e
|
973
973
|
|
974
|
-
|
975
|
-
|
976
|
-
|
974
|
+
if frame.parent_name in self.graph.links_dict:
|
975
|
+
return frame.parent_name
|
976
|
+
elif frame.parent_name in self.graph.frames_dict:
|
977
|
+
return self.find_parent_link_of_frame(name=frame.parent_name)
|
977
978
|
|
978
|
-
|
979
|
-
|
980
|
-
|
981
|
-
case _:
|
982
|
-
msg = f"Failed to find parent element of frame '{name}' with name '{frame.parent.name}'"
|
983
|
-
raise RuntimeError(msg)
|
979
|
+
msg = f"Failed to find parent element of frame '{name}' with name '{frame.parent_name}'"
|
980
|
+
raise RuntimeError(msg)
|
jaxsim/parsers/rod/parser.py
CHANGED
@@ -131,7 +131,7 @@ def extract_model_data(
|
|
131
131
|
name=f.name,
|
132
132
|
mass=jnp.array(0.0, dtype=float),
|
133
133
|
inertia=jnp.zeros(shape=(3, 3)),
|
134
|
-
|
134
|
+
parent_name=f.attached_to,
|
135
135
|
pose=f.pose.transform() if f.pose is not None else jnp.eye(4),
|
136
136
|
)
|
137
137
|
for f in sdf_model.frames()
|
jaxsim/parsers/rod/utils.py
CHANGED
@@ -123,7 +123,11 @@ def create_box_collision(
|
|
123
123
|
# Conditionally add the top corners based on the environment variable.
|
124
124
|
top_corners = (
|
125
125
|
np.array([[0, 0, z], [x, 0, z], [x, y, z], [0, y, z]])
|
126
|
-
if
|
126
|
+
if os.environ.get("JAXSIM_COLLISION_USE_BOTTOM_ONLY", "0").lower()
|
127
|
+
in {
|
128
|
+
"false",
|
129
|
+
"0",
|
130
|
+
}
|
127
131
|
else []
|
128
132
|
)
|
129
133
|
|
@@ -184,7 +188,10 @@ def create_sphere_collision(
|
|
184
188
|
]
|
185
189
|
|
186
190
|
# Filter to keep only the bottom half if required.
|
187
|
-
if os.environ.get("JAXSIM_COLLISION_USE_BOTTOM_ONLY", "0")
|
191
|
+
if os.environ.get("JAXSIM_COLLISION_USE_BOTTOM_ONLY", "0").lower() in {
|
192
|
+
"true",
|
193
|
+
"1",
|
194
|
+
}:
|
188
195
|
# Keep only the points with z <= 0.
|
189
196
|
points = [point for point in points if point[2] <= 0]
|
190
197
|
|
jaxsim/rbda/utils.py
CHANGED
@@ -132,6 +132,12 @@ def process_inputs(
|
|
132
132
|
if W_Q_B.shape != (4,):
|
133
133
|
raise ValueError(W_Q_B.shape, (4,))
|
134
134
|
|
135
|
+
# Check that the quaternion does not contain NaNs.
|
136
|
+
exceptions.raise_value_error_if(
|
137
|
+
condition=jnp.isnan(W_Q_B).any(),
|
138
|
+
msg="A RBDA received a quaternion that contains NaNs.",
|
139
|
+
)
|
140
|
+
|
135
141
|
# Check that the quaternion is unary since our RBDAs make this assumption in order
|
136
142
|
# to prevent introducing additional normalizations that would affect AD.
|
137
143
|
exceptions.raise_value_error_if(
|
@@ -1,6 +1,6 @@
|
|
1
1
|
Metadata-Version: 2.2
|
2
2
|
Name: jaxsim
|
3
|
-
Version: 0.6.2.
|
3
|
+
Version: 0.6.2.dev156
|
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>
|
@@ -98,8 +98,8 @@ Requires-Dist: jaxsim[style,testing,viz]; extra == "all"
|
|
98
98
|
<br/>
|
99
99
|
<table>
|
100
100
|
<tr>
|
101
|
-
<th><img src="https://github.com/user-attachments/assets/f9661fae-9a85-41dd-9a58-218758ec8c9c"
|
102
|
-
<th><img src="https://github.com/user-attachments/assets/62b88b9d-45ea-4d22-99d2-f24fc842dd29"
|
101
|
+
<th><img src="https://github.com/user-attachments/assets/f9661fae-9a85-41dd-9a58-218758ec8c9c"></th>
|
102
|
+
<th><img src="https://github.com/user-attachments/assets/62b88b9d-45ea-4d22-99d2-f24fc842dd29"></th>
|
103
103
|
</tr>
|
104
104
|
</table>
|
105
105
|
<br/>
|
@@ -119,13 +119,16 @@ Requires-Dist: jaxsim[style,testing,viz]; extra == "all"
|
|
119
119
|
|
120
120
|
|
121
121
|
```python
|
122
|
+
import pathlib
|
123
|
+
|
124
|
+
import icub_models
|
122
125
|
import jax.numpy as jnp
|
126
|
+
|
123
127
|
import jaxsim.api as js
|
124
|
-
import icub_models
|
125
|
-
import pathlib
|
126
128
|
|
127
129
|
# Load the iCub model
|
128
130
|
model_path = icub_models.get_model_file("iCubGazeboV2_5")
|
131
|
+
|
129
132
|
joints = ('torso_pitch', 'torso_roll', 'torso_yaw', 'l_shoulder_pitch',
|
130
133
|
'l_shoulder_roll', 'l_shoulder_yaw', 'l_elbow', 'r_shoulder_pitch',
|
131
134
|
'r_shoulder_roll', 'r_shoulder_yaw', 'r_elbow', 'l_hip_pitch',
|
@@ -135,33 +138,45 @@ joints = ('torso_pitch', 'torso_roll', 'torso_yaw', 'l_shoulder_pitch',
|
|
135
138
|
|
136
139
|
# Build and reduce the model
|
137
140
|
model_description = pathlib.Path(model_path)
|
141
|
+
|
138
142
|
full_model = js.model.JaxSimModel.build_from_model_description(
|
139
143
|
model_description=model_description, time_step=0.0001, is_urdf=True
|
140
144
|
)
|
145
|
+
|
141
146
|
model = js.model.reduce(model=full_model, considered_joints=joints)
|
142
147
|
|
148
|
+
# Get the number of degrees of freedom
|
143
149
|
ndof = model.dofs()
|
150
|
+
|
144
151
|
# Initialize data and simulation
|
145
152
|
# Note that the default data representation is mixed velocity representation
|
146
|
-
data = js.data.JaxSimModelData.build(
|
153
|
+
data = js.data.JaxSimModelData.build(
|
154
|
+
model=model, base_position=jnp.array([0.0, 0.0, 1.0])
|
155
|
+
)
|
156
|
+
|
147
157
|
T = jnp.arange(start=0, stop=1.0, step=model.time_step)
|
158
|
+
|
148
159
|
tau = jnp.zeros(ndof)
|
149
160
|
|
150
161
|
# Simulate
|
151
|
-
for
|
152
|
-
data = js.model.step(
|
153
|
-
|
162
|
+
for _ in T:
|
163
|
+
data = js.model.step(
|
164
|
+
model=model, data=data, link_forces=None, joint_force_references=tau
|
165
|
+
)
|
154
166
|
```
|
155
167
|
|
156
168
|
### Using JaxSim as a multibody dynamics library
|
157
169
|
``` python
|
170
|
+
import pathlib
|
171
|
+
|
172
|
+
import icub_models
|
158
173
|
import jax.numpy as jnp
|
174
|
+
|
159
175
|
import jaxsim.api as js
|
160
|
-
import icub_models
|
161
|
-
import pathlib
|
162
176
|
|
163
177
|
# Load the iCub model
|
164
178
|
model_path = icub_models.get_model_file("iCubGazeboV2_5")
|
179
|
+
|
165
180
|
joints = ('torso_pitch', 'torso_roll', 'torso_yaw', 'l_shoulder_pitch',
|
166
181
|
'l_shoulder_roll', 'l_shoulder_yaw', 'l_elbow', 'r_shoulder_pitch',
|
167
182
|
'r_shoulder_roll', 'r_shoulder_yaw', 'r_elbow', 'l_hip_pitch',
|
@@ -171,21 +186,31 @@ joints = ('torso_pitch', 'torso_roll', 'torso_yaw', 'l_shoulder_pitch',
|
|
171
186
|
|
172
187
|
# Build and reduce the model
|
173
188
|
model_description = pathlib.Path(model_path)
|
189
|
+
|
174
190
|
full_model = js.model.JaxSimModel.build_from_model_description(
|
175
191
|
model_description=model_description, time_step=0.0001, is_urdf=True
|
176
192
|
)
|
193
|
+
|
177
194
|
model = js.model.reduce(model=full_model, considered_joints=joints)
|
178
195
|
|
179
196
|
# Initialize model data
|
180
197
|
data = js.data.JaxSimModelData.build(
|
181
198
|
model=model,
|
182
|
-
base_position=jnp.array([0.0, 0.0, 1.0],
|
199
|
+
base_position=jnp.array([0.0, 0.0, 1.0]),
|
183
200
|
)
|
184
201
|
|
185
202
|
# Frame and dynamics computations
|
186
203
|
frame_index = js.frame.name_to_idx(model=model, frame_name="l_foot")
|
187
|
-
|
188
|
-
|
204
|
+
|
205
|
+
# Frame transformation
|
206
|
+
W_H_F = js.frame.transform(
|
207
|
+
model=model, data=data, frame_index=frame_index
|
208
|
+
)
|
209
|
+
|
210
|
+
# Frame Jacobian
|
211
|
+
W_J_F = js.frame.jacobian(
|
212
|
+
model=model, data=data, frame_index=frame_index
|
213
|
+
)
|
189
214
|
|
190
215
|
# Dynamics properties
|
191
216
|
M = js.model.free_floating_mass_matrix(model=model, data=data) # Mass matrix
|
@@ -194,16 +219,15 @@ g = js.model.free_floating_gravity_forces(model=model, data=data) # Gravity for
|
|
194
219
|
C = js.model.free_floating_coriolis_matrix(model=model, data=data) # Coriolis matrix
|
195
220
|
|
196
221
|
# Print dynamics results
|
197
|
-
print(f"
|
198
|
-
|
222
|
+
print(f"{M.shape=} \n{h.shape=} \n{g.shape=} \n{C.shape=}")
|
199
223
|
```
|
224
|
+
|
200
225
|
### Additional features
|
201
226
|
|
202
227
|
- Full support for automatic differentiation of RBDAs (forward and reverse modes) with JAX.
|
203
228
|
- Support for automatically differentiating against kinematics and dynamics parameters.
|
204
229
|
- All fixed-step integrators are forward and reverse differentiable.
|
205
|
-
-
|
206
|
-
- Check the example folder for additional usecase !
|
230
|
+
- Check the example folder for additional use cases!
|
207
231
|
|
208
232
|
[jax]: https://github.com/google/jax/
|
209
233
|
[sdformat]: https://github.com/gazebosim/sdformat
|
@@ -335,23 +359,21 @@ The JaxSim API documentation is available at [jaxsim.readthedocs.io][readthedocs
|
|
335
359
|
|
336
360
|
src/jaxsim
|
337
361
|
|-- api..........................# Package containing the main functional APIs.
|
338
|
-
| |--
|
362
|
+
| |-- actuation_model.py.......# |-- APIs for computing quantities related to the actuation model.
|
339
363
|
| |-- common.py................# |-- Common utilities used in the current package.
|
364
|
+
| |-- com.py...................# |-- APIs for computing quantities related to the center of mass.
|
365
|
+
| |-- contact_model.py.........# |-- APIs for computing quantities related to the contact model.
|
340
366
|
| |-- contact.py...............# |-- APIs for computing quantities related to the collidable points.
|
341
367
|
| |-- data.py..................# |-- Class storing the data of a simulated model.
|
342
368
|
| |-- frame.py.................# |-- APIs for computing quantities related to additional frames.
|
369
|
+
| |-- integrators.py...........# |-- APIs for integrating the system dynamics.
|
343
370
|
| |-- joint.py.................# |-- APIs for computing quantities related to the joints.
|
344
371
|
| |-- kin_dyn_parameters.py....# |-- Class storing kinematic and dynamic parameters of a model.
|
345
372
|
| |-- link.py..................# |-- APIs for computing quantities related to the links.
|
346
373
|
| |-- model.py.................# |-- Class defining a simulated model and APIs for computing related quantities.
|
347
374
|
| |-- ode.py...................# |-- APIs for computing quantities related to the system dynamics.
|
348
|
-
| |-- ode_data.py..............# |-- Set of classes to store the data of the system dynamics.
|
349
375
|
| `-- references.py............# `-- Helper class to create references (link forces and joint torques).
|
350
376
|
|-- exceptions.py................# Module containing functions to raise exceptions from JIT-compiled functions.
|
351
|
-
|-- integrators..................# Package containing the integrators used to simulate the system dynamics.
|
352
|
-
| |-- common.py................# |-- Common utilities used in the current package.
|
353
|
-
| |-- fixed_step.py............# |-- Fixed-step integrators (explicit Runge-Kutta schemes).
|
354
|
-
| `-- variable_step.py.........# `-- Variable-step integrators (embedded Runge-Kutta schemes).
|
355
377
|
|-- logging.py...................# Module containing logging utilities.
|
356
378
|
|-- math.........................# Package containing mathematical utilities.
|
357
379
|
| |-- adjoint.py...............# |-- APIs for creating and manipulating 6D transformations.
|
@@ -361,7 +383,8 @@ src/jaxsim
|
|
361
383
|
| |-- quaternion.py............# |-- APIs for creating and manipulating quaternions.
|
362
384
|
| |-- rotation.py..............# |-- APIs for creating and manipulating rotation matrices.
|
363
385
|
| |-- skew.py..................# |-- APIs for creating and manipulating skew-symmetric matrices.
|
364
|
-
|
|
386
|
+
| |-- transform.py.............# |-- APIs for creating and manipulating homogeneous transformations.
|
387
|
+
| |-- utils.py.................# |-- Common utilities used in the current package.
|
365
388
|
|-- mujoco.......................# Package containing utilities to interact with the Mujoco passive viewer.
|
366
389
|
| |-- loaders.py...............# |-- Utilities for converting JaxSim models to Mujoco models.
|
367
390
|
| |-- model.py.................# |-- Class providing high-level methods to compute quantities using Mujoco.
|
@@ -1,5 +1,5 @@
|
|
1
1
|
jaxsim/__init__.py,sha256=b8dYoVXqtHxHcF56iM2xgKk78lsvmGrfDlvdwaGasgs,3388
|
2
|
-
jaxsim/_version.py,sha256=
|
2
|
+
jaxsim/_version.py,sha256=gPKOajFv_sU8ebr7Lpn9223MX1vvWdNsA0VawLyS46Y,528
|
3
3
|
jaxsim/exceptions.py,sha256=MQ3LRMfVMX2-g3qYj7mUVNV9OLlIA48TANJegbcQyXI,2641
|
4
4
|
jaxsim/logging.py,sha256=STI-D_upXZYX-ZezLrlJJ0UlD5YspST0vZ_DcIwkzO4,1553
|
5
5
|
jaxsim/typing.py,sha256=7msl8t5Jt09RNYfKdPJtpjLfWurldcycDappb045Eso,761
|
@@ -11,12 +11,12 @@ jaxsim/api/contact.py,sha256=PpVtlb7Iw4mW0dCrGWorznzF2ai13N9SlV7DL5ecUfA,16795
|
|
11
11
|
jaxsim/api/contact_model.py,sha256=gY7dRA6UIduD4OO1nRZ4VmnNXIOeDLulw5Am9gBjhUs,3334
|
12
12
|
jaxsim/api/data.py,sha256=vfgzPBCbX9OUiCBKvyib2aevhZpH-MzU4ZNZsnhidQo,20790
|
13
13
|
jaxsim/api/frame.py,sha256=4wg6GsyBQgYhSvc-ry_31JsaL66sZt3TtgwjB7NrHmk,14583
|
14
|
-
jaxsim/api/integrators.py,sha256=
|
14
|
+
jaxsim/api/integrators.py,sha256=O2WmwW-3lZEiA2Pp0-CsyYlnoqaz0zqqNdeCZUI5O6Q,4980
|
15
15
|
jaxsim/api/joint.py,sha256=AnqlNWmBOay-gsoo0y4AbfFQ2OCJm-8T1E0IMhZeLoY,7457
|
16
|
-
jaxsim/api/kin_dyn_parameters.py,sha256=
|
16
|
+
jaxsim/api/kin_dyn_parameters.py,sha256=v1ZDb-Eqn552Zb344sJYy9e5sQT--_SYGIdtNC2JVSg,29828
|
17
17
|
jaxsim/api/link.py,sha256=bSZOYJDY9HJMgY25VzevTTsdFZTJc6yRRpslc5FhGHE,12782
|
18
|
-
jaxsim/api/model.py,sha256=
|
19
|
-
jaxsim/api/ode.py,sha256=
|
18
|
+
jaxsim/api/model.py,sha256=LQmsJ-ipQzZ_tYsH58zu20GRdQsOcj7AHs3FcqRwSSs,68822
|
19
|
+
jaxsim/api/ode.py,sha256=rGOswAcchEdzx8ZfMDTR2F6AjkHymZgTTdvBbjPPpd4,5358
|
20
20
|
jaxsim/api/references.py,sha256=ywNiURwZ0niYB-AzX1eoWCfdrx7TBiRyyfi_2tz-ako,20677
|
21
21
|
jaxsim/math/__init__.py,sha256=lWMswEnITzQ69O761CiHL-r7UrxyIqsHlk4YTNxtwAY,384
|
22
22
|
jaxsim/math/adjoint.py,sha256=mAtCwddK1ZwpVgIp4GIY8EVAW32Z67AJl_BzREf0ih8,4731
|
@@ -33,18 +33,18 @@ jaxsim/mujoco/__main__.py,sha256=GBmB7J-zj75ZnFyuAAmpSOpbxi_HhHhWJeot3ljGDJY,529
|
|
33
33
|
jaxsim/mujoco/loaders.py,sha256=Jjb5Us9ERmLjejM4S1FcqrF12ZVkjBMZXelu9n6HGA4,23138
|
34
34
|
jaxsim/mujoco/model.py,sha256=bRXo1uhWDN37sP9qdejr_2vq_PXHQ7p6eyBlFff_JcE,16492
|
35
35
|
jaxsim/mujoco/utils.py,sha256=dW3LrcM050-eVVdLuCiN3StIrTEfyk_jJyq1GiNh3fg,8396
|
36
|
-
jaxsim/mujoco/visualizer.py,sha256=
|
36
|
+
jaxsim/mujoco/visualizer.py,sha256=7CYxxqPZdCp9Wc77ZGe1t8Z_SIgBpoJAtYcvQMcCJvQ,7746
|
37
37
|
jaxsim/parsers/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
|
38
|
-
jaxsim/parsers/kinematic_graph.py,sha256=
|
38
|
+
jaxsim/parsers/kinematic_graph.py,sha256=YkxnykmGIXYs5fSXUX1mIi81mm7lG4HMt8LyRV-31j0,35759
|
39
39
|
jaxsim/parsers/descriptions/__init__.py,sha256=N_hp8cGI5FyEFiuNx9a-CAGCr0F0QpYEpdMHvwB7_1g,261
|
40
40
|
jaxsim/parsers/descriptions/collision.py,sha256=C6SekAPL2tNG62Y-lTqj1m9BblhoTZDj2F2vgspKNbI,4577
|
41
41
|
jaxsim/parsers/descriptions/joint.py,sha256=MurLIHHmu-y_bwUcR_J1NZ-FjfRZRk166zKsn6VzIwE,4232
|
42
|
-
jaxsim/parsers/descriptions/link.py,sha256=
|
43
|
-
jaxsim/parsers/descriptions/model.py,sha256=
|
42
|
+
jaxsim/parsers/descriptions/link.py,sha256=au5XP_7O_UG7WG07RZ6y5VXkLqjgx9aBmlrPCMxgzbo,3495
|
43
|
+
jaxsim/parsers/descriptions/model.py,sha256=LNmNed6myIhrUkSa0zp5XBt4GsDVB34tFqusNj3rdAk,9876
|
44
44
|
jaxsim/parsers/rod/__init__.py,sha256=G2vqlLajBLUc4gyzXwsEI2Wsi4TMOIF9bLDFeT6KrGU,92
|
45
45
|
jaxsim/parsers/rod/meshes.py,sha256=yAXefG73_zqbVKRUdlcz9yFmypjDIpiP9cO96PeAozE,2842
|
46
|
-
jaxsim/parsers/rod/parser.py,sha256=
|
47
|
-
jaxsim/parsers/rod/utils.py,sha256=
|
46
|
+
jaxsim/parsers/rod/parser.py,sha256=vaOlvlvgQCEGbymFc21praHKEVq_LqqlKeRJNv7tZiE,14398
|
47
|
+
jaxsim/parsers/rod/utils.py,sha256=wmD-wCF1lLO8pknX7A3a8CGt9wDlTS_xCqQulcZ_XlM,8242
|
48
48
|
jaxsim/rbda/__init__.py,sha256=kmy4G9aMkrqPNGdLSaSV3k15dpF52vBEUQXDFDuKIxU,337
|
49
49
|
jaxsim/rbda/aba.py,sha256=QiajR4Uv06u-BX5V2kWpmFmP_kIfTxxcycdJhIbid9A,9029
|
50
50
|
jaxsim/rbda/collidable_points.py,sha256=XyeV1I43GL22j03rkNVocaIPOGYirt3PiDHrFMndziQ,2070
|
@@ -52,7 +52,7 @@ jaxsim/rbda/crba.py,sha256=DC9kBXMG1qXaoAdo8K7OCnVHT_YUaL_t6Li56sRf8ro,5093
|
|
52
52
|
jaxsim/rbda/forward_kinematics.py,sha256=zoyXEZnL-hyJr3c9Jl0EfCleJtVvVsekHgUTLn5EiQ4,4640
|
53
53
|
jaxsim/rbda/jacobian.py,sha256=EaMvf073UnLWJGXm4UZIlYd4erulFAGgj_pp89k6xic,11113
|
54
54
|
jaxsim/rbda/rnea.py,sha256=lMU7xxdPqGGzk0QwteB-IYjL4auHOpd78C1YqAXlp9s,7588
|
55
|
-
jaxsim/rbda/utils.py,sha256=
|
55
|
+
jaxsim/rbda/utils.py,sha256=LDm00VhNjH2STo5y48PAtO4TC75Bybi60-Viuq3Cyss,5575
|
56
56
|
jaxsim/rbda/contacts/__init__.py,sha256=eZKSoGpWH6IQIofhXzU-q9J00PkKTovPVnxubmzoK0E,202
|
57
57
|
jaxsim/rbda/contacts/common.py,sha256=QSvEykjN5MuRD7Vk-rM454qBYdkwH7xZAVOBeulKAUc,5042
|
58
58
|
jaxsim/rbda/contacts/relaxed_rigid.py,sha256=BTHOr0An_6K3UIkGYakaF05gkTsKHDSinDzirlvSY8o,26742
|
@@ -62,8 +62,8 @@ jaxsim/utils/__init__.py,sha256=Y5zyoRevl3EMVQadhZ4EtSwTEkDt2vcnFoRhPJjKTZ0,215
|
|
62
62
|
jaxsim/utils/jaxsim_dataclass.py,sha256=XzmZeIibcaOzaxpprsGSxH3UrM66PAO456rFV91sNXg,11453
|
63
63
|
jaxsim/utils/tracing.py,sha256=Btwxdfhb7fJLk3r5PlQkGYj60Y2KbFT1gANGIA697FU,530
|
64
64
|
jaxsim/utils/wrappers.py,sha256=3IMwydqFgmSPqeuUQ3PRmdhDc1IoT6XC23jPC_LjWXs,4175
|
65
|
-
jaxsim-0.6.2.
|
66
|
-
jaxsim-0.6.2.
|
67
|
-
jaxsim-0.6.2.
|
68
|
-
jaxsim-0.6.2.
|
69
|
-
jaxsim-0.6.2.
|
65
|
+
jaxsim-0.6.2.dev156.dist-info/LICENSE,sha256=eaYdFmdeMbiIoIiPzEK0MjP1S9wtFXjXNR5er49uLR0,1546
|
66
|
+
jaxsim-0.6.2.dev156.dist-info/METADATA,sha256=Uvw7cBgt4g0VyN-Hx017SOHwtVAT5Sn6Jqvb8JFMmno,19991
|
67
|
+
jaxsim-0.6.2.dev156.dist-info/WHEEL,sha256=nn6H5-ilmfVryoAQl3ZQ2l8SH5imPWFpm1A5FgEuFV4,91
|
68
|
+
jaxsim-0.6.2.dev156.dist-info/top_level.txt,sha256=LxGMA8FLtXjQ6oI7N5gd_R_oSUHxpXxUEOfT1xS_ni0,7
|
69
|
+
jaxsim-0.6.2.dev156.dist-info/RECORD,,
|
File without changes
|
File without changes
|