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 40 static const value_type dqMinSquaredNorm = NumTraits::dummy_precision();
46 bool onlyLineSearch =
false;
50 computeValue<true> (arg);
53 previousCost =
datas_.back().error.squaredNorm();
57 if (errorWasBelowThr) {
65 if (optimize && !errorIsAboveThr) qopt = arg;
68 while ( (optimize || (errorIsAboveThr && errorDecreased))) {
78 if (!onlyLineSearch) {
86 if (optimize)
dq_ *= scaling;
87 if (
dq_.squaredNorm () < dqMinSquaredNorm) {
93 lineSearch (*
this, arg,
dq_);
98 computeValue<true> (arg);
114 if (!errorIsAboveThr) {
116 if (cost < previousCost) {
119 if (scaling < 0.5) scaling *= 2;
121 onlyLineSearch =
false;
125 if (qopt.size() > 0) arg = qopt;
126 onlyLineSearch =
true;
133 if (!optimize && errorWasBelowThr) {
141 if (optimize && qopt.size() > 0) arg = qopt;
150 #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:637
size_type maxIterations_
Definition: hierarchical-iterative.hh:632
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:636
std::numeric_limits< value_type > numeric_limits
Definition: by-substitution.hh:23
void computeDescentDirection() const
value_type squaredNorm_
Definition: hierarchical-iterative.hh:658
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:659
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:631
vector_t dq_
Definition: hierarchical-iterative.hh:653