diff --git a/src/jaxsim/high_level/model.py b/src/jaxsim/high_level/model.py index 918a3733d..34e35d88b 100644 --- a/src/jaxsim/high_level/model.py +++ b/src/jaxsim/high_level/model.py @@ -1143,18 +1143,10 @@ def forward_dynamics_crb( KV = jnp.diag( jnp.array(list(self.physics_model._joint_motor_viscous_friction.values())) ) - Γ = jnp.diag(GR) - - # Check on the motor parameters - jax.lax.cond( - pred=(jnp.diag(IM) != 0).any(), - operand=Γ, - true_fun=lambda Γ: jnp.diag(GR), - false_fun=lambda Γ: jnp.eye(GR.size), - ) + # Compute auxiliary quantities + Γ = jnp.diag(GR) Γ_inv = jnp.linalg.inv(Γ) - K̅ᵥ = Γ.T @ KV @ Γ # Compute terms of the floating-base EoM