hpp-constraints
4.9.0
Definition of basic geometric constraints for motion planning
|
Go to the documentation of this file.
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();
43 value_type previousCost = numeric_limits::infinity();
45 bool onlyLineSearch =
false;
49 computeValue<true> (arg);
54 if (errorWasBelowThr) {
62 if (optimize && !errorIsAboveThr) qopt = arg;
65 while ( (optimize || (errorIsAboveThr && errorDecreased))) {
75 if (!onlyLineSearch) {
83 if (optimize)
dq_ *= scaling;
84 if (
dq_.squaredNorm () < dqMinSquaredNorm) {
90 lineSearch (*
this, arg,
dq_);
94 computeValue<true> (arg);
110 if (!errorIsAboveThr) {
112 if (cost < previousCost) {
115 if (scaling < 0.5) scaling *= 2;
117 onlyLineSearch =
false;
121 if (qopt.size() > 0) arg = qopt;
122 onlyLineSearch =
true;
129 if (!optimize && errorWasBelowThr) {
137 if (optimize && qopt.size() > 0) arg = qopt;
146 #endif // HPP_CONSTRAINTS_SOLVER_IMPL_BY_SUBSTITUTION_HH
bool lastIsOptional_
Definition: hierarchical-iterative.hh:604
size_type maxIterations_
Definition: hierarchical-iterative.hh:599
vector_t dq_
Definition: hierarchical-iterative.hh:620
pinocchio::vector_t vector_t
Definition: fwd.hh:45
assert(d.lhs()._blocks()==d.rhs()._blocks())
@ SUCCESS
Definition: hierarchical-iterative.hh:175
size_type reducedDimension_
Definition: hierarchical-iterative.hh:603
Status
Definition: hierarchical-iterative.hh:171
void computeDescentDirection() const
Eigen::NumTraits< value_type > NumTraits
Definition: by-substitution.hh:24
Definition: active-set-differentiable-function.hh:24
pinocchio::size_type size_type
Definition: fwd.hh:35
bool solve(vectorOut_t arg) const
void computeSaturation(vectorIn_t arg) const
void updateJacobian(vectorIn_t arg) const
pinocchio::value_type value_type
Definition: fwd.hh:36
value_type squaredNorm_
Definition: hierarchical-iterative.hh:625
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:47
std::vector< Data > datas_
Definition: hierarchical-iterative.hh:626
void computeError() const
@ MAX_ITERATION_REACHED
Definition: hierarchical-iterative.hh:173
std::numeric_limits< value_type > numeric_limits
Definition: by-substitution.hh:23
@ ERROR_INCREASED
Definition: hierarchical-iterative.hh:172
@ INFEASIBLE
Definition: hierarchical-iterative.hh:174
value_type squaredErrorThreshold_
Definition: hierarchical-iterative.hh:598