### [HierarchicalIterative] Allow to solve equations level by level.

parent db6dd757
Pipeline #18409 failed with stage
in 11 minutes and 27 seconds
 ... ... @@ -168,7 +168,8 @@ namespace hpp { /// priority than 1. /// /// The algorithm used is a Newton-Raphson like algorithm that works as /// follows: let \f$f (\mathbf{q}) = 0\f$ be the system of equations where /// follows: for a single level of priority, let \f$f (\mathbf{q}) = 0\f$ /// be the system of equations where /// \f$f\f$ is a \f$C^1\f$ mapping from the robot configuration space to /// a Lie group space \f$\mathcal{L}\f$. /// ... ... @@ -191,11 +192,23 @@ namespace hpp { /// \li the residual \f$\|f(\mathbf{q})\|\f$ is below an error threshold, or /// \li the maximal number of iterations has been reached. /// /// The computation of the direction of descent in the case of multiple /// level of hierarchy is described in /// this paper. /// /// The error threshold can be accessed by methods /// HierarchicalIterative::errorThreshold. The maximal number of /// iterations can be accessed by methods /// HierarchicalIterative::maxIterations. /// /// \note Solving equations one after the other /// /// For some applications, it can be more efficient to solve a set of /// equations one after the other. In other words, an equation is ignored /// until the previous one is solved (norm below the threshold). To do so, /// introduce the equations using method HierarchicalIterative::add with /// increasing value of \c priority, and call method /// \c HierarchicalIterative::solveLevelByLevel(true). /// \note Lie group /// /// The unknowns \f$\mathbf{q}\f$ may take values in a more general set ... ... @@ -454,6 +467,21 @@ namespace hpp { return lastIsOptional_; } // Set whether the constraints are solved level by level // // If true, the solver will consider only the higher level of // priority that is not satisfied when computing the direction // of descent. void solveLevelByLevel(bool solveLevelByLevel) { solveLevelByLevel_ = solveLevelByLevel; } // Get whether the constraints are solved level by level bool solveLevelByLevel() const { return solveLevelByLevel_; } /// \} /// \name Stack ... ... @@ -644,6 +672,7 @@ namespace hpp { LiegroupSpacePtr_t configSpace_; size_type dimension_, reducedDimension_; bool lastIsOptional_; bool solveLevelByLevel_; /// Unknown of the set of implicit constraints Indices_t freeVariables_; Saturation_t saturate_; ... ...
 ... ... @@ -198,6 +198,7 @@ namespace hpp { squaredErrorThreshold_ (0), inequalityThreshold_ (0), maxIterations_ (0), stacks_ (), configSpace_ (configSpace), dimension_ (0), reducedDimension_ (0), lastIsOptional_ (false), solveLevelByLevel_(false), freeVariables_ (), saturate_ (new saturation::Base()), constraints_ (), iq_ (), iv_ (), priority_ (), sigma_ (0), dq_ (), dqSmall_ (), reducedJ_ (), ... ... @@ -820,7 +821,8 @@ namespace hpp { d.maxRank = std::max(d.maxRank, rank); if (d.maxRank > 0) sigma_ = std::min(sigma_, d.svd.singularValues()[d.maxRank - 1]); if(solveLevelByLevel_ && err.squaredNorm() > squaredErrorThreshold_) break; if (last) break; // No need to compute projector for next step. if (d.svd.matrixV().cols() == rank) break; // The kernel is { 0 } ... ...
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment