jaxsim 0.4.2.dev14__py3-none-any.whl → 0.4.2.dev28__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 +166 -0
- jaxsim/api/data.py +2 -11
- jaxsim/api/model.py +35 -0
- jaxsim/math/adjoint.py +1 -2
- jaxsim/math/joint_model.py +2 -2
- jaxsim/math/quaternion.py +2 -4
- jaxsim/math/transform.py +1 -3
- jaxsim/mujoco/model.py +5 -5
- jaxsim/rbda/aba.py +2 -2
- jaxsim/rbda/collidable_points.py +2 -2
- jaxsim/rbda/forward_kinematics.py +2 -2
- jaxsim/rbda/rnea.py +2 -2
- {jaxsim-0.4.2.dev14.dist-info → jaxsim-0.4.2.dev28.dist-info}/METADATA +1 -1
- {jaxsim-0.4.2.dev14.dist-info → jaxsim-0.4.2.dev28.dist-info}/RECORD +18 -18
- {jaxsim-0.4.2.dev14.dist-info → jaxsim-0.4.2.dev28.dist-info}/WHEEL +1 -1
- {jaxsim-0.4.2.dev14.dist-info → jaxsim-0.4.2.dev28.dist-info}/LICENSE +0 -0
- {jaxsim-0.4.2.dev14.dist-info → jaxsim-0.4.2.dev28.dist-info}/top_level.txt +0 -0
jaxsim/_version.py
CHANGED
@@ -12,5 +12,5 @@ __version__: str
|
|
12
12
|
__version_tuple__: VERSION_TUPLE
|
13
13
|
version_tuple: VERSION_TUPLE
|
14
14
|
|
15
|
-
__version__ = version = '0.4.2.
|
16
|
-
__version_tuple__ = version_tuple = (0, 4, 2, '
|
15
|
+
__version__ = version = '0.4.2.dev28'
|
16
|
+
__version_tuple__ = version_tuple = (0, 4, 2, 'dev28')
|
jaxsim/api/contact.py
CHANGED
@@ -8,6 +8,7 @@ import jax.numpy as jnp
|
|
8
8
|
import jaxsim.api as js
|
9
9
|
import jaxsim.terrain
|
10
10
|
import jaxsim.typing as jtp
|
11
|
+
from jaxsim.math import Adjoint, Cross, Transform
|
11
12
|
from jaxsim.rbda.contacts.soft import SoftContactsParams
|
12
13
|
|
13
14
|
from .common import VelRepr
|
@@ -411,3 +412,168 @@ def jacobian(
|
|
411
412
|
raise ValueError(output_vel_repr)
|
412
413
|
|
413
414
|
return O_J_WC
|
415
|
+
|
416
|
+
|
417
|
+
@functools.partial(jax.jit, static_argnames=["output_vel_repr"])
|
418
|
+
def jacobian_derivative(
|
419
|
+
model: js.model.JaxSimModel,
|
420
|
+
data: js.data.JaxSimModelData,
|
421
|
+
*,
|
422
|
+
output_vel_repr: VelRepr | None = None,
|
423
|
+
) -> jtp.Matrix:
|
424
|
+
r"""
|
425
|
+
Compute the derivative of the free-floating jacobian of the contact points.
|
426
|
+
|
427
|
+
Args:
|
428
|
+
model: The model to consider.
|
429
|
+
data: The data of the considered model.
|
430
|
+
output_vel_repr:
|
431
|
+
The output velocity representation of the free-floating jacobian derivative.
|
432
|
+
|
433
|
+
Returns:
|
434
|
+
The derivative of the :math:`6 \times (6+n)` free-floating jacobian of the contact points.
|
435
|
+
|
436
|
+
Note:
|
437
|
+
The input representation of the free-floating jacobian derivative is the active
|
438
|
+
velocity representation.
|
439
|
+
"""
|
440
|
+
|
441
|
+
output_vel_repr = (
|
442
|
+
output_vel_repr if output_vel_repr is not None else data.velocity_representation
|
443
|
+
)
|
444
|
+
|
445
|
+
# Get the index of the parent link and the position of the collidable point.
|
446
|
+
parent_link_idxs = jnp.array(model.kin_dyn_parameters.contact_parameters.body)
|
447
|
+
L_p_Ci = jnp.array(model.kin_dyn_parameters.contact_parameters.point)
|
448
|
+
contact_idxs = jnp.arange(L_p_Ci.shape[0])
|
449
|
+
|
450
|
+
# Get the transforms of all the parent links.
|
451
|
+
W_H_Li = js.model.forward_kinematics(model=model, data=data)
|
452
|
+
|
453
|
+
# =====================================================
|
454
|
+
# Compute quantities to adjust the input representation
|
455
|
+
# =====================================================
|
456
|
+
|
457
|
+
def compute_T(model: js.model.JaxSimModel, X: jtp.Matrix) -> jtp.Matrix:
|
458
|
+
In = jnp.eye(model.dofs())
|
459
|
+
T = jax.scipy.linalg.block_diag(X, In)
|
460
|
+
return T
|
461
|
+
|
462
|
+
def compute_Ṫ(model: js.model.JaxSimModel, Ẋ: jtp.Matrix) -> jtp.Matrix:
|
463
|
+
On = jnp.zeros(shape=(model.dofs(), model.dofs()))
|
464
|
+
Ṫ = jax.scipy.linalg.block_diag(Ẋ, On)
|
465
|
+
return Ṫ
|
466
|
+
|
467
|
+
# Compute the operator to change the representation of ν, and its
|
468
|
+
# time derivative.
|
469
|
+
match data.velocity_representation:
|
470
|
+
case VelRepr.Inertial:
|
471
|
+
W_H_W = jnp.eye(4)
|
472
|
+
W_X_W = Adjoint.from_transform(transform=W_H_W)
|
473
|
+
W_Ẋ_W = jnp.zeros((6, 6))
|
474
|
+
|
475
|
+
T = compute_T(model=model, X=W_X_W)
|
476
|
+
Ṫ = compute_Ṫ(model=model, Ẋ=W_Ẋ_W)
|
477
|
+
|
478
|
+
case VelRepr.Body:
|
479
|
+
W_H_B = data.base_transform()
|
480
|
+
W_X_B = Adjoint.from_transform(transform=W_H_B)
|
481
|
+
B_v_WB = data.base_velocity()
|
482
|
+
B_vx_WB = Cross.vx(B_v_WB)
|
483
|
+
W_Ẋ_B = W_X_B @ B_vx_WB
|
484
|
+
|
485
|
+
T = compute_T(model=model, X=W_X_B)
|
486
|
+
Ṫ = compute_Ṫ(model=model, Ẋ=W_Ẋ_B)
|
487
|
+
|
488
|
+
case VelRepr.Mixed:
|
489
|
+
W_H_B = data.base_transform()
|
490
|
+
W_H_BW = W_H_B.at[0:3, 0:3].set(jnp.eye(3))
|
491
|
+
W_X_BW = Adjoint.from_transform(transform=W_H_BW)
|
492
|
+
BW_v_WB = data.base_velocity()
|
493
|
+
BW_v_W_BW = BW_v_WB.at[3:6].set(jnp.zeros(3))
|
494
|
+
BW_vx_W_BW = Cross.vx(BW_v_W_BW)
|
495
|
+
W_Ẋ_BW = W_X_BW @ BW_vx_W_BW
|
496
|
+
|
497
|
+
T = compute_T(model=model, X=W_X_BW)
|
498
|
+
Ṫ = compute_Ṫ(model=model, Ẋ=W_Ẋ_BW)
|
499
|
+
|
500
|
+
case _:
|
501
|
+
raise ValueError(data.velocity_representation)
|
502
|
+
|
503
|
+
# =====================================================
|
504
|
+
# Compute quantities to adjust the output representation
|
505
|
+
# =====================================================
|
506
|
+
|
507
|
+
with data.switch_velocity_representation(VelRepr.Inertial):
|
508
|
+
# Compute the Jacobian of the parent link in inertial representation.
|
509
|
+
W_J_WL_W = js.model.generalized_free_floating_jacobian(
|
510
|
+
model=model,
|
511
|
+
data=data,
|
512
|
+
output_vel_repr=VelRepr.Inertial,
|
513
|
+
)
|
514
|
+
# Compute the Jacobian derivative of the parent link in inertial representation.
|
515
|
+
W_J̇_WL_W = js.model.generalized_free_floating_jacobian_derivative(
|
516
|
+
model=model,
|
517
|
+
data=data,
|
518
|
+
output_vel_repr=VelRepr.Inertial,
|
519
|
+
)
|
520
|
+
|
521
|
+
# Get the Jacobian of the collidable points in the mixed representation.
|
522
|
+
with data.switch_velocity_representation(VelRepr.Mixed):
|
523
|
+
CW_J_WC_BW = jacobian(
|
524
|
+
model=model,
|
525
|
+
data=data,
|
526
|
+
output_vel_repr=VelRepr.Mixed,
|
527
|
+
)
|
528
|
+
|
529
|
+
def compute_O_J̇_WC_I(
|
530
|
+
L_p_C: jtp.Vector,
|
531
|
+
contact_idx: jtp.Int,
|
532
|
+
CW_J_WC_BW: jtp.Matrix,
|
533
|
+
W_H_L: jtp.Matrix,
|
534
|
+
) -> jtp.Matrix:
|
535
|
+
|
536
|
+
parent_link_idx = parent_link_idxs[contact_idx]
|
537
|
+
|
538
|
+
match output_vel_repr:
|
539
|
+
case VelRepr.Inertial:
|
540
|
+
O_X_W = W_X_W = Adjoint.from_transform(transform=jnp.eye(4))
|
541
|
+
O_Ẋ_W = W_Ẋ_W = jnp.zeros((6, 6))
|
542
|
+
|
543
|
+
case VelRepr.Body:
|
544
|
+
L_H_C = Transform.from_rotation_and_translation(translation=L_p_C)
|
545
|
+
W_H_C = W_H_L[parent_link_idx] @ L_H_C
|
546
|
+
O_X_W = C_X_W = Adjoint.from_transform(transform=W_H_C, inverse=True)
|
547
|
+
with data.switch_velocity_representation(VelRepr.Inertial):
|
548
|
+
W_nu = data.generalized_velocity()
|
549
|
+
W_v_WC = W_J_WL_W[parent_link_idx] @ W_nu
|
550
|
+
W_vx_WC = Cross.vx(W_v_WC)
|
551
|
+
O_Ẋ_W = C_Ẋ_W = -C_X_W @ W_vx_WC
|
552
|
+
|
553
|
+
case VelRepr.Mixed:
|
554
|
+
L_H_C = Transform.from_rotation_and_translation(translation=L_p_C)
|
555
|
+
W_H_C = W_H_L[parent_link_idx] @ L_H_C
|
556
|
+
W_H_CW = W_H_C.at[0:3, 0:3].set(jnp.eye(3))
|
557
|
+
CW_H_W = Transform.inverse(W_H_CW)
|
558
|
+
O_X_W = CW_X_W = Adjoint.from_transform(transform=CW_H_W)
|
559
|
+
with data.switch_velocity_representation(VelRepr.Mixed):
|
560
|
+
CW_v_WC = CW_J_WC_BW @ data.generalized_velocity()
|
561
|
+
W_v_W_CW = jnp.zeros(6).at[0:3].set(CW_v_WC[0:3])
|
562
|
+
W_vx_W_CW = Cross.vx(W_v_W_CW)
|
563
|
+
O_Ẋ_W = CW_Ẋ_W = -CW_X_W @ W_vx_W_CW
|
564
|
+
|
565
|
+
case _:
|
566
|
+
raise ValueError(output_vel_repr)
|
567
|
+
|
568
|
+
O_J̇_WC_I = jnp.zeros(shape=(6, 6 + model.dofs()))
|
569
|
+
O_J̇_WC_I += O_Ẋ_W @ W_J_WL_W[parent_link_idx] @ T
|
570
|
+
O_J̇_WC_I += O_X_W @ W_J̇_WL_W[parent_link_idx] @ T
|
571
|
+
O_J̇_WC_I += O_X_W @ W_J_WL_W[parent_link_idx] @ Ṫ
|
572
|
+
|
573
|
+
return O_J̇_WC_I
|
574
|
+
|
575
|
+
O_J̇_WC = jax.vmap(compute_O_J̇_WC_I, in_axes=(0, 0, 0, None))(
|
576
|
+
L_p_Ci, contact_idxs, CW_J_WC_BW, W_H_Li
|
577
|
+
)
|
578
|
+
|
579
|
+
return O_J̇_WC
|
jaxsim/api/data.py
CHANGED
@@ -12,7 +12,6 @@ import jaxlie
|
|
12
12
|
import jaxsim.api as js
|
13
13
|
import jaxsim.rbda
|
14
14
|
import jaxsim.typing as jtp
|
15
|
-
from jaxsim.math import Quaternion
|
16
15
|
from jaxsim.rbda.contacts.soft import SoftContacts
|
17
16
|
from jaxsim.utils import Mutability
|
18
17
|
from jaxsim.utils.tracing import not_tracing
|
@@ -191,9 +190,7 @@ class JaxSimModelData(common.ModelDataWithVelocityRepresentation):
|
|
191
190
|
|
192
191
|
W_H_B = jaxlie.SE3.from_rotation_and_translation(
|
193
192
|
translation=base_position,
|
194
|
-
rotation=jaxlie.SO3
|
195
|
-
base_quaternion[jnp.array([1, 2, 3, 0])]
|
196
|
-
),
|
193
|
+
rotation=jaxlie.SO3(wxyz=base_quaternion),
|
197
194
|
).as_matrix()
|
198
195
|
|
199
196
|
v_WB = JaxSimModelData.other_representation_to_inertial(
|
@@ -380,13 +377,7 @@ class JaxSimModelData(common.ModelDataWithVelocityRepresentation):
|
|
380
377
|
on_false=W_Q_B / jnp.linalg.norm(W_Q_B),
|
381
378
|
)
|
382
379
|
|
383
|
-
return (
|
384
|
-
W_Q_B
|
385
|
-
if not dcm
|
386
|
-
else jaxlie.SO3.from_quaternion_xyzw(
|
387
|
-
Quaternion.to_xyzw(wxyz=W_Q_B)
|
388
|
-
).as_matrix()
|
389
|
-
).astype(float)
|
380
|
+
return (W_Q_B if not dcm else jaxlie.SO3(wxyz=W_Q_B).as_matrix()).astype(float)
|
390
381
|
|
391
382
|
@jax.jit
|
392
383
|
def base_transform(self) -> jtp.Matrix:
|
jaxsim/api/model.py
CHANGED
@@ -576,6 +576,41 @@ def generalized_free_floating_jacobian(
|
|
576
576
|
return O_J_WL_I
|
577
577
|
|
578
578
|
|
579
|
+
@functools.partial(jax.jit, static_argnames=["output_vel_repr"])
|
580
|
+
def generalized_free_floating_jacobian_derivative(
|
581
|
+
model: JaxSimModel,
|
582
|
+
data: js.data.JaxSimModelData,
|
583
|
+
*,
|
584
|
+
output_vel_repr: VelRepr | None = None,
|
585
|
+
) -> jtp.Matrix:
|
586
|
+
"""
|
587
|
+
Compute the free-floating jacobian derivatives of all links.
|
588
|
+
|
589
|
+
Args:
|
590
|
+
model: The model to consider.
|
591
|
+
data: The data of the considered model.
|
592
|
+
output_vel_repr:
|
593
|
+
The output velocity representation of the free-floating jacobian derivatives.
|
594
|
+
|
595
|
+
Returns:
|
596
|
+
The `(nL, 6, 6+dofs)` array containing the stacked free-floating
|
597
|
+
jacobian derivatives of the links. The first axis is the link index.
|
598
|
+
"""
|
599
|
+
|
600
|
+
output_vel_repr = (
|
601
|
+
output_vel_repr if output_vel_repr is not None else data.velocity_representation
|
602
|
+
)
|
603
|
+
|
604
|
+
O_J̇_WL_I = jax.vmap(
|
605
|
+
lambda model, data, link_idxs, output_vel_repr: js.link.jacobian_derivative(
|
606
|
+
model, data, link_index=link_idxs, output_vel_repr=output_vel_repr
|
607
|
+
),
|
608
|
+
in_axes=(None, None, 0, None),
|
609
|
+
)(model, data, jnp.arange(model.number_of_links()), output_vel_repr)
|
610
|
+
|
611
|
+
return O_J̇_WL_I
|
612
|
+
|
613
|
+
|
579
614
|
@functools.partial(jax.jit, static_argnames=["prefer_aba"])
|
580
615
|
def forward_dynamics(
|
581
616
|
model: JaxSimModel,
|
jaxsim/math/adjoint.py
CHANGED
@@ -3,7 +3,6 @@ import jaxlie
|
|
3
3
|
|
4
4
|
import jaxsim.typing as jtp
|
5
5
|
|
6
|
-
from .quaternion import Quaternion
|
7
6
|
from .skew import Skew
|
8
7
|
|
9
8
|
|
@@ -31,7 +30,7 @@ class Adjoint:
|
|
31
30
|
assert quaternion.size == 4
|
32
31
|
assert translation.size == 3
|
33
32
|
|
34
|
-
Q_sixd = jaxlie.SO3
|
33
|
+
Q_sixd = jaxlie.SO3(wxyz=quaternion)
|
35
34
|
Q_sixd = Q_sixd if not normalize_quaternion else Q_sixd.normalize()
|
36
35
|
|
37
36
|
return Adjoint.from_rotation_and_translation(
|
jaxsim/math/joint_model.py
CHANGED
@@ -254,8 +254,8 @@ def supported_joint_motion(
|
|
254
254
|
# This is a metadata required by only some joint types.
|
255
255
|
axis = jnp.array(joint_axis).astype(float).squeeze()
|
256
256
|
|
257
|
-
pre_H_suc = jaxlie.SE3.
|
258
|
-
|
257
|
+
pre_H_suc = jaxlie.SE3.from_matrix(
|
258
|
+
matrix=jnp.eye(4).at[:3, :3].set(Rotation.from_axis_angle(vector=s * axis))
|
259
259
|
)
|
260
260
|
|
261
261
|
S = jnp.vstack(jnp.hstack([jnp.zeros(3), axis]))
|
jaxsim/math/quaternion.py
CHANGED
@@ -43,9 +43,7 @@ class Quaternion:
|
|
43
43
|
Returns:
|
44
44
|
jtp.Matrix: Direction cosine matrix (DCM).
|
45
45
|
"""
|
46
|
-
return jaxlie.SO3.
|
47
|
-
xyzw=Quaternion.to_xyzw(quaternion)
|
48
|
-
).as_matrix()
|
46
|
+
return jaxlie.SO3(wxyz=quaternion).as_matrix()
|
49
47
|
|
50
48
|
@staticmethod
|
51
49
|
def from_dcm(dcm: jtp.Matrix) -> jtp.Vector:
|
@@ -158,7 +156,7 @@ class Quaternion:
|
|
158
156
|
A_Q_B = jnp.array(quaternion).squeeze().astype(float)
|
159
157
|
|
160
158
|
# Build the initial SO(3) quaternion.
|
161
|
-
W_Q_B_t0 = jaxlie.SO3
|
159
|
+
W_Q_B_t0 = jaxlie.SO3(wxyz=A_Q_B)
|
162
160
|
|
163
161
|
# Integrate the quaternion on the manifold.
|
164
162
|
W_Q_B_tf = jax.lax.select(
|
jaxsim/math/transform.py
CHANGED
@@ -3,8 +3,6 @@ import jaxlie
|
|
3
3
|
|
4
4
|
import jaxsim.typing as jtp
|
5
5
|
|
6
|
-
from .quaternion import Quaternion
|
7
|
-
|
8
6
|
|
9
7
|
class Transform:
|
10
8
|
|
@@ -35,7 +33,7 @@ class Transform:
|
|
35
33
|
assert W_p_B.size == 3
|
36
34
|
assert W_Q_B.size == 4
|
37
35
|
|
38
|
-
A_R_B = jaxlie.SO3
|
36
|
+
A_R_B = jaxlie.SO3(wxyz=W_Q_B)
|
39
37
|
A_R_B = A_R_B if not normalize_quaternion else A_R_B.normalize()
|
40
38
|
|
41
39
|
A_H_B = jaxlie.SE3.from_rotation_and_translation(
|
jaxsim/mujoco/model.py
CHANGED
@@ -238,9 +238,9 @@ class MujocoModelHelper:
|
|
238
238
|
raise ValueError("The orientation is not a valid element of SO(3)")
|
239
239
|
|
240
240
|
W_Q_B = (
|
241
|
-
Rotation.from_matrix(orientation).as_quat(
|
242
|
-
|
243
|
-
|
241
|
+
Rotation.from_matrix(orientation).as_quat(
|
242
|
+
canonical=True, scalar_first=False
|
243
|
+
)
|
244
244
|
if dcm
|
245
245
|
else orientation
|
246
246
|
)
|
@@ -394,8 +394,8 @@ class MujocoModelHelper:
|
|
394
394
|
if dcm:
|
395
395
|
return R
|
396
396
|
|
397
|
-
q_xyzw = Rotation.from_matrix(R).as_quat(canonical=True)
|
398
|
-
return q_xyzw
|
397
|
+
q_xyzw = Rotation.from_matrix(R).as_quat(canonical=True, scalar_first=False)
|
398
|
+
return q_xyzw
|
399
399
|
|
400
400
|
# ===============
|
401
401
|
# Private methods
|
jaxsim/rbda/aba.py
CHANGED
@@ -4,7 +4,7 @@ import jaxlie
|
|
4
4
|
|
5
5
|
import jaxsim.api as js
|
6
6
|
import jaxsim.typing as jtp
|
7
|
-
from jaxsim.math import Adjoint, Cross,
|
7
|
+
from jaxsim.math import Adjoint, Cross, StandardGravity
|
8
8
|
|
9
9
|
from . import utils
|
10
10
|
|
@@ -77,7 +77,7 @@ def aba(
|
|
77
77
|
|
78
78
|
# Compute the base transform.
|
79
79
|
W_H_B = jaxlie.SE3.from_rotation_and_translation(
|
80
|
-
rotation=jaxlie.SO3
|
80
|
+
rotation=jaxlie.SO3(wxyz=W_Q_B),
|
81
81
|
translation=W_p_B,
|
82
82
|
)
|
83
83
|
|
jaxsim/rbda/collidable_points.py
CHANGED
@@ -4,7 +4,7 @@ import jaxlie
|
|
4
4
|
|
5
5
|
import jaxsim.api as js
|
6
6
|
import jaxsim.typing as jtp
|
7
|
-
from jaxsim.math import Adjoint,
|
7
|
+
from jaxsim.math import Adjoint, Skew
|
8
8
|
|
9
9
|
from . import utils
|
10
10
|
|
@@ -57,7 +57,7 @@ def collidable_points_pos_vel(
|
|
57
57
|
|
58
58
|
# Compute the base transform.
|
59
59
|
W_H_B = jaxlie.SE3.from_rotation_and_translation(
|
60
|
-
rotation=jaxlie.SO3
|
60
|
+
rotation=jaxlie.SO3(wxyz=W_Q_B),
|
61
61
|
translation=W_p_B,
|
62
62
|
)
|
63
63
|
|
@@ -4,7 +4,7 @@ import jaxlie
|
|
4
4
|
|
5
5
|
import jaxsim.api as js
|
6
6
|
import jaxsim.typing as jtp
|
7
|
-
from jaxsim.math import Adjoint
|
7
|
+
from jaxsim.math import Adjoint
|
8
8
|
|
9
9
|
from . import utils
|
10
10
|
|
@@ -42,7 +42,7 @@ def forward_kinematics_model(
|
|
42
42
|
|
43
43
|
# Compute the base transform.
|
44
44
|
W_H_B = jaxlie.SE3.from_rotation_and_translation(
|
45
|
-
rotation=jaxlie.SO3
|
45
|
+
rotation=jaxlie.SO3(wxyz=W_Q_B),
|
46
46
|
translation=W_p_B,
|
47
47
|
)
|
48
48
|
|
jaxsim/rbda/rnea.py
CHANGED
@@ -6,7 +6,7 @@ import jaxlie
|
|
6
6
|
|
7
7
|
import jaxsim.api as js
|
8
8
|
import jaxsim.typing as jtp
|
9
|
-
from jaxsim.math import Adjoint, Cross,
|
9
|
+
from jaxsim.math import Adjoint, Cross, StandardGravity
|
10
10
|
|
11
11
|
from . import utils
|
12
12
|
|
@@ -82,7 +82,7 @@ def rnea(
|
|
82
82
|
|
83
83
|
# Compute the base transform.
|
84
84
|
W_H_B = jaxlie.SE3.from_rotation_and_translation(
|
85
|
-
rotation=jaxlie.SO3
|
85
|
+
rotation=jaxlie.SO3(wxyz=W_Q_B),
|
86
86
|
translation=W_p_B,
|
87
87
|
)
|
88
88
|
|
@@ -1,6 +1,6 @@
|
|
1
1
|
Metadata-Version: 2.1
|
2
2
|
Name: jaxsim
|
3
|
-
Version: 0.4.2.
|
3
|
+
Version: 0.4.2.dev28
|
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=ixsS4dYMPex2wOUUp_rkPnwrPhYzkRh1xO_YuMj3Cr4,2626
|
2
|
-
jaxsim/_version.py,sha256=
|
2
|
+
jaxsim/_version.py,sha256=2qDCKMaCBeWCRwlxCB4UQbl_v3wIgT4nVMoTvL5ZPKA,426
|
3
3
|
jaxsim/exceptions.py,sha256=8_h8iqL8DgNR754dR8SZiQ7361GR5V1sUk3ZuZCHw1Q,2069
|
4
4
|
jaxsim/logging.py,sha256=c4zhwBKf9eAYAHVp62kTEllqdsZgh0K-kPKVy8L3elU,1584
|
5
5
|
jaxsim/typing.py,sha256=IbFx3UkEXi-cm7UBqMPi58rJAFV_HbZ9E_K4JwfNvVM,753
|
6
6
|
jaxsim/api/__init__.py,sha256=8eV22t2S3UwNyCg8karPetG1dmX1VDBXkyv28_FwNQA,210
|
7
7
|
jaxsim/api/com.py,sha256=6TnYCvjmsJ2KLuw3NtZb0pay7ZwGKe9MKphYeQdjpQ0,13474
|
8
8
|
jaxsim/api/common.py,sha256=Ubi6uAw3o6qbdU0TFGzUyHg98EnoMzrnlihrvrs95Sk,6653
|
9
|
-
jaxsim/api/contact.py,sha256=
|
10
|
-
jaxsim/api/data.py,sha256
|
9
|
+
jaxsim/api/contact.py,sha256=RdxPdwYbCpD_Wz0oiG9N569EoXtLiDGUAvCuoOJFYxc,19761
|
10
|
+
jaxsim/api/data.py,sha256=T6m7-NteWrm-K3491Yo76xvlWtFCKqTzEE7Ughcasi8,27197
|
11
11
|
jaxsim/api/frame.py,sha256=yQmhh8fckXnqzs7dQvojOzbuSanNGLwUTWUQDXbVtF4,12874
|
12
12
|
jaxsim/api/joint.py,sha256=Pvg_It2iYA-jAQ2nOlFZxwmITiozO_f46G13BdQtHQ0,5106
|
13
13
|
jaxsim/api/kin_dyn_parameters.py,sha256=CcfSg5Mc8qb1mZeMQ4AK_ffZIsK5yOl7tu397pFhcDA,29369
|
14
14
|
jaxsim/api/link.py,sha256=hn7fbxaebHeXnvwEG9jZiWwzRcfdS8m-18LVsIG3S24,18479
|
15
|
-
jaxsim/api/model.py,sha256=
|
15
|
+
jaxsim/api/model.py,sha256=8TTFVc7HuPLxIRq4h1pzXwuSlQ4sDVmL5KpiO-OnmnM,62662
|
16
16
|
jaxsim/api/ode.py,sha256=NnLTBvpaT4kXnbjAghXIzLv9DTMJ8bele2iOlUQDv3Q,11028
|
17
17
|
jaxsim/api/ode_data.py,sha256=9YZX-SK_KJtoIqG-zYWZsQInb2NA_LtxDn-jtLqm_3U,19759
|
18
18
|
jaxsim/api/references.py,sha256=UA6kSQVBoq-bXSo99EOELf-_MD5MTy2zS0GtG3wQ410,16618
|
@@ -21,18 +21,18 @@ jaxsim/integrators/common.py,sha256=iwFykYZxdchqJcmcx8MFWEVijS5Hx9wCNKLKAJdF4gE,
|
|
21
21
|
jaxsim/integrators/fixed_step.py,sha256=KpjRd6hHtapxDoo6D1kyDrVDSHnke2TepI5grFH7_bM,2693
|
22
22
|
jaxsim/integrators/variable_step.py,sha256=0FCmAZIFnhvQxVbAzNfZgCWN1yMRTGVdBm9UwwaXI1o,21280
|
23
23
|
jaxsim/math/__init__.py,sha256=8oPITEoGwgRcOeG8KxtqxPQ8b5uku1HNRMokpCoi9Tc,352
|
24
|
-
jaxsim/math/adjoint.py,sha256=
|
24
|
+
jaxsim/math/adjoint.py,sha256=MI1A6BgYGpS1dT3bQQBH66jtC08h7x5ieyAW5R6hisg,4349
|
25
25
|
jaxsim/math/cross.py,sha256=U7yEx_l75mSy5g6O-jsjBztApvxC3WaV4MpkS5tThu4,1330
|
26
26
|
jaxsim/math/inertia.py,sha256=UAB7ym4gXFanejcs_ovZMpteHCc6poWYmt-mLmd5hhk,1640
|
27
|
-
jaxsim/math/joint_model.py,sha256=
|
28
|
-
jaxsim/math/quaternion.py,sha256=
|
27
|
+
jaxsim/math/joint_model.py,sha256=VZ3hRCgb0gsyI1wN1UdHkmaRMBxjAYQK1i_3WIvkdUA,9994
|
28
|
+
jaxsim/math/quaternion.py,sha256=_WA7W3iv7px83sWO1V1n0-J78hqAlO4SL1-jofE-UZ4,4754
|
29
29
|
jaxsim/math/rotation.py,sha256=Z90daUjGpuNEVLfWB3SVtM9EtwAIaneVj9A9UpWXqhA,2182
|
30
30
|
jaxsim/math/skew.py,sha256=oOGSSR8PUGROl6IJFlrmu6K3gPH-u16hUPfKIkcVv9o,1177
|
31
|
-
jaxsim/math/transform.py,sha256=
|
31
|
+
jaxsim/math/transform.py,sha256=KXzQgOnCfAtbXCwxhplpJ3F0JT3oEyeLVby1_uRAryQ,2892
|
32
32
|
jaxsim/mujoco/__init__.py,sha256=Zo5GAlN1DYKvX8s1hu1j6HntKIbBMLB9Puv9ouaNAZ8,158
|
33
33
|
jaxsim/mujoco/__main__.py,sha256=GBmB7J-zj75ZnFyuAAmpSOpbxi_HhHhWJeot3ljGDJY,5291
|
34
34
|
jaxsim/mujoco/loaders.py,sha256=He55jmkC5wQpMhEIDHOXXbqgWNjJ2fx16wOTStp_3PA,25111
|
35
|
-
jaxsim/mujoco/model.py,sha256=
|
35
|
+
jaxsim/mujoco/model.py,sha256=ZqqHBDnB-y8ueHydD0Ujbg4ALyLUpB_6r_9r0sENQvI,16359
|
36
36
|
jaxsim/mujoco/visualizer.py,sha256=XvMzGSHM-xnOSYl1Vk6bPe6j6ylQmJeLOgxHgL6I1nw,6966
|
37
37
|
jaxsim/parsers/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
|
38
38
|
jaxsim/parsers/kinematic_graph.py,sha256=88d0EmndVJWdcyFJsW25S78Z8F04cUt08RQMyoil1Xw,34734
|
@@ -45,12 +45,12 @@ jaxsim/parsers/rod/__init__.py,sha256=G2vqlLajBLUc4gyzXwsEI2Wsi4TMOIF9bLDFeT6KrG
|
|
45
45
|
jaxsim/parsers/rod/parser.py,sha256=B8fnnL3LFNfCNTTFhX_OeQZhTlRgwPFCNKcUVL94-rY,13528
|
46
46
|
jaxsim/parsers/rod/utils.py,sha256=5DsF3OeePZGidOJ5GiFSZx-51uIdnFvMW9EK6SgOW6Q,5698
|
47
47
|
jaxsim/rbda/__init__.py,sha256=H7DhXpxkPOi9lpUvg31IMHFfRafke1UoJLc5GQIdyhA,387
|
48
|
-
jaxsim/rbda/aba.py,sha256=
|
49
|
-
jaxsim/rbda/collidable_points.py,sha256=
|
48
|
+
jaxsim/rbda/aba.py,sha256=w7ciyxB0IsmueatT0C7PcBQEl9dyiH9oqJgIi3xeTUE,8983
|
49
|
+
jaxsim/rbda/collidable_points.py,sha256=Rmf1DhflhOTYh9mDalv0agS0CGSbmfoOybwP2KzKuJ0,4883
|
50
50
|
jaxsim/rbda/crba.py,sha256=NhtZO48OUKKor7ddY7mB7h7a6idrmOyf0Vy4p7UCCgI,4724
|
51
|
-
jaxsim/rbda/forward_kinematics.py,sha256=
|
51
|
+
jaxsim/rbda/forward_kinematics.py,sha256=FmqhZD0hQpIuUlmpzlA-5b7EYBaLZarrhiSZ782aC3E,3357
|
52
52
|
jaxsim/rbda/jacobian.py,sha256=I6mrlkk7Cpq3CE7k_tajOHCbT6vf2pW6vMS0TKNCnng,10725
|
53
|
-
jaxsim/rbda/rnea.py,sha256=
|
53
|
+
jaxsim/rbda/rnea.py,sha256=LGXD6s3NigaVy4-WxoROjnbKLZcUoyFmS9UNu_4ldjo,7568
|
54
54
|
jaxsim/rbda/utils.py,sha256=eeT21Y4DiiyhrdF0lUE_VvRuwru5-rR7yOlOlWzCCWE,5381
|
55
55
|
jaxsim/rbda/contacts/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
|
56
56
|
jaxsim/rbda/contacts/common.py,sha256=iMKLP30Qft9eGTiHo2iY-UoACJjg1JphA9_pW8wRdjc,2410
|
@@ -61,8 +61,8 @@ jaxsim/utils/__init__.py,sha256=Y5zyoRevl3EMVQadhZ4EtSwTEkDt2vcnFoRhPJjKTZ0,215
|
|
61
61
|
jaxsim/utils/jaxsim_dataclass.py,sha256=fLl1tY3DDb3lpIhG6BPqA5W34hM84oFzL-5cuz8k-68,11379
|
62
62
|
jaxsim/utils/tracing.py,sha256=KDMoyVPlu2NJvFkhtZwq5AkqMMgajt3munvJom-vEjQ,650
|
63
63
|
jaxsim/utils/wrappers.py,sha256=GOJQCJc5zwzoEGZB62wnWWGvUUQlXvDxz_A2Q-hFv7c,4027
|
64
|
-
jaxsim-0.4.2.
|
65
|
-
jaxsim-0.4.2.
|
66
|
-
jaxsim-0.4.2.
|
67
|
-
jaxsim-0.4.2.
|
68
|
-
jaxsim-0.4.2.
|
64
|
+
jaxsim-0.4.2.dev28.dist-info/LICENSE,sha256=eaYdFmdeMbiIoIiPzEK0MjP1S9wtFXjXNR5er49uLR0,1546
|
65
|
+
jaxsim-0.4.2.dev28.dist-info/METADATA,sha256=yw8qQ3mi7xmCuxJdZghz_SMShsV2mGyInvxo_yCS3j4,17250
|
66
|
+
jaxsim-0.4.2.dev28.dist-info/WHEEL,sha256=Wyh-_nZ0DJYolHNn1_hMa4lM7uDedD_RGVwbmTjyItk,91
|
67
|
+
jaxsim-0.4.2.dev28.dist-info/top_level.txt,sha256=LxGMA8FLtXjQ6oI7N5gd_R_oSUHxpXxUEOfT1xS_ni0,7
|
68
|
+
jaxsim-0.4.2.dev28.dist-info/RECORD,,
|
File without changes
|
File without changes
|