Skip to content

Commit e8aeba5

Browse files
bakpaulalxbilgerfredroyhugtalbot
authored
[Lagrangian] Add corrected Jacobi constraint solving method (sofa-framework#5680)
* First working version of exploded constraint problrms. Still lots of info in GenericCosntraintSolver data that are dependent on the method type. Need to find a way of setting the solving method by adding a component in the scene. * Changed all problems into solvers. It makes more sense to use an inheritance. Now the constraintProblem is only a container * Apply changes to scenes, tests and tutorials * Add stopping ocndition based on constraint force evolutionr ate as proposed in the original paper * Add first version of improved Jacobi * Finish improved jacobi * Finalize the solver in monothread * Remove unwanted file * Add reference * Fix condition to exit loop * Include review comment + fix algorithm from a Gauss-seidel to a real JAcobi * Add parameters * fix compilation * Fix compilation * Clean and fix * Apply suggestions from code review Co-authored-by: Alex Bilger <[email protected]> --------- Co-authored-by: Alex Bilger <[email protected]> Co-authored-by: Frederick Roy <[email protected]> Co-authored-by: Hugo <[email protected]>
1 parent 60d44cf commit e8aeba5

File tree

6 files changed

+385
-0
lines changed

6 files changed

+385
-0
lines changed

‎Sofa/Component/Constraint/Lagrangian/Solver/CMakeLists.txt‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@ set(HEADER_FILES
1111

1212
${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/GenericConstraintSolver.h
1313
${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/BuiltConstraintSolver.h
14+
${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/ImprovedJacobiConstraintSolver.h
1415
${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/NNCGConstraintSolver.h
1516
${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/ProjectedGaussSeidelConstraintSolver.h
1617
${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/UnbuiltConstraintSolver.h
@@ -29,6 +30,7 @@ set(SOURCE_FILES
2930

3031
${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/GenericConstraintSolver.cpp
3132
${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/BuiltConstraintSolver.cpp
33+
${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/ImprovedJacobiConstraintSolver.cpp
3234
${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/NNCGConstraintSolver.cpp
3335
${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/ProjectedGaussSeidelConstraintSolver.cpp
3436
${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/UnbuiltConstraintSolver.cpp

‎Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ImprovedJacobiConstraintSolver.cpp‎

Lines changed: 197 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,197 @@
1+
/******************************************************************************
2+
* SOFA, Simulation Open-Framework Architecture *
3+
* (c) 2006 INRIA, USTL, UJF, CNRS, MGH *
4+
* *
5+
* This program is free software; you can redistribute it and/or modify it *
6+
* under the terms of the GNU Lesser General Public License as published by *
7+
* the Free Software Foundation; either version 2.1 of the License, or (at *
8+
* your option) any later version. *
9+
* *
10+
* This program is distributed in the hope that it will be useful, but WITHOUT *
11+
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or *
12+
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
13+
* for more details. *
14+
* *
15+
* You should have received a copy of the GNU Lesser General Public License *
16+
* along with this program. If not, see <http://www.gnu.org/licenses/>. *
17+
*******************************************************************************
18+
* Authors: The SOFA Team and external contributors (see Authors.txt) *
19+
* *
20+
* Contact information: [email protected] *
21+
******************************************************************************/
22+
23+
#include <sofa/component/constraint/lagrangian/solver/ImprovedJacobiConstraintSolver.h>
24+
#include <sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h>
25+
#include <sofa/helper/AdvancedTimer.h>
26+
#include <sofa/helper/ScopedAdvancedTimer.h>
27+
#include <sofa/core/ObjectFactory.h>
28+
#include <Eigen/Eigenvalues>
29+
30+
namespace sofa::component::constraint::lagrangian::solver
31+
{
32+
33+
ImprovedJacobiConstraintSolver::ImprovedJacobiConstraintSolver()
34+
: BuiltConstraintSolver()
35+
, d_useSpectralCorrection(initData(&d_useSpectralCorrection,false,"useSpectralCorrection","If set to true, the solution found after each iteration will be multiplied by spectralCorrectionFactor*2/spr(W), with spr() denoting the spectral radius."))
36+
, d_spectralCorrectionFactor(initData(&d_spectralCorrectionFactor,1.0,"spectralCorrectionFactor","Factor used to modulate the spectral correction"))
37+
, d_useConjugateResidue(initData(&d_useConjugateResidue,false,"useConjugateResidue","If set to true, the solution found after each iteration will be corrected along the solution direction using `\\lambda^{i+1} -= beta^{i} * (\\lambda^{i} - \\lambda^{i-1})` with beta following the formula beta^{i} = min(1, (i/maxIterations)^{conjugateResidueSpeedFactor}) "))
38+
, d_conjugateResidueSpeedFactor(initData(&d_conjugateResidueSpeedFactor,10.0,"conjugateResidueSpeedFactor","Factor used to modulate the speed in which beta used in the conjugate residue part reaches 1.0. The higher the value, the slower the reach. "))
39+
{
40+
41+
}
42+
43+
44+
void ImprovedJacobiConstraintSolver::doSolve(GenericConstraintProblem * problem , SReal timeout)
45+
{
46+
SCOPED_TIMER_VARNAME(gaussSeidelTimer, "ImprovedJacobiConstraintSolver");
47+
48+
49+
const int dimension = problem->getDimension();
50+
51+
if(!dimension)
52+
{
53+
problem->currentError = 0.0;
54+
problem->currentIterations = 0;
55+
return;
56+
}
57+
58+
SReal *dfree = problem->getDfree();
59+
SReal *force = problem->getF();
60+
SReal **w = problem->getW();
61+
SReal tol = problem->tolerance;
62+
SReal *d = problem->_d.ptr();
63+
64+
std::copy_n(dfree, dimension, d);
65+
66+
for(unsigned i=0; i< dimension; ++i)
67+
{
68+
force[i] = 0;
69+
}
70+
71+
std::vector<SReal> lastF;
72+
lastF.resize(problem->getDimension(), 0.0);
73+
74+
std::vector<SReal> deltaF;
75+
deltaF.resize(problem->getDimension(), 0.0);
76+
77+
std::vector<SReal> correctedD;
78+
correctedD.resize(problem->getDimension(), 0.0);
79+
80+
81+
SReal error=0.0;
82+
bool convergence = false;
83+
if(problem->scaleTolerance && !problem->allVerified)
84+
{
85+
tol *= dimension;
86+
}
87+
88+
for(int i=0; i<dimension; )
89+
{
90+
if(!problem->constraintsResolutions[i])
91+
{
92+
msg_error()<< "Bad size of constraintsResolutions in GenericConstraintSolver" ;
93+
break;
94+
}
95+
problem->constraintsResolutions[i]->init(i, w, force);
96+
i += problem->constraintsResolutions[i]->getNbLines();
97+
}
98+
99+
sofa::type::vector<SReal> tabErrors(dimension);
100+
101+
int iterCount = 0;
102+
103+
SReal rho = 1.0;
104+
105+
if (d_useSpectralCorrection.getValue())
106+
{
107+
Eigen::Map<Eigen::MatrixX<SReal>> EigenW(w[0],dimension, dimension) ;
108+
SReal eigenRadius = 0;
109+
for(auto s : EigenW.eigenvalues())
110+
{
111+
eigenRadius=std::max(eigenRadius,norm(s));
112+
}
113+
rho = d_spectralCorrectionFactor.getValue()*std::min(1.0, 0.9 * 2/eigenRadius);
114+
}
115+
116+
for(int i=0; i<problem->maxIterations; i++)
117+
{
118+
iterCount ++;
119+
bool constraintsAreVerified = true;
120+
121+
error=0.0;
122+
SReal beta = d_useConjugateResidue.getValue() * std::min(1.0, pow( ((float)i)/problem->maxIterations,d_conjugateResidueSpeedFactor.getValue()));
123+
124+
for(int j=0; j<dimension; ) // increment of j realized at the end of the loop
125+
{
126+
// 1. nbLines provide the dimension of the constraint
127+
const unsigned int nb = problem->constraintsResolutions[j]->getNbLines();
128+
129+
for(unsigned l=j; l<j+nb; ++l )
130+
{
131+
for(unsigned k=0; k<dimension; ++k)
132+
{
133+
d[l] += w[l][k] * deltaF[k];
134+
}
135+
correctedD[l] = rho * d[l] ;
136+
}
137+
j += nb;
138+
}
139+
140+
for(int j=0; j<dimension; ) // increment of j realized at the end of the loop
141+
{
142+
// 1. nbLines provide the dimension of the constraint
143+
const unsigned int nb = problem->constraintsResolutions[j]->getNbLines();
144+
145+
problem->constraintsResolutions[j]->resolution(j,w,correctedD.data(), force, dfree);
146+
for(unsigned l=j; l<j+nb; ++l )
147+
{
148+
force[l] += beta * deltaF[l] ;
149+
deltaF[l] = force[l] - lastF[l];
150+
lastF[l] = force[l];
151+
}
152+
153+
SReal cstError = 0.0;
154+
for(unsigned l=j; l<j+nb; ++l )
155+
{
156+
for(unsigned k=0; k<dimension; ++k)
157+
{
158+
cstError += pow(w[l][k] * deltaF[k],2);
159+
}
160+
constraintsAreVerified = constraintsAreVerified && cstError < pow(tol,2);
161+
}
162+
error += sqrt(cstError);
163+
j+= nb;
164+
165+
}
166+
167+
if (problem->allVerified)
168+
{
169+
if (constraintsAreVerified)
170+
{
171+
convergence = true;
172+
break;
173+
}
174+
}
175+
else if(error < tol && i > 0) // do not stop at the first iteration (that is used for initial guess computation)
176+
{
177+
convergence = true;
178+
break;
179+
}
180+
}
181+
182+
sofa::helper::AdvancedTimer::valSet("GS iterations", problem->currentIterations);
183+
184+
problem->result_output(this, force, error, iterCount, convergence);
185+
186+
}
187+
188+
189+
190+
void registerImprovedJacobiConstraintSolver(sofa::core::ObjectFactory* factory)
191+
{
192+
factory->registerObjects(core::ObjectRegistrationData("A Constraint Solver using the Linear Complementarity Problem formulation to solve Constraint based components using a Projected Jacobi iterative method")
193+
.add< ImprovedJacobiConstraintSolver >());
194+
}
195+
196+
197+
}

‎Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/ImprovedJacobiConstraintSolver.h‎

Lines changed: 50 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,50 @@
1+
/******************************************************************************
2+
* SOFA, Simulation Open-Framework Architecture *
3+
* (c) 2006 INRIA, USTL, UJF, CNRS, MGH *
4+
* *
5+
* This program is free software; you can redistribute it and/or modify it *
6+
* under the terms of the GNU Lesser General Public License as published by *
7+
* the Free Software Foundation; either version 2.1 of the License, or (at *
8+
* your option) any later version. *
9+
* *
10+
* This program is distributed in the hope that it will be useful, but WITHOUT *
11+
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or *
12+
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
13+
* for more details. *
14+
* *
15+
* You should have received a copy of the GNU Lesser General Public License *
16+
* along with this program. If not, see <http://www.gnu.org/licenses/>. *
17+
*******************************************************************************
18+
* Authors: The SOFA Team and external contributors (see Authors.txt) *
19+
* *
20+
* Contact information: [email protected] *
21+
******************************************************************************/
22+
#pragma once
23+
24+
#include <sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.h>
25+
#include <sofa/core/behavior/ConstraintResolution.h>
26+
27+
namespace sofa::component::constraint::lagrangian::solver
28+
{
29+
class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API ImprovedJacobiConstraintSolver : public BuiltConstraintSolver
30+
{
31+
public:
32+
SOFA_CLASS(ImprovedJacobiConstraintSolver, BuiltConstraintSolver);
33+
34+
Data<bool> d_useSpectralCorrection;
35+
Data<SReal> d_spectralCorrectionFactor;
36+
Data<bool> d_useConjugateResidue;
37+
Data<SReal> d_conjugateResidueSpeedFactor;
38+
39+
ImprovedJacobiConstraintSolver();
40+
41+
protected:
42+
/**
43+
* Based on paper
44+
* Francu, Mihai & Moldoveanu, Florica. An Improved Jacobi Solver for Particle Simulation.
45+
* VRPHYS 2014
46+
**/
47+
virtual void doSolve(GenericConstraintProblem * problem , SReal timeout = 0.0) override;
48+
49+
};
50+
}

‎Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/init.cpp‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -30,6 +30,7 @@ extern void registerNNCGConstraintSolver(sofa::core::ObjectFactory* factory);
3030
extern void registerProjectedGaussSeidelConstraintSolver(sofa::core::ObjectFactory* factory);
3131
extern void registerUnbuiltGaussSeidelConstraintSolver(sofa::core::ObjectFactory* factory);
3232
extern void registerLCPConstraintSolver(sofa::core::ObjectFactory* factory);
33+
extern void registerImprovedJacobiConstraintSolver(sofa::core::ObjectFactory* factory);
3334

3435
extern "C" {
3536
SOFA_EXPORT_DYNAMIC_LIBRARY void initExternalModule();
@@ -59,6 +60,7 @@ void registerObjects(sofa::core::ObjectFactory* factory)
5960
registerProjectedGaussSeidelConstraintSolver(factory);
6061
registerUnbuiltGaussSeidelConstraintSolver(factory);
6162
registerLCPConstraintSolver(factory);
63+
registerImprovedJacobiConstraintSolver(factory);
6264
}
6365

6466
void init()

‎examples/.scene-tests‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -37,3 +37,6 @@ ignore "Component/Controller/MechanicalStateController.scn"
3737
# To be removed when scenes are fixed.
3838
ignore "Benchmark/TopologicalChanges/ProjectToPlaneConstraint_RemovingMeshTest.scn"
3939
ignore "Benchmark/TopologicalChanges/FixedPlaneConstraint_RemovingMeshTest.scn"
40+
41+
iterations "/Component/Constraint/Lagrangian/Contact_ImprovedJacobi.scn" "50"
42+
timeout "Component/AnimationLoop/FreeMotionAnimationLoop.scn" "180"

0 commit comments

Comments
 (0)