WIP: Add polytopic constraints on forces at QP level #465

Hugo-L3174 · 2025-03-26T19:45:32Z

This PR intends to add the possibility of constraining the forces (currently bound by a linearized friction cone) in a convex polytope instead, allowing to cap each contact force's maximum value in any direction.
The main changes are the following:

The mc_rbdyn::Contact objects now hold a polytope representation (normals and offsets) of the force limits that can be applied on the contacts, one for each robot involved.
Those are intended to be computed by an external library but the default behavior is the regular friction cone with CoP constraints for the contact surfaces.
The mc_rbdyn::Contact also hold TVM nodes representations of the same polytopes, so that the TVM graph can update the constraints automatically if the polytope is updated.
In TVM the 3D forces decision variables at each edge of the contact become one 6D wrench decision variable for the surface contact and their influence on the dynamics constraint is updated accordingly.
A new low weight objective task on these wrench variables is introduced, targetting zero by default to get a minimization, but adding the possibility of targeting a specific force to get a desired wrench distribution at the whole-body QP level.

These changes mostly concern TVM because its formulation allow an easy addition and removal of decision variables to the problem.

Allows to pass the optional feasibility polytope to the TVM QP Solver through the contacts

- The polytope is still not thread safe (for external update) - The function uses a cache for now but is intended to only have the polytope and declare a dependency on it (not working for now)

- Add correct dependencies between FeasiblePolytope and the function - Contact owns his tvm feasible polytope node - Create new function in solver instead of the old one - Still segfault, probably because of rbdyn contact lifetime

- The feasible polytope always builds a 6D polytope, either the force from the optional contact polytope and the moments limit from a basic CoP constraint, or a default friction + CoP - The function builds its jacobian only as force limits or with the whole 6D matrix depending on the variable dimension

- The addContact function for the dynamics constraint is divided in either 3d or 6d (3d is the default original case) - The 6d version adds only one wrench variable at the contact frame and its influence on the dynamics function (instead of 4 force vars at each contact limit) - This allows constraining directly the contact wrench at contact frame

- Added target tasks on force variables in ContactData (for now minimization) - Flag for constraint size change to remove and re add task to solver (no event in tvm to handle this for now) - Handle 3d or 6d correctly for polytope function second member - Polytope index bugfixes

fix pre-commit

…uated joints

- There is now a polytope R1 and R2 for the two robots involved in the contact - This allows handling the force limits when both robots have a dynamic constraint and different actuation capabilities. - This also creates the CoP constraint from the correct robot surface for each side

… at surface using parent robot frame)

- In case a contact is created between two robots with dynamics constraint instead of creating both sides with opposite directions we correctly use the same wrench var with opposite directions. - This way each side has its own set of constraints acting on the same variable. - This also fixes problems occuring if the robot having the dynamic constraint is the second one. - Updating the contact with remove/add is no longer necessary in case of friction change (automatic in feasiblie polytope) so hasWork criterion is updated.

- Instead of the previous dir variable at 1 or -1 (assuming both frames were aligned and opposed on z) we use the dual plücker matrix between both frames of the contact. - This is used both for the polytope constraints and the dynamics constraints, and takes effect only in the case where a contact between two dynamically constrained robots occurs. - This ensures the common interaction wrench between two robots respects both of their feasible polytopes in the correct frames. - This also covers the previously ignored case of a contact with a desired offset between the two frames.

- Correct transform mistake - Conserve X_r2_r1 in contact impl after update - Add GUI arrow for second contact force

- Add a map of transforms from contact wrenches to CoM with wrench variables as keys - This is to be used by other tasks such as balancing, to distribute contact wrenches using the wrench variables

… removing tasks

…ct removal

…ch contacts

This removes the now unnecessay remove and re add of tasks and constraints from the solver every time they are modified. This is handled automatically by TVM using the changes made by jrl-umi3218/tvm#53 and does not need to be tracked at the solver level anymore.

…s string yaml-cpp treats all number-like elements as numbers, even when they are in single or double quotes. Furthermore numbers starting with a leading zero are interpreted as octal: - "123" -> number 123 - "0123" -> number 83 (conversion to decimal) - 0123 -> number 83 now you can do - !!str 0123 -> string "0123" as expected

Hugo-L3174 marked this pull request as draft March 30, 2025 14:35

Hugo-L3174 force-pushed the topic/DynamicConstraint branch from 60da03c to 1ede344 Compare April 8, 2025 10:02

arntanguy force-pushed the topic/DynamicConstraint branch 2 times, most recently from e96d55b to a981e5b Compare April 9, 2025 19:38

arntanguy self-requested a review April 10, 2025 06:56

Hugo-L3174 force-pushed the topic/DynamicConstraint branch from c74d449 to 42305f7 Compare May 5, 2025 09:22

arntanguy and others added 24 commits June 13, 2025 16:49

[solver] Allow to pass polytope instead of friction cone

18c5bbd

[solver] Add feasible polytope to contact

6b9434c

Allows to pass the optional feasibility polytope to the TVM QP Solver through the contacts

[logging] Add optional overloads to mc_rtc::log::*

2b5c2c4

[DynamicPolytope] Pass polytope correctly in addContact

0e93ff4

[bugfix][mc_rbdyn] Properly store feasibilityPolytope in the qp contact

f5984f7

Detail logic with comments

b6f2e50

[WIP] Add linear function and polytope node

74b649d

- The polytope is still not thread safe (for external update) - The function uses a cache for now but is intended to only have the polytope and declare a dependency on it (not working for now)

Mutex protect access of polytope

cbdd705

[WIP] Structures for new node and polytope constraint

0e6edc9

- Add correct dependencies between FeasiblePolytope and the function - Contact owns his tvm feasible polytope node - Create new function in solver instead of the old one - Still segfault, probably because of rbdyn contact lifetime

fix: Lifetime of contact

6b987cc

fix(contacts): Store contacts as pointers

deb9826

feat(tvm_solver): Allow to generate dot file of TVM graph

9f8ee75

fix pre-commit

fix(tvm): Properly register updates of PostureFunction

e8781f0

[Bugfix] Only build kinematic constraint in TVM impl if there are act…

538da12

…uated joints

[temporary bugfix] Correct contact comparison (need to to this better…

cb4234c

… at surface using parent robot frame)

Remove unnecessary external updates for feasible polytopes

ff98baf

[fix bindings] Add shared_ptr contacts to build bindings

4e0c031

fix(tests): fix rebase mistake

b5c3eb9

arntanguy and others added 14 commits June 13, 2025 16:49

[solver] Disable minimization of contact forces

c1d9242

Add access to solver dynamics constraints

6d4ba84

Correct frames for dual robot contact

adbcc33

- Correct transform mistake - Conserve X_r2_r1 in contact impl after update - Add GUI arrow for second contact force

Add transforms map

5109ab4

- Add a map of transforms from contact wrenches to CoM with wrench variables as keys - This is to be used by other tasks such as balancing, to distribute contact wrenches using the wrench variables

Store normals as MatrixXd instead of MatrixX3d expecting 6D extension

d1f953f

[Bug] Remove TVM substitutions to avoid variables bug when adding and…

ce4b1ba

… removing tasks

[Bug] Workaround for missing TVM solver events triggers

66840c4

Add momentum velocity and update momentum function

a98daa5

fix: Workaround for tasks that do addVariable after creation

ba4e910

Fix Momentum function refVel and DynamicFunction erase order at conta…

bb6e549

…ct removal

Generalize polytope constraint for 3D and 6D

06d469f

Add surface torsional friction constraint

78129bf

Bugfix: Use jacobians in contact frame instead of body frame for wren…

ca348a2

…ch contacts

Hugo-L3174 force-pushed the topic/DynamicConstraint branch from fee06ff to 1286ae2 Compare June 17, 2025 16:38

Hugo-L3174 and others added 7 commits June 18, 2025 17:36

Bugfix: Correct opposite frames transform order

87a10ca

fix(gui/config): Properly read color config for PolyhedronConfig

4425c95

feat(mc_rbdyn): Allow variable body mass

df0f7db

fix(Schema): display name of required field in mc_rtc::Schema

7192c62

feat(schema): Add support for Eigen::MatrixXd

b61ea33

feat(mc_rbdyn): Add force sensor frames

a720c23

arntanguy force-pushed the topic/DynamicConstraint branch from 2e6021c to a720c23 Compare October 7, 2025 15:24

fix(ImpedanceTask): Fix loading of target from configuration

536f293

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Uh oh!

WIP: Add polytopic constraints on forces at QP level #465

WIP: Add polytopic constraints on forces at QP level #465

Uh oh!

Hugo-L3174 commented Mar 26, 2025 •

edited

Loading

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

WIP: Add polytopic constraints on forces at QP level #465

Are you sure you want to change the base?

WIP: Add polytopic constraints on forces at QP level #465

Uh oh!

Conversation

Hugo-L3174 commented Mar 26, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

Hugo-L3174 commented Mar 26, 2025 •

edited

Loading