Commit 574ad149 authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

[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
/// <a href="https://hal.archives-ouvertes.fr/lirmm-00796736">this paper</a>.
///
/// 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