hpp-constraints  4.10.1
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 
219  bool isSatisfied (vectorIn_t arg) const
220  {
221  return
223  && explicit_.isSatisfied (arg);
224  }
225 
231  bool isSatisfied (vectorIn_t arg, vectorOut_t error) const
232  {
233  assert (error.size() == dimension() + explicit_.errorSize());
234  bool iterative =
236  residualError(error.head(dimension()));
237  bool _explicit =
238  explicit_.isSatisfied (arg, error.tail(explicit_.errorSize()));
239  return iterative && _explicit;
240  }
241 
251  bool isConstraintSatisfied (const ImplicitPtr_t& constraint,
252  vectorIn_t arg, vectorOut_t error,
253  bool& constraintFound) const;
254 
255  template <typename LineSearchType>
256  bool oneStep (vectorOut_t arg, LineSearchType& lineSearch) const
257  {
258  computeValue<true> (arg);
259  updateJacobian (arg);
260  computeDescentDirection ();
261  lineSearch (*this, arg, dq_);
262  explicit_.solve (arg);
264  }
265 
268  void updateJacobian (vectorIn_t arg) const;
269 
271  void errorThreshold (const value_type& threshold)
272  {
274  explicit_.errorThreshold(threshold);
275  }
278  {
280  }
281 
285  segments_t implicitDof () const;
286 
287  virtual std::ostream& print (std::ostream& os) const;
288 
289  bool integrate(vectorIn_t from, vectorIn_t velocity, vectorOut_t result)
290  const
291  {
292  bool res = solver::HierarchicalIterative::integrate(from, velocity, result);
293  explicit_.solve (result);
294  return res;
295  }
296 
297  protected:
298  void computeActiveRowsOfJ (std::size_t iStack);
299 
300  private:
302 
303  template <typename LineSearchType>
304  Status impl_solve (vectorOut_t arg, bool optimize, LineSearchType ls) const;
305 
306  ExplicitConstraintSet explicit_;
307  mutable matrix_t Je_, JeExpanded_;
308 
309  BySubstitution() {}
310  HPP_SERIALIZABLE_SPLIT();
311  }; // class BySubstitution
313 
314  inline std::ostream& operator<< (std::ostream& os, const BySubstitution& hs)
315  {
316  return hs.print(os);
317  }
318  } // namespace solver
319  } // namespace constraints
320 } // namespace hpp
321 
322 #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:289
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:277
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())
std::ostream & operator<<(std::ostream &os, const BySubstitution &hs)
Definition: by-substitution.hh:314
bool isSatisfied(vectorIn_t arg) const
Definition: by-substitution.hh:219
bool isSatisfied(vectorIn_t arg, vectorOut_t error) const
Definition: by-substitution.hh:231
ExplicitConstraintSet & explicitConstraintSet()
Get explicit constraint set.
Definition: by-substitution.hh:98
bool isSatisfied(vectorIn_t arg) const
Definition: hierarchical-iterative.hh:321
virtual std::ostream & print(std::ostream &os) const
value_type errorThreshold() const
Get error threshold.
Definition: hierarchical-iterative.hh:403
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:256
#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
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:271
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
boost::shared_ptr< Implicit > ImplicitPtr_t
Definition: fwd.hh:163
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:93
Status solve(vectorOut_t arg) const
Definition: by-substitution.hh:171