Draft
Conversation
arntanguy
force-pushed
the
topic/nix-DynamicConstraint
branch
from
73b10e8 to
206a460
Compare
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
- 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
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.
for more information, see https://pre-commit.ci
arntanguy
force-pushed
the
topic/nix-DynamicConstraint
branch
from
aaee00a to
4da724f
Compare
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
2 participants
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
PR #465 rebased on top of #495 for testing nix support of hugo's dynamicPolytope controller