GCC Code Coverage Report

Directory: ./
File: include/hpp/constraints/differentiable-function.hh
Date: 2025-05-05 12:19:30
Exec Total Coverage
Lines: 35 37 94.6%
Branches: 21 38 55.3%
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_CONSTRAINTS_DIFFERENTIABLE_FUNCTION_HH
31 #define HPP_CONSTRAINTS_DIFFERENTIABLE_FUNCTION_HH
32
33 #include <hpp/constraints/config.hh>
34 #include <hpp/constraints/fwd.hh>
35 #include <hpp/pinocchio/liegroup-element.hh>
36 #include <hpp/util/debug.hh>
37 #include <hpp/util/serialization-fwd.hh>
38
39 namespace hpp {
40 namespace constraints {
41
42 /// \addtogroup constraints
43 /// \{
44
45 /// Differentiable function from a \link hpp::pinocchio::LiegroupSpace Lie
46 /// group\endlink, for instance the configuration space of a robot
47 /// (hpp::pinocchio::Device) to a another \link
48 /// hpp::pinocchio::LiegroupSpace Lie group\endlink.
49 ///
50 /// Note that the input Lie group is only represented by the sizes
51 /// of the elements and of the velocities: methods \link
52 /// DifferentiableFunction::inputSize inputSize\endlink and \link
53 /// DifferentiableFunction::inputDerivativeSize
54 /// inputDerivativeSize\endlink
55 ///
56 /// The output space can be accessed by method \link
57 /// DifferentiableFunction::outputSpace outputSpace\endlink.
58 ///
59 /// The value of the function for a given input can be accessed by method
60 /// \link DifferentiableFunction::value value \endlink.
61 /// The Jacobian of the function for a given input can be accessed by
62 /// method \link DifferentiableFunction::jacobian jacobian \endlink.
63 class HPP_CONSTRAINTS_DLLAPI DifferentiableFunction {
64 public:
65 6670 virtual ~DifferentiableFunction() {}
66
67 /// Evaluate the function at a given parameter.
68 ///
69 /// \note parameters should be of the correct size.
70 406 LiegroupElement operator()(vectorIn_t argument) const {
71
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 406 times.
 406 assert(argument.size() == inputSize());
72 406 LiegroupElement result(outputSpace_);
73
3/6
✓ Branch 1 taken 406 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 406 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 406 times.
✗ Branch 8 not taken.
 406 impl_compute(result, argument);
74 406 return result;
75 }
76 /// Evaluate the function at a given parameter.
77 ///
78 /// \note parameters should be of the correct size.
79 56219 void value(LiegroupElementRef result, vectorIn_t argument) const {
80
1/2
✗ Branch 4 not taken.
✓ Branch 5 taken 56219 times.
 56219 assert(result.space()->nq() == outputSize());
81
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 56219 times.
 56219 assert(argument.size() == inputSize());
82
2/4
✓ Branch 2 taken 56219 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 56219 times.
✗ Branch 6 not taken.
 56219 impl_compute(result, argument);
83 56219 }
84 /// Computes the jacobian.
85 ///
86 /// \retval jacobian jacobian will be stored in this argument
87 /// \param argument point at which the jacobian will be computed
88 26019 void jacobian(matrixOut_t jacobian, vectorIn_t argument) const {
89
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 26019 times.
 26019 assert(argument.size() == inputSize());
90
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 26019 times.
 26019 assert(jacobian.rows() == outputDerivativeSize());
91
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 26019 times.
 26019 assert(jacobian.cols() == inputDerivativeSize());
92
2/4
✓ Branch 2 taken 26019 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 26019 times.
✗ Branch 6 not taken.
 26019 impl_jacobian(jacobian, argument);
93 26019 }
94
95 /// Returns a vector of booleans that indicates whether the corresponding
96 /// configuration parameter influences this constraints.
97 1056 const ArrayXb& activeParameters() const { return activeParameters_; }
98
99 /// Returns a vector of booleans that indicates whether the corresponding
100 /// velocity parameter influences this constraints.
101 2258 const ArrayXb& activeDerivativeParameters() const {
102 2258 return activeDerivativeParameters_;
103 }
104
105 /// Get dimension of input vector
106 84536 size_type inputSize() const { return inputSize_; }
107 /// Get dimension of input derivative vector
108 ///
109 /// The dimension of configuration vectors might differ from the dimension
110 /// of velocity vectors since some joints are represented by non minimal
111 /// size vectors: e.g. quaternion for SO(3)
112 36093 size_type inputDerivativeSize() const { return inputDerivativeSize_; }
113 /// Get output space
114 75191 LiegroupSpacePtr_t outputSpace() const { return outputSpace_; }
115 /// Get dimension of output vector
116 120474 size_type outputSize() const { return outputSpace_->nq(); }
117 /// Get dimension of output derivative vector
118 126340 size_type outputDerivativeSize() const { return outputSpace_->nv(); }
119 /// \brief Get function name.
120 ///
121 /// \return Function name.
122 196 const std::string& name() const { return name_; }
123
124 /// Display object in a stream
125 virtual std::ostream& print(std::ostream& o) const;
126
127 std::string context() const { return context_; }
128
129 void context(const std::string& c) { context_ = c; }
130
131 /// Approximate the jacobian using forward finite difference.
132 /// \retval jacobian jacobian will be stored in this argument
133 /// \param arg point at which the jacobian will be computed
134 /// \param robot use to add configuration and velocities. If set to NULL,
135 /// the configuration space is considered a vector space.
136 /// \param eps refers to \f$\epsilon\f$ in
137 /// http://en.wikipedia.org/wiki/Numerical_differentiation
138 /// Evaluate the function (x.size() + 1) times but less precise the
139 /// finiteDifferenceCentral
140 void finiteDifferenceForward(
141 matrixOut_t jacobian, vectorIn_t arg, DevicePtr_t robot = DevicePtr_t(),
142 value_type eps =
143 std::sqrt(Eigen::NumTraits<value_type>::epsilon())) const;
144
145 /// Approximate the jacobian using forward finite difference.
146 /// \retval jacobian jacobian will be stored in this argument
147 /// \param arg point at which the jacobian will be computed
148 /// \param robot use to add configuration and velocities. If set to NULL,
149 /// the configuration space is considered a vector space.
150 /// \param eps refers to \f$\epsilon\f$ in
151 /// http://en.wikipedia.org/wiki/Numerical_differentiation
152 /// Evaluate the function 2*x.size() times but more precise the
153 /// finiteDifferenceForward
154 void finiteDifferenceCentral(
155 matrixOut_t jacobian, vectorIn_t arg, DevicePtr_t robot = DevicePtr_t(),
156 value_type eps =
157 std::sqrt(Eigen::NumTraits<value_type>::epsilon())) const;
158
159 bool operator==(DifferentiableFunction const& other) const;
160 bool operator!=(DifferentiableFunction const& b) const;
161
162 /// Return pair of joints the relative pose between which
163 /// modifies the function value if any
164 ///
165 /// This method is useful to know whether an instance of Implicit constrains
166 /// the relative pose between two joints.
167 /// \return the pair of joints involved, arranged in order of increasing
168 /// joint index, or a pair of empty shared pointers.
169 /// \note
170 /// \li if absolute pose (relative pose with respect to "universe"),
171 /// "universe" is returned as empty shared pointer
172 /// \li child class reimplementing this may require a valid "robot"
173 /// argument, which the constraints are applied on.
174 ✗ virtual std::pair<JointConstPtr_t, JointConstPtr_t> dependsOnRelPoseBetween(
175 DeviceConstPtr_t /*robot*/) const {
176 ✗ return std::pair<JointConstPtr_t, JointConstPtr_t>(nullptr, nullptr);
177 };
178
179 protected:
180 /// \brief Concrete class constructor should call this constructor.
181 ///
182 /// \param sizeInput dimension of the function input
183 /// \param sizeInputDerivative dimension of the function input derivative,
184 /// \param sizeOutput dimension of the output,
185 /// \param name function's name
186 DifferentiableFunction(size_type sizeInput, size_type sizeInputDerivative,
187 size_type sizeOutput,
188 std::string name = std::string());
189
190 /// \brief Concrete class constructor should call this constructor.
191 ///
192 /// \param sizeInput dimension of the function input
193 /// \param sizeInputDerivative dimension of the function input derivative,
194 /// \param outputSpace output space of the function.
195 /// \param name function name
196 DifferentiableFunction(size_type sizeInput, size_type sizeInputDerivative,
197 const LiegroupSpacePtr_t& outputSpace,
198 std::string name = std::string());
199
200 /// User implementation of function evaluation
201 virtual void impl_compute(LiegroupElementRef result,
202 vectorIn_t argument) const = 0;
203
204 virtual void impl_jacobian(matrixOut_t jacobian, vectorIn_t arg) const = 0;
205
206 29288 virtual bool isEqual(const DifferentiableFunction& other) const {
207
2/2
✓ Branch 1 taken 13 times.
✓ Branch 2 taken 29275 times.
 29288 if (name_ != other.name_) return false;
208
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 29275 times.
 29275 if (inputSize_ != other.inputSize_) return false;
209
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 29275 times.
 29275 if (inputDerivativeSize_ != other.inputDerivativeSize_) return false;
210
2/2
✓ Branch 3 taken 6 times.
✓ Branch 4 taken 29269 times.
 29275 if (*outputSpace_ != *(other.outputSpace_)) return false;
211
212 29269 return true;
213 }
214
215 /// Dimension of input vector.
216 size_type inputSize_;
217 /// Dimension of input derivative
218 size_type inputDerivativeSize_;
219 /// Dimension of output vector
220 LiegroupSpacePtr_t outputSpace_;
221
222 /// Initialized to true by this class. Child class are responsible for
223 /// updating it.
224 /// \sa activeParameters
225 ArrayXb activeParameters_;
226 /// Initialized to true by this class. Child class are responsible for
227 /// updating it.
228 /// \sa activeDerivativeParameters
229 ArrayXb activeDerivativeParameters_;
230
231 private:
232 std::string name_;
233 /// Context of creation of function
234 std::string context_;
235
236 friend class DifferentiableFunctionSet;
237
238 protected:
239
2/4
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 13 times.
✗ Branch 6 not taken.
 13 DifferentiableFunction() {}
240
241 private:
242 HPP_SERIALIZABLE();
243 }; // class DifferentiableFunction
244 31 inline std::ostream& operator<<(std::ostream& os,
245 const DifferentiableFunction& f) {
246 31 return f.print(os);
247 }
248 /// \}
249 } // namespace constraints
250 } // namespace hpp
251
252 /*
253 // This will override boost::shared_ptr 's equality operator
254 // between 2 DifferentiableFunctionPtr_t
255 namespace boost {
256 using namespace hpp::constraints;
257 typedef DifferentiableFunction T;
258 typedef shared_ptr<T> TPtr;
259
260 template<> inline bool operator==<T, T> (TPtr const & a, TPtr const & b)
261 BOOST_SP_NOEXCEPT
262 {
263 return *a == *b;
264 }
265
266 template<> inline bool operator!=<T, T> (TPtr const & a, TPtr const & b)
267 BOOST_SP_NOEXCEPT
268 {
269 return !(a == b);
270 }
271 }
272 */
273
274 #endif // HPP_CONSTRAINTS_DIFFERENTIABLE_FUNCTION_HH
275