GCC Code Coverage Report

Directory: ./
File: include/hpp/constraints/differentiable-function-set.hh
Date: 2025-05-05 12:19:30
Exec Total Coverage
Lines: 40 48 83.3%
Branches: 28 62 45.2%
Line Branch Exec Source
1 //
2 // Copyright (c) 2016 CNRS
3 // Authors: Joseph Mirabel
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_SET_HH
31 #define HPP_CONSTRAINTS_DIFFERENTIABLE_FUNCTION_SET_HH
32
33 #include <hpp/constraints/differentiable-function.hh>
34 #include <hpp/constraints/fwd.hh>
35
36 namespace hpp {
37 namespace constraints {
38 /// \addtogroup constraints
39 /// \{
40
41 /// Set of differentiable functions
42 ///
43 /// This class also handles selection of cols of the output matrix.
44 class HPP_CONSTRAINTS_DLLAPI DifferentiableFunctionSet
45 : public DifferentiableFunction {
46 public:
47 typedef std::vector<DifferentiableFunctionPtr_t> Functions_t;
48
49 /// Return a shared pointer to a new instance
50 ///
51 /// \param name the name of the constraints,
52 2072 static DifferentiableFunctionSetPtr_t create(const std::string& name) {
53
1/2
✓ Branch 2 taken 2072 times.
✗ Branch 3 not taken.
 2072 return DifferentiableFunctionSetPtr_t(new DifferentiableFunctionSet(name));
54 }
55
56 8288 virtual ~DifferentiableFunctionSet() {}
57
58 /// \name Function stack management
59 /// \{
60
61 /// Get the stack of functions
62 const Functions_t& functions() const { return functions_; }
63
64 1053 void add(const DifferentiableFunctionPtr_t& func) {
65
2/2
✓ Branch 1 taken 1040 times.
✓ Branch 2 taken 13 times.
 1053 if (functions_.empty()) {
66 1040 inputSize_ = func->inputSize();
67 1040 inputDerivativeSize_ = func->inputDerivativeSize();
68 1040 activeParameters_ = func->activeParameters();
69 1040 activeDerivativeParameters_ = func->activeDerivativeParameters();
70 } else {
71
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 13 times.
 13 assert(inputSize_ == func->inputSize());
72
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 13 times.
 13 assert(inputDerivativeSize_ == func->inputDerivativeSize());
73
74
1/2
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 13 activeParameters_ = activeParameters_ || func->activeParameters();
75 activeDerivativeParameters_ =
76
1/2
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 13 activeDerivativeParameters_ || func->activeDerivativeParameters();
77 }
78 1053 functions_.push_back(func);
79
2/4
✓ Branch 3 taken 1053 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1053 times.
✗ Branch 7 not taken.
 1053 result_.push_back(LiegroupElement(func->outputSpace()));
80
1/2
✓ Branch 5 taken 1053 times.
✗ Branch 6 not taken.
 1053 *outputSpace_ *= func->outputSpace();
81 1053 }
82
83 /// The output columns selection of other is not taken into account.
84 void merge(const DifferentiableFunctionSetPtr_t& other) {
85 const Functions_t& functions = other->functions();
86 for (Functions_t::const_iterator _f = functions.begin();
87 _f != functions.end(); ++_f)
88 add(*_f);
89 }
90
91 /// \}
92
93 std::ostream& print(std::ostream& os) const;
94
95 /// Constructor
96 ///
97 /// \param name the name of the constraints,
98 2072 DifferentiableFunctionSet(const std::string& name)
99
1/2
✓ Branch 2 taken 2072 times.
✗ Branch 3 not taken.
 2072 : DifferentiableFunction(0, 0, 0, name) {}
100
101 DifferentiableFunctionSet() : DifferentiableFunction(0, 0, 0, "Stack") {}
102
103 protected:
104 27592 void impl_compute(LiegroupElementRef result, ConfigurationIn_t arg) const {
105 27592 size_type row = 0;
106 27592 std::size_t i = 0;
107 27592 for (Functions_t::const_iterator _f = functions_.begin();
108
2/2
✓ Branch 2 taken 28715 times.
✓ Branch 3 taken 27592 times.
 56307 _f != functions_.end(); ++_f) {
109 28715 const DifferentiableFunction& f = **_f;
110
3/6
✓ Branch 1 taken 28715 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 28715 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 28715 times.
✗ Branch 9 not taken.
 28715 f.impl_compute(result_[i], arg);
111
2/4
✓ Branch 2 taken 28715 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 28715 times.
 28715 assert(hpp::pinocchio::checkNormalized(result_[i]));
112
2/4
✓ Branch 5 taken 28715 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 28715 times.
✗ Branch 9 not taken.
 28715 result.vector().segment(row, f.outputSize()) = result_[i].vector();
113 28715 row += f.outputSize();
114 28715 ++i;
115 }
116
2/4
✓ Branch 1 taken 27592 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 27592 times.
 27592 assert(hpp::pinocchio::checkNormalized(result));
117 27592 }
118 23744 void impl_jacobian(matrixOut_t jacobian, ConfigurationIn_t arg) const {
119 23744 size_type row = 0;
120 23744 for (Functions_t::const_iterator _f = functions_.begin();
121
2/2
✓ Branch 2 taken 24251 times.
✓ Branch 3 taken 23744 times.
 47995 _f != functions_.end(); ++_f) {
122 24251 const DifferentiableFunction& f = **_f;
123
4/8
✓ Branch 1 taken 24251 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 24251 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 24251 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 24251 times.
✗ Branch 12 not taken.
 24251 f.impl_jacobian(jacobian.middleRows(row, f.outputDerivativeSize()), arg);
124 24251 row += f.outputDerivativeSize();
125 }
126 23744 }
127
128 ✗ bool isEqual(const DifferentiableFunction& other) const {
129 const DifferentiableFunctionSet& castother =
130 ✗ dynamic_cast<const DifferentiableFunctionSet&>(other);
131 ✗ if (!DifferentiableFunction::isEqual(other)) return false;
132
133 ✗ if (functions_ != castother.functions_) return false;
134 ✗ if (result_.size() != castother.result_.size()) return false;
135 ✗ for (std::size_t i = 0; i < result_.size(); i++)
136 ✗ if (result_[i] != castother.result_[i]) return false;
137
138 ✗ return true;
139 }
140
141 private:
142 Functions_t functions_;
143 mutable std::vector<LiegroupElement> result_;
144 }; // class DifferentiableFunctionSet
145 /// \}
146 } // namespace constraints
147 } // namespace hpp
148
149 #endif // HPP_CONSTRAINTS_DIFFERENTIABLE_FUNCTION_SET_HH
150