Fix ABA for viscous friction

flferretti · Nov 8, 2023 · a32b505 · a32b505
1 parent c0b5d44
commit a32b505
Showing 1 changed file with 2 additions and 1 deletion.
diff --git a/src/jaxsim/physics/algos/aba.py b/src/jaxsim/physics/algos/aba.py
@@ -104,6 +104,7 @@ def aba(
     def loop_body_pass1(carry: Pass1Carry, i: jtp.Int) -> Tuple[Pass1Carry, None]:
         ii = i - 1
         i_X_λi, v, c, m_v, m_c, MA, pA, pR, i_X_0 = carry
+        has_motors = (Γ[i] != 1).any()
 
         # Compute parent-to-child transform
         i_X_λi_i = i_X_pre[i] @ pre_X_λi[i]
@@ -136,7 +137,7 @@ def loop_body_pass1(carry: Pass1Carry, i: jtp.Int) -> Tuple[Pass1Carry, None]:
         pA_i = Cross.vx_star(v[i]) @ M[i] @ v[i] - i_Xf_W @ jnp.vstack(f_ext[i])
         pA = pA.at[i].set(pA_i)
 
-        pR_i = Cross.vx_star(m_v[i]) @ IM[i] @ m_v[i] + K̅ᵥ[i] * m_v[i]
+        pR_i = Cross.vx_star(m_v[i]) @ IM[i] @ m_v[i] + K̅ᵥ[i] * m_v[i] * has_motors
         pR = pR.at[i].set(pR_i)
 
         return (i_X_λi, v, c, m_v, m_c, MA, pA, pR, i_X_0), None