[Lagrangian.Solver] UnbuiltConstraintSolver: implement hotstart #5875
There are no files selected for viewing
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -24,9 +24,18 @@ | |
| #include <sofa/component/constraint/lagrangian/solver/UnbuiltConstraintProblem.h> | ||
| #include <sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h> | ||
|
|
||
| #include <sofa/helper/ScopedAdvancedTimer.h> | ||
|
|
||
| namespace sofa::component::constraint::lagrangian::solver | ||
| { | ||
|
|
||
| UnbuiltConstraintSolver::UnbuiltConstraintSolver() | ||
| : GenericConstraintSolver() | ||
| , d_initialGuess(initData(&d_initialGuess, false, "initialGuess", "Activate constraint force history to possibly improve convergence (hot start).")) | ||
| { | ||
|
|
||
| } | ||
|
|
||
| void UnbuiltConstraintSolver::doBuildSystem( const core::ConstraintParams *cParams, GenericConstraintProblem * problem, unsigned int numConstraints) | ||
| { | ||
| SOFA_UNUSED(cParams); | ||
|
|
@@ -37,13 +46,14 @@ void UnbuiltConstraintSolver::doBuildSystem( const core::ConstraintParams *cPara | |
| return; | ||
| } | ||
|
|
||
| // Initialize constraint sequence ONCE before iterating over constraint corrections | ||
| c_current_cp->constraints_sequence.resize(numConstraints); | ||
| std::iota(c_current_cp->constraints_sequence.begin(), c_current_cp->constraints_sequence.end(), 0); | ||
|
|
||
| for (const auto& cc : l_constraintCorrections) | ||
| { | ||
| if (!cc->isActive()) continue; | ||
|
|
||
| // some constraint corrections (e.g LinearSolverConstraintCorrection) | ||
| // can change the order of the constraints, to optimize later computations | ||
| cc->resetForUnbuiltResolution(c_current_cp->getF(), c_current_cp->constraints_sequence); | ||
|
|
@@ -59,11 +69,9 @@ void UnbuiltConstraintSolver::doBuildSystem( const core::ConstraintParams *cPara | |
| for (unsigned int i = 0; i < numConstraints; i++) | ||
| c_current_cp->cclist_elems[i].resize(nbCC, nullptr); | ||
|
|
||
| for (unsigned int c_id = 0; c_id < numConstraints;) | ||
| { | ||
| bool foundCC = false; | ||
| const unsigned int l = c_current_cp->constraintsResolutions[c_id]->getNbLines(); | ||
|
|
||
| for (unsigned int j = 0; j < l_constraintCorrections.size(); j++) | ||
|
|
@@ -93,6 +101,24 @@ void UnbuiltConstraintSolver::doBuildSystem( const core::ConstraintParams *cPara | |
|
|
||
| } | ||
|
|
||
| void UnbuiltConstraintSolver::doPreApplyCorrection() | ||
| { | ||
| if (!d_initialGuess.getValue()) | ||
| return; | ||
|
|
||
| // Save forces for hot-start in next timestep | ||
| keepContactForcesValue(); | ||
| } | ||
| void UnbuiltConstraintSolver::doPreClearCorrection(const core::ConstraintParams* cparams) | ||
| { | ||
| getConstraintInfo(cparams); | ||
| } | ||
|
|
||
| void UnbuiltConstraintSolver::doPostClearCorrection() | ||
| { | ||
| computeInitialGuess(); | ||
| } | ||
|
|
||
| void UnbuiltConstraintSolver::initializeConstraintProblems() | ||
| { | ||
| for (unsigned i=0; i< CP_BUFFER_SIZE; ++i) | ||
|
|
@@ -102,5 +128,107 @@ void UnbuiltConstraintSolver::initializeConstraintProblems() | |
| current_cp = m_cpBuffer[0].get(); | ||
| } | ||
|
|
||
| void UnbuiltConstraintSolver::getConstraintInfo(const core::ConstraintParams* cparams) | ||
| { | ||
| if (d_initialGuess.getValue() && (m_numConstraints != 0)) | ||
| { | ||
| SCOPED_TIMER("GetConstraintInfo"); | ||
| m_constraintBlockInfo.clear(); | ||
| m_constraintIds.clear(); | ||
| simulation::mechanicalvisitor::MechanicalGetConstraintInfoVisitor(cparams, m_constraintBlockInfo, m_constraintIds).execute(getContext()); | ||
| } | ||
| } | ||
|
|
||
| void UnbuiltConstraintSolver::computeInitialGuess() | ||
| { | ||
| if (!d_initialGuess.getValue() || m_numConstraints == 0) | ||
| return; | ||
|
|
||
| SCOPED_TIMER("InitialGuess"); | ||
|
|
||
| SReal* force = current_cp->getF(); | ||
| const int numConstraints = current_cp->getDimension(); | ||
|
|
||
| // First, zero all forces | ||
| for (int c = 0; c < numConstraints; c++) | ||
| { | ||
| force[c] = 0.0; | ||
| } | ||
|
|
||
| // Then restore forces from previous timestep for matching persistent IDs | ||
| for (const ConstraintBlockInfo& info : m_constraintBlockInfo) | ||
| { | ||
| if (!info.parent) continue; | ||
| if (!info.hasId) continue; | ||
|
|
||
| auto previt = m_previousConstraints.find(info.parent); | ||
|
Contributor
There was a problem hiding this comment. Using the parent as a key is somewhat strange because it it irrelevant @the level of the solver. What is relevant is the element on which the force is applied. This information is only kept in the constraint jacobian matrix. At this level I don't think it is possible to get an actual proper hotstart. What is done here it is looking at the id of the constraint at the level of the component which transform the collision detection output into the constraint jacobian. So what you are comparing here is the place of the force in the original vector of the collision detection. So for me this hotstart is just as good as copying back the content of the old force vector into the new one while clamping or padding if size mismatch. This is only by chance that you get forces that are good because the collision detection is somewhat deterministic. The only strong difference would be when there are two component responsible of transforming this collision output into contact force, this would keep the force at least in the good part of the vector (the old forces will be used a starting point of forces coming from the same collision detection), but there is no garantee that the force is applied in the same region of the object as before. This happens when you have multiple objects colliding. But really, I don't know how much you gain with respect of a simple copy/clipping/padding work. Could you try out your example with this simple implementation to see if you get worst convergence ? |
||
| if (previt == m_previousConstraints.end()) continue; | ||
|
|
||
| const ConstraintBlockBuf& buf = previt->second; | ||
| const int c0 = info.const0; | ||
| const int nbl = (info.nbLines < buf.nbLines) ? info.nbLines : buf.nbLines; | ||
|
|
||
| for (int c = 0; c < info.nbGroups; ++c) | ||
| { | ||
| auto it = buf.persistentToConstraintIdMap.find(m_constraintIds[info.offsetId + c]); | ||
| if (it == buf.persistentToConstraintIdMap.end()) continue; | ||
|
|
||
| const int prevIndex = it->second; | ||
| if (prevIndex >= 0 && prevIndex + nbl <= static_cast<int>(m_previousForces.size())) | ||
| { | ||
| for (int l = 0; l < nbl; ++l) | ||
| { | ||
| force[c0 + c * nbl + l] = m_previousForces[prevIndex + l]; | ||
| } | ||
| } | ||
| } | ||
| } | ||
| } | ||
|
|
||
| void UnbuiltConstraintSolver::keepContactForcesValue() | ||
| { | ||
| SCOPED_TIMER("KeepForces"); | ||
|
|
||
| const SReal* force = current_cp->getF(); | ||
| const unsigned int numConstraints = current_cp->getDimension(); | ||
|
|
||
| // Store current forces | ||
| m_previousForces.resize(numConstraints); | ||
| for (unsigned int c = 0; c < numConstraints; ++c) | ||
| { | ||
| m_previousForces[c] = force[c]; | ||
| } | ||
|
|
||
| // Clear previous history (mark all as invalid) | ||
| for (auto& previousConstraint : m_previousConstraints) | ||
| { | ||
| ConstraintBlockBuf& buf = previousConstraint.second; | ||
| for (auto& it2 : buf.persistentToConstraintIdMap) | ||
| { | ||
| it2.second = -1; | ||
| } | ||
| } | ||
|
Comment on lines
+203
to
+210
Contributor
There was a problem hiding this comment. Wouldn't it be better to simply clear the whole map and start fresh ? Otherwise if the parent keep changing, this map will grow without getting properly cleaned |
||
|
|
||
| // Fill info from current constraint IDs | ||
| for (const ConstraintBlockInfo& info : m_constraintBlockInfo) | ||
| { | ||
| if (!info.parent) continue; | ||
| if (!info.hasId) continue; | ||
|
|
||
| ConstraintBlockBuf& buf = m_previousConstraints[info.parent]; | ||
| buf.nbLines = info.nbLines; | ||
|
|
||
| for (int c = 0; c < info.nbGroups; ++c) | ||
| { | ||
| buf.persistentToConstraintIdMap[m_constraintIds[info.offsetId + c]] = info.const0 + c * info.nbLines; | ||
| } | ||
| } | ||
|
|
||
| // Update constraint count for next iteration | ||
| m_numConstraints = numConstraints; | ||
| } | ||
|
|
||
|
|
||
|
|
||
|
|
||
| } | ||
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I don't get why this is needed. When you look at the implementation of the doPreClear, clearing the curren_cp wouldn't change a thing in what MechanicalGetConstraintInfoVisitor will get. So for me the pre/post is not necessary. could you try mergin both and calling the merged method after the clear ?