Add relaxed rigid contacts model #223

flferretti · 2024-08-23T11:55:05Z

This PR adds an alternative rigid contact model in which the holonomic constraint is relaxed. This allows some penetration, while guaranteeing more stability with respect to the spring-damper model. It is worth noting that the slipping behavior is not considered.

Note

This model is based on https://ieeexplore.ieee.org/document/5979814 and the code is based on the brax implementation in https://github.com/google/brax/blob/main/brax/generalized/constraint.py

📚 Documentation preview 📚: https://jaxsim--223.org.readthedocs.build//223/

diegoferigo

Great work @flferretti! I left an initial review with some comment. Let's align on the merging strategy as soon as @xela-95 is back and #218 gets reviewed as well.

src/jaxsim/terrain/terrain.py

src/jaxsim/rbda/contacts/relaxed_rigid.py

diegoferigo

Great, well done @flferretti. All looks good - just minor suggestions excluding a methodological question on the free acceleration. I'll give you green flag as soon as it will be answered.

src/jaxsim/typing.py

src/jaxsim/rbda/contacts/relaxed_rigid.py

diegoferigo

Thanks for the updates @flferretti. I still have a doubt on the representation used in $\dot{\boldsymbol{\nu}}$. Can you please verify if it is sound?

src/jaxsim/rbda/contacts/relaxed_rigid.py

Co-authored-by: Diego Ferigo <diego.ferigo@iit.it>

diegoferigo

Awesome, good to go now. Just the last minor suggestions. Great work @flferretti.

diegoferigo · 2024-09-13T09:23:17Z

src/jaxsim/rbda/contacts/rigid.py

-        # Convert the inertial-fixed base acceleration to a mixed base acceleration.
-        W_H_B = data.base_transform()
-        W_H_BW = W_H_B.at[0:3, 0:3].set(jnp.eye(3))
-        BW_X_W = math.Adjoint.from_transform(W_H_BW, inverse=True)
-        term1 = BW_X_W @ W_v̇_WB
-        term2 = jnp.zeros(6).at[0:3].set(jnp.cross(W_ṗ_B, W_ω_WB))
-        BW_v̇_WB = term1 - term2
-
-        BW_ν̇ = jnp.hstack([BW_v̇_WB, s̈])


@xela-95 not necessarily before this PR is merged, can you provide some feedback that this computation is equivalent with the new one? Just want to be sure that everything stays consistent.

src/jaxsim/rbda/contacts/relaxed_rigid.py

src/jaxsim/api/model.py

src/jaxsim/api/ode.py

Co-authored-by: Alessandro Croci <alessandro.croci@iit.it> Co-authored-by: Diego Ferigo <diego.ferigo@iit.it>

diegoferigo · 2024-09-13T10:03:25Z

Good to go! Last, can you please link in the first comment the url to code/documentation of Mujoco/Brax from where this contact model got inspired? Thanks!

flferretti self-assigned this Aug 23, 2024

flferretti requested a review from diegoferigo as a code owner August 23, 2024 11:55

diegoferigo requested changes Aug 28, 2024

View reviewed changes

flferretti force-pushed the feature/relaxed_rigid_contacts branch 9 times, most recently from 4fb23ed to a562aea Compare September 11, 2024 21:03

flferretti requested a review from diegoferigo September 11, 2024 21:04

diegoferigo requested changes Sep 12, 2024

View reviewed changes

flferretti force-pushed the feature/relaxed_rigid_contacts branch from 7a0758a to f588068 Compare September 12, 2024 13:21

flferretti requested a review from diegoferigo September 12, 2024 13:39

flferretti force-pushed the feature/relaxed_rigid_contacts branch from f588068 to 9537c2c Compare September 12, 2024 14:08

diegoferigo reviewed Sep 12, 2024

View reviewed changes

src/jaxsim/rbda/contacts/relaxed_rigid.py Outdated Show resolved Hide resolved

flferretti requested a review from diegoferigo September 12, 2024 15:27

diegoferigo requested changes Sep 13, 2024

View reviewed changes

src/jaxsim/rbda/contacts/relaxed_rigid.py Outdated Show resolved Hide resolved

flferretti added 10 commits September 13, 2024 11:03

Remove unhashable member in set

d15149f

Unify variable names in terrain module

7d41b29

Fix return types

10695c0

Add QuasiRigid contact model

31ea9b2

Add the new soft contacts to api.contact

b04051a

Normalize base quaternion when set by the user

057f699

Add the new soft contacts to api.ode

b34909c

Add the new soft contacts to api.ode_data

663a2ea

Add jaxopt dependency

870bf68

Add QuasiRigid contact model

75218c1

flferretti and others added 6 commits September 13, 2024 11:03

Speed up compatibility computation between pytrees

1e40a20

Use 3D velocity when warm-starting the optimizer

56371ac

Apply suggestions from code review

9afbd20

Co-authored-by: Diego Ferigo <diego.ferigo@iit.it>

Cast builder arguments to jnp.array

25596db

Update docstrings

aea3d07

Restore lstsq for the mass matrix inversion

d7d7ec9

Co-authored-by: Diego Ferigo <diego.ferigo@iit.it>

flferretti force-pushed the feature/relaxed_rigid_contacts branch from fb3cb28 to 43e50d2 Compare September 13, 2024 09:03

flferretti requested a review from diegoferigo September 13, 2024 09:07

flferretti force-pushed the feature/relaxed_rigid_contacts branch from 3319598 to ae1ff74 Compare September 13, 2024 09:20

diegoferigo approved these changes Sep 13, 2024

View reviewed changes

flferretti force-pushed the feature/relaxed_rigid_contacts branch from ae1ff74 to 3c75735 Compare September 13, 2024 09:29

diegoferigo reviewed Sep 13, 2024

View reviewed changes

src/jaxsim/api/model.py Outdated Show resolved Hide resolved

diegoferigo reviewed Sep 13, 2024

View reviewed changes

src/jaxsim/api/ode.py Outdated Show resolved Hide resolved

diegoferigo reviewed Sep 13, 2024

View reviewed changes

src/jaxsim/api/ode.py Outdated Show resolved Hide resolved

flferretti and others added 5 commits September 13, 2024 11:57

Fix resetting logic in case of overflow of simulation time

fc88b0c

Make ode.system_acceleration return in the active representation

a4875f1

Drop _compute_mixed_nu_dot_free in favor of ode.system_acceleration

e9ac7ea

Fix system acceleration representation in relaxed_rigid_contacts

71ecac2

Update variable names in ode.system_acceleration

a09bc0d

Co-authored-by: Alessandro Croci <alessandro.croci@iit.it> Co-authored-by: Diego Ferigo <diego.ferigo@iit.it>

flferretti force-pushed the feature/relaxed_rigid_contacts branch from 3c75735 to a09bc0d Compare September 13, 2024 09:58

flferretti enabled auto-merge September 13, 2024 10:03

flferretti disabled auto-merge September 13, 2024 10:03

flferretti enabled auto-merge September 13, 2024 10:13

flferretti disabled auto-merge September 13, 2024 10:18

Add reference sources and update comments

42aed4b

flferretti force-pushed the feature/relaxed_rigid_contacts branch from 6c47479 to 42aed4b Compare September 13, 2024 10:22

flferretti enabled auto-merge September 13, 2024 10:27

flferretti merged commit 9129e41 into main Sep 13, 2024
24 checks passed

flferretti deleted the feature/relaxed_rigid_contacts branch September 13, 2024 10:41

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add relaxed rigid contacts model #223

Add relaxed rigid contacts model #223

flferretti commented Aug 23, 2024 •

edited

Loading

diegoferigo left a comment •

edited

Loading

diegoferigo left a comment

diegoferigo left a comment

diegoferigo left a comment

diegoferigo Sep 13, 2024

diegoferigo commented Sep 13, 2024 •

edited

Loading

Add relaxed rigid contacts model #223

Add relaxed rigid contacts model #223

Conversation

flferretti commented Aug 23, 2024 • edited Loading

diegoferigo left a comment • edited Loading

Choose a reason for hiding this comment

diegoferigo left a comment

Choose a reason for hiding this comment

diegoferigo left a comment

Choose a reason for hiding this comment

diegoferigo left a comment

Choose a reason for hiding this comment

diegoferigo Sep 13, 2024

Choose a reason for hiding this comment

diegoferigo commented Sep 13, 2024 • edited Loading

flferretti commented Aug 23, 2024 •

edited

Loading

diegoferigo left a comment •

edited

Loading

diegoferigo commented Sep 13, 2024 •

edited

Loading