Skip to content

Commit

Permalink
Fix contact params estimation in fixed-base models
Browse files Browse the repository at this point in the history
If the `model.physics_model.is_floating_base` is False, with the current version, the whole `W_p_CoM` variable is returned (`shape=(3,)`), but it should have `shape=()`
  • Loading branch information
flferretti authored Mar 1, 2024
1 parent b9160b7 commit b994c81
Showing 1 changed file with 2 additions and 2 deletions.
4 changes: 2 additions & 2 deletions src/jaxsim/api/contact.py
Original file line number Diff line number Diff line change
Expand Up @@ -164,14 +164,14 @@ def estimate_model_height(model: Model.JaxSimModel) -> jtp.Float:
""""""

zero_data = Data.JaxSimModelData.build(
model=model, soft_contacts_params=soft_contacts.SoftContactsParams()
model=model, soft_contacts_params=soft_contacts.SoftContactsParams()[2]
)

W_p_CoM = Model.com_position(model=model, data=zero_data)

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_p_CoM - W_pz_C.min())

return 2 * W_p_CoM

Expand Down

0 comments on commit b994c81

Please sign in to comment.