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);
120 return explicitConstraintSet().notOutDers().nbIndices();
126 template <
typename LineSearchType>
129 return solve <LineSearchType> (arg,
false, ls);
132 template <
typename LineSearchType>
141 return impl_solve (arg, optimize, ls);
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);
314 template <
typename LineSearchType>
321 HPP_SERIALIZABLE_SPLIT();
Definition: explicit-constraint-set.hh:92
Definition: by-substitution.hh:64
void computeActiveRowsOfJ(std::size_t iStack)
BySubstitution(const LiegroupSpacePtr_t &configSpace)
virtual ~BySubstitution()
Definition: by-substitution.hh:69
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:133
Status solve(vectorOut_t arg) const
Definition: by-substitution.hh:171
bool oneStep(vectorOut_t arg, LineSearchType &lineSearch) const
Definition: by-substitution.hh:267
Status solve(vectorOut_t arg, LineSearchType ls=LineSearchType()) const
Definition: by-substitution.hh:127
value_type errorThreshold() const
Get error threshold.
Definition: by-substitution.hh:288
bool isSatisfied(vectorIn_t arg, vectorOut_t error) const
Definition: by-substitution.hh:242
virtual std::ostream & print(std::ostream &os) const
size_type errorSize() const
Definition: by-substitution.hh:212
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:104
ExplicitConstraintSet & explicitConstraintSet()
Get explicit constraint set.
Definition: by-substitution.hh:98
bool rightHandSideFromConfig(const ImplicitPtr_t &constraint, ConfigurationIn_t config)
bool isSatisfied(vectorIn_t arg, value_type errorThreshold) const
Definition: by-substitution.hh:231
bool add(const ImplicitPtr_t &numericalConstraint, const segments_t &passiveDofs, const std::size_t priority=0) HPP_CONSTRAINTS_DEPRECATED
Definition: by-substitution.hh:80
const NumericalConstraints_t & numericalConstraints() const
Get the numerical constraints implicit and explicit.
Definition: by-substitution.hh:92
segments_t implicitDof() const
size_type numberFreeVariables() const
Return the number of free variables.
Definition: by-substitution.hh:118
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:300
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
virtual bool contains(const ImplicitPtr_t &numericalConstraint) const
bool isSatisfied(vectorIn_t arg) const
Compares to internal error threshold.
Definition: by-substitution.hh:220
void projectVectorOnKernel(ConfigurationIn_t from, vectorIn_t velocity, ConfigurationOut_t result) const
void errorThreshold(const value_type &threshold)
Set error threshold.
Definition: by-substitution.hh:282
Definition: hierarchical-iterative.hh:219
virtual bool integrate(vectorIn_t from, vectorIn_t velocity, vectorOut_t result) const
Status
Definition: hierarchical-iterative.hh:224
bool isSatisfied(vectorIn_t arg) const
Compares to internal error threshold.
Definition: hierarchical-iterative.hh:324
value_type errorThreshold() const
Get error threshold.
Definition: hierarchical-iterative.hh:417
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:64
assert(d.lhs()._blocks()==d.rhs()._blocks())
pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t
Definition: fwd.hh:57
shared_ptr< Implicit > ImplicitPtr_t
Definition: fwd.hh:163
DifferentiableFunctionSet DifferentiableFunctionStack HPP_CONSTRAINTS_DEPRECATED
Definition: fwd.hh:105
std::vector< constraints::ImplicitPtr_t > NumericalConstraints_t
Definition: fwd.hh:165
pinocchio::size_type size_type
Definition: fwd.hh:36
std::vector< segment_t > segments_t
Definition: fwd.hh:72
pinocchio::value_type value_type
Definition: fwd.hh:37
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:48
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:94
pinocchio::matrix_t matrix_t
Definition: fwd.hh:44
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:49
pinocchio::vector_t vector_t
Definition: fwd.hh:47
pinocchio::ConfigurationOut_t ConfigurationOut_t
Definition: fwd.hh:95
Definition: active-set-differentiable-function.hh:24
Definition: hierarchical-iterative.hh:65