[SofaCore SofaConstraint] Update FreeMotionAnimationLoop so that it can compute a linearised version of the constraint force.

[fjourdes]

This PR diffs against the sofa-framework::issofa_constrainsolving branch until it has been merged into master.

Objectives

• Adjust FreeMotionAnimationLoop so that it is more faithful to the actual equations, most notably by allowing only one linearisation of the mappings within the time step.
• Update the API in order to compute a linearised version of the constraint forces, in a similar fashion to what is done in the Compliant plugin.

Requirements

The following PR are required in order to build this

• 484 ✔️

• 450 ✔️

• 452 ✔️

• 456 ✔️

For testing:

• SOFA_USE_MASK must be deactivated, otherwise derivatives quantities ( velocity at contact points for example) are not propagated at all since all dofs are "masked" by default. After having a rather quick look, apparently the SOFA_USE_MASK impose some strict (undocumented) requirements (on top of other limitations like the fact that it does not support changes in the topology) about where in the time step the "apply" method of mappings is called.

• There is a branch on my local fork that integrates all the requirements for testing that can be found at the following location fjourdes:insimo_freemotion_integration

Changelog

API Breaking

[SofaCore]

• ConstraintSolver can now define the VecId where the constraint lambdas and constraint correction are stored. Previously, these were fixed locations, usually VecId::externalForce() and VecId::dx(), but it could vary depending on the implementation of both the ConstraintSolver and the ConstraintCorrection API. Components can retrieve these locations when inspecting their ConstraintParams object which is usually a parameter of the API methods for constraint solving.
• ConstraintParams contains the MultiMatrixDerivId that can be used by components related to the ConstraintSolver to retrieve the location of the constraint jacobian.
• ConstraintResolution API constructor needs to be passed the number of lines that are involved in the solving of this constraint equation. The previous implementation was silently initializing this value to one. If that property has to modified because it depends on some other state variables of a concrete implementation of the ConstraintResolution, the setNbLines method has to be called to reflect it. Also added some getters/setters method.
• BaseConstraintCorrection API separates the methods which computes the constraint displacement in motion space from the methods which apply it.
• As result, factorized quite significantly the methods related to the ConstraintCorrection API
• BaseConstraint API defines a storeLambda method which is used to store the result of the constraint force computation (stored in a BaseVector form) inside a dedicated state vector of a MechanicalState. Requires PR #456. This value can be used later on to discover the stiffness coming from the non linear mapping that result from it.
• BaseMechanicalState resetConstraint and getConstraintJacobian methods need to be given a ConstraintParams to be able to retrieve the MatrixDerivId containing the constraint jacobian.
• LinearSolver API
  • rename applyContactForce API method into applyConstraintForce. This method is no longer responsible for applying the corrective motion, but just to compute it.
  • buildComplianceMatrix API method needs to be passed a ConstraintParams so that concrete instances of LinearSolver can retrieve the MatrixDerivId that points to the constraint jacobian.

FIX

[SofaConstraint]

• GenericConstraintSolver block gauss seidel can support arbitrary size of blocks. The previous implementation had an hard coded limit of 6 for the block size.

UPDATES

[SofaSimulationCore]

• Adapted AnimateVisitor, MechanicalGetConstraintJacobianVisitor and MechanicalResetConstraintVisitor to the changes introduced in BaseMechanicalState API

[SofaBaseMechanics]

• MechanicalObject dynamically allocated state vectors are given a name and a owner like any other Data. As a result, these dynamic state vectors can be displayed by the GUI. Also adapted the code to the API changes introduced in BaseMechanicalState API

[SofaMeshCollision]

• Some contact mappers updateXfree() method were not calling applyJ and therefore were not propagating the free velocity towards contact dofs, resulting in segfault when solving constraints in velocity mode.

[SofaConstraint]

• GenericConstraintSolver accumulates the constraint lambdas towards independant dofs, so that non linear mappings can compute the geometric stiffness induced by the constraint forces from the previous time step.
• FreeMotionAnimationLoop assembles mapping geometric stiffness when possible. It also uses a single mapping linearisation within the time step. The solving step becomes very similar to the one explained in the "Stable Constraints" paper, when the compliance of the constraint is equal to zero.
• BilateralConstraintResolutionNDofs uses Eigen library to factorize a constraint block of arbitrary size using LDLT decomposition.
• ConstraintAnimationLoop minor changes introduced to reflect API changes.

[SofaMiscMapping]

• DistanceMapping implements the applyJT method for MatrixDeriv types using the utility methods provided in #452

[SofaGeneralAnimationLoop]

• MultiStepAnimationLoop and MultiTagAnimationLoop : adapted code to reflect API change.

ADD

[SofaConstraint]

• MappingGeometricStiffnessForceField can assemble the geometric stiffness of a mapping.
• UniformConstraint defines a constraint with a uniform direction in the contact space, internally it represents an identity matrix. It computes the right hand side of constraint equations in velocity mode as rhs = phi / dt + dvfree, following the same calculus notation and derivation as the one explained in the "Stable Constraints" paper. In position mode the constraint rhs differs from the constraint rhs in velocity mode by a dt factor. rhs = phi + dvfree * dt which gives rhs = dxfree which is similar to what was done by the previous implementation.
• ConstraintStoreLambdaVisitor which is used by the GenericConstraintSolver to store the lambda resulting from the constraint solving step into a specific location ( by default VecId::externalForce(), but a dynamic VecId can be used also)

[Examples]

• InextensiblePendulum.scn shows the benefits of the linearisation of the constraint force

Remarks

• MappingGeometricStiffnessForceField suffers the same limitation as any other forcefield, ie the mapping must be directly connected to the independant dofs, otherwise it would require an additional unsupported computation to project the mapped stiffness matrix into the space of the independant degrees of freedom. Multimappings support is not there, since it would probably require some adapatation in the API, so that it is easy to retrieve the stiffness block associated with each of the mapping parent dofs.

• ContactMappers : It is (very) questionable to let contact mappers propagate the unconstrained dynamics solution vector towards contact dofs by calling updateXfree() since it will induce inconsistencies in mapping linearisation if the contact mappers are non linear, since contact mappings will therefore be linearised around the free motion configuration, and the rest being linearised around the configuration at the beginning of the time step. This PR does not address this problem which is left for future work. It arises only with non linear contact mappers (like the RigidContactMapper for instance), but this should be kept in mind.

• LinearSolver API : As a general note, I believe the LinearSolver API is a bloated with virtual methods which are difficult to understand since they are not directly related to the computation / factorisation of a positive definite matrix, and are mostly optional especially for non assembled solver like CG. Most of the methods should reside in the ConstraintCorrection API, and concrete instance of ConstraintCorrection should be templated on the type of LinearSolver in my opinion, since it is the only objects that actually make use of these methods, and really what you want is to have access to the concrete type of Matrix and Vector used by the solver. Also from my understanding only LinearSolver instances that derive from MatrixLinearSolver implement these kind of methods, so maybe a first step would be to move them here.

---

This PR:

• [ ] builds with SUCCESS for all platforms on the CI.
• [ ] does not generate new warnings.
• [ ] does not generate new unit test failures.
• [ ] does not generate new scene test failures.
• [ ] does not break API compatibility.
• [ ] is more than 1 week old (or has fast-merge label).

Reviewers will merge only if all these checks are true.

[fjourdes]

I believe that if that branch makes it way to master, someone at mimesis should have close look too.

[guparan]

I am ready to rebase your branch on issofa_constraintsolving (updated with master). Are you OK for me to force push?

[ChristianDuriez]

You will do it in a branch of Master ? I was suppose to push it on sofaDefrost but if you are doing it on the Master, that's even better...

[fjourdes]

@guparan : I think it is fine to rebase with the updated issofa_constraint_solving branch. I believe the conflicts come from the override keyword that was added in many places in the code recently. The integration branch fjourdes:insimo_freemotion_integration will still work if someone wants want to try.

[guparan]

It's done. Yes most of the conflicts went from override keywords. @ChristianDuriez I did it on this branch to be able to easily rebase it on master when #484 will be merged.

[hugtalbot]

This PR will be rebased and reviewed this week. Any help in review would be most welcome @EulalieCoevoet ;)

[guparan]

This PR is now rebased and aiming master. Review shall begin!

[damienmarchal]

[ci-build][with-scene-tests]

[guparan]

ping @EulalieCoevoet :-)

[guparan]

[ci-build]

[hugtalbot]

hey @EulalieCoevoet will you be available to review this PR by next week ? This PR starts being a bit old. Thanks !

[EulalieCoevoet]

Hi, Big sorry for the late answer... I'm looking at it now.

[EulalieCoevoet]

So I have tested the branch fjourdes:insimo_freemotion_integration with our plugins. Our tests and examples ran fine. I have read the very well detailed changelog (thanks for that) and from what I know it looks fine. I didn't review the files changes, because there is too much to look at... but I would agree to merge. Again, I'm really sorry for the late answer.

[hugtalbot]

Thank you for your feedback Eulalie! I guess some fixes / merges are required to fit the current master.

To start with, let's [ci-build]

[guparan]

@fjourdes could you please confirm that my fix in 792d95c is not breaking your changes?

[guparan]

Wrong fix replaced with correct one :-)

[hugtalbot]

[ci-build][with-scene-tests]

[hugtalbot]

Damned, it seems there's some test failing with BilateralInteractionConstraint_test/0.checkVec3ConstrainedPositions

[hugtalbot]

@EulalieCoevoet there is an issue with the test, do you have time within the 7 upcoming days to have a look at it ? either the test needs to be updated, or the code to be fixed. Let us know if you don't ;)

[hugtalbot]

Ok, this PR is almost there. Only one test is failing. @fjourdes have you few minutes to check it out ? thx

[hugtalbot]

[ci-build][with-scene-tests]

[hugtalbot]

test BilateralInteractionConstraint_test failing due to the constraint which is not exactly met

[ChristianDuriez]

Hi, I think I have solved the test problem.. by changing the test ! It was not designed to take into account the tolerance of the solver... see commit: https://github.com/fjourdes/sofa/commit/78b84592651836809078c18971300f6abde6806a

[hugtalbot]

Hi Christian,

Using a tolerance is working, but is this normal that the constraint is not exactly applied? (although it was before)

[damienmarchal]

[ci-build][with-scene-tests]

[damienmarchal]

@hugtalbot , @ChristianDuriez Can you reproduce locally the problem that happens in ubuntu or is this a CI issue ?

[ChristianDuriez]

I am not on Ubuntu. It seems to be a problem with a test in the Flexible plugin.

[hugtalbot]

@ChristianDuriez One scene was crashing in SensableEmulation plugin : testOmniDriverEmu.scn I fixed it by adding the option : solveVelocityConstraintFirst="true". I have no idea why actually! Could you give some insight?

I fixed some added warnings, let me know if this was normal: Note that :

• in GenericConstraintSolver.cpp line 368 : none of the function parameter (cParams, res1 and res2) were unused.
• in GenericConstraintCorrection.cpp line 277 : the complianceFactor is not used

What appears really necessary, is to have some documentation on this constraint pipeline. This could be a good task for the STC#5 don't you think?

[hugtalbot]

[ci-build][with-scene-tests]

[hugtalbot]

Conflicts are again fixed, if it compiles the PR will have to be merged asap

[hugtalbot]

[ci-build][with-scene-tests]

[damienmarchal]

Merge ...

[fangzhouzisb]

Delay problem still exists using Geomagic plugin. See https://www.sofa-framework.org/community/forum/topic/problem-about-geomagic-plugin-in-v18-12/

[hugtalbot]

Thanks @fangzhouzisb This is not the correct place to ask, here the pull-request has been merged and focuses on FreeAnimationLopp Do not hesitate to create a dedicated issue referring (as you did) the forum topic.