17 #ifndef HPP_CONSTRAINTS_SOLVER_IMPL_BY_SUBSTITUTION_HH 18 #define HPP_CONSTRAINTS_SOLVER_IMPL_BY_SUBSTITUTION_HH 21 namespace constraints {
26 template <
typename LineSearchType>
30 LineSearchType lineSearch)
const 39 static const value_type dqMinSquaredNorm = NumTraits::dummy_precision();
45 bool onlyLineSearch =
false;
49 computeValue<true> (arg);
52 previousCost =
datas_.back().error.squaredNorm();
56 if (errorWasBelowThr) {
64 if (optimize && !errorIsAboveThr) qopt = arg;
67 while ( (optimize || (errorIsAboveThr && errorDecreased))) {
77 if (!onlyLineSearch) {
85 if (optimize)
dq_ *= scaling;
86 if (
dq_.squaredNorm () < dqMinSquaredNorm) {
92 lineSearch (*
this, arg,
dq_);
96 computeValue<true> (arg);
112 if (!errorIsAboveThr) {
114 if (cost < previousCost) {
117 if (scaling < 0.5) scaling *= 2;
119 onlyLineSearch =
false;
123 if (qopt.size() > 0) arg = qopt;
124 onlyLineSearch =
true;
131 if (!optimize && errorWasBelowThr) {
139 if (optimize && qopt.size() > 0) arg = qopt;
148 #endif // HPP_CONSTRAINTS_SOLVER_IMPL_BY_SUBSTITUTION_HH pinocchio::vector_t vector_t
Definition: fwd.hh:47
Definition: hierarchical-iterative.hh:225
bool lastIsOptional_
Definition: hierarchical-iterative.hh:623
size_type maxIterations_
Definition: hierarchical-iterative.hh:618
Definition: active-set-differentiable-function.hh:24
void updateJacobian(vectorIn_t arg) const
Status
Definition: hierarchical-iterative.hh:224
assert(d.lhs()._blocks()==d.rhs()._blocks())
Definition: hierarchical-iterative.hh:228
bool solve(vectorOut_t arg) const
size_type reducedDimension_
Definition: hierarchical-iterative.hh:622
std::numeric_limits< value_type > numeric_limits
Definition: by-substitution.hh:23
void computeDescentDirection() const
value_type squaredNorm_
Definition: hierarchical-iterative.hh:644
void computeSaturation(vectorIn_t arg) const
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:49
void computeError() const
std::vector< Data > datas_
Definition: hierarchical-iterative.hh:645
pinocchio::size_type size_type
Definition: fwd.hh:36
Eigen::NumTraits< value_type > NumTraits
Definition: by-substitution.hh:24
Definition: hierarchical-iterative.hh:227
pinocchio::value_type value_type
Definition: fwd.hh:37
Definition: hierarchical-iterative.hh:226
value_type squaredErrorThreshold_
Definition: hierarchical-iterative.hh:617
vector_t dq_
Definition: hierarchical-iterative.hh:639