BySubstitution (const LiegroupSpacePtr_t &configSpace )
BySubstitution (const BySubstitution &other)
virtual ~BySubstitution ()
bool add (const ImplicitPtr_t &numericalConstraint, const std::size_t &priority=0)
const NumericalConstraints_t & numericalConstraints () const
Get the numerical constraints implicit and explicit.
ExplicitConstraintSet & explicitConstraintSet ()
Get explicit constraint set.
const ExplicitConstraintSet & explicitConstraintSet () const
Set explicit constraint set.
size_type numberFreeVariables () const
Return the number of free variables.
void explicitConstraintSetHasChanged ()
Should be called whenever explicit solver is modified.
template<typename LineSearchType >
Status solve (vectorOut_t arg, LineSearchType ls=LineSearchType()) const
template<typename LineSearchType >
Status solve (vectorOut_t arg, bool optimize, LineSearchType ls=LineSearchType()) const
void projectVectorOnKernel (ConfigurationIn_t from, vectorIn_t velocity, vectorOut_t result) const
virtual void projectOnKernel (ConfigurationIn_t from, ConfigurationIn_t to, ConfigurationOut_t result)
Status solve (vectorOut_t arg) const
size_type errorSize () const
bool isSatisfied (vectorIn_t arg) const
bool isSatisfied (vectorIn_t arg, value_type errorThreshold ) const
bool isSatisfied (vectorIn_t arg, vectorOut_t error) const
bool isConstraintSatisfied (const ImplicitPtr_t &constraint, vectorIn_t arg, vectorOut_t error, bool &constraintFound) const
template<typename LineSearchType >
bool oneStep (vectorOut_t arg, LineSearchType &lineSearch) const
void updateJacobian (vectorIn_t arg) const
void errorThreshold (const value_type &threshold)
Set error threshold.
value_type errorThreshold () const
Get error threshold.
segments_t implicitDof () const
virtual std::ostream & print (std::ostream &os) const
bool integrate (vectorIn_t from, vectorIn_t velocity, vectorOut_t result) const
template<typename LineSearchType >
HierarchicalIterative::Status impl_solve (vectorOut_t arg, bool _optimize, LineSearchType lineSearch) const
vector_t rightHandSideFromConfig (ConfigurationIn_t config)
bool rightHandSideFromConfig (const ImplicitPtr_t &constraint, ConfigurationIn_t config)
bool rightHandSide (const ImplicitPtr_t &constraint, vectorIn_t rhs)
bool getRightHandSide (const ImplicitPtr_t &constraint, vectorOut_t rhs) const
void rightHandSide (vectorIn_t rhs)
vector_t rightHandSide () const
size_type rightHandSideSize () const
HierarchicalIterative (const LiegroupSpacePtr_t &configSpace )
HierarchicalIterative (const HierarchicalIterative &other)
virtual ~HierarchicalIterative ()
value_type residualError () const
Returns the squared norm of the error vector.
void residualError (vectorOut_t error) const
Returns the error vector.
bool definesSubmanifoldOf (const HierarchicalIterative &solver) const
template<typename LineSearchType >
solver::HierarchicalIterative::Status solve (vectorOut_t arg, LineSearchType lineSearch) const
const LiegroupSpacePtr_t & configSpace () const
Get configuration space on which constraints are defined.
virtual bool contains (const ImplicitPtr_t &numericalConstraint) const
virtual void merge (const HierarchicalIterative &other)
void saturation (const Saturation_t &saturate )
Set the saturation function.
const Saturation_t & saturation () const
Get the saturation function.
template<typename LineSearchType >
Status solve (vectorOut_t arg, LineSearchType ls=LineSearchType()) const
Status solve (vectorOut_t arg) const
bool isSatisfied (vectorIn_t arg) const
bool isSatisfied (vectorIn_t arg, value_type errorThreshold ) const
bool isConstraintSatisfied (const ImplicitPtr_t &constraint, vectorIn_t arg, vectorOut_t error, bool &constraintFound) const
const value_type & sigma () const
void freeVariables (const segments_t intervals)
void freeVariables (const Indices_t &indices)
const Indices_t & freeVariables () const
Get free velocity variables.
void maxIterations (size_type iterations)
Set maximal number of iterations.
size_type maxIterations () const
Get maximal number of iterations in config projector.
void errorThreshold (const value_type &threshold)
Set error threshold.
value_type errorThreshold () const
Get error threshold.
value_type squaredErrorThreshold () const
Get error threshold.
value_type inequalityThreshold () const
Get the inequality threshold.
void inequalityThreshold (const value_type &it)
set the inequality threshold
void lastIsOptional (bool optional)
bool lastIsOptional () const
void solveLevelByLevel (bool solveLevelByLevel)
bool solveLevelByLevel () const
const ImplicitConstraintSet & constraints (const std::size_t priority)
Get set of constraints for a give priority level.
const NumericalConstraints_t & constraints () const
Get constraints (implicit and explicit)
std::size_t numberStacks () const
const size_type & dimension () const
const size_type & reducedDimension () const
ArrayXb activeParameters () const
Configuration parameters involved in the constraint resolution.
ArrayXb activeDerivativeParameters () const
Velocity parameters involved in the constraint resolution.
vector_t rightHandSideFromConfig (ConfigurationIn_t config)
void rightHandSideAt (const value_type &s)
vector_t rightHandSide () const
size_type rightHandSideSize () const
template<bool ComputeJac>
void computeValue (vectorIn_t arg) const
Compute the value of each level, and the jacobian if ComputeJac is true.
void computeSaturation (vectorIn_t arg) const
void getValue (vectorOut_t v) const
void getReducedJacobian (matrixOut_t J) const
void computeError () const
const vector_t & lastStep () const
Accessor to the last step done.
Solve a non-linear system equations with explicit and implicit constraints
This solver is defined in paper https://hal.archives-ouvertes.fr/hal-01804774/file/paper.pdf . We give here only a brief description
The unknows (denoted by ) of the system of equations is a Lie group. It is usually a robot configuration space or the Cartesian product of robot configuration spaces.
The solver stores a set of implicit numerical constraints: . These implicit constraints are added using method HierarchicalIterative::add .
The solver also stores explicit numerical constraints (constraints where some configuration variables depend on others) in an instance of class ExplicitConstraintSet . This instance is accessible via method BySubstitution::explicitConstraintSet .
When an explicit constraint is added using method ExplicitConstraintSet::add , this method checks that the explicit constraint is compatible with the previously added ones. If so, the constraint is stored in the explicit constraint set. Otherwise, it has to be added as an implicit constraint.
See Section III of the above mentioned paper for the description of the constraint resolution.