17 #ifndef HPP_CONSTRAINTS_SOLVER_BY_SUBSTITUTION_HH 18 #define HPP_CONSTRAINTS_SOLVER_BY_SUBSTITUTION_HH 30 namespace constraints {
77 const std::size_t& priority = 0);
84 if(passiveDofs.size() > 0)
85 throw std::invalid_argument(
"Passive dof in the solver are not " 86 "supported anymore. You must build an " 87 "ActiveSetDifferentiableFunction yourself.");
88 return add (numericalConstraint, priority);
115 virtual bool contains (
const ImplicitPtr_t& numericalConstraint)
const;
120 return explicitConstraintSet().notOutDers().nbIndices();
124 void explicitConstraintSetHasChanged();
126 template <
typename LineSearchType>
129 return solve <LineSearchType> (arg,
false, ls);
132 template <
typename LineSearchType>
141 return impl_solve (arg, optimize, ls);
183 bool rightHandSideFromConfig (
const ImplicitPtr_t& constraint,
214 return dimension() + explicit_.outDers().nbIndices();
224 && explicit_.isSatisfied (arg);
235 && explicit_.isSatisfied (arg, errorThreshold);
244 assert (error.size() == dimension() + explicit_.errorSize());
247 residualError(error.head(dimension()));
249 explicit_.isSatisfied (arg, error.tail(explicit_.errorSize()));
250 return iterative && _explicit;
264 bool& constraintFound)
const;
266 template <
typename LineSearchType>
269 computeValue<true> (arg);
270 updateJacobian (arg);
271 computeDescentDirection ();
272 lineSearch (*
this, arg, dq_);
273 explicit_.solve (arg);
285 explicit_.errorThreshold(threshold);
298 virtual std::ostream& print (std::ostream& os)
const;
304 explicit_.solve (result);
309 void computeActiveRowsOfJ (std::size_t iStack);
314 template <
typename LineSearchType>
315 Status impl_solve (
vectorOut_t arg,
bool optimize, LineSearchType ls)
const;
321 HPP_SERIALIZABLE_SPLIT();
329 #endif // HPP_CONSTRAINTS_SOLVER_BY_SUBSTITUTION_HH Status solve(vectorOut_t arg, LineSearchType ls=LineSearchType()) const
Definition: by-substitution.hh:127
pinocchio::vector_t vector_t
Definition: fwd.hh:47
bool integrate(vectorIn_t from, vectorIn_t velocity, vectorOut_t result) const
Definition: by-substitution.hh:300
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:48
pinocchio::ConfigurationOut_t ConfigurationOut_t
Definition: fwd.hh:95
value_type errorThreshold() const
Get error threshold.
Definition: by-substitution.hh:288
Definition: active-set-differentiable-function.hh:24
pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t
Definition: fwd.hh:57
DifferentiableFunctionSet DifferentiableFunctionStack HPP_CONSTRAINTS_DEPRECATED
Definition: fwd.hh:105
std::vector< segment_t > segments_t
Definition: fwd.hh:72
pinocchio::matrix_t matrix_t
Definition: fwd.hh:44
virtual ~BySubstitution()
Definition: by-substitution.hh:69
Status
Definition: hierarchical-iterative.hh:224
Definition: hierarchical-iterative.hh:65
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:94
const NumericalConstraints_t & numericalConstraints() const
Get the numerical constraints implicit and explicit.
Definition: by-substitution.hh:92
assert(d.lhs()._blocks()==d.rhs()._blocks())
bool isSatisfied(vectorIn_t arg) const
Compares to internal error threshold.
Definition: by-substitution.hh:220
bool isSatisfied(vectorIn_t arg, vectorOut_t error) const
Definition: by-substitution.hh:242
ExplicitConstraintSet & explicitConstraintSet()
Get explicit constraint set.
Definition: by-substitution.hh:98
bool isSatisfied(vectorIn_t arg) const
Compares to internal error threshold.
Definition: hierarchical-iterative.hh:324
bool isSatisfied(vectorIn_t arg, value_type errorThreshold) const
Definition: by-substitution.hh:231
value_type errorThreshold() const
Get error threshold.
Definition: hierarchical-iterative.hh:417
size_type errorSize() const
Definition: by-substitution.hh:212
Definition: by-substitution.hh:62
bool oneStep(vectorOut_t arg, LineSearchType &lineSearch) const
Definition: by-substitution.hh:267
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:64
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:49
Status solve(vectorOut_t arg, bool optimize, LineSearchType ls=LineSearchType()) const
Definition: by-substitution.hh:133
pinocchio::size_type size_type
Definition: fwd.hh:36
shared_ptr< Implicit > ImplicitPtr_t
Definition: fwd.hh:163
pinocchio::value_type value_type
Definition: fwd.hh:37
size_type numberFreeVariables() const
Return the number of free variables.
Definition: by-substitution.hh:118
void errorThreshold(const value_type &threshold)
Set error threshold.
Definition: by-substitution.hh:282
bool add(const ImplicitPtr_t &numericalConstraint, const segments_t &passiveDofs, const std::size_t priority=0) HPP_CONSTRAINTS_DEPRECATED
Definition: by-substitution.hh:80
virtual bool integrate(vectorIn_t from, vectorIn_t velocity, vectorOut_t result) const
Definition: hierarchical-iterative.hh:218
const ExplicitConstraintSet & explicitConstraintSet() const
Set explicit constraint set.
Definition: by-substitution.hh:104
std::vector< constraints::ImplicitPtr_t > NumericalConstraints_t
Definition: fwd.hh:165
Definition: explicit-constraint-set.hh:91
Status solve(vectorOut_t arg) const
Definition: by-substitution.hh:171