hpp-constraints  4.14.0
Definition of basic geometric constraints for motion planning
by-substitution.hh
Go to the documentation of this file.
1 // Copyright (c) 2017, 2018 CNRS
2 // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
3 //
4 
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // 1. Redistributions of source code must retain the above copyright
10 // notice, this list of conditions and the following disclaimer.
11 //
12 // 2. Redistributions in binary form must reproduce the above copyright
13 // notice, this list of conditions and the following disclaimer in the
14 // documentation and/or other materials provided with the distribution.
15 //
16 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27 // DAMAGE.
28 
29 #ifndef HPP_CONSTRAINTS_SOLVER_BY_SUBSTITUTION_HH
30 #define HPP_CONSTRAINTS_SOLVER_BY_SUBSTITUTION_HH
31 
34 #include <hpp/constraints/fwd.hh>
37 #include <vector>
38 
39 namespace hpp {
40 namespace constraints {
41 namespace solver {
44 
74  public:
75  BySubstitution(const LiegroupSpacePtr_t& configSpace);
77 
78  virtual ~BySubstitution() {}
79 
85  bool add(const ImplicitPtr_t& numericalConstraint,
86  const std::size_t& priority = 0);
87 
89  bool add(const ImplicitPtr_t& numericalConstraint,
90  const segments_t& passiveDofs,
91  const std::size_t priority = 0) HPP_CONSTRAINTS_DEPRECATED {
92  if (passiveDofs.size() > 0)
93  throw std::invalid_argument(
94  "Passive dof in the solver are not "
95  "supported anymore. You must build an "
96  "ActiveSetDifferentiableFunction yourself.");
97  return add(numericalConstraint, priority);
98  }
99 
102  return constraints_;
103  }
104 
107 
110  return explicit_;
111  }
112 
115  return explicitConstraintSet().notOutDers().nbIndices();
116  }
117 
120 
121  template <typename LineSearchType>
122  Status solve(vectorOut_t arg, LineSearchType ls = LineSearchType()) const {
123  return solve<LineSearchType>(arg, false, ls);
124  }
125 
126  template <typename LineSearchType>
127  Status solve(vectorOut_t arg, bool optimize,
128  LineSearchType ls = LineSearchType()) const {
129  // TODO when there are only locked joint explicit constraints,
130  // there is no need for this intricated loop.
131  // if (explicit_.isConstant()) {
132  // explicit_.solve(arg);
133  // iterative_.solve(arg, ls);
134  // } else {
135  return impl_solve(arg, optimize, ls);
136  // }
137  }
138 
149  vectorOut_t result) const;
150 
162  ConfigurationOut_t result);
163 
164  inline Status solve(vectorOut_t arg) const {
165  return solve(arg, DefaultLineSearch());
166  }
167 
170 
173 
176  bool rightHandSideFromConfig(const ImplicitPtr_t& constraint,
177  ConfigurationIn_t config);
178 
181  bool rightHandSide(const ImplicitPtr_t& constraint, vectorIn_t rhs);
182 
185  bool getRightHandSide(const ImplicitPtr_t& constraint, vectorOut_t rhs) const;
186 
189 
195 
198 
200 
207  return dimension() + explicit_.outDers().nbIndices();
208  }
209 
213  bool isSatisfied(vectorIn_t arg) const {
215  explicit_.isSatisfied(arg);
216  }
217 
222  bool isSatisfied(vectorIn_t arg, value_type errorThreshold) const {
223  return solver::HierarchicalIterative::isSatisfied(arg, errorThreshold) &&
224  explicit_.isSatisfied(arg, errorThreshold);
225  }
231  bool isSatisfied(vectorIn_t arg, vectorOut_t error) const {
232  assert(error.size() == dimension() + explicit_.errorSize());
233  bool iterative = solver::HierarchicalIterative::isSatisfied(arg);
234  residualError(error.head(dimension()));
235  bool _explicit =
236  explicit_.isSatisfied(arg, error.tail(explicit_.errorSize()));
237  return iterative && _explicit;
238  }
239 
249  bool isConstraintSatisfied(const ImplicitPtr_t& constraint, vectorIn_t arg,
250  vectorOut_t error, bool& constraintFound) const;
251 
252  template <typename LineSearchType>
253  bool oneStep(vectorOut_t arg, LineSearchType& lineSearch) const {
254  computeValue<true>(arg);
255  updateJacobian(arg);
256  computeDescentDirection();
257  lineSearch(*this, arg, dq_);
258  explicit_.solve(arg);
260  }
261 
264  void updateJacobian(vectorIn_t arg) const;
265 
267  void errorThreshold(const value_type& threshold) {
269  explicit_.errorThreshold(threshold);
270  }
274  }
275 
280 
281  virtual std::ostream& print(std::ostream& os) const;
282 
283  bool integrate(vectorIn_t from, vectorIn_t velocity,
284  vectorOut_t result) const {
285  bool res = solver::HierarchicalIterative::integrate(from, velocity, result);
286  explicit_.solve(result);
287  return res;
288  }
289 
290  protected:
291  void computeActiveRowsOfJ(std::size_t iStack);
292 
293  private:
295 
296  template <typename LineSearchType>
297  Status impl_solve(vectorOut_t arg, bool optimize, LineSearchType ls) const;
298 
299  ExplicitConstraintSet explicit_;
300  mutable matrix_t Je_, JeExpanded_;
301 
302  BySubstitution() {}
303  HPP_SERIALIZABLE_SPLIT();
304 }; // class BySubstitution
306 
307 } // namespace solver
308 } // namespace constraints
309 } // namespace hpp
310 
311 #endif // HPP_CONSTRAINTS_SOLVER_BY_SUBSTITUTION_HH
Definition: explicit-constraint-set.hh:99
Definition: by-substitution.hh:73
void computeActiveRowsOfJ(std::size_t iStack)
BySubstitution(const LiegroupSpacePtr_t &configSpace)
virtual ~BySubstitution()
Definition: by-substitution.hh:78
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:127
Status solve(vectorOut_t arg) const
Definition: by-substitution.hh:164
bool oneStep(vectorOut_t arg, LineSearchType &lineSearch) const
Definition: by-substitution.hh:253
Status solve(vectorOut_t arg, LineSearchType ls=LineSearchType()) const
Definition: by-substitution.hh:122
value_type errorThreshold() const
Get error threshold.
Definition: by-substitution.hh:272
bool isSatisfied(vectorIn_t arg, vectorOut_t error) const
Definition: by-substitution.hh:231
virtual std::ostream & print(std::ostream &os) const
size_type errorSize() const
Definition: by-substitution.hh:206
bool getRightHandSide(const ImplicitPtr_t &constraint, vectorOut_t rhs) const
const ExplicitConstraintSet & explicitConstraintSet() const
Set explicit constraint set.
Definition: by-substitution.hh:109
void projectVectorOnKernel(ConfigurationIn_t from, vectorIn_t velocity, vectorOut_t result) const
ExplicitConstraintSet & explicitConstraintSet()
Get explicit constraint set.
Definition: by-substitution.hh:106
bool rightHandSideFromConfig(const ImplicitPtr_t &constraint, ConfigurationIn_t config)
bool isSatisfied(vectorIn_t arg, value_type errorThreshold) const
Definition: by-substitution.hh:222
bool add(const ImplicitPtr_t &numericalConstraint, const segments_t &passiveDofs, const std::size_t priority=0) HPP_CONSTRAINTS_DEPRECATED
Definition: by-substitution.hh:89
const NumericalConstraints_t & numericalConstraints() const
Get the numerical constraints implicit and explicit.
Definition: by-substitution.hh:101
size_type numberFreeVariables() const
Return the number of free variables.
Definition: by-substitution.hh:114
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:283
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
bool isSatisfied(vectorIn_t arg) const
Definition: by-substitution.hh:213
void errorThreshold(const value_type &threshold)
Set error threshold.
Definition: by-substitution.hh:267
Definition: hierarchical-iterative.hh:232
virtual bool integrate(vectorIn_t from, vectorIn_t velocity, vectorOut_t result) const
Status
Definition: hierarchical-iterative.hh:237
bool isSatisfied(vectorIn_t arg) const
Definition: hierarchical-iterative.hh:320
value_type errorThreshold() const
Get error threshold.
Definition: hierarchical-iterative.hh:395
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:64
assert(d.lhs()._blocks()==d.rhs()._blocks())
pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t
Definition: fwd.hh:69
shared_ptr< Implicit > ImplicitPtr_t
Definition: fwd.hh:175
DifferentiableFunctionSet DifferentiableFunctionStack HPP_CONSTRAINTS_DEPRECATED
Definition: fwd.hh:116
pinocchio::size_type size_type
Definition: fwd.hh:47
pinocchio::value_type value_type
Definition: fwd.hh:48
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:60
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:106
pinocchio::matrix_t matrix_t
Definition: fwd.hh:56
std::vector< segment_t > segments_t
Definition: fwd.hh:84
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:61
std::vector< constraints::ImplicitPtr_t > NumericalConstraints_t
Definition: fwd.hh:177
pinocchio::vector_t vector_t
Definition: fwd.hh:59
pinocchio::ConfigurationOut_t ConfigurationOut_t
Definition: fwd.hh:107
Definition: active-set-differentiable-function.hh:36
Definition: hierarchical-iterative.hh:70