hpp-core: include/hpp/core/path-optimization/gradient-based.hh Source File

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

Loading...
Searching...
No Matches
gradient-based.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_PATH_OPTIMIZATION_GRADIENT_BASED_HH
31#define HPP_CORE_PATH_OPTIMIZATION_GRADIENT_BASED_HH
32
33#include <hpp/core/path-optimizer.hh>
34#include <hpp/core/path-vector.hh>
35#include <hpp/core/steering-method/straight.hh>
36
37namespace hpp {
38namespace core {
41namespace pathOptimization {
42class CollisionConstraintsResult;
43 typedef std::vector<CollisionConstraintsResult> CollisionConstraintsResults_t;
44
45 class HPP_CORE_DLLAPI GradientBased : public PathOptimizer {
46 public:
49 static GradientBasedPtr_t create(const ProblemConstPtr_t& problem);
50
52 virtual PathVectorPtr_t optimize(const PathVectorPtr_t& path);
53
54 protected:
55 GradientBased(const ProblemConstPtr_t& problem);
56
57 private:
58 typedef Eigen::JacobiSVD<matrix_t> Jacobi_t;
59
60 void compressHessian(matrixIn_t normal, matrixOut_t small) const;
61 void compressVector(vectorIn_t normal, vectorOut_t small) const;
62 void uncompressVector(vectorIn_t small, vectorOut_t normal) const;
63
64 void initialize(const PathVectorPtr_t& path);
65
67 void pathToVector(const PathVectorPtr_t& path, vectorOut_t x) const {
68 size_type index = 0;
69 for (std::size_t i = 0; i < path->numberPaths() - 1; ++i) {
70 const PathPtr_t& localPath = path->pathAtRank(i);
71 value_type t1 = localPath->timeRange().second;
72 (*localPath)(x.segment(index, configSize_), t1);
73 index += configSize_;
74 }
75 assert(index == x.size());
76 }
77
79 void vectorToPath(vectorIn_t x, const PathVectorPtr_t& result) const {
80 Configuration_t q0 = initial_, q1;
81 size_type index = 0;
82 while (index < x.size()) {
83 q1 = x.segment(index, configSize_);
84 PathPtr_t p = (*steeringMethod_)(q0, q1);
85 result->appendPath(p);
86 q0 = q1;
87 index += configSize_;
88 }
89 q1 = end_;
90 PathPtr_t p = (*steeringMethod_)(q0, q1);
91 result->appendPath(p);
92 }
93
94 void initializeProblemConstraints();
102 void updateProblemConstraints(vectorIn_t x);
103
115 bool constraintsSatisfied(
116 vectorOut_t& x, PathVectorPtr_t& path,
117 CollisionConstraintsResults_t& collisionConstraints);
118
129 bool solveConstraints(vectorOut_t x, PathVectorPtr_t& path,
130 CollisionConstraintsResults_t& collisionConstraints);
132 void integrate(vectorIn_t x0, vectorIn_t step, vectorOut_t x1) const;
133
135 vector_t computeIterate(vectorIn_t x) const;
136
138 void displayPath(vectorIn_t x, std::string
139#ifdef HPP_DEBUG
140 prefix
141#endif
142 ) const {
143 hppDout(info, prefix);
144 for (int i = 0; i < x.size(); i++) hppDout(info, x[i]);
145 hppDout(info, "finish path parsing");
146 }
147
155 void addCollisionConstraint(const CollisionConstraintsResult& ccr,
156 const PathVectorPtr_t& path) const;
157
165 void updateRightHandSide(
166 const CollisionConstraintsResults_t& collisionConstraints,
167 const PathVectorPtr_t& path) const;
168
170 bool getProblemConstraints();
171
172 mutable CostPtr_t cost_;
173 DevicePtr_t robot_;
174 size_type configSize_;
175 size_type robotNumberDofs_;
176 size_type robotNbNonLockedDofs_;
177 size_type numberDofs_;
178 size_type fSize_; // dimension of collision constraints
179 mutable Configuration_t initial_;
180 mutable Configuration_t end_;
181 WeighedDistancePtr_t distance_;
182 steeringMethod::StraightPtr_t steeringMethod_;
187 mutable matrix_t H_, Hinverse_, J_;
188 mutable matrix_t Hz_, gz_;
189 mutable vector_t stepNormal_;
190 mutable bool fullRank_;
191 mutable size_type nbWaypoints_;
192 mutable value_type alpha_;
193 mutable rowvector_t rgrad_;
194 mutable vector_t rhs_;
195 mutable vector_t value_;
196 mutable vector_t p_, p0_;
197 value_type epsilon_;
198 std::size_t iterMax_;
199 value_type alphaInit_; // .1, .2, .4
200 value_type alphaMax_;
201}; // GradientBased
202} // namespace pathOptimization
203} // namespace core
204} // namespace hpp
205#endif // HPP_CORE_PATH_OPTIMIZATION_GRADIENT_BASED_HH
hpp::core::PathOptimizer
Definition path-optimizer.hh:44
hpp::core::pathOptimization::GradientBased
Definition gradient-based.hh:45
HPP_CORE_DLLAPI
#define HPP_CORE_DLLAPI
Definition config.hh:88
hppDout
#define hppDout(channel, data)
hpp::core::pathOptimization::GradientBased::optimize
virtual PathVectorPtr_t optimize(const PathVectorPtr_t &path)
Optimize path.
hpp::core::pathOptimization::CollisionConstraintsResults_t
std::vector< CollisionConstraintsResult > CollisionConstraintsResults_t
Definition gradient-based.hh:43
hpp::core::pathOptimization::GradientBased::create
static GradientBasedPtr_t create(const ProblemConstPtr_t &problem)
hpp::core::pathOptimization::GradientBased::GradientBased
GradientBased(const ProblemConstPtr_t &problem)
hpp
hpp::constraints::Configuration_t
pinocchio::Configuration_t Configuration_t
hpp::constraints::DevicePtr_t
pinocchio::DevicePtr_t DevicePtr_t
hpp::constraints::size_type
pinocchio::size_type size_type
hpp::constraints::value_type
pinocchio::value_type value_type
hpp::constraints::matrix_t
pinocchio::matrix_t matrix_t
hpp::constraints::vector_t
pinocchio::vector_t vector_t
hpp::core::pathOptimization::GradientBasedPtr_t
shared_ptr< GradientBased > GradientBasedPtr_t
Definition fwd.hh:297
hpp::core::path::CostPtr_t
shared_ptr< Cost > CostPtr_t
Definition fwd.hh:249
hpp::core::value_type
pinocchio::value_type value_type
Definition fwd.hh:174
hpp::core::PathVectorPtr_t
shared_ptr< PathVector > PathVectorPtr_t
Definition fwd.hh:193
hpp::core::rowvector_t
Eigen::Matrix< value_type, 1, Eigen::Dynamic > rowvector_t
Definition fwd.hh:223
hpp::core::vectorIn_t
pinocchio::vectorIn_t vectorIn_t
Definition fwd.hh:221
hpp::core::WeighedDistancePtr_t
shared_ptr< WeighedDistance > WeighedDistancePtr_t
Definition fwd.hh:226
hpp::core::vectorOut_t
pinocchio::vectorOut_t vectorOut_t
Definition fwd.hh:222
hpp::core::size_type
pinocchio::size_type size_type
Definition fwd.hh:173
hpp::core::ProblemConstPtr_t
shared_ptr< const Problem > ProblemConstPtr_t
Definition fwd.hh:197
hpp::core::Configuration_t
pinocchio::Configuration_t Configuration_t
Definition fwd.hh:107
hpp::core::matrixOut_t
constraints::matrixOut_t matrixOut_t
Definition fwd.hh:167
hpp::core::matrixIn_t
constraints::matrixIn_t matrixIn_t
Definition fwd.hh:166
hpp::core::PathPtr_t
shared_ptr< Path > PathPtr_t
Definition fwd.hh:187
path-optimizer.hh
path-vector.hh
straight.hh