Fix AD test

ami-iit · May 7, 2024 · dfe9ad4 · dfe9ad4
1 parent 3b2aa51
commit dfe9ad4
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 14 deletions.
diff --git a/src/jaxsim/api/rigid_contacts.py b/src/jaxsim/api/rigid_contacts.py
@@ -95,7 +95,7 @@ def contact_model(
         τ = jnp.atleast_1d(tau.squeeze())
 
         def process_point_dynamics(
-            body_position: jtp.Vector,  # , model: JaxSimModel, data: JaxSimModelData
+            body_position: jtp.Vector,
         ) -> tuple[jtp.Vector, jtp.Vector]:
             """
             Process the dynamics of a single point.
@@ -111,28 +111,25 @@ def process_point_dynamics(
             body, position = body_position
             W_p_Ci = jnp.array(position)
 
-            # W_H_B = link.transform(model=model, data=data, link_index=body)
-            # B_H_C = W_H_B.at[0:3, 3].set(W_p_Ci)
-
             B_Jh = link.jacobian(
                 model=model,
                 data=data,
                 link_index=body,
                 output_vel_repr=VelRepr.Body,
             )
 
-            # C_Xf_B = Adjoint.from_transform(transform=W_H_B @ B_H_C).T
-
             C_Xf_B = jnp.vstack(
                 [
-                    jnp.block([jnp.eye(3), jnp.zeros(shape=(3, 3))]),
-                    jnp.block([Skew.wedge(W_p_Ci), jnp.eye(3)]),
+                    jnp.block([jnp.eye(3), Skew.wedge(W_p_Ci)]),
+                    jnp.block([jnp.zeros(shape=(3, 3)), jnp.eye(3)]),
                 ]
             )
 
             JC_i = C_Xf_B @ B_Jh
 
-            f_i = JC_i @ M_inv @ JC_i.T @ (J̇ν[body] + JC_i @ M_inv @ (S @ τ - h))
+            f_i = -jnp.linalg.inv(JC_i @ M_inv @ JC_i.T) @ (
+                J̇ν[body] + JC_i @ M_inv @ (S @ τ - h)
+            )
 
             return f_i
 

diff --git a/tests/test_automatic_differentiation.py b/tests/test_automatic_differentiation.py
@@ -305,9 +305,9 @@ def close_over_inputs_and_parameters(
         p: jtp.VectorLike,
         v: jtp.VectorLike,
         m: jtp.VectorLike,
-        params: jaxsim.rbda.SoftContactsParams,
+        params: js.soft_contacts.SoftContactsParams,
     ) -> tuple[jtp.Vector, jtp.Vector]:
-        return jaxsim.rbda.SoftContacts(parameters=params).contact_model(
+        return js.soft_contacts.SoftContacts(parameters=params).contact_model(
             position=p, velocity=v, tangential_deformation=m
         )
 
@@ -341,7 +341,7 @@ def test_ad_integration(
     s = data.joint_positions(model=model)
     W_v_WB = data.base_velocity()
     ṡ = data.joint_velocities(model=model)
-    m = data.state.soft_contacts.tangential_deformation
+    m = data.state.contact_state.tangential_deformation
 
     # Inputs.
     W_f_L = references.link_forces(model=model)
@@ -395,7 +395,7 @@ def step(
                     base_angular_velocity=W_v_WB[3:6],
                     joint_velocities=ṡ,
                 ),
-                soft_contacts_state=js.rbda.soft_contacs.SoftContactsState.build(
+                contacts_state=js.soft_contacts.SoftContactsState.build(
                     tangential_deformation=m
                 ),
             ),
@@ -416,7 +416,7 @@ def step(
         xf_s = data_xf.joint_positions(model=model)
         xf_W_v_WB = data_xf.base_velocity()
         xf_ṡ = data_xf.joint_velocities(model=model)
-        xf_m = data_xf.state.soft_contacts.tangential_deformation
+        xf_m = data_xf.state.contact_state.tangential_deformation
 
         return xf_W_p_B, xf_W_Q_B, xf_s, xf_W_v_WB, xf_ṡ, xf_m