hpp-constraints 4.15.1
Definition of basic geometric constraints for motion planning
Loading...
Searching...
No Matches
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
37#include <vector>
38
39namespace hpp {
40namespace constraints {
41namespace solver {
44
74 public:
77
78 virtual ~BySubstitution() {}
79
85 bool add(const ImplicitPtr_t& numericalConstraint,
86 const std::size_t& priority = 0);
87
89 HPP_CONSTRAINTS_DEPRECATED bool add(const ImplicitPtr_t& numericalConstraint,
90 const segments_t& passiveDofs,
91 const std::size_t priority = 0) {
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
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 {
214 return solver::HierarchicalIterative::isSatisfied(arg) &&
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);
259 return solver::HierarchicalIterative::isSatisfied(arg);
260 }
261
264 void updateJacobian(vectorIn_t arg) const;
265
267 void errorThreshold(const value_type& threshold) {
268 solver::HierarchicalIterative::errorThreshold(threshold);
269 explicit_.errorThreshold(threshold);
270 }
273 return solver::HierarchicalIterative::errorThreshold();
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
size_type errorSize() const
Definition: by-substitution.hh:206
bool getRightHandSide(const ImplicitPtr_t &constraint, vectorOut_t rhs) const
virtual std::ostream & print(std::ostream &os) const
void projectVectorOnKernel(ConfigurationIn_t from, vectorIn_t velocity, vectorOut_t result) const
bool rightHandSideFromConfig(const ImplicitPtr_t &constraint, ConfigurationIn_t config)
const NumericalConstraints_t & numericalConstraints() const
Get the numerical constraints implicit and explicit.
Definition: by-substitution.hh:101
ExplicitConstraintSet & explicitConstraintSet()
Get explicit constraint set.
Definition: by-substitution.hh:106
bool isSatisfied(vectorIn_t arg, value_type errorThreshold) const
Definition: by-substitution.hh:222
const ExplicitConstraintSet & explicitConstraintSet() const
Set explicit constraint set.
Definition: by-substitution.hh:109
HPP_CONSTRAINTS_DEPRECATED bool add(const ImplicitPtr_t &numericalConstraint, const segments_t &passiveDofs, const std::size_t priority=0)
Definition: by-substitution.hh:89
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
Status
Definition: hierarchical-iterative.hh:237
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:64
#define HPP_CONSTRAINTS_DEPRECATED
Definition: deprecated.hh:37
pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t
Definition: fwd.hh:69
shared_ptr< Implicit > ImplicitPtr_t
Definition: fwd.hh:175
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