hpp-core: include/hpp/core/config-projector.hh Source File

hpp-core 9.0.2 Implement basic classes for canonical path planning for kinematic chains.

Loading...
Searching...
No Matches
config-projector.hh
Go to the documentation of this file.
1//
2// Copyright (c) 2014 CNRS
3// Authors: Florent Lamiraux
4//
5
6// Redistribution and use in source and binary forms, with or without
7// modification, are permitted provided that the following conditions are
8// met:
9//
10// 1. Redistributions of source code must retain the above copyright
11// notice, this list of conditions and the following disclaimer.
12//
13// 2. Redistributions in binary form must reproduce the above copyright
14// notice, this list of conditions and the following disclaimer in the
15// documentation and/or other materials provided with the distribution.
16//
17// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
18// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
19// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
20// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
21// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
22// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
23// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
24// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
25// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
28// DAMAGE.
29
30#ifndef HPP_CORE_CONFIG_PROJECTOR_HH
31#define HPP_CORE_CONFIG_PROJECTOR_HH
32
33#include <Eigen/SVD>
34#include <hpp/core/config.hh>
35#include <hpp/core/constraint.hh>
36#include <hpp/core/deprecated.hh>
37#include <hpp/statistics/success-bin.hh>
38
39namespace hpp {
40namespace core {
41 typedef constraints::solver::BySubstitution BySubstitution;
42
45
66 class HPP_CORE_DLLAPI ConfigProjector : public Constraint {
67 public:
68 enum LineSearchType { Backtracking, ErrorNormBased, FixedSequence, Constant };
69
76 static ConfigProjectorPtr_t create(const DevicePtr_t& robot,
77 const std::string& name,
78 value_type errorThreshold,
79 size_type maxIterations);
80
83 static ConfigProjectorPtr_t createCopy(const ConfigProjectorPtr_t cp);
84
86 virtual ConstraintPtr_t copy() const;
87
89 virtual ~ConfigProjector();
90
95 bool contains(const constraints::ImplicitPtr_t& numericalConstraint) const;
96
106 bool add(const constraints::ImplicitPtr_t& numericalConstraint,
107 const std::size_t priority = 0);
108
109 void lastIsOptional(bool optional);
110
111 bool lastIsOptional() const;
112
117 bool optimize(ConfigurationOut_t config, std::size_t maxIter = 0);
118
120 const DevicePtr_t& robot() const { return robot_; }
121
131 void projectVectorOnKernel(ConfigurationIn_t from, vectorIn_t velocity,
132 vectorOut_t result);
133
143 virtual void projectOnKernel(ConfigurationIn_t from, ConfigurationIn_t to,
144 ConfigurationOut_t result);
145
156 void computeValueAndJacobian(ConfigurationIn_t configuration,
157 vectorOut_t value, matrixOut_t reducedJacobian);
158
166
168 size_type numberFreeVariables() const;
169
171 size_type dimension() const;
172
177 void compressVector(vectorIn_t normal, vectorOut_t small) const;
178
184 void uncompressVector(vectorIn_t small, vectorOut_t normal) const;
185
191 void compressMatrix(matrixIn_t normal, matrixOut_t small,
192 bool rows = true) const;
193
199 void uncompressMatrix(matrixIn_t small, matrixOut_t normal,
200 bool rows = true) const;
201
203
205 void maxIterations(size_type iterations);
207 size_type maxIterations() const;
208
210 void errorThreshold(const value_type& threshold);
212 value_type errorThreshold() const;
213
214 value_type residualError() const;
215
216 const value_type& sigma() const;
217
220
236 vector_t rightHandSideFromConfig(ConfigurationIn_t config);
237
240 void rightHandSideFromConfig(const constraints::ImplicitPtr_t& nm,
241 ConfigurationIn_t config);
242
245 void rightHandSide(const vector_t& param);
246
249 void rightHandSide(const constraints::ImplicitPtr_t& nm, vectorIn_t rhs);
250
253 vector_t rightHandSide() const;
254
256 void rightHandSideAt(const value_type& s);
257
259
262 virtual bool isSatisfied(ConfigurationIn_t config);
263
268 virtual bool isSatisfied(ConfigurationIn_t config, value_type errorThreshold);
269
276 virtual bool isSatisfied(ConfigurationIn_t config, vector_t& error);
277
279 ::hpp::statistics::SuccessStatistics& statistics() { return statistics_; }
280
283 const NumericalConstraints_t& numericalConstraints() const;
284
285 const BySubstitution& solver() const { return *solver_; }
286
287 BySubstitution& solver() { return *solver_; }
288
290 void lineSearchType(LineSearchType ls) { lineSearchType_ = ls; }
291
293 LineSearchType lineSearchType() const { return lineSearchType_; }
294
295 static void defaultLineSearch(LineSearchType ls);
296
297 protected:
304 ConfigProjector(const DevicePtr_t& robot, const std::string& name,
305 value_type errorThreshold, size_type maxIterations);
307 ConfigProjector(const ConfigProjector& cp);
308
310 void init(const ConfigProjectorPtr_t& self) {
311 Constraint::init(self);
312 weak_ = self;
313 }
315 virtual bool impl_compute(ConfigurationOut_t configuration);
316
317 private:
318 virtual std::ostream& print(std::ostream& os) const;
319
320 DevicePtr_t robot_;
321 static LineSearchType defaultLineSearch_;
322 LineSearchType lineSearchType_;
323 BySubstitution* solver_;
324
325 bool solverOneStep(ConfigurationOut_t config) const;
326 int solverSolve(ConfigurationOut_t config) const;
327
328 ConfigProjectorWkPtr_t weak_;
329 ::hpp::statistics::SuccessStatistics statistics_;
330
331 ConfigProjector() {}
332 HPP_SERIALIZABLE();
333}; // class ConfigProjector
335} // namespace core
336} // namespace hpp
337#endif // HPP_CORE_CONFIG_PROJECTOR_HH
hpp::constraints::solver::BySubstitution
hpp::core::ConfigProjector
Definition config-projector.hh:66
hpp::core::ConfigProjector::numberFreeVariables
size_type numberFreeVariables() const
Return the number of free variables.
hpp::core::ConfigProjector::uncompressMatrix
void uncompressMatrix(matrixIn_t small, matrixOut_t normal, bool rows=true) const
hpp::core::ConfigProjector::init
void init(const ConfigProjectorPtr_t &self)
Store weak pointer to itself.
Definition config-projector.hh:310
hpp::core::ConfigProjector::errorThreshold
void errorThreshold(const value_type &threshold)
Set error threshold.
hpp::core::ConfigProjector::create
static ConfigProjectorPtr_t create(const DevicePtr_t &robot, const std::string &name, value_type errorThreshold, size_type maxIterations)
hpp::core::ConfigProjector::lastIsOptional
void lastIsOptional(bool optional)
hpp::core::ConfigProjector::contains
bool contains(const constraints::ImplicitPtr_t &numericalConstraint) const
hpp::core::ConfigProjector::LineSearchType
LineSearchType
Definition config-projector.hh:68
hpp::core::ConfigProjector::Backtracking
@ Backtracking
Definition config-projector.hh:68
hpp::core::ConfigProjector::dimension
size_type dimension() const
Get constraint dimension.
hpp::core::ConfigProjector::compressVector
void compressVector(vectorIn_t normal, vectorOut_t small) const
hpp::core::ConfigProjector::lineSearchType
LineSearchType lineSearchType() const
Get the line search type.
Definition config-projector.hh:293
hpp::core::ConfigProjector::copy
virtual ConstraintPtr_t copy() const
return shared pointer to copy
hpp::core::ConfigProjector::robot
const DevicePtr_t & robot() const
Get robot.
Definition config-projector.hh:120
hpp::core::ConfigProjector::impl_compute
virtual bool impl_compute(ConfigurationOut_t configuration)
Numerically solve constraint.
hpp::core::ConfigProjector::rightHandSideAt
void rightHandSideAt(const value_type &s)
Update the right hand side using Implicit::rightHandSideAt.
hpp::core::ConfigProjector::add
bool add(const constraints::ImplicitPtr_t &numericalConstraint, const std::size_t priority=0)
hpp::core::ConfigProjector::ConfigProjector
ConfigProjector(const ConfigProjector &cp)
Copy constructor.
hpp::core::ConfigProjector::rightHandSide
void rightHandSide(const vector_t &param)
hpp::core::ConfigProjector::numericalConstraints
const NumericalConstraints_t & numericalConstraints() const
hpp::core::ConfigProjector::residualError
value_type residualError() const
hpp::core::ConfigProjector::defaultLineSearch
static void defaultLineSearch(LineSearchType ls)
hpp::core::ConfigProjector::projectOnKernel
virtual void projectOnKernel(ConfigurationIn_t from, ConfigurationIn_t to, ConfigurationOut_t result)
hpp::core::ConfigProjector::rightHandSideFromConfig
vector_t rightHandSideFromConfig(ConfigurationIn_t config)
hpp::core::ConfigProjector::isSatisfied
virtual bool isSatisfied(ConfigurationIn_t config, vector_t &error)
hpp::core::ConfigProjector::sigma
const value_type & sigma() const
hpp::core::ConfigProjector::~ConfigProjector
virtual ~ConfigProjector()
Destructor.
hpp::core::ConfigProjector::maxIterations
size_type maxIterations() const
Get maximal number of iterations in config projector.
hpp::core::ConfigProjector::createCopy
static ConfigProjectorPtr_t createCopy(const ConfigProjectorPtr_t cp)
hpp::core::ConfigProjector::statistics
::hpp::statistics::SuccessStatistics & statistics()
Get the statistics.
Definition config-projector.hh:279
hpp::core::ConfigProjector::computeValueAndJacobian
void computeValueAndJacobian(ConfigurationIn_t configuration, vectorOut_t value, matrixOut_t reducedJacobian)
hpp::core::ConfigProjector::rightHandSide
void rightHandSide(const constraints::ImplicitPtr_t &nm, vectorIn_t rhs)
hpp::core::ConfigProjector::compressMatrix
void compressMatrix(matrixIn_t normal, matrixOut_t small, bool rows=true) const
hpp::core::ConfigProjector::ConfigProjector
ConfigProjector(const DevicePtr_t &robot, const std::string &name, value_type errorThreshold, size_type maxIterations)
hpp::core::ConfigProjector::solver
const BySubstitution & solver() const
Definition config-projector.hh:285
hpp::core::ConfigProjector::errorThreshold
value_type errorThreshold() const
Get errorimal number of threshold in config projector.
hpp::core::ConfigProjector::solver
BySubstitution & solver()
Definition config-projector.hh:287
hpp::core::ConfigProjector::isSatisfied
virtual bool isSatisfied(ConfigurationIn_t config)
hpp::core::ConfigProjector::lineSearchType
void lineSearchType(LineSearchType ls)
Set the line search type.
Definition config-projector.hh:290
hpp::core::ConfigProjector::uncompressVector
void uncompressVector(vectorIn_t small, vectorOut_t normal) const
hpp::core::ConfigProjector::rightHandSide
vector_t rightHandSide() const
hpp::core::ConfigProjector::projectVectorOnKernel
void projectVectorOnKernel(ConfigurationIn_t from, vectorIn_t velocity, vectorOut_t result)
hpp::core::ConfigProjector::isSatisfied
virtual bool isSatisfied(ConfigurationIn_t config, value_type errorThreshold)
hpp::core::ConfigProjector::maxIterations
void maxIterations(size_type iterations)
Set maximal number of iterations.
hpp::core::ConfigProjector::rightHandSideFromConfig
void rightHandSideFromConfig(const constraints::ImplicitPtr_t &nm, ConfigurationIn_t config)
hpp::core::ConfigProjector::optimize
bool optimize(ConfigurationOut_t config, std::size_t maxIter=0)
hpp::core::ConfigProjector::lastIsOptional
bool lastIsOptional() const
hpp::core::Constraint
Definition constraint.hh:49
hpp::statistics::SuccessStatistics
config.hh
HPP_CORE_DLLAPI
#define HPP_CORE_DLLAPI
Definition config.hh:88
constraint.hh
deprecated.hh
hpp
hpp::constraints::ImplicitPtr_t
shared_ptr< Implicit > ImplicitPtr_t
hpp::core::NumericalConstraints_t
constraints::NumericalConstraints_t NumericalConstraints_t
Definition fwd.hh:232
hpp::core::value_type
pinocchio::value_type value_type
Definition fwd.hh:174
hpp::core::ConstraintPtr_t
shared_ptr< Constraint > ConstraintPtr_t
Definition fwd.hh:129
hpp::core::vectorIn_t
pinocchio::vectorIn_t vectorIn_t
Definition fwd.hh:221
hpp::core::vectorOut_t
pinocchio::vectorOut_t vectorOut_t
Definition fwd.hh:222
hpp::core::ConfigurationOut_t
pinocchio::ConfigurationOut_t ConfigurationOut_t
Definition fwd.hh:109
hpp::core::vector_t
pinocchio::vector_t vector_t
Definition fwd.hh:220
hpp::core::ConfigurationIn_t
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition fwd.hh:108
hpp::core::size_type
pinocchio::size_type size_type
Definition fwd.hh:173
hpp::core::BySubstitution
constraints::solver::BySubstitution BySubstitution
Definition config-projector.hh:41
hpp::core::ConfigProjectorPtr_t
shared_ptr< ConfigProjector > ConfigProjectorPtr_t
Definition fwd.hh:114
hpp::core::matrixOut_t
constraints::matrixOut_t matrixOut_t
Definition fwd.hh:167
hpp::core::DevicePtr_t
pinocchio::DevicePtr_t DevicePtr_t
Definition fwd.hh:134
hpp::core::matrixIn_t
constraints::matrixIn_t matrixIn_t
Definition fwd.hh:166
HPP_SERIALIZABLE
#define HPP_SERIALIZABLE()
success-bin.hh