hpp-constraints  4.12.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 // This file is part of hpp-constraints.
5 // hpp-constraints is free software: you can redistribute it
6 // and/or modify it under the terms of the GNU Lesser General Public
7 // License as published by the Free Software Foundation, either version
8 // 3 of the License, or (at your option) any later version.
9 //
10 // hpp-constraints is distributed in the hope that it will be
11 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
12 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 // General Lesser Public License for more details. You should have
14 // received a copy of the GNU Lesser General Public License along with
15 // hpp-constraints. If not, see <http://www.gnu.org/licenses/>.
16 
17 #ifndef HPP_CONSTRAINTS_SOLVER_BY_SUBSTITUTION_HH
18 #define HPP_CONSTRAINTS_SOLVER_BY_SUBSTITUTION_HH
19 
20 #include <vector>
21 
22 #include <hpp/constraints/fwd.hh>
24 
28 
29 namespace hpp {
30  namespace constraints {
31  namespace solver {
34 
64  {
65  public:
66  BySubstitution (const LiegroupSpacePtr_t& configSpace);
67  BySubstitution (const BySubstitution& other);
68 
69  virtual ~BySubstitution () {}
70 
76  bool add (const ImplicitPtr_t& numericalConstraint,
77  const std::size_t& priority = 0);
78 
80  bool add (const ImplicitPtr_t& numericalConstraint,
81  const segments_t& passiveDofs,
82  const std::size_t priority = 0) HPP_CONSTRAINTS_DEPRECATED
83  {
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);
89  }
90 
93  {
94  return constraints_;
95  }
96 
99  {
100  return explicit_;
101  }
102 
105  {
106  return explicit_;
107  }
108 
115  virtual bool contains (const ImplicitPtr_t& numericalConstraint) const;
116 
119  {
120  return explicitConstraintSet().notOutDers().nbIndices();
121  }
122 
124  void explicitConstraintSetHasChanged();
125 
126  template <typename LineSearchType>
127  Status solve (vectorOut_t arg, LineSearchType ls = LineSearchType()) const
128  {
129  return solve <LineSearchType> (arg, false, ls);
130  }
131 
132  template <typename LineSearchType>
133  Status solve (vectorOut_t arg, bool optimize, LineSearchType ls = LineSearchType()) const
134  {
135  // TODO when there are only locked joint explicit constraints,
136  // there is no need for this intricated loop.
137  // if (explicit_.isConstant()) {
138  // explicit_.solve(arg);
139  // iterative_.solve(arg, ls);
140  // } else {
141  return impl_solve (arg, optimize, ls);
142  // }
143  }
144 
154  void projectVectorOnKernel (ConfigurationIn_t from, vectorIn_t velocity,
155  ConfigurationOut_t result) const;
156 
167  virtual void projectOnKernel (ConfigurationIn_t from,
169  ConfigurationOut_t result);
170 
171  inline Status solve (vectorOut_t arg) const
172  {
173  return solve(arg, DefaultLineSearch());
174  }
175 
178 
180  vector_t rightHandSideFromConfig (ConfigurationIn_t config);
181 
183  bool rightHandSideFromConfig (const ImplicitPtr_t& constraint,
184  ConfigurationIn_t config);
185 
187  bool rightHandSide (const ImplicitPtr_t& constraint,
188  vectorIn_t rhs);
189 
191  bool getRightHandSide (const ImplicitPtr_t& constraint,vectorOut_t rhs) const;
192 
194  void rightHandSide (vectorIn_t rhs);
195 
200  vector_t rightHandSide () const;
201 
203  size_type rightHandSideSize () const;
204 
206 
213  {
214  return dimension() + explicit_.outDers().nbIndices();
215  }
216 
220  bool isSatisfied (vectorIn_t arg) const
221  {
222  return
224  && explicit_.isSatisfied (arg);
225  }
226 
231  bool isSatisfied (vectorIn_t arg, value_type errorThreshold) const
232  {
233  return
234  solver::HierarchicalIterative::isSatisfied (arg, errorThreshold)
235  && explicit_.isSatisfied (arg, errorThreshold);
236  }
242  bool isSatisfied (vectorIn_t arg, vectorOut_t error) const
243  {
244  assert (error.size() == dimension() + explicit_.errorSize());
245  bool iterative =
247  residualError(error.head(dimension()));
248  bool _explicit =
249  explicit_.isSatisfied (arg, error.tail(explicit_.errorSize()));
250  return iterative && _explicit;
251  }
252 
262  bool isConstraintSatisfied (const ImplicitPtr_t& constraint,
263  vectorIn_t arg, vectorOut_t error,
264  bool& constraintFound) const;
265 
266  template <typename LineSearchType>
267  bool oneStep (vectorOut_t arg, LineSearchType& lineSearch) const
268  {
269  computeValue<true> (arg);
270  updateJacobian (arg);
271  computeDescentDirection ();
272  lineSearch (*this, arg, dq_);
273  explicit_.solve (arg);
275  }
276 
279  void updateJacobian (vectorIn_t arg) const;
280 
282  void errorThreshold (const value_type& threshold)
283  {
285  explicit_.errorThreshold(threshold);
286  }
289  {
291  }
292 
296  segments_t implicitDof () const;
297 
298  virtual std::ostream& print (std::ostream& os) const;
299 
300  bool integrate(vectorIn_t from, vectorIn_t velocity, vectorOut_t result)
301  const
302  {
303  bool res = solver::HierarchicalIterative::integrate(from, velocity, result);
304  explicit_.solve (result);
305  return res;
306  }
307 
308  protected:
309  void computeActiveRowsOfJ (std::size_t iStack);
310 
311  private:
312  typedef solver::HierarchicalIterative parent_t;
313 
314  template <typename LineSearchType>
315  Status impl_solve (vectorOut_t arg, bool optimize, LineSearchType ls) const;
316 
317  ExplicitConstraintSet explicit_;
318  mutable matrix_t Je_, JeExpanded_;
319 
320  BySubstitution() {}
321  HPP_SERIALIZABLE_SPLIT();
322  }; // class BySubstitution
324 
325  } // namespace solver
326  } // namespace constraints
327 } // namespace hpp
328 
329 #endif // HPP_CONSTRAINTS_SOLVER_BY_SUBSTITUTION_HH
Status solve(vectorOut_t arg, LineSearchType ls=LineSearchType()) const
Definition: by-substitution.hh:127
pinocchio::vector_t vector_t
Definition: fwd.hh:47
bool integrate(vectorIn_t from, vectorIn_t velocity, vectorOut_t result) const
Definition: by-substitution.hh:300
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:48
pinocchio::ConfigurationOut_t ConfigurationOut_t
Definition: fwd.hh:95
value_type errorThreshold() const
Get error threshold.
Definition: by-substitution.hh:288
Definition: active-set-differentiable-function.hh:24
pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t
Definition: fwd.hh:57
DifferentiableFunctionSet DifferentiableFunctionStack HPP_CONSTRAINTS_DEPRECATED
Definition: fwd.hh:105
std::vector< segment_t > segments_t
Definition: fwd.hh:72
pinocchio::matrix_t matrix_t
Definition: fwd.hh:44
virtual ~BySubstitution()
Definition: by-substitution.hh:69
Status
Definition: hierarchical-iterative.hh:224
Definition: hierarchical-iterative.hh:65
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:94
const NumericalConstraints_t & numericalConstraints() const
Get the numerical constraints implicit and explicit.
Definition: by-substitution.hh:92
assert(d.lhs()._blocks()==d.rhs()._blocks())
bool isSatisfied(vectorIn_t arg) const
Compares to internal error threshold.
Definition: by-substitution.hh:220
bool isSatisfied(vectorIn_t arg, vectorOut_t error) const
Definition: by-substitution.hh:242
ExplicitConstraintSet & explicitConstraintSet()
Get explicit constraint set.
Definition: by-substitution.hh:98
bool isSatisfied(vectorIn_t arg) const
Compares to internal error threshold.
Definition: hierarchical-iterative.hh:324
bool isSatisfied(vectorIn_t arg, value_type errorThreshold) const
Definition: by-substitution.hh:231
value_type errorThreshold() const
Get error threshold.
Definition: hierarchical-iterative.hh:417
size_type errorSize() const
Definition: by-substitution.hh:212
Definition: by-substitution.hh:62
bool oneStep(vectorOut_t arg, LineSearchType &lineSearch) const
Definition: by-substitution.hh:267
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:64
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:49
Status solve(vectorOut_t arg, bool optimize, LineSearchType ls=LineSearchType()) const
Definition: by-substitution.hh:133
pinocchio::size_type size_type
Definition: fwd.hh:36
shared_ptr< Implicit > ImplicitPtr_t
Definition: fwd.hh:163
pinocchio::value_type value_type
Definition: fwd.hh:37
size_type numberFreeVariables() const
Return the number of free variables.
Definition: by-substitution.hh:118
void errorThreshold(const value_type &threshold)
Set error threshold.
Definition: by-substitution.hh:282
bool add(const ImplicitPtr_t &numericalConstraint, const segments_t &passiveDofs, const std::size_t priority=0) HPP_CONSTRAINTS_DEPRECATED
Definition: by-substitution.hh:80
virtual bool integrate(vectorIn_t from, vectorIn_t velocity, vectorOut_t result) const
Definition: hierarchical-iterative.hh:218
const ExplicitConstraintSet & explicitConstraintSet() const
Set explicit constraint set.
Definition: by-substitution.hh:104
std::vector< constraints::ImplicitPtr_t > NumericalConstraints_t
Definition: fwd.hh:165
Definition: explicit-constraint-set.hh:91
Status solve(vectorOut_t arg) const
Definition: by-substitution.hh:171