hpp-constraints: include/hpp/constraints/solver/by-substitution.hh Source File

hpp-constraints 9.0.2 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
32#include <hpp/constraints/config.hh>
33#include <hpp/constraints/explicit-constraint-set.hh>
34#include <hpp/constraints/fwd.hh>
35#include <hpp/constraints/locked-joint.hh>
36#include <hpp/constraints/solver/hierarchical-iterative.hh>
37#include <vector>
38
39 namespace hpp {
40 namespace constraints {
41 namespace solver {
44
72 class HPP_CONSTRAINTS_DLLAPI BySubstitution
73 : public solver::HierarchicalIterative {
74 public:
75 BySubstitution(const LiegroupSpacePtr_t& configSpace);
76 BySubstitution(const BySubstitution& other);
77
78 virtual ~BySubstitution() {}
79
85 bool add(const ImplicitPtr_t& numericalConstraint,
86 const std::size_t& priority = 0);
87
96 BySubstitution extract(interval_t interval);
97
99 const NumericalConstraints_t& numericalConstraints() const {
100 return constraints_;
101 }
102
104 ExplicitConstraintSet& explicitConstraintSet() { return explicit_; }
105
107 const ExplicitConstraintSet& explicitConstraintSet() const {
108 return explicit_;
109 }
110
112 size_type numberFreeVariables() const {
113 return explicitConstraintSet().notOutDers().nbIndices();
114 }
115
117 void explicitConstraintSetHasChanged();
118
119 template <typename LineSearchType>
120 Status solve(vectorOut_t arg, LineSearchType ls = LineSearchType()) const {
121 return solve<LineSearchType>(arg, false, ls);
122 }
123
124 template <typename LineSearchType>
125 Status solve(vectorOut_t arg, bool optimize,
126 LineSearchType ls = LineSearchType()) const {
127 // TODO when there are only locked joint explicit constraints,
128 // there is no need for this intricated loop.
129 // if (explicit_.isConstant()) {
130 // explicit_.solve(arg);
131 // iterative_.solve(arg, ls);
132 // } else {
133 return impl_solve(arg, optimize, ls);
134 // }
135 }
136
146 void projectVectorOnKernel(ConfigurationIn_t from, vectorIn_t velocity,
147 vectorOut_t result) const;
148
159 virtual void projectOnKernel(ConfigurationIn_t from, ConfigurationIn_t to,
160 ConfigurationOut_t result);
161
162 inline Status solve(vectorOut_t arg) const {
163 return solve(arg, DefaultLineSearch());
164 }
165
168
170 vector_t rightHandSideFromConfig(ConfigurationIn_t config);
171
174 bool rightHandSideFromConfig(const ImplicitPtr_t& constraint,
175 ConfigurationIn_t config);
176
179 bool rightHandSide(const ImplicitPtr_t& constraint, vectorIn_t rhs);
180
183 bool getRightHandSide(const ImplicitPtr_t& constraint, vectorOut_t rhs) const;
184
186 void rightHandSide(vectorIn_t rhs);
187
192 vector_t rightHandSide() const;
193
195 size_type rightHandSideSize() const;
196
198
204 size_type errorSize() const {
205 return dimension() + explicit_.outDers().nbIndices();
206 }
207
211 bool isSatisfied(vectorIn_t arg) const {
212 return solver::HierarchicalIterative::isSatisfied(arg) &&
213 explicit_.isSatisfied(arg);
214 }
215
220 bool isSatisfied(vectorIn_t arg, value_type errorThreshold) const {
221 return solver::HierarchicalIterative::isSatisfied(arg, errorThreshold) &&
222 explicit_.isSatisfied(arg, errorThreshold);
223 }
229 bool isSatisfied(vectorIn_t arg, vectorOut_t error) const {
230 assert(error.size() == dimension() + explicit_.errorSize());
231 bool iterative = solver::HierarchicalIterative::isSatisfied(arg);
232 residualError(error.head(dimension()));
233 bool _explicit =
234 explicit_.isSatisfied(arg, error.tail(explicit_.errorSize()));
235 return iterative && _explicit;
236 }
237
247 bool isConstraintSatisfied(const ImplicitPtr_t& constraint, vectorIn_t arg,
248 vectorOut_t error, bool& constraintFound) const;
249
250 template <typename LineSearchType>
251 bool oneStep(vectorOut_t arg, LineSearchType& lineSearch) const {
252 computeValue<true>(arg);
253 updateJacobian(arg);
254 computeDescentDirection();
255 lineSearch(*this, arg, dq_);
256 explicit_.solve(arg);
257 return solver::HierarchicalIterative::isSatisfied(arg);
258 }
259
262 void updateJacobian(vectorIn_t arg) const;
263
265 void errorThreshold(const value_type& threshold) {
266 solver::HierarchicalIterative::errorThreshold(threshold);
267 explicit_.errorThreshold(threshold);
268 }
270 value_type errorThreshold() const {
271 return solver::HierarchicalIterative::errorThreshold();
272 }
273
277 segments_t implicitDof() const;
278
279 virtual std::ostream& print(std::ostream& os) const;
280
281 bool integrate(vectorIn_t from, vectorIn_t velocity,
282 vectorOut_t result) const {
283 bool res = solver::HierarchicalIterative::integrate(from, velocity, result);
284 explicit_.solve(result);
285 return res;
286 }
287
288 protected:
289 void computeActiveRowsOfJ(std::size_t iStack);
290
291 private:
292 typedef solver::HierarchicalIterative parent_t;
293
294 template <typename LineSearchType>
295 Status impl_solve(vectorOut_t arg, bool optimize, LineSearchType ls) const;
296
297 ExplicitConstraintSet explicit_;
298 mutable matrix_t Je_, JeExpanded_;
299
300 BySubstitution() {}
301 HPP_SERIALIZABLE_SPLIT();
302}; // class BySubstitution
304
305} // namespace solver
306} // namespace constraints
307} // namespace hpp
308
309#endif // HPP_CONSTRAINTS_SOLVER_BY_SUBSTITUTION_HH
hpp::constraints::ExplicitConstraintSet
Definition explicit-constraint-set.hh:99
hpp::constraints::solver::BySubstitution
Definition by-substitution.hh:73
hpp::constraints::solver::BySubstitution::computeActiveRowsOfJ
void computeActiveRowsOfJ(std::size_t iStack)
hpp::constraints::solver::BySubstitution::BySubstitution
BySubstitution(const LiegroupSpacePtr_t &configSpace)
hpp::constraints::solver::BySubstitution::~BySubstitution
virtual ~BySubstitution()
Definition by-substitution.hh:78
hpp::constraints::solver::BySubstitution::rightHandSide
vector_t rightHandSide() const
hpp::constraints::solver::BySubstitution::explicitConstraintSetHasChanged
void explicitConstraintSetHasChanged()
Should be called whenever explicit solver is modified.
hpp::constraints::solver::BySubstitution::solve
Status solve(vectorOut_t arg, bool optimize, LineSearchType ls=LineSearchType()) const
Definition by-substitution.hh:125
hpp::constraints::solver::BySubstitution::solve
Status solve(vectorOut_t arg) const
Definition by-substitution.hh:162
hpp::constraints::solver::BySubstitution::oneStep
bool oneStep(vectorOut_t arg, LineSearchType &lineSearch) const
Definition by-substitution.hh:251
hpp::constraints::solver::BySubstitution::solve
Status solve(vectorOut_t arg, LineSearchType ls=LineSearchType()) const
Definition by-substitution.hh:120
hpp::constraints::solver::BySubstitution::errorThreshold
value_type errorThreshold() const
Get error threshold.
Definition by-substitution.hh:270
hpp::constraints::solver::BySubstitution::isSatisfied
bool isSatisfied(vectorIn_t arg, vectorOut_t error) const
Definition by-substitution.hh:229
hpp::constraints::solver::BySubstitution::errorSize
size_type errorSize() const
Definition by-substitution.hh:204
hpp::constraints::solver::BySubstitution::getRightHandSide
bool getRightHandSide(const ImplicitPtr_t &constraint, vectorOut_t rhs) const
hpp::constraints::solver::BySubstitution::print
virtual std::ostream & print(std::ostream &os) const
hpp::constraints::solver::BySubstitution::rightHandSide
void rightHandSide(vectorIn_t rhs)
hpp::constraints::solver::BySubstitution::projectVectorOnKernel
void projectVectorOnKernel(ConfigurationIn_t from, vectorIn_t velocity, vectorOut_t result) const
hpp::constraints::solver::BySubstitution::rightHandSideFromConfig
bool rightHandSideFromConfig(const ImplicitPtr_t &constraint, ConfigurationIn_t config)
hpp::constraints::solver::BySubstitution::numericalConstraints
const NumericalConstraints_t & numericalConstraints() const
Get the numerical constraints implicit and explicit.
Definition by-substitution.hh:99
hpp::constraints::solver::BySubstitution::explicitConstraintSet
ExplicitConstraintSet & explicitConstraintSet()
Get explicit constraint set.
Definition by-substitution.hh:104
hpp::constraints::solver::BySubstitution::isSatisfied
bool isSatisfied(vectorIn_t arg, value_type errorThreshold) const
Definition by-substitution.hh:220
hpp::constraints::solver::BySubstitution::explicitConstraintSet
const ExplicitConstraintSet & explicitConstraintSet() const
Set explicit constraint set.
Definition by-substitution.hh:107
hpp::constraints::solver::BySubstitution::implicitDof
segments_t implicitDof() const
hpp::constraints::solver::BySubstitution::numberFreeVariables
size_type numberFreeVariables() const
Return the number of free variables.
Definition by-substitution.hh:112
hpp::constraints::solver::BySubstitution::add
bool add(const ImplicitPtr_t &numericalConstraint, const std::size_t &priority=0)
hpp::constraints::solver::BySubstitution::projectOnKernel
virtual void projectOnKernel(ConfigurationIn_t from, ConfigurationIn_t to, ConfigurationOut_t result)
hpp::constraints::solver::BySubstitution::integrate
bool integrate(vectorIn_t from, vectorIn_t velocity, vectorOut_t result) const
Definition by-substitution.hh:281
hpp::constraints::solver::BySubstitution::rightHandSideFromConfig
vector_t rightHandSideFromConfig(ConfigurationIn_t config)
hpp::constraints::solver::BySubstitution::isConstraintSatisfied
bool isConstraintSatisfied(const ImplicitPtr_t &constraint, vectorIn_t arg, vectorOut_t error, bool &constraintFound) const
hpp::constraints::solver::BySubstitution::rightHandSide
bool rightHandSide(const ImplicitPtr_t &constraint, vectorIn_t rhs)
hpp::constraints::solver::BySubstitution::BySubstitution
BySubstitution(const BySubstitution &other)
hpp::constraints::solver::BySubstitution::updateJacobian
void updateJacobian(vectorIn_t arg) const
hpp::constraints::solver::BySubstitution::extract
BySubstitution extract(interval_t interval)
hpp::constraints::solver::BySubstitution::rightHandSideSize
size_type rightHandSideSize() const
hpp::constraints::solver::BySubstitution::isSatisfied
bool isSatisfied(vectorIn_t arg) const
Definition by-substitution.hh:211
hpp::constraints::solver::BySubstitution::errorThreshold
void errorThreshold(const value_type &threshold)
Set error threshold.
Definition by-substitution.hh:265
hpp::constraints::solver::HierarchicalIterative
Definition hierarchical-iterative.hh:240
hpp::constraints::solver::HierarchicalIterative::Status
Status
Definition hierarchical-iterative.hh:245
config.hh
HPP_CONSTRAINTS_DLLAPI
#define HPP_CONSTRAINTS_DLLAPI
Definition config.hh:88
explicit-constraint-set.hh
fwd.hh
hierarchical-iterative.hh
locked-joint.hh
hpp::constraints::LiegroupSpacePtr_t
pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t
Definition fwd.hh:70
hpp::constraints::ImplicitPtr_t
shared_ptr< Implicit > ImplicitPtr_t
Definition fwd.hh:175
hpp::constraints::interval_t
std::pair< value_type, value_type > interval_t
Definition fwd.hh:85
hpp::constraints::size_type
pinocchio::size_type size_type
Definition fwd.hh:48
hpp::constraints::value_type
pinocchio::value_type value_type
Definition fwd.hh:49
hpp::constraints::vectorIn_t
pinocchio::vectorIn_t vectorIn_t
Definition fwd.hh:61
hpp::constraints::ConfigurationIn_t
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition fwd.hh:108
hpp::constraints::matrix_t
pinocchio::matrix_t matrix_t
Definition fwd.hh:57
hpp::constraints::segments_t
std::vector< segment_t > segments_t
Definition fwd.hh:86
hpp::constraints::vectorOut_t
pinocchio::vectorOut_t vectorOut_t
Definition fwd.hh:62
hpp::constraints::NumericalConstraints_t
std::vector< constraints::ImplicitPtr_t > NumericalConstraints_t
Definition fwd.hh:177
hpp::constraints::vector_t
pinocchio::vector_t vector_t
Definition fwd.hh:60
hpp::constraints::ConfigurationOut_t
pinocchio::ConfigurationOut_t ConfigurationOut_t
Definition fwd.hh:109
hpp
HPP_SERIALIZABLE_SPLIT
#define HPP_SERIALIZABLE_SPLIT()
hpp::constraints::solver::lineSearch::FixedSequence
Definition hierarchical-iterative.hh:70