GCC Code Coverage Report

Line	Branch	Exec	Source
1			// Copyright (c) 2017, Joseph Mirabel
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_EXPLICIT_CONSTRAINT_SET_HH
30			#define HPP_CONSTRAINTS_EXPLICIT_CONSTRAINT_SET_HH
31
32			#include <hpp/constraints/config.hh>
33			#include <hpp/constraints/differentiable-function-set.hh>
34			#include <hpp/constraints/fwd.hh>
35			#include <hpp/constraints/matrix-view.hh>
36			#include <vector>
37
38			namespace hpp {
39			namespace constraints {
40			/// \addtogroup solvers
41			/// \{
42
43			/**
44			Set of explicit constraints
45
46			This class combines compatible explicit constraints as
47			defined in the following paper published in Robotics Science and System
48			2018: https://hal.archives-ouvertes.fr/hal-01804774/file/paper.pdf,
49			Section II-B Definition 4.
50
51			An explicit constraint \f$E=(in,out,f)\f$ on a robot
52			configuration space \f$\mathcal{C}\f$ is defined by
53			\li a subset of input indices
54			\f$in\subset\{1,\cdots, \dim\mathcal{C}\}\f$,
55			\li a subset of output indices
56			\f$out\subset\{1,\cdots, \dim\mathcal{C}\}\f$,
57			\li a smooth mapping \f$f\f$ from \f$\mathbf{R}^{\|in\|}\f$ to
58			\f$\mathbf{R}^{\|out\|}\f$, satisfying the following properties: \li \f$in\cap out
59			= \emptyset\f$, \li for any \f$\mathbf{p}\in\mathcal{C}\f$, \f$\mathbf{q} =
60			E(\mathbf{p})\f$ is defined by \f{eqnarray}
61			&\mathbf{q}_{\bar{out}} = \mathbf{p}_{\bar{out}}\\
62			&\mathbf{q}_{out} = f (\mathbf{p}_{in}).
63			\f}
64
65			\note Right hand side.
66
67			For manipulation planning, it is useful to handle a parameterizable
68			right hand side \f$rhs\f$. The expression above thus becomes
69
70			\f{equation}
71			\mathbf{q}_{out} = f (\mathbf{p}_{in}) + rhs.
72			\f}
73
74			The right hand side may be set using the various methods
75			ExplicitConstraintSet::rightHandSide and
76			ExplicitConstraintSet::rightHandSideFromInput.
77
78			\note For some applications like manipulation planning, an
79			invertible function \f$ g \f$ (of known inverse \f$ g^{-1} \f$)
80			can be specified for each explicit constraint \f$E\f$. The above expression
81			then becomes:
82			\f{equation}
83			g(\mathbf{q}_{out}) = f(\mathbf{p}_{in}) + rhs
84			\f}
85
86			To add explicit constraints, use methods ExplicitConstraintSet::add. If the
87			constraint to add is not compatible with the previous one, this method
88			returns -1.
89
90			Method ExplicitConstraintSet::solve solves the explicit constraints.
91
92			The combination of compatible explicit constraints is an explicit
93			constraint. As such this class can be considered as an explicit constraint.
94
95			We will therefore use the following notation
96			\li \f$in\f$ for the set of indices of input variables,
97			\li \f$out\f$ for the set of indices of output variables.
98			**/
99			class HPP_CONSTRAINTS_DLLAPI ExplicitConstraintSet {
100			public:
101			typedef Eigen::RowBlockIndices RowBlockIndices;
102			typedef Eigen::ColBlockIndices ColBlockIndices;
103			typedef Eigen::MatrixBlockView<matrix_t, Eigen::Dynamic, Eigen::Dynamic,
104			false, false>
105			MatrixBlockView;
106
107			/// \name Resolution
108			/// \{
109
110			/// Solve the explicit constraints
111			/// \param arg input configuration,
112			/// \retval arg output configuration satisfying the explicit
113			/// constraints:
114			/// \f$\mathbf{q}_{out} = g^{-1}
115			/// \left(f(\mathbf{q}_{in}) + rhs\right)\f$
116			bool solve(vectorOut_t arg) const;
117
118			/// Size of error vector
119			/// \note that this size may differ from size of \link
120			/// ExplicitConstraintSet::outDers outDers output\endlink,
121		2012	size_type errorSize() const { return errorSize_; }
122
123			/// Whether input vector satisfies the constraints of the solver
124			/// \param arg input vector
125			/// \param errorThreshold threshold to compare against the norms of
126			/// the constraint errors. Default value is accessible via
127			/// methods errorThreshold.
128			bool isSatisfied(vectorIn_t arg, value_type errorThreshold = -1) const;
129
130			/// Whether input vector satisfies the constraints of the solver
131			/// \param arg input vector
132			/// \retval error the constraint errors
133			/// \param errorThreshold threshold to compare against the norms of
134			/// the constraint errors. Default value is accessible via
135			/// methods errorThreshold.
136			bool isSatisfied(vectorIn_t arg, vectorOut_t error,
137			value_type errorThreshold = -1) const;
138
139			/// Whether a constraint is satisfied for an input vector
140			///
141			/// \param constraint, the constraint in the solver,
142			/// \param arg the input vector
143			/// \retval error the error of the constraint.
144			/// \retval constraintFound whether the constraint belongs to the
145			/// solver,
146			/// \return true if constraint belongs to solver and error is below
147			/// the threshold, false otherwise.
148			bool isConstraintSatisfied(const ImplicitPtr_t& constraint, vectorIn_t arg,
149			vectorOut_t error, bool& constraintFound) const;
150			/// \}
151
152			/// \name Construction of the problem
153			/// \{
154
155			/// Attempt to add an explicit constraint
156			///
157			/// \param constraint explicit constraint
158			/// \return the index of the function if the function was added,
159			/// -1 otherwise.
160			/// \note A function can be added iff it is compatible with the
161			/// previously added functions.
162			size_type add(const ExplicitPtr_t& constraint);
163
164			/// Check whether an explicit numerical constraint has been added
165			/// \param numericalConstraint explicit numerical constraint
166			/// \return true if the constraint is in the set.
167			/// \note Comparison between constraints is performed by
168			/// function names. This means that two constraints with the
169			/// same function names are considered as equal.
170			bool contains(const ExplicitPtr_t& numericalConstraint) const;
171
172			/// Constructor
173			///
174			/// \param space Lie group on which constraints are defined.
175		7437	ExplicitConstraintSet(const LiegroupSpacePtr_t& space)
176		7437	: configSpace_(space),
177	1/2 ✓ Branch 1 taken 7437 times. ✗ Branch 2 not taken.	7437	inArgs_(),
178	1/2 ✓ Branch 1 taken 7437 times. ✗ Branch 2 not taken.	7437	notOutArgs_(),
179	1/2 ✓ Branch 1 taken 7437 times. ✗ Branch 2 not taken.	7437	inDers_(),
180	1/2 ✓ Branch 1 taken 7437 times. ✗ Branch 2 not taken.	7437	notOutDers_(),
181	1/2 ✓ Branch 1 taken 7437 times. ✗ Branch 2 not taken.	7437	outArgs_(),
182	1/2 ✓ Branch 1 taken 7437 times. ✗ Branch 2 not taken.	7437	outDers_(),
183	2/4 ✓ Branch 3 taken 7437 times. ✗ Branch 4 not taken. ✓ Branch 6 taken 7437 times. ✗ Branch 7 not taken.	7437	argFunction_(Eigen::VectorXi::Constant(space->nq(), -1)),
184	2/4 ✓ Branch 3 taken 7437 times. ✗ Branch 4 not taken. ✓ Branch 6 taken 7437 times. ✗ Branch 7 not taken.	7437	derFunction_(Eigen::VectorXi::Constant(space->nv(), -1)),
185		7437	errorThreshold_(Eigen::NumTraits<value_type>::epsilon()),
186		7437	errorSize_(0)
187			// , Jg (nv, nv)
188			,
189	1/2 ✓ Branch 3 taken 7437 times. ✗ Branch 4 not taken.	7437	arg_(space->nq()),
190	1/2 ✓ Branch 3 taken 7437 times. ✗ Branch 4 not taken.	7437	diff_(space->nv()),
191	2/4 ✓ Branch 1 taken 7437 times. ✗ Branch 2 not taken. ✓ Branch 6 taken 7437 times. ✗ Branch 7 not taken.	14874	diffSmall_() {
192	1/2 ✓ Branch 3 taken 7437 times. ✗ Branch 4 not taken.	7437	notOutArgs_.addRow(0, space->nq());
193	1/2 ✓ Branch 3 taken 7437 times. ✗ Branch 4 not taken.	7437	notOutDers_.addCol(0, space->nv());
194		7437	}
195
196			/// \}
197
198			/// \name Parameters
199			/// \{
200
201			/// Set error threshold
202		1015	void errorThreshold(const value_type& threshold) {
203		1015	errorThreshold_ = threshold;
204		1015	}
205			/// Get error threshold
206			value_type errorThreshold() const { return errorThreshold_; }
207			/// Get error threshold
208			value_type squaredErrorThreshold() const {
209			return errorThreshold_ * errorThreshold_;
210			}
211
212			/// \}
213
214			/// \name Input and outputs
215			/// \{
216
217			/// Set \f$in\f$ of input configuration variables
218		9	const RowBlockIndices& inArgs() const { return inArgs_; }
219
220			/// Set of input velocity variables
221		13397	const ColBlockIndices& inDers() const { return inDers_; }
222
223			/// Set \f$i\overline{n\cup ou}t\f$ of other configuration variables
224			///
225			/// Configuration variables that are not output variables
226		7	const RowBlockIndices& notOutArgs() const { return notOutArgs_; }
227
228			/// Set \f$i\overline{n\cup ou}t\f$ of other velocity variables
229			///
230			/// Velocity variables that are not output variables
231		30	const ColBlockIndices& notOutDers() const { return notOutDers_; }
232
233			/// Same as \ref inArgs
234			ColBlockIndices activeParameters() const;
235
236			/// Same as \ref inDers
237			const ColBlockIndices& activeDerivativeParameters() const;
238
239			/// Returns a matrix of integer whose:
240			/// - rows correspond to functions
241			/// - cols correspond to DoF
242			/// - values correspond to the dependency degree of a function wrt to
243			/// a DoF
244		3	const Eigen::MatrixXi& inOutDependencies() const {
245		3	return inOutDependencies_;
246			}
247
248			/// Same as \ref inOutDependencies except that cols correpond to DoFs.
249			Eigen::MatrixXi inOutDofDependencies() const;
250
251		2	const Eigen::VectorXi& derFunction() const { return derFunction_; }
252
253			/// Set \f$out\f$ of output configuration variables
254			/// \return the set of intervals corresponding the the configuration
255			/// variables that are ouputs of the combination of explicit
256			/// constraints.
257		9	const RowBlockIndices& outArgs() const { return outArgs_; }
258
259			/// Set of output velocity variables
260			/// \return the set of intervals corresponding the the velocity
261			/// variables that are ouputs of the combination of explicit
262			/// constraints.
263		188	const RowBlockIndices& outDers() const { return outDers_; }
264
265			/// The Lie group on which constraints are defined
266		1	LiegroupSpacePtr_t configSpace() const { return configSpace_; }
267
268			/// The number of variables
269		1	std::size_t nq() const { return configSpace_->nq(); }
270
271			/// The number of derivative variables
272		1059	std::size_t nv() const { return configSpace_->nv(); }
273
274			/// \}
275
276			/// Return Jacobian matrix of output variable wrt not output variables
277			///
278			/// \retval jacobian Jacobian matrix of the mapping from non output
279			/// variables to output variables. The columns of this matrix
280			/// corresponding to variables \f$in\f$ are filled with the
281			/// Jacobian of \f$f\f$:
282			/// \f{equation}
283			/// \frac{\partial f}{\partial \mathbf{q}_{in}}
284			/// (\mathbf{q}_{in}).
285			/// \f}
286			/// The columns corresponding to variables
287			/// \f$i\overline{n\cup ou}t\f$ are set to 0.
288		187	inline MatrixBlockView jacobianNotOutToOut(matrix_t& jacobian) const {
289		187	return MatrixBlockView(jacobian, outDers_.nbIndices(), outDers_.indices(),
290		374	notOutDers_.nbIndices(), notOutDers_.indices());
291			}
292
293			/** Compute the Jacobian of the explicit constraint resolution
294
295			\param q input configuration
296			\param jacobian square Jacobian matrix of same size as velocity
297			i.e. given by \ref nv method.
298
299			The result is the Jacobian of the explicit constraint set considered
300			as a projector that maps to any \f$\mathbf{p}\in\mathcal{C}\f$,
301			\f$\mathbf{q} = E(\mathbf{p})\f$ defined by
302			\f{eqnarray}
303			\mathbf{q}_{\bar{out}} &=& \mathbf{p}_{\bar{out}} \\
304			\mathbf{q}_{out} &=& f (\mathbf{p}_{in}) + rhs
305			\f}
306
307			\warning it is assumed solve(q) has been called before.
308			*/
309			void jacobian(matrixOut_t jacobian, vectorIn_t q) const;
310
311			/// \name Right hand side accessors
312			/// \{
313
314			/// Compute right hand side of constraints using input configuration.
315			///
316			/// \param p vector in \f$\mathcal{C}\f$.
317			///
318			/// For each explicit constraint \f$E=(in,out,f)\f$, compute the right
319			/// hand side as follows:
320			/// \f{equation}
321			/// rhs = f (\mathbf{q}),
322			/// \f}
323			/// where in general
324			///\f{equation}
325			/// f(\mathbf{q}) = \mathbf{p}_{out} - f(\mathbf{q}_{in),
326			/// \f}
327			/// in such a way that all \f$\mathbf{q}\f$ satisfies the explicit
328			/// constraint.
329			/// \note For hpp::constraints::explicit_::RelativePose, the implicit
330			/// formulation does not derive from the explicit one. The
331			/// right hand side considered is the right hand side of the
332			/// implicit formulation.
333			vector_t rightHandSideFromInput(vectorIn_t p);
334
335			/// Compute right hand side of constraint using input configuration.
336			///
337			/// \param constraint explicit constraint,
338			/// \param p vector in \f$\mathcal{C}\f$.
339			///
340			/// Let \f$E=(in,out,f)\f$ be the explicit constraint, compute the right
341			/// hand side as follows:
342			/// \f{equation}
343			/// rhs = f (\mathbf{q}),
344			/// \f}
345			/// where in general
346			///\f{equation}
347			/// f(\mathbf{q}) = \mathbf{p}_{out} - f(\mathbf{q}_{in),
348			/// \f}
349			/// in such a way that all \f$\mathbf{q}\f$ satisfies the explicit
350			/// constraint.
351			/// \note For hpp::constraints::explicit_::RelativePose, the implicit
352			/// formulation does not derive from the explicit one. The
353			/// right hand side considered is the right hand side of the
354			/// implicit formulation.
355			bool rightHandSideFromInput(const ExplicitPtr_t& constraint, vectorIn_t p);
356
357			/// Compute right hand side of constraint using input configuration.
358			///
359			/// \param i index of the explicit constraint,
360			/// \param p vector in \f$\mathcal{C}\f$.
361			///
362			/// Let \f$E=(in,out,f)\f$ be the explicit constraint, compute the right
363			/// hand side as follows:
364			/// \f{equation}
365			/// rhs = f (\mathbf{q}),
366			/// \f}
367			/// where in general
368			///\f{equation}
369			/// f(\mathbf{q}) = \mathbf{p}_{out} - f(\mathbf{q}_{in),
370			/// \f}
371			/// in such a way that all \f$\mathbf{q}\f$ satisfies the explicit
372			/// constraint.
373			/// \note For hpp::constraints::explicit_::RelativePose, the implicit
374			/// formulation does not derive from the explicit one. The
375			/// right hand side considered is the right hand side of the
376			/// implicit formulation.
377			void rightHandSideFromInput(const size_type& i, vectorIn_t p);
378
379			/// Set the right hand sides of the explicit constraints.
380			///
381			/// \param rhs the right hand side.
382			///
383			/// The components of rhs are dispatched to the right hand sides of the
384			/// explicit constraints in the order they are added.
385			void rightHandSide(vectorIn_t rhs);
386
387			/// Set the right hand side for a given explicit constraint
388			///
389			/// \param constraint the explicit constraint,
390			/// \param rhs right hand side.
391			bool rightHandSide(const ExplicitPtr_t& constraint, vectorIn_t rhs);
392
393			/// Get the right hand side for a given explicit constraint
394			///
395			/// \param constraint the explicit constraint,
396			/// \param rhs right hand side.
397			bool getRightHandSide(const ExplicitPtr_t& constraint, vectorOut_t rhs) const;
398
399			/// Set the right hand side for a given explicit constraint
400			///
401			/// \param i order of the explicit constraint,
402			/// \param rhs right hand side.
403			void rightHandSide(const size_type& i, vectorIn_t rhs);
404
405			/// Get the right hand sides
406			/// \return the right hand sides of the explicit constraints stacked
407			/// into a vector
408			vector_t rightHandSide() const;
409
410			/// Get size of the right hand side
411			///
412			/// See documentation of classes Implicit and Explicit for details.
413			size_type rightHandSideSize() const;
414
415			/// \}
416
417			std::ostream& print(std::ostream& os) const;
418
419			private:
420			typedef std::vector<bool> Computed_t;
421
422			/// Compute output variables with respect to input variables
423			/// \param i index of explicit constraint,
424			/// \retval arg configuration of the system in which output variables
425			/// are set to their values.
426			void solveExplicitConstraint(const std::size_t& i, vectorOut_t arg) const;
427			/// Compute rows of Jacobian corresponding to output of function
428			///
429			/// \param i index of the explicit constraint,
430			/// \retval J Jacobian in which rows are computed
431			///
432			/// Let
433			/// \li E = (f, in, out) be the explicit constraint of index i,
434			/// \li E.jacobian be the Jacobian of f,
435			/// \li E.in the input velocity variables of the constraints,
436			/// \li E.out the output velocity variables of the constraints,
437			/// \li Jin the matrix composed of E.in rows of J,
438			/// \li Jout the matrix composed of E.out rows of J,
439			/// then,
440			/// Jout = E.jacobian * Jin
441			void computeJacobian(const std::size_t& i, matrixOut_t J) const;
442			void computeOrder(const std::size_t& iF, std::size_t& iOrder,
443			Computed_t& computed);
444
445			LiegroupSpacePtr_t configSpace_;
446
447			struct Data {
448			Data(const ExplicitPtr_t& constraint);
449			ExplicitPtr_t constraint;
450			RowBlockIndices equalityIndices;
451			LiegroupElement rhs_implicit;
452			// implicit formulation
453			mutable LiegroupElement h_value;
454			// explicit formulation
455			mutable vector_t qin, qout;
456			mutable LiegroupElement f_value, res_qout;
457			// jacobian of f
458			mutable matrix_t jacobian;
459			}; // struct Data
460
461			RowBlockIndices inArgs_, notOutArgs_;
462			ColBlockIndices inDers_, notOutDers_;
463			/// Output indices
464			RowBlockIndices outArgs_, outDers_;
465
466			Eigen::MatrixXi inOutDependencies_;
467
468			std::vector<Data> data_;
469			std::vector<std::size_t> computationOrder_;
470			/// For each configuration variable i, argFunction_[i] is the index in
471			/// data_ of the function that computes this configuration
472			/// variable.
473			/// -1 means that the configuration variable is not ouput of any
474			/// function in data_.
475			Eigen::VectorXi argFunction_, derFunction_;
476			value_type errorThreshold_;
477			size_type errorSize_;
478			// mutable matrix_t Jg;
479			mutable vector_t arg_, diff_, diffSmall_;
480
481			/// Constructor for serialization
482		✗	ExplicitConstraintSet()
483		✗	: inArgs_(),
484		✗	notOutArgs_(),
485		✗	inDers_(),
486		✗	notOutDers_(),
487		✗	outArgs_(),
488		✗	outDers_(),
489		✗	errorThreshold_(Eigen::NumTraits<value_type>::epsilon()),
490		✗	errorSize_(0) {}
491			/// Initialization for serialization
492		1	void init(const LiegroupSpacePtr_t& space) {
493		1	configSpace_ = space;
494	2/4 ✓ Branch 3 taken 1 times. ✗ Branch 4 not taken. ✓ Branch 6 taken 1 times. ✗ Branch 7 not taken.	1	argFunction_ = Eigen::VectorXi::Constant(space->nq(), -1);
495	2/4 ✓ Branch 3 taken 1 times. ✗ Branch 4 not taken. ✓ Branch 6 taken 1 times. ✗ Branch 7 not taken.	1	derFunction_ = Eigen::VectorXi::Constant(space->nv(), -1);
496		1	arg_.resize(space->nq());
497		1	diff_.resize(space->nv());
498
499	1/2 ✓ Branch 3 taken 1 times. ✗ Branch 4 not taken.	1	notOutArgs_.addRow(0, space->nq());
500	1/2 ✓ Branch 3 taken 1 times. ✗ Branch 4 not taken.	1	notOutDers_.addCol(0, space->nv());
501		1	}
502
503			friend class solver::BySubstitution;
504			}; // class ExplicitConstraintSet
505			/// \}
506			} // namespace constraints
507			} // namespace hpp
508
509			#endif // HPP_CONSTRAINTS_EXPLICIT_CONSTRAINT_SET_HH
510