jaxsim 0.6.2.dev253__tar.gz → 0.6.2.dev259__tar.gz

{jaxsim-0.6.2.dev253 → jaxsim-0.6.2.dev259}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: jaxsim
-Version: 0.6.2.dev253
+Version: 0.6.2.dev259
 Summary: A differentiable physics engine and multibody dynamics library for control and robot learning.
 Author-email: Diego Ferigo <dgferigo@gmail.com>, Filippo Luca Ferretti <filippoluca.ferretti@outlook.com>
 Maintainer-email: Filippo Luca Ferretti <filippo.ferretti@iit.it>, Alessandro Croci <alessandro.croci@iit.it>

{jaxsim-0.6.2.dev253 → jaxsim-0.6.2.dev259}/src/jaxsim/_version.py

@@ -17,5 +17,5 @@ __version__: str
 __version_tuple__: VERSION_TUPLE
 version_tuple: VERSION_TUPLE
 
-__version__ = version = '0.6.2.dev253'
-__version_tuple__ = version_tuple = (0, 6, 2, 'dev253')
+__version__ = version = '0.6.2.dev259'
+__version_tuple__ = version_tuple = (0, 6, 2, 'dev259')

{jaxsim-0.6.2.dev253 → jaxsim-0.6.2.dev259}/src/jaxsim/api/data.py

@@ -431,13 +431,6 @@ class JaxSimModelData(common.ModelDataWithVelocityRepresentation):
         if contact_state is None:
             contact_state = self.contact_state
 
-        if isinstance(model.contact_model, jaxsim.rbda.contacts.SoftContacts):
-            contact_state = {
-                "tangential_deformation": jnp.zeros_like(
-                    contact_state["tangential_deformation"]
-                )
-            }
-
         # Normalize the quaternion to avoid numerical issues.
         base_quaternion_norm = jaxsim.math.safe_norm(
             base_quaternion, axis=-1, keepdims=True
@@ -525,6 +518,7 @@ class JaxSimModelData(common.ModelDataWithVelocityRepresentation):
             _joint_transforms=joint_transforms,
             _link_transforms=link_transforms,
             _link_velocities=link_velocities,
+            contact_state=contact_state,
             validate=validate,
         )
 

{jaxsim-0.6.2.dev253 → jaxsim-0.6.2.dev259}/src/jaxsim/api/integrators.py

@@ -156,9 +156,117 @@ def rk4_integration(
     return data_tf.replace(model=model)
 
 
+def rk4fast_integration(
+    model: js.model.JaxSimModel,
+    data: JaxSimModelData,
+    link_forces: jtp.Vector,
+    joint_torques: jtp.Vector,
+) -> JaxSimModelData:
+    """
+    Integrate the system state using the Runge-Kutta 4 fast method.
+
+    Note:
+        This method is a faster version of the RK4 method, but it may not be as accurate.
+        It computes the contact forces only once at the beginning of the integration step.
+    """
+
+    dt = model.time_step
+
+    if len(model.kin_dyn_parameters.contact_parameters.body) > 0:
+
+        # Compute the 6D forces W_f ∈ ℝ^{n_L × 6} applied to links due to contact
+        # with the terrain.
+        W_f_L_terrain, contact_state_derivative = js.contact.link_contact_forces(
+            model=model,
+            data=data,
+            link_forces=link_forces,
+            joint_torques=joint_torques,
+        )
+
+    W_f_L_total = link_forces + W_f_L_terrain
+
+    # Update the contact state data. This is necessary only for the contact models
+    # that require propagation and integration of contact state.
+    contact_state = model.contact_model.update_contact_state(contact_state_derivative)
+
+    def f(x) -> dict[str, jtp.Matrix]:
+
+        with data.switch_velocity_representation(jaxsim.VelRepr.Inertial):
+
+            data_ti = data.replace(model=model, **x)
+
+            W_v̇_WB, s̈ = js.model.forward_dynamics_aba(
+                model=model,
+                data=data_ti,
+                joint_forces=joint_torques,
+                link_forces=W_f_L_total,
+            )
+
+            W_ṗ_B, W_Q̇_B, ṡ = js.ode.system_position_dynamics(
+                data=data,
+                baumgarte_quaternion_regularization=1.0,
+            )
+
+        return dict(
+            base_position=W_ṗ_B,
+            base_quaternion=W_Q̇_B,
+            joint_positions=ṡ,
+            base_linear_velocity=W_v̇_WB[0:3],
+            base_angular_velocity=W_v̇_WB[3:6],
+            joint_velocities=s̈,
+            # The contact state is not updated here, as it is assumed to be constant.
+            contact_state=data_ti.contact_state,
+        )
+
+    base_quaternion_norm = jaxsim.math.safe_norm(data._base_quaternion, axis=-1)
+    base_quaternion = data._base_quaternion / jnp.where(
+        base_quaternion_norm == 0, 1.0, base_quaternion_norm
+    )
+
+    x_t0 = dict(
+        base_position=data._base_position,
+        base_quaternion=base_quaternion,
+        joint_positions=data._joint_positions,
+        base_linear_velocity=data._base_linear_velocity,
+        base_angular_velocity=data._base_angular_velocity,
+        joint_velocities=data._joint_velocities,
+        contact_state=contact_state,
+    )
+
+    euler_mid = lambda x, dxdt: x + (0.5 * dt) * dxdt
+    euler_fin = lambda x, dxdt: x + dt * dxdt
+
+    k1 = f(x_t0)
+    k2 = f(jax.tree.map(euler_mid, x_t0, k1))
+    k3 = f(jax.tree.map(euler_mid, x_t0, k2))
+    k4 = f(jax.tree.map(euler_fin, x_t0, k3))
+
+    # Average the slopes and compute the RK4 state derivative.
+    average = lambda k1, k2, k3, k4: (k1 + 2 * k2 + 2 * k3 + k4) / 6
+
+    dxdt = jax.tree_util.tree_map(average, k1, k2, k3, k4)
+
+    # Integrate the dynamics
+    x_tf = jax.tree_util.tree_map(euler_fin, x_t0, dxdt)
+
+    data_tf = dataclasses.replace(
+        data,
+        _base_position=x_tf["base_position"],
+        _base_quaternion=x_tf["base_quaternion"],
+        _joint_positions=x_tf["joint_positions"],
+        _base_linear_velocity=x_tf["base_linear_velocity"],
+        _base_angular_velocity=x_tf["base_angular_velocity"],
+        _joint_velocities=x_tf["joint_velocities"],
+        contact_state=x_tf["contact_state"],
+    )
+
+    return data_tf.replace(model=model)
+
+
 _INTEGRATORS_MAP: dict[
     js.model.IntegratorType, Callable[..., js.data.JaxSimModelData]
 ] = {
     js.model.IntegratorType.SemiImplicitEuler: semi_implicit_euler_integration,
     js.model.IntegratorType.RungeKutta4: rk4_integration,
+    js.model.IntegratorType.RungeKutta4Fast: rk4fast_integration,
 }

{jaxsim-0.6.2.dev253 → jaxsim-0.6.2.dev259}/src/jaxsim/api/model.py

@@ -29,6 +29,7 @@ class IntegratorType(enum.IntEnum):
 
     SemiImplicitEuler = enum.auto()
     RungeKutta4 = enum.auto()
+    RungeKutta4Fast = enum.auto()
 
 
 @jax_dataclasses.pytree_dataclass(eq=False, unsafe_hash=False)

{jaxsim-0.6.2.dev253 → jaxsim-0.6.2.dev259}/src/jaxsim.egg-info/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: jaxsim
-Version: 0.6.2.dev253
+Version: 0.6.2.dev259
 Summary: A differentiable physics engine and multibody dynamics library for control and robot learning.
 Author-email: Diego Ferigo <dgferigo@gmail.com>, Filippo Luca Ferretti <filippoluca.ferretti@outlook.com>
 Maintainer-email: Filippo Luca Ferretti <filippo.ferretti@iit.it>, Alessandro Croci <alessandro.croci@iit.it>

{jaxsim-0.6.2.dev253 → jaxsim-0.6.2.dev259}/tests/conftest.py

@@ -133,6 +133,7 @@ def velocity_representation(request) -> jaxsim.VelRepr:
     params=[
         pytest.param(IntegratorType.SemiImplicitEuler, id="semi_implicit_euler"),
         pytest.param(IntegratorType.RungeKutta4, id="runge_kutta_4"),
+        pytest.param(IntegratorType.RungeKutta4Fast, id="runge_kutta_4_fast"),
     ],
 )
 def integrator(request) -> str: