GCC Code Coverage Report

Directory: ./
File: include/hpp/core/config-projector.hh
Date: 2025-03-10 11:18:21
Exec Total Coverage
Lines: 7 8 87.5%
Branches: 0 4 0.0%
Line Branch Exec Source
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
39 namespace hpp {
40 namespace core {
41 typedef constraints::solver::BySubstitution BySubstitution;
42
43 /// \addtogroup constraints
44 /// \{
45
46 /** Implicit non-linear constraint
47
48 This class defines a numerical constraints on a robot configuration
49 of the form:
50 \f{eqnarray*}
51 f_1 (\mathbf{q}) & = \mbox{or} \leq & f_1^0 \\
52 & \vdots\\
53 f_m (\mathbf{q}) & = \mbox{or} \leq & f_m^0
54 \f}
55 Functions \f$f_i\f$ are \ref constraints::DifferentiableFunction
56 "differentiable functions". Vectors \f$f_i^0\f$ are called
57 <b>right hand side</b>.
58
59 The constraints are solved numerically by a Newton Raphson like method.
60
61 Numerical constraints can be added using method
62 ConfigProjector::add. Default parameter of this method define
63 equality constraints, but inequality constraints can also be defined by
64 passing an object of type ComparisonType to method.
65 */
66 class HPP_CORE_DLLAPI ConfigProjector : public Constraint {
67 public:
68 enum LineSearchType { Backtracking, ErrorNormBased, FixedSequence, Constant };
69
70 /// Return shared pointer to new object
71 /// \param robot robot the constraint applies to.
72 /// \param errorThreshold norm of the value of the constraint under which
73 /// the constraint is considered satified,
74 /// \param maxIterations maximal number of iteration in the resolution of
75 /// the constraint.
76 static ConfigProjectorPtr_t create(const DevicePtr_t& robot,
77 const std::string& name,
78 value_type errorThreshold,
79 size_type maxIterations);
80
81 /// Return shared pointer to copy
82 /// \param cp shared pointer to object to copy
83 static ConfigProjectorPtr_t createCopy(const ConfigProjectorPtr_t cp);
84
85 /// return shared pointer to copy
86 virtual ConstraintPtr_t copy() const;
87
88 /// Destructor
89 virtual ~ConfigProjector();
90
91 /// Check that numerical constraint is in config projector
92 /// \param numericalConstraint numerical constraint
93 /// \return true if numerical constraint is already in config projector
94 /// whatever the passive dofs are.
95 bool contains(const constraints::ImplicitPtr_t& numericalConstraint) const;
96
97 /// Add a numerical constraint
98 ///
99 /// \note The intervals are interpreted as a list of couple
100 /// (index_start, length) and NOT as (index_start, index_end).
101 ///
102 /// \param numericalConstraint The numerical constraint.
103 /// \param priority priority of the function. The last level might be
104 /// optional.
105 /// \return false if numerical constraint had already been inserted.
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
113 /// Optimize the configuration while respecting the constraints
114 /// The input configuration must already satisfy the constraints.
115 /// \return true if the configuration was optimized.
116 /// \param maxIter if 0, use maxIterations().
117 bool optimize(ConfigurationOut_t config, std::size_t maxIter = 0);
118
119 /// Get robot
120 4242 const DevicePtr_t& robot() const { return robot_; }
121
122 /// Project velocity on constraint tangent space in "from"
123 ///
124 /// \param from configuration,
125 /// \param velocity velocity to project
126 ///
127 /// \f[
128 /// \textbf{q}_{res} = \left(I_n -
129 /// J^{+}J(\textbf{q}_{from})\right) (\textbf{v})
130 /// \f]
131 void projectVectorOnKernel(ConfigurationIn_t from, vectorIn_t velocity,
132 vectorOut_t result);
133
134 /// Project configuration "to" on constraint tangent space in "from"
135 ///
136 /// \param from configuration,
137 /// \param to configuration to project
138 ///
139 /// \f[
140 /// \textbf{q}_{res} = \textbf{q}_{from} + \left(I_n -
141 /// J^{+}J(\textbf{q}_{from})\right) (\textbf{q}_{to} - \textbf{q}_{from})
142 /// \f]
143 virtual void projectOnKernel(ConfigurationIn_t from, ConfigurationIn_t to,
144 ConfigurationOut_t result);
145
146 /// Compute value and reduced jacobian at a given configuration
147 ///
148 /// \param configuration input configuration
149 /// \retval value values of the differentiable functions stacked in a
150 /// vector,
151 /// \retval reducedJacobian Reduced Jacobian of the differentiable
152 /// functions stacked in a matrix. Reduced Jacobian is defined
153 /// as the Jacobian to which columns corresponding to explicit
154 /// constraints have been removed and to which columns corresponding
155 /// to passive dofs are set to 0.
156 void computeValueAndJacobian(ConfigurationIn_t configuration,
157 vectorOut_t value, matrixOut_t reducedJacobian);
158
159 /// \name Compression of locked degrees of freedom
160 ///
161 /// Degrees of freedom related to locked joint are not taken into
162 /// account in numerical constraint resolution. The following methods
163 /// Compress or uncompress vectors or matrices by removing lines and
164 /// columns corresponding to locked degrees of freedom.
165 /// \{
166
167 /// Return the number of free variables
168 size_type numberFreeVariables() const;
169
170 /// Get constraint dimension
171 size_type dimension() const;
172
173 /// Compress Velocity vector by removing output of explicit constraints
174 ///
175 /// \param normal input velocity vector
176 /// \retval small compressed velocity vectors
177 void compressVector(vectorIn_t normal, vectorOut_t small) const;
178
179 /// Expand compressed velocity vector
180 ///
181 /// \param small compressed velocity vector without output of explicit
182 /// constraints
183 /// \retval normal uncompressed velocity vector.
184 void uncompressVector(vectorIn_t small, vectorOut_t normal) const;
185
186 /// Compress matrix
187 ///
188 /// \param normal input matrix
189 /// \retval small compressed matrix
190 /// \param rows whether to compress rows and colums or only columns
191 void compressMatrix(matrixIn_t normal, matrixOut_t small,
192 bool rows = true) const;
193
194 /// Uncompress matrix
195 ///
196 /// \param small input matrix
197 /// \retval normal uncompressed matrix
198 /// \param rows whether to uncompress rows and colums or only columns
199 void uncompressMatrix(matrixIn_t small, matrixOut_t normal,
200 bool rows = true) const;
201
202 /// \}
203
204 /// Set maximal number of iterations
205 void maxIterations(size_type iterations);
206 /// Get maximal number of iterations in config projector
207 size_type maxIterations() const;
208
209 /// Set error threshold
210 void errorThreshold(const value_type& threshold);
211 /// Get errorimal number of threshold in config projector
212 value_type errorThreshold() const;
213
214 value_type residualError() const;
215
216 const value_type& sigma() const;
217
218 /// \name Right hand side of equalities - inequalities
219 /// @{
220
221 /// Set the right hand side from a configuration
222 ///
223 /// in such a way that the configuration satisfies the numerical
224 /// constraints
225 /// \param config the input configuration.
226 /// \return the right hand side
227 ///
228 /// \warning Only values of the right hand side corresponding to
229 /// \link Equality "equality constraints" \endlink are set. As a
230 /// result, the input configuration may not satisfy the other constraints.
231 /// The rationale is the following. Equality constraints define a
232 /// foliation of the configuration space. Leaves of the foliation are
233 /// defined by the value of the right hand side of the equality
234 /// constraints. This method is mainly used in manipulation planning
235 /// to retrieve the leaf a configuration lies on.
236 vector_t rightHandSideFromConfig(ConfigurationIn_t config);
237
238 /// Same as rightHandSideFromConfig(ConfigurationIn_t) but only for
239 /// the specified constraints::Implicit
240 void rightHandSideFromConfig(const constraints::ImplicitPtr_t& nm,
241 ConfigurationIn_t config);
242
243 /// Set the level set parameter.
244 /// \param param the level set parameter.
245 void rightHandSide(const vector_t& param);
246
247 /// Same as rightHandSide(vectorIn_t) but only for
248 /// the specified constraints::Implicit
249 void rightHandSide(const constraints::ImplicitPtr_t& nm, vectorIn_t rhs);
250
251 /// Get the level set parameter.
252 /// \return the parameter.
253 vector_t rightHandSide() const;
254
255 /// Update the right hand side using Implicit::rightHandSideAt
256 void rightHandSideAt(const value_type& s);
257
258 /// @}
259
260 /// Check whether a configuration statisfies the constraint.
261 /// \param config the configuration to test,
262 virtual bool isSatisfied(ConfigurationIn_t config);
263
264 /// Check whether a configuration statisfies the constraint.
265 /// \param config the configuration to test,
266 /// \param errorThreshold the threshold the norm of each constraint should
267 /// stay below.
268 virtual bool isSatisfied(ConfigurationIn_t config, value_type errorThreshold);
269
270 /// Check whether a configuration statisfies the constraint.
271 ///
272 /// \param config the configuration to check
273 /// \retval error concatenation of differences between value of numerical
274 /// constraints and
275 /// right hand side.
276 virtual bool isSatisfied(ConfigurationIn_t config, vector_t& error);
277
278 /// Get the statistics
279 ::hpp::statistics::SuccessStatistics& statistics() { return statistics_; }
280
281 /// Get the numerical constraints of the config-projector (and so of the
282 /// Constraint Set)
283 const NumericalConstraints_t& numericalConstraints() const;
284
285 const BySubstitution& solver() const { return *solver_; }
286
287 9131 BySubstitution& solver() { return *solver_; }
288
289 /// Set the line search type.
290 64 void lineSearchType(LineSearchType ls) { lineSearchType_ = ls; }
291
292 /// Get the line search type.
293 LineSearchType lineSearchType() const { return lineSearchType_; }
294
295 static void defaultLineSearch(LineSearchType ls);
296
297 protected:
298 /// Constructor
299 /// \param robot robot the constraint applies to.
300 /// \param errorThreshold norm of the value of the constraint under which
301 /// the constraint is considered satified,
302 /// \param maxIterations maximal number of iteration in the resolution of
303 /// the constraint.
304 ConfigProjector(const DevicePtr_t& robot, const std::string& name,
305 value_type errorThreshold, size_type maxIterations);
306 /// Copy constructor
307 ConfigProjector(const ConfigProjector& cp);
308
309 /// Store weak pointer to itself
310 3706013 void init(const ConfigProjectorPtr_t& self) {
311 3706013 Constraint::init(self);
312 3706013 weak_ = self;
313 3706013 }
314 /// Numerically solve constraint
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
334 /// \}
335 } // namespace core
336 } // namespace hpp
337 #endif // HPP_CORE_CONFIG_PROJECTOR_HH
338