Merge pull request #98 from ami-iit/flferretti-patch-1

Fix contact params estimation in fixed-base models
ami-iit · Mar 4, 2024 · f41c11b · f41c11b
2 parents 75f7257 + 669d538
commit f41c11b
Showing 1 changed file with 3 additions and 3 deletions.
diff --git a/src/jaxsim/api/contact.py b/src/jaxsim/api/contact.py
@@ -167,13 +167,13 @@ def estimate_model_height(model: Model.JaxSimModel) -> jtp.Float:
             model=model, soft_contacts_params=soft_contacts.SoftContactsParams()
         )
 
-        W_p_CoM = Model.com_position(model=model, data=zero_data)
+        W_pz_CoM = Model.com_position(model=model, data=zero_data)[2]
 
         if model.physics_model.is_floating_base:
             W_pz_C = collidable_point_positions(model=model, data=zero_data)[:, -1]
-            return 2 * (W_p_CoM[2] - W_pz_C.min())
+            return 2 * (W_pz_CoM - W_pz_C.min())
 
-        return 2 * W_p_CoM
+        return 2 * W_pz_CoM
 
     max_δ = (
         max_penetration