29 #ifndef HPP_CONSTRAINTS_SOLVER_BY_SUBSTITUTION_HH
30 #define HPP_CONSTRAINTS_SOLVER_BY_SUBSTITUTION_HH
40 namespace constraints {
86 const std::size_t& priority = 0);
92 if (passiveDofs.size() > 0)
93 throw std::invalid_argument(
94 "Passive dof in the solver are not "
95 "supported anymore. You must build an "
96 "ActiveSetDifferentiableFunction yourself.");
97 return add(numericalConstraint, priority);
115 return explicitConstraintSet().notOutDers().nbIndices();
121 template <
typename LineSearchType>
123 return solve<LineSearchType>(arg,
false, ls);
126 template <
typename LineSearchType>
128 LineSearchType ls = LineSearchType())
const {
135 return impl_solve(arg, optimize, ls);
207 return dimension() + explicit_.outDers().nbIndices();
215 explicit_.isSatisfied(arg);
224 explicit_.isSatisfied(arg, errorThreshold);
232 assert(error.size() == dimension() + explicit_.errorSize());
234 residualError(error.head(dimension()));
236 explicit_.isSatisfied(arg, error.tail(explicit_.errorSize()));
237 return iterative && _explicit;
252 template <
typename LineSearchType>
254 computeValue<true>(arg);
256 computeDescentDirection();
257 lineSearch(*
this, arg, dq_);
258 explicit_.solve(arg);
269 explicit_.errorThreshold(threshold);
281 virtual std::ostream&
print(std::ostream& os)
const;
286 explicit_.solve(result);
296 template <
typename LineSearchType>
303 HPP_SERIALIZABLE_SPLIT();
Definition: explicit-constraint-set.hh:99
Definition: by-substitution.hh:73
void computeActiveRowsOfJ(std::size_t iStack)
BySubstitution(const LiegroupSpacePtr_t &configSpace)
virtual ~BySubstitution()
Definition: by-substitution.hh:78
vector_t rightHandSide() const
void explicitConstraintSetHasChanged()
Should be called whenever explicit solver is modified.
Status solve(vectorOut_t arg, bool optimize, LineSearchType ls=LineSearchType()) const
Definition: by-substitution.hh:127
Status solve(vectorOut_t arg) const
Definition: by-substitution.hh:164
bool oneStep(vectorOut_t arg, LineSearchType &lineSearch) const
Definition: by-substitution.hh:253
Status solve(vectorOut_t arg, LineSearchType ls=LineSearchType()) const
Definition: by-substitution.hh:122
value_type errorThreshold() const
Get error threshold.
Definition: by-substitution.hh:272
bool isSatisfied(vectorIn_t arg, vectorOut_t error) const
Definition: by-substitution.hh:231
virtual std::ostream & print(std::ostream &os) const
size_type errorSize() const
Definition: by-substitution.hh:206
bool getRightHandSide(const ImplicitPtr_t &constraint, vectorOut_t rhs) const
void rightHandSide(vectorIn_t rhs)
const ExplicitConstraintSet & explicitConstraintSet() const
Set explicit constraint set.
Definition: by-substitution.hh:109
void projectVectorOnKernel(ConfigurationIn_t from, vectorIn_t velocity, vectorOut_t result) const
ExplicitConstraintSet & explicitConstraintSet()
Get explicit constraint set.
Definition: by-substitution.hh:106
bool rightHandSideFromConfig(const ImplicitPtr_t &constraint, ConfigurationIn_t config)
bool isSatisfied(vectorIn_t arg, value_type errorThreshold) const
Definition: by-substitution.hh:222
bool add(const ImplicitPtr_t &numericalConstraint, const segments_t &passiveDofs, const std::size_t priority=0) HPP_CONSTRAINTS_DEPRECATED
Definition: by-substitution.hh:89
const NumericalConstraints_t & numericalConstraints() const
Get the numerical constraints implicit and explicit.
Definition: by-substitution.hh:101
segments_t implicitDof() const
size_type numberFreeVariables() const
Return the number of free variables.
Definition: by-substitution.hh:114
bool add(const ImplicitPtr_t &numericalConstraint, const std::size_t &priority=0)
virtual void projectOnKernel(ConfigurationIn_t from, ConfigurationIn_t to, ConfigurationOut_t result)
bool integrate(vectorIn_t from, vectorIn_t velocity, vectorOut_t result) const
Definition: by-substitution.hh:283
vector_t rightHandSideFromConfig(ConfigurationIn_t config)
bool isConstraintSatisfied(const ImplicitPtr_t &constraint, vectorIn_t arg, vectorOut_t error, bool &constraintFound) const
bool rightHandSide(const ImplicitPtr_t &constraint, vectorIn_t rhs)
BySubstitution(const BySubstitution &other)
void updateJacobian(vectorIn_t arg) const
size_type rightHandSideSize() const
bool isSatisfied(vectorIn_t arg) const
Definition: by-substitution.hh:213
void errorThreshold(const value_type &threshold)
Set error threshold.
Definition: by-substitution.hh:267
Definition: hierarchical-iterative.hh:232
virtual bool integrate(vectorIn_t from, vectorIn_t velocity, vectorOut_t result) const
Status
Definition: hierarchical-iterative.hh:237
bool isSatisfied(vectorIn_t arg) const
Definition: hierarchical-iterative.hh:320
value_type errorThreshold() const
Get error threshold.
Definition: hierarchical-iterative.hh:395
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:64
assert(d.lhs()._blocks()==d.rhs()._blocks())
pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t
Definition: fwd.hh:69
shared_ptr< Implicit > ImplicitPtr_t
Definition: fwd.hh:175
DifferentiableFunctionSet DifferentiableFunctionStack HPP_CONSTRAINTS_DEPRECATED
Definition: fwd.hh:116
pinocchio::size_type size_type
Definition: fwd.hh:47
pinocchio::value_type value_type
Definition: fwd.hh:48
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:60
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:106
pinocchio::matrix_t matrix_t
Definition: fwd.hh:56
std::vector< segment_t > segments_t
Definition: fwd.hh:84
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:61
std::vector< constraints::ImplicitPtr_t > NumericalConstraints_t
Definition: fwd.hh:177
pinocchio::vector_t vector_t
Definition: fwd.hh:59
pinocchio::ConfigurationOut_t ConfigurationOut_t
Definition: fwd.hh:107
Definition: active-set-differentiable-function.hh:36
Definition: hierarchical-iterative.hh:70