GCC Code Coverage Report

Directory: ./
File: src/implicit.cc
Date: 2025-05-05 12:19:30
Exec Total Coverage
Lines: 126 143 88.1%
Branches: 101 226 44.7%
Line Branch Exec Source
1 // Copyright (c) 2015, LAAS-CNRS
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 #include "hpp/constraints/implicit.hh"
30
31 #include <boost/serialization/export.hpp>
32 #include <boost/serialization/vector.hpp>
33 #include <boost/serialization/weak_ptr.hpp>
34 #include <hpp/constraints/differentiable-function.hh>
35 #include <hpp/util/serialization.hh>
36 #include <pinocchio/serialization/eigen.hpp>
37
38 namespace hpp {
39 namespace constraints {
40 1165 bool Implicit::operator==(const Implicit& other) const {
41 1165 bool res = isEqual(other, true);
42 1165 return res;
43 }
44 9604 size_type computeParameterSize(const ComparisonTypes_t& comp) {
45 9604 size_type size = 0;
46
2/2
✓ Branch 1 taken 48716 times.
✓ Branch 2 taken 9604 times.
 58320 for (std::size_t i = 0; i < comp.size(); ++i) {
47
2/2
✓ Branch 1 taken 25304 times.
✓ Branch 2 taken 23412 times.
 48716 if (comp[i] == Equality) {
48 25304 ++size;
49 }
50 }
51 9604 return size;
52 }
53
54 ✗ void Implicit::rightHandSideFunction(const DifferentiableFunctionPtr_t& rhsF) {
55 ✗ if (rhsF) {
56 ✗ if (rhsF->inputSize() != 1 || rhsF->inputDerivativeSize() != 1) {
57 ✗ HPP_THROW(std::invalid_argument,
58 "Right hand side functions must take"
59 "only one real value as input. Got "
60 << rhsF->inputSize() << " and "
61 << rhsF->inputDerivativeSize());
62 }
63 ✗ if (*rhsF->outputSpace() != *function_->outputSpace()) {
64 ✗ HPP_THROW(std::invalid_argument,
65 "The right hand side function output"
66 " space ("
67 << *rhsF->outputSpace()
68 << ") differs from the function"
69 " output space ("
70 << *function_->outputSpace() << ").");
71 }
72 // Check that the right hand side is non-constant on all axis.
73 ✗ for (std::size_t i = 0; i < comparison_.size(); ++i) {
74 ✗ if (comparison_[i] != constraints::Equality) {
75 ✗ HPP_THROW(std::invalid_argument,
76 "The comparison type of implicit "
77 "constraint with right hand side function should be Equality "
78 "on all dimensions.");
79 }
80 }
81 }
82
83 ✗ rhsFunction_ = rhsF;
84 }
85
86 ✗ vectorIn_t Implicit::rightHandSideAt(const value_type& s) {
87 ✗ if (rhsFunction_) {
88 ✗ vector_t S(1);
89 ✗ S[0] = s;
90 ✗ rhsFunction_->value(output_, S);
91 }
92 ✗ return output_.vector();
93 }
94
95 ✗ size_type Implicit::parameterSize() const { return parameterSize_; }
96
97 12 size_type Implicit::rightHandSideSize() const {
98 12 return function_->outputSpace()->nq();
99 }
100 94136 const ComparisonTypes_t& Implicit::comparisonType() const {
101 94136 return comparison_;
102 }
103
104 6402 void Implicit::comparisonType(const ComparisonTypes_t& comp) {
105 6402 comparison_ = comp;
106 6402 parameterSize_ = computeParameterSize(comparison_);
107 6402 computeIndices();
108 6402 }
109
110 25931 void Implicit::setInactiveRowsToZero(vectorOut_t error) const {
111
1/2
✓ Branch 2 taken 25931 times.
✗ Branch 3 not taken.
 25931 inactiveRows_.lview(error).setZero();
112 25931 }
113
114 3199 Implicit::Implicit(const DifferentiableFunctionPtr_t& function,
115 3199 ComparisonTypes_t comp, std::vector<bool> mask)
116 3199 : comparison_(comp),
117
2/4
✓ Branch 3 taken 3199 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3199 times.
✗ Branch 7 not taken.
 3199 rhs_(vector_t::Zero(function->outputSize())),
118
1/2
✓ Branch 1 taken 3199 times.
✗ Branch 2 not taken.
 3199 parameterSize_(computeParameterSize(comparison_)),
119 3199 function_(function),
120
1/2
✓ Branch 1 taken 3199 times.
✗ Branch 2 not taken.
 3199 mask_(mask),
121 3199 activeRows_(),
122
1/2
✓ Branch 1 taken 3199 times.
✗ Branch 2 not taken.
 3199 inactiveRows_(),
123 3199 inequalityIndices_(),
124
1/2
✓ Branch 1 taken 3199 times.
✗ Branch 2 not taken.
 3199 equalityIndices_(),
125
1/2
✓ Branch 3 taken 3199 times.
✗ Branch 4 not taken.
 3199 output_(function->outputSpace()),
126
1/2
✓ Branch 8 taken 3199 times.
✗ Branch 9 not taken.
 12796 logOutput_(function->outputSpace()->nv())
127
128 {
129 // This constructor used to set comparison types to Equality if an
130 // empty vector was given as input. Now you should provide the
131 // comparison type at construction.
132
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 3199 times.
 3199 assert(function_->outputDerivativeSize() == (size_type)comparison_.size());
133
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 3199 times.
 3199 assert(function_->outputDerivativeSize() == (size_type)mask.size());
134
1/2
✓ Branch 1 taken 3199 times.
✗ Branch 2 not taken.
 3199 computeActiveRows();
135
1/2
✓ Branch 1 taken 3199 times.
✗ Branch 2 not taken.
 3199 computeIndices();
136 3199 }
137
138 // Compute active rows
139 4253 void Implicit::computeActiveRows() {
140 4253 segments_t inactiveRows;
141
2/2
✓ Branch 1 taken 20459 times.
✓ Branch 2 taken 4253 times.
 24712 for (std::size_t i = 0; i < mask_.size(); ++i) {
142
3/4
✓ Branch 1 taken 20459 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20426 times.
✓ Branch 5 taken 33 times.
 20459 if (mask_[i])
143
1/2
✓ Branch 2 taken 20426 times.
✗ Branch 3 not taken.
 20426 activeRows_.push_back(segment_t(i, 1));
144 else
145
1/2
✓ Branch 2 taken 33 times.
✗ Branch 3 not taken.
 33 inactiveRows.push_back(segment_t(i, 1));
146 }
147
1/2
✓ Branch 1 taken 4253 times.
✗ Branch 2 not taken.
 4253 Eigen::BlockIndex::shrink(activeRows_);
148
1/2
✓ Branch 1 taken 4253 times.
✗ Branch 2 not taken.
 4253 Eigen::BlockIndex::shrink(inactiveRows);
149
2/4
✓ Branch 1 taken 4253 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4253 times.
✗ Branch 5 not taken.
 4253 inactiveRows_ = Eigen::RowBlockIndices(inactiveRows);
150 4253 }
151
152 10655 void Implicit::computeIndices() {
153 10655 inequalityIndices_.clear();
154 10655 equalityIndices_.clearRows();
155
2/2
✓ Branch 1 taken 58861 times.
✓ Branch 2 taken 10655 times.
 69516 for (std::size_t i = 0; i < comparison_.size(); ++i) {
156
6/6
✓ Branch 1 taken 58843 times.
✓ Branch 2 taken 18 times.
✓ Branch 4 taken 27 times.
✓ Branch 5 taken 58816 times.
✓ Branch 6 taken 45 times.
✓ Branch 7 taken 58816 times.
 58861 if ((comparison_[i] == Superior) || (comparison_[i] == Inferior)) {
157
1/2
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
 45 inequalityIndices_.push_back((size_type)i);
158
2/2
✓ Branch 1 taken 31380 times.
✓ Branch 2 taken 27436 times.
 58816 } else if (comparison_[i] == Equality) {
159
1/2
✓ Branch 1 taken 31380 times.
✗ Branch 2 not taken.
 31380 equalityIndices_.addRow((size_type)i, 1);
160 }
161 }
162 10655 equalityIndices_.updateRows<true, true, true>();
163 10655 }
164
165 19 Implicit::Implicit(const Implicit& other)
166 19 : comparison_(other.comparison_),
167
1/2
✓ Branch 1 taken 19 times.
✗ Branch 2 not taken.
 19 rhs_(other.rhs_),
168 19 parameterSize_(other.parameterSize_),
169 19 function_(other.function_),
170 19 rhsFunction_(other.rhsFunction_),
171
1/2
✓ Branch 1 taken 19 times.
✗ Branch 2 not taken.
 19 mask_(other.mask_),
172
1/2
✓ Branch 1 taken 19 times.
✗ Branch 2 not taken.
 19 activeRows_(other.activeRows_),
173
1/2
✓ Branch 1 taken 19 times.
✗ Branch 2 not taken.
 19 inactiveRows_(other.inactiveRows_),
174
1/2
✓ Branch 1 taken 19 times.
✗ Branch 2 not taken.
 19 inequalityIndices_(other.inequalityIndices_),
175
1/2
✓ Branch 1 taken 19 times.
✗ Branch 2 not taken.
 19 equalityIndices_(other.equalityIndices_),
176
1/2
✓ Branch 1 taken 19 times.
✗ Branch 2 not taken.
 19 output_(other.output_),
177
1/2
✓ Branch 3 taken 19 times.
✗ Branch 4 not taken.
 57 logOutput_(other.logOutput_)
178
179 19 {}
180
181 2190 bool Implicit::isEqual(const Implicit& other, bool swapAndTest) const {
182
2/2
✓ Branch 1 taken 80 times.
✓ Branch 2 taken 2110 times.
 2190 if (comparison_ != other.comparison_) return false;
183
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 2110 times.
 2110 if (rhs_.size() != other.rhs_.size()) return false;
184
5/6
✓ Branch 1 taken 20 times.
✓ Branch 2 taken 2090 times.
✓ Branch 6 taken 20 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 20 times.
✓ Branch 9 taken 2090 times.
 2110 if (function_ != other.function_ && *function_ != *other.function_)
185 20 return false;
186
2/2
✓ Branch 0 taken 1055 times.
✓ Branch 1 taken 1035 times.
 2090 if (swapAndTest) return other.isEqual(*this, false);
187 1035 return true;
188 }
189
190 1066 ImplicitPtr_t Implicit::create(const DifferentiableFunctionPtr_t& function,
191 ComparisonTypes_t comp, std::vector<bool> mask) {
192
2/2
✓ Branch 1 taken 1053 times.
✓ Branch 2 taken 13 times.
 1066 if (mask.empty()) {
193
1/2
✓ Branch 6 taken 1053 times.
✗ Branch 7 not taken.
 1053 mask = std::vector<bool>(function->outputSpace()->nv(), true);
194 }
195
4/8
✓ Branch 1 taken 1066 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1066 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1066 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1066 times.
✗ Branch 11 not taken.
 1066 Implicit* ptr = new Implicit(function, comp, mask);
196
1/2
✓ Branch 1 taken 1066 times.
✗ Branch 2 not taken.
 1066 ImplicitPtr_t shPtr(ptr);
197 1066 ImplicitWkPtr_t wkPtr(shPtr);
198 1066 ptr->init(wkPtr);
199 2132 return shPtr;
200 1066 }
201
202 7 ImplicitPtr_t Implicit::createCopy(const ImplicitPtr_t& other) {
203
2/4
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
 7 Implicit* ptr = new Implicit(*other);
204
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 ImplicitPtr_t shPtr(ptr);
205 7 ImplicitWkPtr_t wkPtr(shPtr);
206 7 ptr->init(wkPtr);
207 14 return shPtr;
208 7 }
209
210
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 14 ImplicitPtr_t Implicit::copy() const { return createCopy(weak_.lock()); }
211
212 3606 void Implicit::rightHandSideFromConfig(ConfigurationIn_t config,
213 LiegroupElementRef rhs) {
214
2/4
✓ Branch 6 taken 3606 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 3606 times.
 3606 assert(*rhs.space() == *function_->outputSpace());
215
2/4
✓ Branch 3 taken 3606 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3606 times.
✗ Branch 7 not taken.
 3606 function_->value(output_, config);
216 3606 logOutput_.setZero();
217
3/6
✓ Branch 2 taken 3606 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3606 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3606 times.
✗ Branch 9 not taken.
 3606 equalityIndices_.lview(logOutput_) = equalityIndices_.rview(log(output_));
218 // rhs = exp(logOutput_)
219
2/4
✓ Branch 4 taken 3606 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3606 times.
✗ Branch 8 not taken.
 3606 rhs = rhs.space()->exp(logOutput_);
220 3606 }
221
222 2101 bool Implicit::checkRightHandSide(LiegroupElementConstRef rhs) const {
223
1/2
✗ Branch 5 not taken.
✓ Branch 6 taken 2101 times.
 2101 if (rhs.space() != function_->outputSpace()) return false;
224 2101 logOutput_ = log(rhs);
225
2/2
✓ Branch 1 taken 16601 times.
✓ Branch 2 taken 2101 times.
 18702 for (std::size_t i = 0; i < comparison_.size(); ++i)
226
4/6
✓ Branch 1 taken 7000 times.
✓ Branch 2 taken 9601 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 7000 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 16601 times.
 16601 if ((comparison_[i] != Equality) && (logOutput_[i] != 0)) return false;
227
228 2101 return true;
229 }
230
231 std::pair<JointConstPtr_t, JointConstPtr_t>
232 9 Implicit::doesConstrainRelPoseBetween(DeviceConstPtr_t robot) const {
233 std::pair<JointConstPtr_t, JointConstPtr_t> joints =
234
1/2
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
 9 function_->dependsOnRelPoseBetween(robot);
235 // check that the constraint fully constrains the relative pose
236
8/12
✓ Branch 4 taken 5 times.
✓ Branch 5 taken 4 times.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 2 times.
✓ Branch 10 taken 3 times.
✓ Branch 11 taken 9 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 6 times.
✓ Branch 15 taken 3 times.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
 9 if (function_->outputSpace()->nv() != 6 || !checkAllRowsActive()) {
237 hppDout(
238 info, "Constraint "
239 << function_->name()
240 << " does not fully constrain the relative pose of joints");
241 6 return std::pair<JointConstPtr_t, JointConstPtr_t>(nullptr, nullptr);
242 }
243 3 return joints;
244 9 }
245
246 template <class Archive>
247 12 void Implicit::serialize(Archive& ar, const unsigned int version) {
248 (void)version;
249
1/2
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
 12 ar& BOOST_SERIALIZATION_NVP(comparison_);
250
1/2
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
 12 ar& BOOST_SERIALIZATION_NVP(rhs_);
251 if (!Archive::is_saving::value)
252 6 parameterSize_ = computeParameterSize(comparison_);
253
1/2
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
 12 ar& BOOST_SERIALIZATION_NVP(function_);
254
1/2
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
 12 ar& BOOST_SERIALIZATION_NVP(rhsFunction_);
255
1/2
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
 12 ar& BOOST_SERIALIZATION_NVP(mask_);
256
1/2
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
 12 ar& BOOST_SERIALIZATION_NVP(weak_);
257 if (!Archive::is_saving::value) {
258 6 computeActiveRows();
259 6 computeIndices();
260 }
261 }
262
263 HPP_SERIALIZATION_IMPLEMENT(Implicit);
264 } // namespace constraints
265 } // namespace hpp
266