GCC Code Coverage Report

Directory: ./
File: src/explicit.cc
Date: 2025-05-05 12:19:30
Exec Total Coverage
Lines: 78 98 79.6%
Branches: 76 174 43.7%
Line Branch Exec Source
1 // Copyright (c) 2015, LAAS-CNRS
2 // Authors: Florent Lamiraux
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 #include <boost/serialization/utility.hpp>
30 #include <boost/serialization/vector.hpp>
31 #include <boost/serialization/weak_ptr.hpp>
32 #include <hpp/constraints/differentiable-function.hh>
33 #include <hpp/constraints/explicit.hh>
34 #include <hpp/constraints/explicit/implicit-function.hh>
35 #include <hpp/constraints/matrix-view.hh>
36 #include <hpp/pinocchio/device.hh>
37 #include <hpp/util/serialization.hh>
38
39 namespace hpp {
40 namespace constraints {
41 typedef constraints::Implicit Implicit;
42 typedef constraints::ImplicitPtr_t ImplicitPtr_t;
43 ✗ void complement(size_type size, const segments_t& intervals,
44 segments_t& result) {
45 ✗ std::vector<bool> unionOfIntervals(size + 1, false);
46 ✗ for (segments_t::const_iterator it = intervals.begin(); it != intervals.end();
47 ✗ ++it) {
48 ✗ for (size_type i = it->first; i < it->first + it->second; ++i) {
49 ✗ unionOfIntervals[i] = true;
50 }
51 }
52 ✗ unionOfIntervals[size] = true;
53 ✗ unsigned int state = 0;
54 ✗ segment_t interval;
55 ✗ for (size_type i = 0; i <= (size_type)unionOfIntervals.size(); ++i) {
56 ✗ if ((state == 0) && (unionOfIntervals[i] == false)) {
57 // start a new interval
58 ✗ state = 1;
59 ✗ interval.first = i;
60 ✗ } else if ((state == 1) && unionOfIntervals[i] == true) {
61 // finish current interval
62 ✗ state = 0;
63 ✗ interval.second = i - interval.first;
64 ✗ result.push_back(interval);
65 }
66 }
67 }
68
69 47 inline ComparisonTypes_t defaultCompTypes(const segments_t& outputVelocity,
70 const ComparisonTypes_t& comp) {
71
1/2
✓ Branch 1 taken 47 times.
✗ Branch 2 not taken.
 47 if (comp.size() == 0) {
72 47 size_type n = Eigen::BlockIndex::cardinal(outputVelocity);
73
2/4
✓ Branch 0 taken 47 times.
✗ Branch 1 not taken.
✓ Branch 4 taken 47 times.
✗ Branch 5 not taken.
 94 if (n > 0) return ComparisonTypes_t(n, constraints::EqualToZero);
74 }
75 ✗ return comp;
76 }
77
78 47 ExplicitPtr_t Explicit::create(const LiegroupSpacePtr_t& configSpace,
79 const DifferentiableFunctionPtr_t& function,
80 const segments_t& inputConf,
81 const segments_t& outputConf,
82 const segments_t& inputVelocity,
83 const segments_t& outputVelocity,
84 const ComparisonTypes_t& comp,
85 std::vector<bool> mask) {
86
1/2
✓ Branch 1 taken 47 times.
✗ Branch 2 not taken.
 47 if (mask.empty()) {
87
2/4
✓ Branch 2 taken 47 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 47 times.
✗ Branch 6 not taken.
 47 mask = std::vector<bool>(Eigen::BlockIndex::cardinal(outputVelocity), true);
88 }
89 Explicit* ptr = new Explicit(configSpace, function, inputConf, outputConf,
90 inputVelocity, outputVelocity,
91
4/8
✓ Branch 1 taken 47 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 47 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 47 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 47 times.
✗ Branch 11 not taken.
 47 defaultCompTypes(outputVelocity, comp), mask);
92
1/2
✓ Branch 1 taken 47 times.
✗ Branch 2 not taken.
 47 ExplicitPtr_t shPtr(ptr);
93 47 ExplicitWkPtr_t wkPtr(shPtr);
94
1/2
✓ Branch 1 taken 47 times.
✗ Branch 2 not taken.
 47 ptr->init(wkPtr);
95 94 return shPtr;
96 47 }
97
98 7 ExplicitPtr_t Explicit::createCopy(const ExplicitPtr_t& other) {
99
2/4
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
 7 Explicit* ptr = new Explicit(*other);
100
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 ExplicitPtr_t shPtr(ptr);
101 7 ExplicitWkPtr_t wkPtr(shPtr);
102
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 ptr->init(wkPtr);
103 14 return shPtr;
104 7 }
105
106 516 void Explicit::outputValue(LiegroupElementRef result, vectorIn_t qin,
107 LiegroupElementConstRef rhs) const {
108
3/6
✓ Branch 3 taken 516 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 516 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 516 times.
✗ Branch 10 not taken.
 516 explicitFunction()->value(result, qin);
109
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 516 times.
 516 assert(rhs.space()->isVectorSpace());
110
1/2
✓ Branch 3 taken 516 times.
✗ Branch 4 not taken.
 516 result += rhs.vector();
111 516 }
112
113 265 void Explicit::jacobianOutputValue(vectorIn_t qin,
114 LiegroupElementConstRef g_value,
115 LiegroupElementConstRef rhs,
116 matrixOut_t jacobian) const {
117
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 265 times.
 265 assert(rhs.space()->isVectorSpace());
118
3/6
✓ Branch 3 taken 265 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 265 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 265 times.
✗ Branch 10 not taken.
 265 explicitFunction()->jacobian(jacobian, qin);
119
3/4
✓ Branch 3 taken 265 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 2 times.
✓ Branch 6 taken 263 times.
 265 if (!rhs.vector().isZero()) {
120 4 explicitFunction()
121 4 ->outputSpace()
122
4/8
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2 times.
✗ Branch 13 not taken.
 2 ->dIntegrate_dq<pinocchio::DerivativeTimesInput>(g_value, rhs.vector(),
123 jacobian);
124 }
125 265 }
126
127 120 bool Explicit::isEqual(const Implicit& other, bool swapAndTest) const {
128
6/6
✓ Branch 0 taken 60 times.
✓ Branch 1 taken 60 times.
✓ Branch 3 taken 30 times.
✓ Branch 4 taken 30 times.
✓ Branch 5 taken 30 times.
✓ Branch 6 taken 90 times.
 120 if (swapAndTest && !Implicit::isEqual(other, swapAndTest)) return false;
129 try {
130
1/2
✓ Branch 0 taken 90 times.
✗ Branch 1 not taken.
 90 const Explicit& castother = dynamic_cast<const Explicit&>(other);
131
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 90 times.
 90 if (inputToOutput_ != castother.inputToOutput_) return false;
132
2/4
✓ Branch 1 taken 90 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 90 times.
 90 if (inputConf_ != castother.inputConf_) return false;
133
2/4
✓ Branch 1 taken 90 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 90 times.
 90 if (outputConf_ != castother.outputConf_) return false;
134
2/4
✓ Branch 1 taken 90 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 90 times.
 90 if (inputVelocity_ != castother.inputVelocity_) return false;
135
2/4
✓ Branch 1 taken 90 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 90 times.
 90 if (outputVelocity_ != castother.outputVelocity_) return false;
136
3/4
✓ Branch 0 taken 30 times.
✓ Branch 1 taken 60 times.
✓ Branch 3 taken 30 times.
✗ Branch 4 not taken.
 90 if (swapAndTest) return castother.isEqual(*this, false);
137 60 return true;
138 ✗ } catch (const std::bad_cast& err) {
139 ✗ return false;
140 }
141 }
142
143
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 ImplicitPtr_t Explicit::copy() const { return createCopy(weak_.lock()); }
144
145 53 Explicit::Explicit(const LiegroupSpacePtr_t& configSpace,
146 const DifferentiableFunctionPtr_t& explicitFunction,
147 const segments_t& inputConf, const segments_t& outputConf,
148 const segments_t& inputVelocity,
149 const segments_t& outputVelocity,
150 53 const ComparisonTypes_t& comp, std::vector<bool> mask)
151
1/2
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
 53 : Implicit(explicit_::ImplicitFunction::create(
152 configSpace, explicitFunction, inputConf, outputConf,
153 inputVelocity, outputVelocity),
154 comp, mask),
155 53 inputToOutput_(explicitFunction),
156
1/2
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
 53 inputConf_(inputConf),
157
1/2
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
 53 outputConf_(outputConf),
158
1/2
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
 53 inputVelocity_(inputVelocity),
159
3/6
✓ Branch 2 taken 53 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 53 times.
✗ Branch 7 not taken.
✓ Branch 14 taken 53 times.
✗ Branch 15 not taken.
 159 outputVelocity_(outputVelocity) {}
160
161 9 Explicit::Explicit(const DifferentiableFunctionPtr_t& implicitFunction,
162 const DifferentiableFunctionPtr_t& explicitFunction,
163 const segments_t& inputConf, const segments_t& outputConf,
164 const segments_t& inputVelocity,
165 const segments_t& outputVelocity,
166 9 const ComparisonTypes_t& comp, std::vector<bool> mask)
167 : Implicit(implicitFunction, comp, mask),
168 9 inputToOutput_(explicitFunction),
169
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 inputConf_(inputConf),
170
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 outputConf_(outputConf),
171
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 inputVelocity_(inputVelocity),
172
3/6
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 9 times.
✗ Branch 12 not taken.
 18 outputVelocity_(outputVelocity) {}
173
174 12 Explicit::Explicit(const Explicit& other)
175 : Implicit(other),
176 12 inputToOutput_(other.inputToOutput_),
177
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 inputConf_(other.inputConf_),
178
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 outputConf_(other.outputConf_),
179
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 inputVelocity_(other.inputVelocity_),
180
1/2
✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
 24 outputVelocity_(other.outputVelocity_) {}
181 74 void Explicit::init(const ExplicitWkPtr_t& weak) {
182 74 Implicit::init(weak);
183 74 weak_ = weak;
184 74 }
185
186 template <class Archive>
187 4 void Explicit::serialize(Archive& ar, const unsigned int version) {
188 (void)version;
189
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 ar& boost::serialization::make_nvp(
190 4 "base", boost::serialization::base_object<Implicit>(*this));
191
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 4 ar& BOOST_SERIALIZATION_NVP(inputToOutput_);
192
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 4 ar& BOOST_SERIALIZATION_NVP(inputConf_);
193
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 4 ar& BOOST_SERIALIZATION_NVP(outputConf_);
194
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 4 ar& BOOST_SERIALIZATION_NVP(inputVelocity_);
195
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 4 ar& BOOST_SERIALIZATION_NVP(outputVelocity_);
196
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 4 ar& BOOST_SERIALIZATION_NVP(weak_);
197 }
198
199 HPP_SERIALIZATION_IMPLEMENT(Explicit);
200 } // namespace constraints
201 } // namespace hpp
202
203 BOOST_CLASS_EXPORT_IMPLEMENT(hpp::constraints::Explicit)
204