diff --git a/SofaKernel/modules/SofaImplicitOdeSolver/SofaImplicitOdeSolver_test/CMakeLists.txt b/SofaKernel/modules/SofaImplicitOdeSolver/SofaImplicitOdeSolver_test/CMakeLists.txt index dfac253329d..6739db03725 100644 --- a/SofaKernel/modules/SofaImplicitOdeSolver/SofaImplicitOdeSolver_test/CMakeLists.txt +++ b/SofaKernel/modules/SofaImplicitOdeSolver/SofaImplicitOdeSolver_test/CMakeLists.txt @@ -6,7 +6,8 @@ set(SOURCE_FILES loadPlugins.cpp EulerImplicitSolverStatic_test.cpp EulerImplicitSolverDynamic_test.cpp - SpringSolverDynamic_test.cpp) + SpringSolverDynamic_test.cpp + StaticSolver_test.cpp) add_definitions("-DSOFAIMPLICITODESOLVER_TEST_SCENES_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/scenes\"") add_executable(${PROJECT_NAME} ${SOURCE_FILES}) diff --git a/SofaKernel/modules/SofaImplicitOdeSolver/SofaImplicitOdeSolver_test/StaticSolver_test.cpp b/SofaKernel/modules/SofaImplicitOdeSolver/SofaImplicitOdeSolver_test/StaticSolver_test.cpp new file mode 100644 index 00000000000..0fc258848a7 --- /dev/null +++ b/SofaKernel/modules/SofaImplicitOdeSolver/SofaImplicitOdeSolver_test/StaticSolver_test.cpp @@ -0,0 +1,255 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#include +#include +#include +#include +#include + +#include +#include + +using namespace sofa::simulation; +using namespace sofa::simpleapi; +using namespace sofa::helper::logging; + +using sofa::simulation::graph::DAGSimulation; +using sofa::component::odesolver::StaticSolver; + +static constexpr SReal poissonRatio = 0; +static constexpr SReal youngModulus = 3000; +static constexpr SReal mu = youngModulus / (2.0 * (1.0 + poissonRatio)); +static constexpr SReal l = youngModulus * poissonRatio / ((1.0 + poissonRatio) * (1.0 - 2.0 * poissonRatio)); + +/** + * Create a bending rectangular beam simulation using the StaticSolver. + * + * Domain: 15x15x80 + * Discretization: 3x3x9 nodes, linear tetrahedral mesh + * Material: StVenantKirchhoff (Young modulus = 3000, Poisson ratio = 0.4) + * + */ +class StaticSolverTest : public sofa::testing::BaseTest +{ +public: + void onSetUp() override { + + if (! getSimulation()) { + setSimulation(new DAGSimulation()) ; + } + + root = getSimulation()->createNewNode("root"); + + createObject(root, "RequiredPlugin", {{"pluginName", "SofaBoundaryCondition SofaEngine SofaMiscFem SofaImplicitOdeSolver SofaSparseSolver SofaTopologyMapping"}}); + createObject(root, "RegularGridTopology", {{"name", "grid"}, {"min", "-7.5 -7.5 0"}, {"max", "7.5 7.5 80"}, {"n", "3 3 9"}}); + auto s = createObject(root, "StaticSolver", {{"newton_iterations", "10"}}); + createObject(root, "SparseLDLSolver"); + createObject(root, "MechanicalObject", {{"name", "mo"}, {"src", "@grid"}}); + createObject(root, "TetrahedronSetTopologyContainer", {{"name", "mechanical_topology"}}); + createObject(root, "TetrahedronSetTopologyModifier"); + createObject(root, "Hexa2TetraTopologicalMapping", {{"input", "@grid"}, {"output", "@mechanical_topology"}}); + createObject(root, "TetrahedronHyperelasticityFEMForceField", { + {"materialName", "StVenantKirchhoff"}, + {"ParameterSet", std::to_string(mu) + " " + std::to_string(l)}, + {"topology", "@mechanical_topology"} + }); + + createObject(root, "BoxROI", {{"name", "top_roi"}, {"box", "-7.5 -7.5 -0.9 7.5 7.5 0.1"}}); + createObject(root, "FixedConstraint", {{"indices", "@top_roi.indices"}}); + + createObject(root, "BoxROI", {{"name", "base_roi"}, {"box", "-7.5 -7.5 79.9 7.5 7.5 80.1"}}); + createObject(root, "SurfacePressureForceField", {{"pressure", "100"}, {"mainDirection", "0 -1 0"}, {"triangleIndices", "@base_roi.trianglesInROI"}}); + + solver = dynamic_cast (s.get()); + } + + void onTearDown() override { + getSimulation()->unload(root); + } + + auto execute() -> std::pair, std::vector> { + using namespace std; + getSimulation()->init(root.get()); + getSimulation()->animate(root.get(), 1); + auto residuals = solver->squared_residual_norms(); + auto corrections = solver->squared_increment_norms(); + transform(begin(residuals), end(residuals), begin(residuals), [](SReal r) {return sqrt(r);}); + transform(begin(corrections), end(corrections), begin(corrections), [](SReal r) {return sqrt(r);}); + return {residuals, corrections}; + } + + + NodeSPtr root; + StaticSolver::SPtr solver; +}; + +TEST_F(StaticSolverTest, Residuals) { + using namespace sofa::core::objectmodel; + // These are the expected force residual if we do not activate any convergence threshold + // and force the solve to do 10 Newton iterations + std::vector expected_force_residual_norms = { + 1.237102e+03, + 6.931312e+00, + 2.634097e-01, + 2.829366e-02, + 2.928456e-03, + 3.017181e-04, + 3.108847e-05, + 3.203062e-06, + 3.300435e-07, + 3.398669e-08 + }; + + // Disable all convergence criteria + dynamic_cast< Data * > ( this->solver->findData("newton_iterations") )->setValue(10); + dynamic_cast< Data * > ( this->solver->findData("absolute_correction_tolerance_threshold") )->setValue(-1); + dynamic_cast< Data * > ( this->solver->findData("relative_correction_tolerance_threshold") )->setValue(-1); + dynamic_cast< Data * > ( this->solver->findData("absolute_residual_tolerance_threshold") )->setValue(-1); + dynamic_cast< Data * > ( this->solver->findData("relative_residual_tolerance_threshold") )->setValue(-1); + dynamic_cast< Data * > ( this->solver->findData("should_diverge_when_residual_is_growing") )->setValue(false); + ; + std::vector actual_force_residual_norms = this->execute().first; + + EXPECT_EQ(actual_force_residual_norms.size(), expected_force_residual_norms.size()) + << "The static ODE solver is supposed to execute 10 Newton steps since the convergence criteria were deactivated."; + + for (std::size_t newton_it = 0; newton_it < actual_force_residual_norms.size(); ++newton_it) { + EXPECT_NEAR(actual_force_residual_norms[newton_it], expected_force_residual_norms[newton_it], 1e-3) + << "The actual force residual norm doesn't match the expected one."; + } +} + +TEST_F(StaticSolverTest, RelativeResiduals) { + using namespace sofa::core::objectmodel; + // Disable all convergence criteria BUT the relative residual + dynamic_cast< Data * > ( this->solver->findData("newton_iterations") )->setValue(10); + dynamic_cast< Data * > ( this->solver->findData("absolute_correction_tolerance_threshold") )->setValue(-1); + dynamic_cast< Data * > ( this->solver->findData("relative_correction_tolerance_threshold") )->setValue(-1); + dynamic_cast< Data * > ( this->solver->findData("absolute_residual_tolerance_threshold") )->setValue(-1); + dynamic_cast< Data * > ( this->solver->findData("relative_residual_tolerance_threshold") )->setValue(1e-5); + dynamic_cast< Data * > ( this->solver->findData("should_diverge_when_residual_is_growing") )->setValue(false); + + // The list of relative residuals are + // 1 2 3 4 5 6 7 8 9 10 + // 1.000000e+00, 5.602864e-03, 2.129249e-04, 2.287093e-05, 2.367191e-06, 2.438911e-07, 2.513007e-08, 2.589194e-09, 2.668053e-10, 2.748057e-11 + // Setting a relative criterion of 1e-5 should therefore converge after the 5th Newton iteration. + + std::vector actual_force_residual_norms = this->execute().first; + EXPECT_EQ(actual_force_residual_norms.size(), 5) + << "The static ODE solver is supposed to converge after 5 Newton steps when using a relative residual threshold of 1e-5."; +} + +TEST_F(StaticSolverTest, AbsoluteResiduals) { + using namespace sofa::core::objectmodel; + // Disable all convergence criteria BUT the absolute residual + dynamic_cast< Data * > ( this->solver->findData("newton_iterations") )->setValue(10); + dynamic_cast< Data * > ( this->solver->findData("absolute_correction_tolerance_threshold") )->setValue(-1); + dynamic_cast< Data * > ( this->solver->findData("relative_correction_tolerance_threshold") )->setValue(-1); + dynamic_cast< Data * > ( this->solver->findData("absolute_residual_tolerance_threshold") )->setValue(1e-5); + dynamic_cast< Data * > ( this->solver->findData("relative_residual_tolerance_threshold") )->setValue(-1); + dynamic_cast< Data * > ( this->solver->findData("should_diverge_when_residual_is_growing") )->setValue(false); + + // The list of relative residuals are + // 1 2 3 4 5 6 7 8 9 10 + // 1.237102e+03, 6.931312e+00, 2.634097e-01, 2.829366e-02, 2.928456e-03, 3.017181e-04, 3.108845e-05, 3.203097e-06, 3.300652e-07, 3.399625e-08 + // Setting an absolute criterion of 1e-5 should therefore converge after the 8th Newton iteration. + + std::vector actual_force_residual_norms = this->execute().first; + EXPECT_EQ(actual_force_residual_norms.size(), 8) + << "The static ODE solver is supposed to converge after 8 Newton steps when using an absolute residual threshold of 1e-5."; +} + +TEST_F(StaticSolverTest, Increments) { + using namespace sofa::core::objectmodel; + // These are the expected correction increment norms if we do not activate any convergence threshold + // and force the solve to do 10 Newton iterations + std::vector expected_increment_norms = { + 1.781729e+01, + 5.043276e-01, + 1.201313e-02, + 1.237255e-03, + 1.250073e-04, + 1.289042e-05, + 1.329191e-06, + 1.369818e-07, + 1.411500e-08, + 1.454314e-09, + }; + + // Disable all convergence criteria + dynamic_cast< Data * > ( this->solver->findData("newton_iterations") )->setValue(10); + dynamic_cast< Data * > ( this->solver->findData("absolute_correction_tolerance_threshold") )->setValue(-1); + dynamic_cast< Data * > ( this->solver->findData("relative_correction_tolerance_threshold") )->setValue(-1); + dynamic_cast< Data * > ( this->solver->findData("absolute_residual_tolerance_threshold") )->setValue(-1); + dynamic_cast< Data * > ( this->solver->findData("relative_residual_tolerance_threshold") )->setValue(-1); + dynamic_cast< Data * > ( this->solver->findData("should_diverge_when_residual_is_growing") )->setValue(false); + ; + std::vector actual_increment_norms = this->execute().second; + + EXPECT_EQ(actual_increment_norms.size(), expected_increment_norms.size()) + << "The static ODE solver is supposed to execute 10 Newton steps since the convergence criteria were deactivated."; + + for (std::size_t newton_it = 0; newton_it < actual_increment_norms.size(); ++newton_it) { + EXPECT_NEAR(actual_increment_norms[newton_it], expected_increment_norms[newton_it], 1e-3) + << "The actual increment norm doesn't match the expected one."; + } +} + +TEST_F(StaticSolverTest, RelativeIncrements) { + using namespace sofa::core::objectmodel; + // Disable all convergence criteria BUT the relative increment corrections + dynamic_cast< Data * > ( this->solver->findData("newton_iterations") )->setValue(10); + dynamic_cast< Data * > ( this->solver->findData("absolute_correction_tolerance_threshold") )->setValue(-1); + dynamic_cast< Data * > ( this->solver->findData("relative_correction_tolerance_threshold") )->setValue(1e-5); + dynamic_cast< Data * > ( this->solver->findData("absolute_residual_tolerance_threshold") )->setValue(-1); + dynamic_cast< Data * > ( this->solver->findData("relative_residual_tolerance_threshold") )->setValue(-1); + dynamic_cast< Data * > ( this->solver->findData("should_diverge_when_residual_is_growing") )->setValue(false); + + // The list of relative corrections are + // 1 2 3 4 5 6 7 8 9 10 + // 1.000000e+00, 2.819276e-02, 6.711671e-04, 6.912001e-05, 6.983561e-06, 7.201258e-07, 7.425552e-08, 7.652517e-09, 7.885371e-10, 8.124548e-11 + // Setting a relative criterion of 1e-5 should therefore converge after the 5th Newton iteration. + + std::vector actual_increment_norms = this->execute().second; + EXPECT_EQ(actual_increment_norms.size(), 5) + << "The static ODE solver is supposed to converge after 5 Newton steps when using a relative correction threshold of 1e-5."; +} + +TEST_F(StaticSolverTest, AbsoluteIncrements) { + using namespace sofa::core::objectmodel; + // Disable all convergence criteria BUT the absolute increment corrections + dynamic_cast< Data * > ( this->solver->findData("newton_iterations") )->setValue(10); + dynamic_cast< Data * > ( this->solver->findData("absolute_correction_tolerance_threshold") )->setValue(1e-5); + dynamic_cast< Data * > ( this->solver->findData("relative_correction_tolerance_threshold") )->setValue(-1); + dynamic_cast< Data * > ( this->solver->findData("absolute_residual_tolerance_threshold") )->setValue(-1); + dynamic_cast< Data * > ( this->solver->findData("relative_residual_tolerance_threshold") )->setValue(-1); + dynamic_cast< Data * > ( this->solver->findData("should_diverge_when_residual_is_growing") )->setValue(false); + + // The list of absolute corrections are + // 1 2 3 4 5 6 7 8 9 10 + // 1.781729e+01, 5.043276e-01, 1.201313e-02, 1.237255e-03, 1.250073e-04, 1.289042e-05, 1.329191e-06, 1.369819e-07, 1.411500e-08, 1.454313e-09 + // Setting an absolute criterion of 1e-5 should therefore converge after the 7th Newton iteration. + + std::vector actual_increment_norms = this->execute().second; + EXPECT_EQ(actual_increment_norms.size(), 7) + << "The static ODE solver is supposed to converge after 7 Newton steps when using a relative correction threshold of 1e-5."; +} \ No newline at end of file diff --git a/SofaKernel/modules/SofaImplicitOdeSolver/src/SofaImplicitOdeSolver/StaticSolver.cpp b/SofaKernel/modules/SofaImplicitOdeSolver/src/SofaImplicitOdeSolver/StaticSolver.cpp index f05c60b9075..bcc8abcb637 100644 --- a/SofaKernel/modules/SofaImplicitOdeSolver/src/SofaImplicitOdeSolver/StaticSolver.cpp +++ b/SofaKernel/modules/SofaImplicitOdeSolver/src/SofaImplicitOdeSolver/StaticSolver.cpp @@ -26,8 +26,13 @@ #include #include #include - +#include #include + +#include +#include +#include + using sofa::simulation::mechanicalvisitor::MechanicalPropagateOnlyPositionAndVelocityVisitor; namespace sofa::component::odesolver @@ -42,14 +47,23 @@ StaticSolver::StaticSolver() (unsigned) 1, "newton_iterations", "Number of newton iterations between each load increments (normally, one load increment per simulation time-step.")) - , d_correction_tolerance_threshold(initData(&d_correction_tolerance_threshold, + , d_absolute_correction_tolerance_threshold(initData(&d_absolute_correction_tolerance_threshold, (double) 1e-5, - "correction_tolerance_threshold", - "Convergence criterion: The newton iterations will stop when the norm of correction |du| reach this threshold.")) - , d_residual_tolerance_threshold( initData(&d_residual_tolerance_threshold, + "absolute_correction_tolerance_threshold", + "Convergence criterion: The newton iterations will stop when the norm |du| is smaller than this threshold.")) + , d_relative_correction_tolerance_threshold(initData(&d_relative_correction_tolerance_threshold, (double) 1e-5, - "residual_tolerance_threshold", - "Convergence criterion: The newton iterations will stop when the norm of the residual |f - K(u)| reach this threshold. " + "relative_correction_tolerance_threshold", + "Convergence criterion: The newton iterations will stop when the ratio |du| / |U| is smaller than this threshold.")) + , d_absolute_residual_tolerance_threshold( initData(&d_absolute_residual_tolerance_threshold, + (double) 1e-5, + "absolute_residual_tolerance_threshold", + "Convergence criterion: The newton iterations will stop when the norm |R| is smaller than this threshold. " + "Use a negative value to disable this criterion.")) + , d_relative_residual_tolerance_threshold( initData(&d_relative_residual_tolerance_threshold, + (double) 1e-5, + "relative_residual_tolerance_threshold", + "Convergence criterion: The newton iterations will stop when the ratio |R|/|R0| is smaller than this threshold. " "Use a negative value to disable this criterion.")) , d_should_diverge_when_residual_is_growing( initData(&d_should_diverge_when_residual_is_growing, false, @@ -65,7 +79,7 @@ void StaticSolver::parse(sofa::core::objectmodel::BaseObjectDescription* arg) const char* val=arg->getAttribute("massCoef",nullptr) ; if(val) { - msg_deprecated() << "The attribute 'massCoef' is deprecated since Sofa 19.06'" << msgendl + msg_deprecated() << "The attribute 'massCoef' is deprecated since SOFA 19.06'" << msgendl << "This data was previously used for stabilization purposes but it was preventing" << msgendl << "from computing a strictly-static system (Use the forum for any question)"; @@ -73,7 +87,7 @@ void StaticSolver::parse(sofa::core::objectmodel::BaseObjectDescription* arg) val=arg->getAttribute("dampingCoef",nullptr) ; if(val) { - msg_deprecated() << "The attribute 'dampingCoef' is deprecated since Sofa 19.06'" << msgendl + msg_deprecated() << "The attribute 'dampingCoef' is deprecated since SOFA 19.06'" << msgendl << "This data was previously used for stabilization purposes but it was preventing" << msgendl << "from computing a strictly-static system (Use the forum for any question)"; @@ -81,7 +95,7 @@ void StaticSolver::parse(sofa::core::objectmodel::BaseObjectDescription* arg) val=arg->getAttribute("stiffnessCoef",nullptr) ; if(val) { - msg_deprecated() << "The attribute 'stiffnessCoef' is deprecated since Sofa 19.06'" << msgendl + msg_deprecated() << "The attribute 'stiffnessCoef' is deprecated since SOFA 19.06'" << msgendl << "This data was previously used for stabilization purposes but it was preventing" << msgendl << "from computing a strictly-static system (Use the forum for any question)"; @@ -89,121 +103,358 @@ void StaticSolver::parse(sofa::core::objectmodel::BaseObjectDescription* arg) val=arg->getAttribute("applyIncrementFactor",nullptr) ; if(val) { - msg_deprecated() << "The attribute 'applyIncrementFactor' is deprecated since Sofa 19.06'" << msgendl + msg_deprecated() << "The attribute 'applyIncrementFactor' is deprecated since SOFA 19.06'" << msgendl << "The incremental loading is now supposed to be done within the desired ForceField." << msgendl << "(Use the forum for any question)"; } + val=arg->getAttribute("correction_tolerance_threshold",nullptr) ; + if(val) + { + msg_deprecated() << "The attribute 'correction_tolerance_threshold' is deprecated since SOFA 21.06'" << msgendl + << "The attribute was renamed for '" << d_absolute_correction_tolerance_threshold.getName() << "'."; + arg->setAttribute(d_absolute_correction_tolerance_threshold.getName(), val); + } + val=arg->getAttribute("residual_tolerance_threshold",nullptr) ; + if(val) + { + msg_deprecated() << "The attribute 'residual_tolerance_threshold' is deprecated since SOFA 21.06'" << msgendl + << "The attribute was renamed for '" << d_absolute_residual_tolerance_threshold.getName() << "'."; + arg->setAttribute(d_absolute_residual_tolerance_threshold.getName(), val); + } sofa::core::behavior::OdeSolver::parse(arg) ; } -void StaticSolver::solve(const sofa::core::ExecParams* params, double dt, sofa::core::MultiVecCoordId xResult, sofa::core::MultiVecDerivId /*vResult*/) { +void StaticSolver::solve(const sofa::core::ExecParams* params, double dt, sofa::core::MultiVecCoordId xResult, sofa::core::MultiVecDerivId vResult) +{ + using namespace sofa::helper::logging; + using namespace std::chrono; - SOFA_UNUSED(dt); + using std::chrono::steady_clock; + using sofa::helper::ScopedAdvancedTimer; + using sofa::core::behavior::MultiMatrix; + using sofa::simulation::common::VectorOperations; + using sofa::simulation::common::MechanicalOperations; - sofa::simulation::common::VectorOperations vop( params, this->getContext() ); - sofa::simulation::common::MechanicalOperations mop( params, this->getContext() ); + static constexpr auto epsilon = std::numeric_limits::epsilon(); - MultiVecCoord x_start(&vop, sofa::core::VecCoordId::position() ); - MultiVecCoord x(&vop, xResult ); - MultiVecDeriv force( &vop, sofa::core::VecDerivId::force() ); - dx.realloc( &vop, true ); + // Get the current context + const auto context = this->getContext(); - // MO vector dx is not allocated by default, it will seg fault if the CG is used (dx is taken by default) with an IdentityMapping - MultiVecDeriv tempdx(&vop, sofa::core::VecDerivId::dx() ); tempdx.realloc( &vop, true, true ); + // Create the vector and mechanical operations tools. These are used to execute special operations (multiplication, + // additions, etc.) on multi-vectors (a vector that is stored in different buffers inside the mechanical objects) + VectorOperations vop( params, context ); + MechanicalOperations mop( params, context ); - // Set implicit param to true to trigger nonlinear stiffness matrix recomputation + // Initialize the set of multi-vectors used by this solver + MultiVecCoord x(&vop, xResult ); + MultiVecDeriv force( &vop, sofa::core::VecDerivId::force() ); + MultiVecDeriv dx( &vop, sofa::core::VecDerivId::dx() ); + dx.realloc( &vop , true, true); + U.realloc( &vop ); + U.clear(); + dx.clear(); + + // Set the multi-vector identifier inside the mechanical parameters. + sofa::core::MechanicalParams mechanical_parameters (*params); + mechanical_parameters.setX(xResult); + mechanical_parameters.setV(vResult); + mechanical_parameters.setF(force); + mechanical_parameters.setDx(dx); + mechanical_parameters.setDt(dt); + + // Let the mechanical operations know that this is a non-linear solver. This will be propagated back to the + // force fields during the addForce and addKToMatrix phase, which will let them recompute their internal + // stresses if they have a non-linear relationship with the displacement. mop->setImplicit(true); - msg_info() << "======= Starting static ODE solver in time step " << this->getTime(); - msg_info() << "(doing a maximum of " << d_newton_iterations.getValue() << " newton iterations)"; - + // Options for the Newton-Raphson + const auto & relative_correction_tolerance_threshold = d_relative_correction_tolerance_threshold.getValue(); + const auto & absolute_correction_tolerance_threshold = d_absolute_correction_tolerance_threshold.getValue(); + const auto & relative_residual_tolerance_threshold = d_relative_residual_tolerance_threshold.getValue(); + const auto & absolute_residual_tolerance_threshold = d_absolute_residual_tolerance_threshold.getValue(); + const auto & max_number_of_newton_iterations = d_newton_iterations.getValue(); + const auto & should_diverge_when_residual_is_growing = d_should_diverge_when_residual_is_growing.getValue(); + const auto & print_log = f_printLog.getValue(); + auto info = MessageDispatcher::info(Message::Runtime, std::make_shared(this->getClassName()), SOFA_FILE_INFO); + + // Local variables used for the iterations unsigned n_it=0; - double dx_norm = -1.0, f_norm; - - sofa::helper::AdvancedTimer::stepBegin("StaticSolver::Solve"); + double dx_squared_norm = 0, U_squared_norm = 0, R_squared_norm = 0, R0_squared_norm = 0, R_previous_squared_norm = 0; + const auto absolute_squared_residual_threshold = absolute_residual_tolerance_threshold*absolute_residual_tolerance_threshold; + const auto relative_squared_residual_tolerance_threshold = relative_residual_tolerance_threshold*relative_residual_tolerance_threshold; + const auto absolute_squared_correction_threshold = absolute_correction_tolerance_threshold*absolute_correction_tolerance_threshold; + const auto relative_squared_correction_threshold = relative_correction_tolerance_threshold*relative_correction_tolerance_threshold; + bool converged = false, diverged = false; + steady_clock::time_point t; + + // Reset the list of residual norms for this time step + p_squared_residual_norms.clear(); + p_squared_residual_norms.reserve(max_number_of_newton_iterations); + + // Reset the list of increment norms for this time step + p_squared_increment_norms.clear(); + p_squared_increment_norms.reserve(max_number_of_newton_iterations); + + if (print_log) + { + info << "======= Starting static ODE solver =======\n"; + info << "Time step : " << this->getTime() << "\n"; + info << "Context : " << dynamic_cast(context)->getPathName() << "\n"; + info << "Max number of iterations : " << max_number_of_newton_iterations << "\n"; + info << "Residual tolerance (abs) : " << absolute_residual_tolerance_threshold << "\n"; + info << "Residual tolerance (rel) : " << relative_residual_tolerance_threshold << "\n"; + info << "Correction tolerance (abs) : " << absolute_correction_tolerance_threshold << "\n"; + info << "Correction tolerance (rel) : " << relative_correction_tolerance_threshold << "\n"; + } - // compute addForce, in mapped: addForce + applyJT (vec) - // Initial computation - force.clear(); - mop.computeForce(force); + // Start the advanced timer + ScopedAdvancedTimer timer ("StaticSolver::Solve"); + + // ########################################################################### + // # First residual # + // ########################################################################### + // # Before starting any newton iterations, we first need to compute # + // # the residual with the updated right-hand side (the new load increment) # + // ########################################################################### + sofa::helper::AdvancedTimer::stepBegin("ComputeForce"); + // Step 1 Assemble the force vector + // 1. Clear the force vector (F := 0) + // 2. Go down in the current context tree calling addForce on every forcefields + // 3. Go up from the current context tree leaves calling applyJT on every mechanical mappings + mop.computeForce(force, true /* clear */); + + // Step 2 Projective constraints + // Calls the "projectResponse" method of every BaseProjectiveConstraintSet objects found in the + // current context tree. An example of such constraint set is the FixedConstraint. In this case, + // it will set to 0 every row (i, _) of the right-hand side (force) vector for the ith degree of + // freedom. mop.projectResponse(force); - f_norm = sqrt(force.dot(force)); + sofa::helper::AdvancedTimer::stepEnd("ComputeForce"); - if (d_residual_tolerance_threshold.getValue() > 0 && f_norm <= d_residual_tolerance_threshold.getValue()) { - msg_info() << "The ODE has already reached an equilibrium state"; - } else { + // Compute the initial residual + R_squared_norm = force.dot(force); - while (n_it < d_newton_iterations.getValue()) { - std::string stepname = "step_" + std::to_string(n_it); - sofa::helper::AdvancedTimer::stepBegin(stepname.c_str()); + if (absolute_residual_tolerance_threshold > 0 && R_squared_norm <= absolute_squared_residual_threshold) + { + converged = true; + if (print_log) + { + info << "The ODE has already reached an equilibrium state." + << std::scientific + << "The residual's ratio |R| is " << std::setw(12) << std::sqrt(R_squared_norm) + << " (criterion is " << std::setw(12) << absolute_residual_tolerance_threshold << ") \n" + << std::defaultfloat; + } + } + // ########################################################################### + // # Newton iterations # + // ########################################################################### - // Assemble matrix, CG: does nothing - // LDL non-mapped: addKToMatrix added to system matrix - // LDL mapped: addKToMatrix not added to the system matrix, needs mapped FF (TODO) - sofa::core::behavior::MultiMatrix matrix(&mop); + while (! converged && n_it < max_number_of_newton_iterations) + { + ScopedAdvancedTimer step_timer ("NewtonStep"); + t = steady_clock::now(); + + // Part I. Assemble the system matrix. + MultiMatrix matrix(&mop); + { + ScopedAdvancedTimer _t_("MBKBuild"); + // 1. The MechanicalMatrix::K is a simple structure that stores three floats called factors: m, b and k. + // 2. the * operator simply multiplies each of the three factors with a value. No matrix is built yet. + // 3. The = operator first search for a linear solver in the current context. It then calls the + // "setSystemMBKMatrix" method of the linear solver. + + // A. For LinearSolver using a GraphScatteredMatrix (ie, non-assembled matrices), nothing appends. + // B. For LinearSolver using other type of matrices (FullMatrix, SparseMatrix, CompressedRowSparseMatrix), + // the "addMBKToMatrix" method is called on each BaseForceField objects and the "applyConstraint" method + // is called on every BaseProjectiveConstraintSet objects. An example of such constraint set is the + // FixedConstraint. In this case, it will set to 0 every column (_, i) and row (i, _) of the assembled + // matrix for the ith degree of freedom. matrix = MechanicalMatrix::K * -1.0; + } + // Part II. Solve the unknown increment. + { + ScopedAdvancedTimer _t_("MBKSolve"); + // Calls methods "setSystemRHVector", "setSystemLHVector" and "solveSystem" of the LinearSolver component // for CG: calls iteratively addDForce, mapped: [applyJ, addDForce, applyJt(vec)]+ - // for LDL: solves the system, everything's already assembled + // for Direct: solves the system, everything's already assembled matrix.solve(dx, force); + } + + // Part III. Propagate the solution increment and update geometry. + { + ScopedAdvancedTimer _t_("PropagateDx"); + // Updating the geometry + x.peq(dx); // x := x + dx - x.eq(x_start, dx, 1); + // Calls "solveConstraint" method of every ConstraintSolver objects found in the current context tree. + // todo(jnbrunet): Shouldn't this be done AFTER the position propagation of the mapped nodes? mop.solveConstraint(x, sofa::core::ConstraintParams::POS); - // Propagate positions to mapped nodes: taken from AnimateVisitor::processNodeTopDown executed by the animation loop - // calls apply, applyJ - sofa::core::MechanicalParams mp; - MechanicalPropagateOnlyPositionAndVelocityVisitor(&mp).execute( - this->getContext()); // propagate the changes to mappings below + // Propagate positions to mapped mechanical objects, for example, identity mappings, barycentric mappings, ... + // This will call the methods apply and applyJ on every mechanical mappings. + MechanicalPropagateOnlyPositionAndVelocityVisitor(&mechanical_parameters).execute(context); + } + + // At this point, we completed one iteration, increment the counter. + // The rest is only for convergence tests and logging. + n_it++; + + // The rest of the step is only necessary when doing more than one Newton iteration. Otherwise, we will + // waste computation time to reassemble the residual and compute the norms for a convergence that will + // never happen (we will always reach the maximum number of iterations, which is 1) + if (max_number_of_newton_iterations == 1) + { + converged = true; // Not really, but we won't warn about divergence when it is always the case + diverged = false; + break; + } + + // Part IV. Update the force vector. + { + ScopedAdvancedTimer _t_("UpdateForce"); - // Compute addForce, in mapped: addForce + applyJT (vec) - force.clear(); mop.computeForce(force); mop.projectResponse(force); - double f_cur_norm = sqrt(force.dot(force)); + } + + // Part V. Compute the updated norms. + { + ScopedAdvancedTimer _t_("ComputeNorms"); + + // Residual norm + R_squared_norm = force.dot(force); - dx_norm = sqrt(dx.dot(dx)); + if (n_it == 1) + { + R0_squared_norm = R_squared_norm; + R_previous_squared_norm = R0_squared_norm; + } + p_squared_residual_norms.emplace_back(R_squared_norm); + + // Displacement norm + U.peq(dx); + dx_squared_norm = dx.dot(dx); + U_squared_norm= U.dot(U); + + p_squared_increment_norms.emplace_back(dx_squared_norm); + } + + // Part VI. Stop timers and print step information. + { + auto iteration_time = duration_cast(steady_clock::now() - t).count(); + + if (print_log) + { + info << "Newton iteration #" << std::left << std::setw(5) << n_it + << std::scientific + << " |R| = " << std::setw(12) << std::sqrt(R_squared_norm) + << " |R|/|R0| = " << std::setw(12) << (R0_squared_norm < epsilon*epsilon ? 0 : std::sqrt(R_squared_norm / R0_squared_norm)) + << " |du| = " << std::setw(12) << std::sqrt(dx_squared_norm) + << " |du| / |U| = " << std::setw(12) << (U_squared_norm < epsilon*epsilon ? 0 : std::sqrt(dx_squared_norm / U_squared_norm)) + << std::defaultfloat; + info << " Time = " << iteration_time / 1000 / 1000 << " ms"; + info << "\n"; + } + } + + // Part VII. Check for convergence/divergence + { + if (std::isnan(R_squared_norm) || std::isnan(dx_squared_norm) || U_squared_norm < epsilon*epsilon) + { + diverged = true; + if (print_log) + { + info << "[DIVERGED]"; + if (std::isnan(R_squared_norm)) + { + info << " The residual's ratio |R| is NaN."; + } + if (std::isnan(dx_squared_norm)) + { + info << " The correction's ratio |du| is NaN."; + } + if (U_squared_norm < epsilon) + { + info << " The correction's ratio |du|/|U| is NaN (|U| is zero)."; + } + info << "\n"; + } + break; + } - msg_info() << "Newton iteration #" << n_it << ": |f - K(x0 + dx)| = " << f_cur_norm << " |dx| = " << dx_norm; - sofa::helper::AdvancedTimer::valSet("residual", f_cur_norm); - sofa::helper::AdvancedTimer::valSet("correction", dx_norm); - sofa::helper::AdvancedTimer::stepEnd(stepname.c_str()); + if (absolute_correction_tolerance_threshold > 0 && dx_squared_norm < absolute_squared_correction_threshold) + { + converged = true; + if (print_log) + { + info << "[CONVERGED] The correction's norm |du| = " << std::sqrt(dx_squared_norm) << " is smaller than the threshold of " << absolute_correction_tolerance_threshold << ".\n"; + } + break; + } - if (d_should_diverge_when_residual_is_growing.getValue() && f_cur_norm > f_norm && n_it>1) { - msg_info() << "[DIVERGED] residual's norm increased"; + if (relative_correction_tolerance_threshold > 0 && dx_squared_norm < relative_squared_correction_threshold*U_squared_norm) + { + converged = true; + if (print_log) + { + info << "[CONVERGED] The correction's ratio |du|/|U| = " << std::sqrt(dx_squared_norm/U_squared_norm) << " is smaller than the threshold of " << relative_correction_tolerance_threshold << ".\n"; + } break; } - if (dx_norm <= this->d_correction_tolerance_threshold.getValue()) { - msg_info() << "[CONVERGED] The correction's norm |dx| is smaller than the threshold of " - << d_correction_tolerance_threshold; + if (absolute_residual_tolerance_threshold > 0 && R_squared_norm < absolute_squared_residual_threshold) + { + converged = true; + if (print_log) + { + info << "[CONVERGED] The residual's norm |R| = " << std::sqrt(R_squared_norm) << " is smaller than the threshold of " << absolute_residual_tolerance_threshold << ".\n"; + } break; } - if (d_residual_tolerance_threshold.getValue() > 0 && f_cur_norm <= d_residual_tolerance_threshold.getValue()) { - msg_info() << "[CONVERGED] The residual's norm |f - K(x0 + dx)| is smaller than the threshold of " - << d_residual_tolerance_threshold; + if (relative_residual_tolerance_threshold > 0 && R_squared_norm < relative_squared_residual_tolerance_threshold*R0_squared_norm) + { + converged = true; + if (print_log) + { + info << "[CONVERGED] The residual's ratio |R|/|R0| = " << std::sqrt(R_squared_norm/R0_squared_norm) << " is smaller than the threshold of " << relative_residual_tolerance_threshold << ".\n"; + } break; } - f_norm = f_cur_norm; - n_it++; + if (should_diverge_when_residual_is_growing && R_squared_norm > R_previous_squared_norm) + { + diverged = true; + if (print_log) + { + info << "[DIVERGED] The current residual norm |R| = " << std::sqrt(R_squared_norm) + << " is greater than at the previous Newton iteration (" << std::sqrt(R_previous_squared_norm) << ").\n"; + } + } + } - } // End while (n_it < d_newton_iterations.getValue()) + // This is used to detect a rise of residual (divergence test) + R_previous_squared_norm = R_squared_norm; } - if (n_it >= d_newton_iterations.getValue()) { - n_it--; - msg_info() << "[DIVERGED] The number of Newton iterations reached the threshold of " << d_newton_iterations << " iterations"; + n_it--; // Reset to the actual index of the last iteration completed + + if (! converged && ! diverged && n_it == (max_number_of_newton_iterations-1)) + { + if (print_log) + { + info << "[DIVERGED] The number of Newton iterations reached the maximum of " << max_number_of_newton_iterations << " iterations" << ".\n"; + } } sofa::helper::AdvancedTimer::valSet("nb_iterations", n_it+1); - sofa::helper::AdvancedTimer::valSet("residual", f_norm); - sofa::helper::AdvancedTimer::valSet("correction", dx_norm); - sofa::helper::AdvancedTimer::stepEnd("StaticSolver::Solve"); + sofa::helper::AdvancedTimer::valSet("residual", std::sqrt(R_squared_norm)); + sofa::helper::AdvancedTimer::valSet("correction", std::sqrt(dx_squared_norm)); } diff --git a/SofaKernel/modules/SofaImplicitOdeSolver/src/SofaImplicitOdeSolver/StaticSolver.h b/SofaKernel/modules/SofaImplicitOdeSolver/src/SofaImplicitOdeSolver/StaticSolver.h index bdadc816bd0..b973fd73e50 100644 --- a/SofaKernel/modules/SofaImplicitOdeSolver/src/SofaImplicitOdeSolver/StaticSolver.h +++ b/SofaKernel/modules/SofaImplicitOdeSolver/src/SofaImplicitOdeSolver/StaticSolver.h @@ -30,6 +30,32 @@ namespace sofa::component::odesolver using sofa::core::objectmodel::Data; +/** + * Implementation of a static ODE solver compatible with non-linear materials. + * + * We are trying to solve to following + * \f{eqnarray*}{ + * \vec{R}(\vec{x}) - \vec{P} = 0 + * \f} + * + * Where \f$\vec{R}\f$ is the (possibly non-linear) internal elastic force residual and \f$\vec{P}\f$ is the external + * force vector (for example, gravitation force or surface traction). + * + * Following the Newton-Raphson method, + * we pose + * + * \f{align*}{ + * \vec{F}(\vec{x}_{n+1}) &= \vec{R}(\vec{x}_{n+1}) - \vec{P}_n \\ + * \mat{J} = \frac{\partial \vec{F}}{\partial \vec{x}_{n+1}} \bigg\rvert_{\vec{x}_{n+1}^i} &= \mat{K}(\vec{x}_{n+1}) + * \f} + * + * where \f$\vec{x}_{n+1}\f$ is the unknown position vector at the \f$n\f$th time step. We then iteratively solve + * + * \f{align*}{ + * \mat{K}(\vec{x}_{n+1}^i) \left [ \Delta \vec{x}_{n+1}^{i+1} \right ] &= - \vec{F}(\vec{x}_{n+1}^i) \\ + * \vec{x}_{n+1}^{i+1} &= \vec{x}_{n+1}^{i} + \Delta \vec{x}_{n+1}^{i+1} + * \f} + */ class SOFA_SOFAIMPLICITODESOLVER_API StaticSolver : public sofa::core::behavior::OdeSolver { public: @@ -39,6 +65,12 @@ class SOFA_SOFAIMPLICITODESOLVER_API StaticSolver : public sofa::core::behavior: public: void solve (const sofa::core::ExecParams* params /* PARAMS FIRST */, double dt, sofa::core::MultiVecCoordId xResult, sofa::core::MultiVecDerivId vResult) override; + /** The list of squared residual norms (r.dot(r) = ||r||^2) of every newton iterations of the last solve call. */ + auto squared_residual_norms() const -> const std::vector & { return p_squared_residual_norms; } + + /** The list of squared correction increment norms (dx.dot(dx) = ||dx||^2) of every newton iterations of the last solve call. */ + auto squared_increment_norms() const -> const std::vector & { return p_squared_increment_norms; } + /// Given a displacement as computed by the linear system inversion, how much will it affect the velocity /// /// This method is used to compute the compliance for contact corrections @@ -116,13 +148,22 @@ class SOFA_SOFAIMPLICITODESOLVER_API StaticSolver : public sofa::core::behavior: protected: - /// the solution vector is stored for warm-start - sofa::core::behavior::MultiVecDeriv dx; - Data d_newton_iterations; ///< Number of newton iterations between each load increments (normally, one load increment per simulation time-step. - Data d_correction_tolerance_threshold; ///< Convergence criterion: The newton iterations will stop when the norm of correction |du| reach this threshold. - Data d_residual_tolerance_threshold; ///< Convergence criterion: The newton iterations will stop when the norm of the residual |f - K(u)| reach this threshold. Use a negative value to disable this criterion. + Data d_absolute_correction_tolerance_threshold; ///< Convergence criterion: The newton iterations will stop when the norm |du| is smaller than this threshold. + Data d_relative_correction_tolerance_threshold; ///< Convergence criterion: The newton iterations will stop when the ratio |du| / |U| is smaller than this threshold. + Data d_absolute_residual_tolerance_threshold; ///< Convergence criterion: The newton iterations will stop when the norm of the residual |R| is smaller than this threshold. Use a negative value to disable this criterion. + Data d_relative_residual_tolerance_threshold; ///< Convergence criterion: The newton iterations will stop when the ratio |R|/|R0| is smaller than this threshold. Use a negative value to disable this criterion. Data d_should_diverge_when_residual_is_growing; ///< Divergence criterion: The newton iterations will stop when the residual is greater than the one from the previous iteration. + +private: + /// Sum of displacement increments since the beginning of the time step + sofa::core::behavior::MultiVecDeriv U; + + /// List of squared residual norms (r.dot(R) = ||r||^2) of every newton iterations of the last solve call. + std::vector p_squared_residual_norms; + + /// List of squared correction increment norms (dx.dot(dx) = ||dx||^2) of every newton iterations of the last solve call. + std::vector p_squared_increment_norms; }; } // namespace sofa::component::odesolver