From c193fecc2569259afcfc0507f0fff8dce699dfae Mon Sep 17 00:00:00 2001 From: Jean-Nicolas Brunet Date: Fri, 18 Mar 2022 20:23:42 -0400 Subject: [PATCH] Add callback mechanism to Newton-Raphson solvers Signed-off-by: Jean-Nicolas Brunet --- CMakeLists.txt | 2 +- src/SofaCaribou/Ode/NewtonRaphsonSolver.cpp | 26 ++-- src/SofaCaribou/Ode/NewtonRaphsonSolver.h | 83 +++++++++++++ src/SofaCaribou/Python/CMakeLists.txt | 25 +++- .../Python/Ode/NewtonRaphsonSolver.cpp | 114 ++++++++++++++++++ .../Python/Ode/NewtonRaphsonSolver.h | 10 ++ .../Python/Ode/StaticODESolver.cpp | 5 +- src/SofaCaribou/Python/SofaCaribou.cpp | 2 + .../Python/pytest/SofaCaribou_StaticSolver.py | 92 ++++++++++++++ unittest/SofaCaribou/ODE/test_static.cpp | 59 +++++++++ 10 files changed, 402 insertions(+), 16 deletions(-) create mode 100644 src/SofaCaribou/Python/Ode/NewtonRaphsonSolver.cpp create mode 100644 src/SofaCaribou/Python/Ode/NewtonRaphsonSolver.h create mode 100644 src/SofaCaribou/Python/pytest/SofaCaribou_StaticSolver.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 3fbf9f06..a78918ca 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.12) -project(Caribou VERSION 21.06.00) +project(Caribou VERSION 22.06.99) # Policies cmake_policy(SET CMP0072 NEW) diff --git a/src/SofaCaribou/Ode/NewtonRaphsonSolver.cpp b/src/SofaCaribou/Ode/NewtonRaphsonSolver.cpp index 34b52f1b..1278a7d2 100644 --- a/src/SofaCaribou/Ode/NewtonRaphsonSolver.cpp +++ b/src/SofaCaribou/Ode/NewtonRaphsonSolver.cpp @@ -159,7 +159,6 @@ void NewtonRaphsonSolver::solve(const ExecParams *params, SReal dt, MultiVecCoor } // Local variables used for the iterations - unsigned n_it=0; double dx_squared_norm, du_squared_norm, R_squared_norm = 0; const auto squared_residual_threshold = residual_tolerance_threshold*residual_tolerance_threshold; const auto squared_correction_threshold = correction_tolerance_threshold*correction_tolerance_threshold; @@ -248,16 +247,20 @@ void NewtonRaphsonSolver::solve(const ExecParams *params, SReal dt, MultiVecCoor // # Newton iterations # // ########################################################################### - while (not converged and n_it < newton_iterations) { + p_current_iteration = 0; + while (not converged and p_current_iteration < newton_iterations) { sofa::helper::ScopedAdvancedTimer step_timer ("NewtonStep"); t = steady_clock::now(); + trigger_event(Event::ITERATION_BEGIN); + // Part 1. Assemble the system matrix. { sofa::helper::ScopedAdvancedTimer _t_("MBKBuild"); p_A->clear(); this->assemble_system_matrix(mechanical_parameters, accessor, p_A.get()); linear_solver->set_system_matrix(p_A.get()); + trigger_event(Event::MATRIX_ASSEMBLED); } // Part 2. Analyze the pattern of the matrix in order to compute a permutation matrix. @@ -281,6 +284,8 @@ void NewtonRaphsonSolver::solve(const ExecParams *params, SReal dt, MultiVecCoor break; } + trigger_event(Event::MATRIX_ANALYZED); + p_has_already_analyzed_the_pattern = true; } } @@ -293,6 +298,8 @@ void NewtonRaphsonSolver::solve(const ExecParams *params, SReal dt, MultiVecCoor diverged = true; break; } + + trigger_event(Event::MATRIX_FACTORIZED); } // Part 4. Solve the unknown increment. @@ -303,12 +310,14 @@ void NewtonRaphsonSolver::solve(const ExecParams *params, SReal dt, MultiVecCoor diverged = true; break; } + trigger_event(Event::INCREMENT_SOLVED); } // Part 5. Propagating the solution increment and update geometry. { sofa::helper::ScopedAdvancedTimer _t_("PropagateDx"); this->propagate_solution_increment(mechanical_parameters, accessor, p_DX.get(), x_id, v_id, dx_id); + trigger_event(Event::INCREMENT_PROPAGATED); } // The next two parts are only necessary when doing more than one Newton iteration @@ -323,6 +332,7 @@ void NewtonRaphsonSolver::solve(const ExecParams *params, SReal dt, MultiVecCoor sofa::helper::AdvancedTimer::stepBegin("UpdateResidual"); R_squared_norm = SofaCaribou::Algebra::dot(p_F.get(), p_F.get()); sofa::helper::AdvancedTimer::stepEnd("UpdateResidual"); + trigger_event(Event::RESIDUAL_UPDATED); } // Part 8. Compute the updated displacement residual. @@ -341,11 +351,13 @@ void NewtonRaphsonSolver::solve(const ExecParams *params, SReal dt, MultiVecCoor p_squared_residuals.emplace_back(R_squared_norm); + trigger_event(Event::ITERATION_END); + // We completed one iteration, increment the counter - n_it++; + p_current_iteration++; if( print_log ) { - info << "Newton iteration #" << std::left << std::setw(5) << n_it + info << "Newton iteration #" << std::left << std::setw(5) << p_current_iteration << std::scientific << " |R| = " << std::setw(12) << sqrt(R_squared_norm) << " |R|/|R0| = " << std::setw(12) << sqrt(R_squared_norm / p_squared_residuals[0]) @@ -405,9 +417,9 @@ void NewtonRaphsonSolver::solve(const ExecParams *params, SReal dt, MultiVecCoor vop.v_clear(dx_id); } // End while (not converged and not diverged and n_it < newton_iterations) - n_it--; // Reset to the actual index of the last iteration completed + p_current_iteration--; // Reset to the actual index of the last iteration completed - if (not converged and not diverged and n_it == (newton_iterations-1)) { + if (not converged and not diverged and p_current_iteration == (newton_iterations-1)) { if (print_log) { info << "[DIVERGED] The number of Newton iterations reached the maximum of " << newton_iterations << " iterations" << ".\n"; } @@ -416,7 +428,7 @@ void NewtonRaphsonSolver::solve(const ExecParams *params, SReal dt, MultiVecCoor d_converged.setValue(converged); sofa::helper::AdvancedTimer::valSet("has_converged", converged ? 1 : 0); - sofa::helper::AdvancedTimer::valSet("nb_iterations", n_it+1); + sofa::helper::AdvancedTimer::valSet("nb_iterations", p_current_iteration+1); } void NewtonRaphsonSolver::init() { diff --git a/src/SofaCaribou/Ode/NewtonRaphsonSolver.h b/src/SofaCaribou/Ode/NewtonRaphsonSolver.h index 91c92a87..de8952f1 100644 --- a/src/SofaCaribou/Ode/NewtonRaphsonSolver.h +++ b/src/SofaCaribou/Ode/NewtonRaphsonSolver.h @@ -14,6 +14,8 @@ DISABLE_ALL_WARNINGS_BEGIN DISABLE_ALL_WARNINGS_END #include +#include +#include namespace SofaCaribou::ode { @@ -63,6 +65,35 @@ class NewtonRaphsonSolver : public sofa::core::behavior::OdeSolver { ALWAYS }; + /** + * Events associated to registered callback function for different time in the NR iteration process. + */ + enum class Event : unsigned int { + /** Event triggered at the very beginning of the Newton iteration. */ + ITERATION_BEGIN = 0, + + /** Event triggered after the system matrix has been assembled. */ + MATRIX_ASSEMBLED, + + /** Event triggered after the system matrix has been analyzed. */ + MATRIX_ANALYZED, + + /** Event triggered after the system matrix has been factorized. */ + MATRIX_FACTORIZED, + + /** Event triggered after the system solution vector has been solved. */ + INCREMENT_SOLVED, + + /** Event triggered after the system solution vector has been propagated through mappings. */ + INCREMENT_PROPAGATED, + + /** Event triggered after the system force vector has been updated. */ + RESIDUAL_UPDATED, + + /** Event triggered at the very end of the Newton iteration. */ + ITERATION_END, + }; + NewtonRaphsonSolver(); @@ -92,6 +123,45 @@ class NewtonRaphsonSolver : public sofa::core::behavior::OdeSolver { void set_pattern_analysis_strategy(const PatternAnalysisStrategy & strategy); + /** Get the complete force vector of the system */ + auto F() const -> const sofa::defaulttype::BaseVector * { + return p_F.get(); + } + + /** Get the complete increment vector (solution) of the system */ + auto dx() const -> const sofa::defaulttype::BaseVector * { + return p_DX.get(); + } + + /** Get the complete matrix of the linearized part of system */ + auto A() const -> const sofa::defaulttype::BaseMatrix * { + return p_A.get(); + } + + /** Get the current newton iteration within the time step solve call. First iteration is 1 (index starts from 1). */ + auto current_iteration() const -> unsigned int { + return p_current_iteration+1; + } + + /** + * Register a callback function to be called at the specific NR event (see NewtonRaphsonSolver::Event for the list + * of events). + * + * @example + * @code + * using NewtonRaphson::Event; + * solver->register_callback(MATRIX_ASSEMBLED, [](const NewtonRaphsonSolver & solver){ + * std::cout << "The system tangent matrix has been assembled in NR iteration #" << solver.current_iteration << '\n'; + * }); + * @endcode + * @param event + */ + inline void register_callback(const Event & event, const std::function& fct) { + const auto event_id = static_cast(event); + caribou_assert(event_id >= 0 and event_id <= static_cast(Event::ITERATION_END)); + p_iteration_callbacks[event_id].push_back(fct); + } + private: /** @@ -164,6 +234,13 @@ class NewtonRaphsonSolver : public sofa::core::behavior::OdeSolver { sofa::core::MultiVecDerivId & v_id, sofa::core::MultiVecDerivId & dx_id) = 0; + void trigger_event(const Event & event) const { + const auto event_id = static_cast(event); + for (const auto & cb : p_iteration_callbacks[event_id]) { + cb(*this); + } + } + protected: /** Check that the linked linear solver is not null and that it implements the SofaCaribou::solver::LinearSolver interface */ @@ -207,5 +284,11 @@ class NewtonRaphsonSolver : public sofa::core::behavior::OdeSolver { /// Either or not the pattern of the system matrix was analyzed at the beginning of the simulation bool p_has_already_analyzed_the_pattern = false; + + /// Current newton iteration within a time step solve call + unsigned int p_current_iteration; + + /// List of callback function to be called at each events of a NR iteration + std::array>, static_cast(Event::ITERATION_END)+1> p_iteration_callbacks; }; } diff --git a/src/SofaCaribou/Python/CMakeLists.txt b/src/SofaCaribou/Python/CMakeLists.txt index a63f95e5..23f7e2a1 100644 --- a/src/SofaCaribou/Python/CMakeLists.txt +++ b/src/SofaCaribou/Python/CMakeLists.txt @@ -7,6 +7,7 @@ set(HEADER_FILES Forcefield/HyperelasticForcefield.h Mass/CaribouMass.h Ode/LegacyStaticODESolver.h + Ode/NewtonRaphsonSolver.h Ode/StaticODESolver.h Solver/ConjugateGradientSolver.h Solver/LDLTSolver.h @@ -22,6 +23,7 @@ set(SOURCE_FILES Forcefield/HyperelasticForcefield.cpp Mass/CaribouMass.cpp Ode/LegacyStaticODESolver.cpp + Ode/NewtonRaphsonSolver.cpp Ode/StaticODESolver.cpp Solver/ConjugateGradientSolver.cpp Solver/LDLTSolver.cpp @@ -38,6 +40,7 @@ set(PYTHON_FILES set(PYTHON_TEST_FILES pytest/SofaCaribou_Forcefield_HyperelasticForcefield.py pytest/SofaCaribou_Mass_CaribouMass.py + pytest/SofaCaribou_StaticSolver.py ) find_package(SofaPython3 REQUIRED) @@ -64,10 +67,20 @@ caribou_add_python_module(SofaCaribou ) list(APPEND target_rpath - "$ORIGIN/../../../../../../lib" - "$loader_path/../../../../../../lib" - "$ORIGIN/../../../../lib" - "$loader_path/../../../../lib" + "$ORIGIN/../../../../../../lib" + "$$ORIGIN/../../../../../../lib" + "$loader_path/../../../../../../lib" + "@loader_path/../../../../../../lib" + "$executable_path/../../../../../../lib" + "@executable_path/../../../../../../lib" + "$ORIGIN/../../../../lib" + "$$ORIGIN/../../../../lib" + "$loader_path/../../../../lib" + "@loader_path/../../../../lib" + "$executable_path/../../../../lib" + "@executable_path/../../../../lib" + "$loader_path/../Caribou" + "@loader_path/../Caribou" ) set_target_properties( ${PROJECT_NAME} @@ -75,6 +88,10 @@ set_target_properties( INSTALL_RPATH "${target_rpath}" ) + +# Make the target relocatable +set_target_properties(${PROJECT_NAME} PROPERTIES RELOCATABLE_INSTALL_DIR "plugins/SofaCaribou") +set_target_properties(${PROJECT_NAME} PROPERTIES EXPORT_PROPERTIES "RELOCATABLE_INSTALL_DIR") if (SOFA_VERSION VERSION_GREATER_EQUAL "20.12.99") # Set RPATH towards relocatable dependencies sofa_auto_set_target_rpath(TARGETS ${PROJECT_NAME} RELOCATABLE "plugins") diff --git a/src/SofaCaribou/Python/Ode/NewtonRaphsonSolver.cpp b/src/SofaCaribou/Python/Ode/NewtonRaphsonSolver.cpp new file mode 100644 index 00000000..970c83cd --- /dev/null +++ b/src/SofaCaribou/Python/Ode/NewtonRaphsonSolver.cpp @@ -0,0 +1,114 @@ +#include "NewtonRaphsonSolver.h" + +#include +#include +#include +#define NDEBUG +#include +#include +#include + +#include +#include + + +namespace py = pybind11; + +namespace SofaCaribou::ode::python { + +void addNewtonRaphsonSolver(pybind11::module &m) { + using namespace sofa::core::objectmodel; + py::class_> c(m, "NewtonRaphsonSolver"); + + py::enum_( + c, "PatternAnalysisStrategy", + "Different strategies to determine when the pattern of the system matrix should be " + "analyzed in order to, for example, compute a permutation matrix before factorizing it.") + .value("ALWAYS", NewtonRaphsonSolver::PatternAnalysisStrategy::ALWAYS, "Analyze the pattern of the matrix at each Newton iterations.") + .value("NEVER", NewtonRaphsonSolver::PatternAnalysisStrategy::NEVER, "Never analyze the pattern of the matrix.") + .value("BEGINNING_OF_THE_SIMULATION", NewtonRaphsonSolver::PatternAnalysisStrategy::BEGINNING_OF_THE_SIMULATION, "Analyze the pattern of the matrix once at the beginning of the simulation.") + .value("BEGINNING_OF_THE_TIME_STEP", NewtonRaphsonSolver::PatternAnalysisStrategy::BEGINNING_OF_THE_TIME_STEP, "Analyze the pattern of the matrix once per time step.") + .export_values(); + + py::enum_( + c, "Event", + "Events associated to registered callback function for different time in the NR iteration process.") + .value("ITERATION_BEGIN", NewtonRaphsonSolver::Event::ITERATION_BEGIN, "Event triggered at the very beginning of the Newton iteration.") + .value("MATRIX_ASSEMBLED", NewtonRaphsonSolver::Event::MATRIX_ASSEMBLED, "Event triggered after the system matrix has been assembled.") + .value("MATRIX_ANALYZED", NewtonRaphsonSolver::Event::MATRIX_ANALYZED, "Event triggered after the system matrix has been analyzed.") + .value("MATRIX_FACTORIZED", NewtonRaphsonSolver::Event::MATRIX_FACTORIZED, "Event triggered after the system matrix has been factorized.") + .value("INCREMENT_SOLVED", NewtonRaphsonSolver::Event::INCREMENT_SOLVED, "Event triggered after the system solution vector has been solved.") + .value("INCREMENT_PROPAGATED", NewtonRaphsonSolver::Event::INCREMENT_PROPAGATED, "Event triggered after the system solution vector has been propagated through mappings.") + .value("RESIDUAL_UPDATED", NewtonRaphsonSolver::Event::RESIDUAL_UPDATED, "Event triggered after the system force vector has been updated.") + .value("ITERATION_END", NewtonRaphsonSolver::Event::ITERATION_END, "Event triggered at the very end of the Newton iteration.") + .export_values(); + + c.def_property_readonly("iteration_times", &NewtonRaphsonSolver::iteration_times, "List of times (in nanoseconds) that each Newton-Raphson iteration took to compute in the last call to Solve()."); + c.def_property_readonly("squared_residuals", &NewtonRaphsonSolver::squared_residuals, "The list of squared residual norms (||r||^2) of every newton iterations of the last solve call."); + c.def_property_readonly("squared_initial_residual", &NewtonRaphsonSolver::squared_initial_residual, "The initial squared residual (||r0||^2) of the last solve call."); + c.def_property("pattern_analysis_strategy", &NewtonRaphsonSolver::pattern_analysis_strategy, &NewtonRaphsonSolver::set_pattern_analysis_strategy, "Get the current strategy that determine when the pattern of the system matrix should be analyzed."); + c.def_property_readonly("current_iteration", &NewtonRaphsonSolver::current_iteration, "Get the current newton iteration within the time step solve call. First iteration is 1 (index starts from 1)."); + + c.def_property_readonly("A", [](const NewtonRaphsonSolver & solver) -> py::object { + if (solver.A() == nullptr) { + return py::none(); + } + + if (const auto * a = dynamic_cast> *>(solver.A())) { + return py::cast(a->matrix()); + } + + if (const auto * a = dynamic_cast> *>(solver.A())) { + return py::cast(a->matrix()); + } + + if (const auto * a = dynamic_cast> *>(solver.A())) { + return py::cast(a->matrix()); + } + + if (const auto * a = dynamic_cast> *>(solver.A())) { + return py::cast(a->matrix()); + } + + throw std::runtime_error("Cannot bind the system matrix type to python."); + }, "Complete matrix of the linearized part of system"); + + c.def_property_readonly("dx", [](const NewtonRaphsonSolver & solver) -> py::object { + if (solver.dx() == nullptr) { + return py::none(); + } + + if (const auto * a = dynamic_cast> *>(solver.dx())) { + return py::cast(a->vector()); + } + + if (const auto * a = dynamic_cast> *>(solver.dx())) { + return py::cast(a->vector()); + } + + throw std::runtime_error("Cannot bind the system increment vector type to python."); + }, "Solution of the linearized system"); + + c.def_property_readonly("F", [](const NewtonRaphsonSolver & solver) -> py::object { + if (solver.F() == nullptr) { + return py::none(); + } + + if (const auto * a = dynamic_cast> *>(solver.F())) { + return py::cast(a->vector()); + } + + if (const auto * a = dynamic_cast> *>(solver.F())) { + return py::cast(a->vector()); + } + + throw std::runtime_error("Cannot bind the system RHS vector type to python."); + }, "Force vector (right-hand side) of the system"); + + c.def("register_callback", [](NewtonRaphsonSolver & solver, const NewtonRaphsonSolver::Event & e, py::function callback) { + solver.register_callback(e, [callback](const NewtonRaphsonSolver & s) { + callback(py::cast(s, py::return_value_policy::reference)); + }); + }, "Register a callback function to be called at the specific NR event (see NewtonRaphsonSolver::Event for the list of events)."); +} +} \ No newline at end of file diff --git a/src/SofaCaribou/Python/Ode/NewtonRaphsonSolver.h b/src/SofaCaribou/Python/Ode/NewtonRaphsonSolver.h new file mode 100644 index 00000000..34431361 --- /dev/null +++ b/src/SofaCaribou/Python/Ode/NewtonRaphsonSolver.h @@ -0,0 +1,10 @@ +#pragma once + +#undef NDEBUG +#include + +namespace SofaCaribou::ode::python { + +void addNewtonRaphsonSolver(pybind11::module &m); + +} diff --git a/src/SofaCaribou/Python/Ode/StaticODESolver.cpp b/src/SofaCaribou/Python/Ode/StaticODESolver.cpp index 8f00fd6c..a4408bce 100644 --- a/src/SofaCaribou/Python/Ode/StaticODESolver.cpp +++ b/src/SofaCaribou/Python/Ode/StaticODESolver.cpp @@ -17,10 +17,7 @@ namespace SofaCaribou::ode::python { void addStaticODESolver(py::module &m) { using namespace sofa::core::objectmodel; - py::class_> c (m, "StaticODESolver"); - c.def_property_readonly("iteration_times", &StaticODESolver::iteration_times); - c.def_property_readonly("squared_residuals", &StaticODESolver::squared_residuals); - c.def_property_readonly("squared_initial_residual", &StaticODESolver::squared_initial_residual); + py::class_> c (m, "StaticODESolver"); sofapython3::PythonFactory::registerType([](sofa::core::objectmodel::Base* o) { return py::cast(dynamic_cast(o)); diff --git a/src/SofaCaribou/Python/SofaCaribou.cpp b/src/SofaCaribou/Python/SofaCaribou.cpp index e109c66b..0f0c760e 100644 --- a/src/SofaCaribou/Python/SofaCaribou.cpp +++ b/src/SofaCaribou/Python/SofaCaribou.cpp @@ -1,6 +1,7 @@ #include #include +#include #include #include #include @@ -23,6 +24,7 @@ PYBIND11_MODULE(SofaCaribou, m) { SofaCaribou::topology::python::addCaribouTopology(m); // ODE bindings + SofaCaribou::ode::python::addNewtonRaphsonSolver(m); SofaCaribou::ode::python::addLegacyStaticODESolver(m); SofaCaribou::ode::python::addStaticODESolver(m); diff --git a/src/SofaCaribou/Python/pytest/SofaCaribou_StaticSolver.py b/src/SofaCaribou/Python/pytest/SofaCaribou_StaticSolver.py new file mode 100644 index 00000000..436ce0ab --- /dev/null +++ b/src/SofaCaribou/Python/pytest/SofaCaribou_StaticSolver.py @@ -0,0 +1,92 @@ +import Sofa, SofaRuntime +import sys +import unittest +import numpy as np +from scipy.sparse import csr_matrix +from pathlib import Path + +current_dir = Path(__file__).parent +site_packages_dir = (current_dir / '..' / '..' / 'lib' / 'python3' / 'site-packages').resolve() +sys.path.insert(0, str(site_packages_dir)) +print(f'Adding {site_packages_dir} to sys.path') + +import SofaCaribou +from SofaCaribou import NewtonRaphsonSolver + +E = NewtonRaphsonSolver.Event + +number_of_steps = 10 +number_of_newton_iterations = 10 +threshold = 1e-15 +radius = 7.5 +length = 80 +nx = 3 +nz = 9 +eps = 1e-5 + + +def createScene(node): + node.addObject('RequiredPlugin', pluginName=[ + 'SofaBaseMechanics', 'SofaEngine', 'SofaGraphComponent', 'SofaOpenglVisual', + 'SofaPreconditioner', 'SofaBoundaryCondition', 'SofaTopologyMapping']) + node.addObject('RegularGridTopology', name='grid', min=[-radius, -radius, 0], max=[radius, radius, length], n=[nx, nx, nz]) + node.addObject('StaticODESolver', name='solver', newton_iterations=number_of_newton_iterations, correction_tolerance_threshold=1e-5, residual_tolerance_threshold=1e-5, printLog=False) + node.addObject('LDLTSolver', backend='Pardiso') + node.addObject('MechanicalObject', name='mo', position='@grid.position') + node.addObject('HexahedronSetTopologyContainer', name='mechanical_topology', src='@grid') + node.addObject('SaintVenantKirchhoffMaterial', young_modulus=3000, poisson_ratio=0.499) + node.addObject('HyperelasticForcefield', name='ff', topology='@mechanical_topology') + node.addObject('BoxROI', name='base_roi', box=[-radius - eps, -radius - eps, 0 - eps, radius + eps, radius + eps, 0 + eps]) + node.addObject('BoxROI', name='top_roi', box=[ -radius - eps, -radius - eps, length - eps, radius + eps, radius + eps, length + eps], quad='@mechanical_topology.quads') + node.addObject('FixedConstraint', indices='@base_roi.indices') + node.addObject('QuadSetTopologyContainer', name='neumann_topology', quads='@top_roi.quadInROI') + node.addObject('TractionForcefield', topology='@neumann_topology', traction=[0, -30, 0], slope=0.2) + + +class TestStaticSolver(unittest.TestCase): + def assertMatrixEqual(self, A, B): + return self.assertTrue(np.allclose(A.todense(), B.todense()), f"Matrices are not equal, with \nA={A} \nand\nB={B}") + def assertVectorAlmostEqual(self, A, B): + return self.assertTrue(np.allclose(A, B), f"Vectors are not equal, with \nA={A} \nand\nB={B}") + + def test_residuals(self): + root = Sofa.Core.Node() + createScene(root) + Sofa.Simulation.init(root) + + # The simulation is supposed to converged in 5 load increments. Let's check the norms of force residuals. + # todo(jnbrunet): These values should be validated against another external software + force_residuals = [ + [1.000000000000000e+00, 4.802014099846802e-03, 3.457785823647809e-04, 6.417769508095073e-08], # Step 1 + [1.000000000000000e+00, 4.700052322118300e-03, 3.771851553383748e-04, 1.289757638852382e-05, 2.125041655083045e-08], # Step 2 + [1.000000000000000e+00, 4.416064536488316e-03, 4.703733156787435e-04, 3.659722934024732e-05, 7.791507698853802e-08], # Step 3 + [1.000000000000000e+00, 4.003458215116851e-03, 6.263051713537671e-04, 4.996976075176631e-05, 1.422069948624354e-07], # Step 4 + [1.000000000000000e+00, 3.526942203674829e-03, 8.307813177405512e-04, 4.667215114394798e-05, 1.646730071539153e-07], # Step 5 + ] + + def increment(n): n +=1 + + step_number = 0 + count = [0, 0, 0, 0, 0, 0, 0, 0] + root.solver.register_callback(E.ITERATION_BEGIN, lambda s: increment(count[E.ITERATION_BEGIN])) + root.solver.register_callback(E.MATRIX_ASSEMBLED, lambda s: increment(count[E.MATRIX_ASSEMBLED])) + root.solver.register_callback(E.MATRIX_ANALYZED, lambda s: increment(count[E.MATRIX_ANALYZED])) + root.solver.register_callback(E.MATRIX_FACTORIZED, lambda s: increment(count[E.MATRIX_FACTORIZED])) + root.solver.register_callback(E.INCREMENT_SOLVED, lambda s: increment(count[E.INCREMENT_SOLVED])) + root.solver.register_callback(E.INCREMENT_PROPAGATED, lambda s: increment(count[E.INCREMENT_PROPAGATED])) + root.solver.register_callback(E.RESIDUAL_UPDATED, lambda s: increment(count[E.RESIDUAL_UPDATED])) + root.solver.register_callback(E.ITERATION_END, lambda s: increment(count[E.ITERATION_END])) + + for step_number in range(5): + Sofa.Simulation.animate(root, 1) + self.assertEqual(root.solver.current_iteration, len(force_residuals[step_number]), f"for step {step_number}") + r0 = root.solver.squared_residuals[0] + residuals = [np.sqrt(r/r0) for r in root.solver.squared_residuals] + self.assertVectorAlmostEqual(residuals, force_residuals[step_number]) + + self.assertEqual(count[E.ITERATION_BEGIN], root.solver.current_iteration) + count = [0, 0, 0, 0, 0, 0, 0, 0] + + +if __name__ == '__main__': + unittest.main() diff --git a/unittest/SofaCaribou/ODE/test_static.cpp b/unittest/SofaCaribou/ODE/test_static.cpp index 89226117..50ead8ca 100644 --- a/unittest/SofaCaribou/ODE/test_static.cpp +++ b/unittest/SofaCaribou/ODE/test_static.cpp @@ -145,15 +145,74 @@ TEST(StaticODESolver, Beam) { {1.000000000000000e+00, 3.526942203674829e-03, 8.307813177405512e-04, 4.667215114394798e-05, 1.646730071539153e-07} // Step 5 }}; + // Make sure callbacks work and are in the good order + using E = SofaCaribou::ode::NewtonRaphsonSolver::Event; + using NR = SofaCaribou::ode::NewtonRaphsonSolver; + constexpr unsigned int nb_events = 8; + std::array event_count = {}; + + solver->register_callback(E::ITERATION_BEGIN, [&event_count](const NR & /*solver*/) { + auto c = ++event_count[static_cast(E::ITERATION_BEGIN)]; + EXPECT_EQ(c, event_count[static_cast(E::MATRIX_ASSEMBLED)] + 1); + }); + + solver->register_callback(E::MATRIX_ASSEMBLED, [&event_count](const NR & /*solver*/) { + auto c = ++event_count[static_cast(E::MATRIX_ASSEMBLED)]; + EXPECT_EQ(c, event_count[static_cast(E::ITERATION_BEGIN)]); + }); + + solver->register_callback(E::MATRIX_ANALYZED, [&event_count](const NR & solver) { + auto c = ++event_count[static_cast(E::MATRIX_ANALYZED)]; + EXPECT_EQ(c, solver.current_iteration()+1); + }); + + solver->register_callback(E::MATRIX_FACTORIZED, [&event_count](const NR & /*solver*/) { + auto c = ++event_count[static_cast(E::MATRIX_FACTORIZED)]; + EXPECT_EQ(c, event_count[static_cast(E::INCREMENT_SOLVED)] + 1); + }); + + solver->register_callback(E::INCREMENT_SOLVED, [&event_count](const NR & /*solver*/) { + auto c = ++event_count[static_cast(E::INCREMENT_SOLVED)]; + EXPECT_EQ(c, event_count[static_cast(E::INCREMENT_PROPAGATED)] + 1); + }); + + solver->register_callback(E::INCREMENT_PROPAGATED, [&event_count](const NR & solver) { + auto c = ++event_count[static_cast(E::INCREMENT_PROPAGATED)]; + EXPECT_EQ(c, event_count[static_cast(E::RESIDUAL_UPDATED)] + 1); + }); + + solver->register_callback(E::RESIDUAL_UPDATED, [&event_count](const NR & /*solver*/) { + auto c = ++event_count[static_cast(E::RESIDUAL_UPDATED)]; + EXPECT_EQ(c, event_count[static_cast(E::ITERATION_END)] + 1); + }); + + solver->register_callback(E::ITERATION_END, [&event_count](const NR & /*solver*/) { + auto c = ++event_count[static_cast(E::ITERATION_END)]; + EXPECT_EQ(c, event_count[static_cast(E::ITERATION_BEGIN)]); + }); + getSimulation()->init(root.get()); for (unsigned int step_id = 0; step_id < force_residuals.size(); ++step_id) { + for (unsigned int event_id = 0; event_id < nb_events; ++event_id) { + event_count[event_id] = 0; + } + getSimulation()->animate(root.get(), 1); + EXPECT_EQ(solver->squared_residuals().size(), force_residuals[step_id].size()); for (unsigned int newton_step_id = 0; newton_step_id < solver->squared_residuals().size(); ++newton_step_id) { double residual = solver->squared_residuals()[newton_step_id] / solver->squared_residuals()[0]; EXPECT_NEAR(sqrt(residual), force_residuals[step_id][newton_step_id], 1e-10); } + + for (unsigned int event_id = 0; event_id < nb_events; ++event_id) { + if (event_id == static_cast(E::MATRIX_ANALYZED)) { + EXPECT_EQ(event_count[event_id], 1); + } else { + EXPECT_EQ(event_count[event_id], solver->squared_residuals().size()); + } + } } // Let's also validate the position of the node positioned at the center of the end-surface of the beam