From 3ba0e375c9f85dcb3bc7670c3191a1651ba5b1a5 Mon Sep 17 00:00:00 2001 From: Jean-Nicolas Brunet Date: Wed, 21 Apr 2021 20:47:00 +0200 Subject: [PATCH 1/3] [SofaKernel] Rewrite of the static Newton-Raphson ODE solver This commit rewrites the static ODE solver, implements two additional convergence criteria, improves the documentation of every step found in the "solve" method, and adds useful doxygen documentation of the Newton algorithms executed by the component. Up until this commit, the only convergence criteria of the static ODE solver were absolute force residual norms, and absolute (displacement) increment norm. Using absolute values for convergence criteria means that the value set by the user is dependent on the simulation scale. For example, setting an absolute displacement norm of 1e-5 might seem enough for a liver mesh set in millimetres, but is definitely not adequate for the same liver using this time a mesh scaled in metres. This is a source of user error, and is probably why many FEM software will use relative convergence criteria. The two additional criteria are: The relative residual norm |R|/|R0| where |R| is the norm of the force vector after the last Newton iteration, and |R0| is the norm of the force vector after the first Newton iteration. The relative displacement norm |dx|/|U| where |dx| is the norm of the latest solution vector, and |U| is the norm of the sum over all previous solution vectors of the current time step. To remove any confusion between absolute and relative criteria, the convergence data field "correction_tolerance_threshold" and "residual_tolerance_threshold" were renamed for "absolute_correction_tolerance_threshold" and "absolute_residual_tolerance_threshold", respectively. Note that to keep a compatibility layer with older scene files, these old data field names are still available, but a deprecated warning is shown to the user, inviting him to update its scene. --- .../SofaImplicitOdeSolver_test/CMakeLists.txt | 3 +- .../StaticSolver_test.cpp | 255 ++++++++++++ .../SofaImplicitOdeSolver/StaticSolver.cpp | 379 ++++++++++++++---- .../src/SofaImplicitOdeSolver/StaticSolver.h | 51 ++- 4 files changed, 605 insertions(+), 83 deletions(-) create mode 100644 SofaKernel/modules/SofaImplicitOdeSolver/SofaImplicitOdeSolver_test/StaticSolver_test.cpp 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 5789e41336a..c7ee5b7b6e4 100644 --- a/SofaKernel/modules/SofaImplicitOdeSolver/src/SofaImplicitOdeSolver/StaticSolver.cpp +++ b/SofaKernel/modules/SofaImplicitOdeSolver/src/SofaImplicitOdeSolver/StaticSolver.cpp @@ -26,6 +26,11 @@ #include #include #include +#include + +#include +#include +#include namespace sofa::component::odesolver { @@ -39,14 +44,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, + "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, - "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, + "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, - "residual_tolerance_threshold", - "Convergence criterion: The newton iterations will stop when the norm of the residual |f - K(u)| reach this threshold. " + "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, @@ -62,7 +76,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)"; @@ -70,7 +84,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)"; @@ -78,7 +92,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)"; @@ -86,121 +100,332 @@ 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() ); + 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)); - - 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 { - - while (n_it < d_newton_iterations.getValue()) { - std::string stepname = "step_" + std::to_string(n_it); - sofa::helper::AdvancedTimer::stepBegin(stepname.c_str()); - + sofa::helper::AdvancedTimer::stepEnd("ComputeForce"); + + // Compute the initial residual + R_squared_norm = force.dot(force); + + 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; + } + } - // 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); + // ########################################################################### + // # Newton iterations # + // ########################################################################### + + 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); + } - x.eq(x_start, dx, 1); + // Part III. Propagate the solution increment and update geometry. + { + ScopedAdvancedTimer _t_("PropagateDx"); + // Updating the geometry + x.peq(dx); // x := x + dx + + // 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; - sofa::simulation::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. + sofa::simulation::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)); + } - dx_norm = sqrt(dx.dot(dx)); + // Part V. Compute the updated norms. + { + ScopedAdvancedTimer _t_("ComputeNorms"); + // Residual norm + R_squared_norm = force.dot(force); - 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 (n_it == 1) { + R0_squared_norm = R_squared_norm; + R_previous_squared_norm = R0_squared_norm; + } - if (d_should_diverge_when_residual_is_growing.getValue() && f_cur_norm > f_norm && n_it>1) { - msg_info() << "[DIVERGED] residual's norm increased"; + 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) << std::sqrt(R_squared_norm / R0_squared_norm) + << " |du| = " << std::setw(12) << std::sqrt(dx_squared_norm) + << " |du| / |U| = " << std::setw(12) << 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; } - 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_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_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_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; } - f_norm = f_cur_norm; - n_it++; + 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 (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; + } + + 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 b9b9369e1ff..341cdf4659e 100644 --- a/SofaKernel/modules/SofaImplicitOdeSolver/src/SofaImplicitOdeSolver/StaticSolver.h +++ b/SofaKernel/modules/SofaImplicitOdeSolver/src/SofaImplicitOdeSolver/StaticSolver.h @@ -31,6 +31,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: @@ -40,6 +66,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 @@ -117,13 +149,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 From 94ef04a63c5b063b066602fd388289e91840ae4d Mon Sep 17 00:00:00 2001 From: Jean-Nicolas Brunet Date: Wed, 5 May 2021 10:00:10 +0200 Subject: [PATCH 2/3] fix formatting --- .../SofaImplicitOdeSolver/StaticSolver.cpp | 83 ++++++++++++------- 1 file changed, 54 insertions(+), 29 deletions(-) diff --git a/SofaKernel/modules/SofaImplicitOdeSolver/src/SofaImplicitOdeSolver/StaticSolver.cpp b/SofaKernel/modules/SofaImplicitOdeSolver/src/SofaImplicitOdeSolver/StaticSolver.cpp index c7ee5b7b6e4..d4ce022510d 100644 --- a/SofaKernel/modules/SofaImplicitOdeSolver/src/SofaImplicitOdeSolver/StaticSolver.cpp +++ b/SofaKernel/modules/SofaImplicitOdeSolver/src/SofaImplicitOdeSolver/StaticSolver.cpp @@ -122,7 +122,8 @@ void StaticSolver::parse(sofa::core::objectmodel::BaseObjectDescription* arg) 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; @@ -191,7 +192,8 @@ void StaticSolver::solve(const sofa::core::ExecParams* params, double dt, sofa:: p_squared_increment_norms.clear(); p_squared_increment_norms.reserve(max_number_of_newton_iterations); - if (print_log) { + if (print_log) + { info << "======= Starting static ODE solver =======\n"; info << "Time step : " << this->getTime() << "\n"; info << "Context : " << dynamic_cast(context)->getPathName() << "\n"; @@ -229,9 +231,11 @@ void StaticSolver::solve(const sofa::core::ExecParams* params, double dt, sofa:: // Compute the initial residual R_squared_norm = force.dot(force); - if (absolute_residual_tolerance_threshold > 0 && R_squared_norm <= absolute_squared_residual_threshold) { + if (absolute_residual_tolerance_threshold > 0 && R_squared_norm <= absolute_squared_residual_threshold) + { converged = true; - if (print_log) { + 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) @@ -244,7 +248,8 @@ void StaticSolver::solve(const sofa::core::ExecParams* params, double dt, sofa:: // # Newton iterations # // ########################################################################### - while (! converged && n_it < max_number_of_newton_iterations) { + while (! converged && n_it < max_number_of_newton_iterations) + { ScopedAdvancedTimer step_timer ("NewtonStep"); t = steady_clock::now(); @@ -297,7 +302,8 @@ void StaticSolver::solve(const sofa::core::ExecParams* params, double dt, sofa:: // 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) { + 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; @@ -318,7 +324,8 @@ void StaticSolver::solve(const sofa::core::ExecParams* params, double dt, sofa:: // Residual norm R_squared_norm = force.dot(force); - if (n_it == 1) { + if (n_it == 1) + { R0_squared_norm = R_squared_norm; R_previous_squared_norm = R0_squared_norm; } @@ -337,13 +344,14 @@ void StaticSolver::solve(const sofa::core::ExecParams* params, double dt, sofa:: { auto iteration_time = duration_cast(steady_clock::now() - t).count(); - if (print_log) { + 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) << std::sqrt(R_squared_norm / R0_squared_norm) - << " |du| = " << std::setw(12) << std::sqrt(dx_squared_norm) - << " |du| / |U| = " << std::setw(12) << std::sqrt(dx_squared_norm / U_squared_norm) + << " |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"; @@ -352,17 +360,22 @@ void StaticSolver::solve(const sofa::core::ExecParams* params, double dt, sofa:: // Part VII. Check for convergence/divergence { - if (std::isnan(R_squared_norm) || std::isnan(dx_squared_norm) || U_squared_norm < epsilon*epsilon) { + if (std::isnan(R_squared_norm) || std::isnan(dx_squared_norm) || U_squared_norm < epsilon*epsilon) + { diverged = true; - if (print_log) { + if (print_log) + { info << "[DIVERGED]"; - if (std::isnan(R_squared_norm)) { + if (std::isnan(R_squared_norm)) + { info << " The residual's ratio |R| is NaN."; } - if (std::isnan(dx_squared_norm)) { + if (std::isnan(dx_squared_norm)) + { info << " The correction's ratio |du| is NaN."; } - if (U_squared_norm < epsilon) { + if (U_squared_norm < epsilon) + { info << " The correction's ratio |du|/|U| is NaN (|U| is zero)."; } info << "\n"; @@ -370,41 +383,51 @@ void StaticSolver::solve(const sofa::core::ExecParams* params, double dt, sofa:: break; } - if (absolute_correction_tolerance_threshold > 0 && dx_squared_norm < absolute_squared_correction_threshold) { + if (absolute_correction_tolerance_threshold > 0 && dx_squared_norm < absolute_squared_correction_threshold) + { converged = true; - if (print_log) { + 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 (relative_correction_tolerance_threshold > 0 && dx_squared_norm < relative_squared_correction_threshold*U_squared_norm) { + if (relative_correction_tolerance_threshold > 0 && dx_squared_norm < relative_squared_correction_threshold*U_squared_norm) + { converged = true; - if (print_log) { + 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 (absolute_residual_tolerance_threshold > 0 && R_squared_norm < absolute_squared_residual_threshold) { + if (absolute_residual_tolerance_threshold > 0 && R_squared_norm < absolute_squared_residual_threshold) + { converged = true; - if (print_log) { + 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 (relative_residual_tolerance_threshold > 0 && R_squared_norm < relative_squared_residual_tolerance_threshold*R0_squared_norm) { + if (relative_residual_tolerance_threshold > 0 && R_squared_norm < relative_squared_residual_tolerance_threshold*R0_squared_norm) + { converged = true; - if (print_log) { + 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; } - if (should_diverge_when_residual_is_growing && R_squared_norm > R_previous_squared_norm) { + if (should_diverge_when_residual_is_growing && R_squared_norm > R_previous_squared_norm) + { diverged = true; - if (print_log) { + 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"; } @@ -417,8 +440,10 @@ void StaticSolver::solve(const sofa::core::ExecParams* params, double dt, sofa:: 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) { + 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"; } } From 128c4a79b244799bc82c8eecb745f444b205422f Mon Sep 17 00:00:00 2001 From: Jean-Nicolas Brunet Date: Thu, 6 May 2021 08:59:08 +0200 Subject: [PATCH 3/3] fix dx not allocated in mapped nodes --- .../src/SofaImplicitOdeSolver/StaticSolver.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/SofaKernel/modules/SofaImplicitOdeSolver/src/SofaImplicitOdeSolver/StaticSolver.cpp b/SofaKernel/modules/SofaImplicitOdeSolver/src/SofaImplicitOdeSolver/StaticSolver.cpp index 526d9b3f959..bcc8abcb637 100644 --- a/SofaKernel/modules/SofaImplicitOdeSolver/src/SofaImplicitOdeSolver/StaticSolver.cpp +++ b/SofaKernel/modules/SofaImplicitOdeSolver/src/SofaImplicitOdeSolver/StaticSolver.cpp @@ -150,6 +150,7 @@ void StaticSolver::solve(const sofa::core::ExecParams* params, double dt, sofa:: 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();