GCC Code Coverage Report

Directory: ./
File: src/path.cc
Date: 2025-03-10 11:18:21
Exec Total Coverage
Lines: 102 160 63.8%
Branches: 62 267 23.2%
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 #include <boost/serialization/utility.hpp>
31 #include <boost/serialization/weak_ptr.hpp>
32 #include <hpp/core/config-projector.hh>
33 #include <hpp/core/path.hh>
34 #include <hpp/core/time-parameterization.hh>
35 #include <hpp/pinocchio/configuration.hh>
36 #include <hpp/util/debug.hh>
37 #include <hpp/util/indent.hh>
38 #include <hpp/util/serialization.hh>
39
40 #include "extracted-path.hh"
41
42 namespace hpp {
43 namespace core {
44 namespace timeParameterization {
45 class HPP_CORE_LOCAL Shift : public TimeParameterization {
46 public:
47 typedef shared_ptr<Shift> Ptr_t;
48
49 2 static TimeParameterizationPtr_t createWithCheck(TimeParameterizationPtr_t tp,
50 value_type t, value_type s) {
51
2/4
✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 if (t == 0 && s == 0)
52 2 return tp;
53 else
54 ✗ return create(tp, t, s);
55 }
56
57 ✗ static Ptr_t create(TimeParameterizationPtr_t tp, value_type t,
58 value_type s) {
59 ✗ Ptr_t shift = HPP_DYNAMIC_PTR_CAST(Shift, tp);
60 ✗ if (shift)
61 ✗ return Ptr_t(new Shift(shift->tp_, shift->t_ + t, shift->s_ + s));
62 else
63 ✗ return Ptr_t(new Shift(tp, t, s));
64 }
65
66 ✗ Shift(TimeParameterizationPtr_t tp, value_type t, value_type s)
67 ✗ : tp_(tp), t_(t), s_(s) {}
68
69 ✗ value_type value(const value_type& t) const {
70 ✗ return tp_->value(t + t_) + s_;
71 }
72 ✗ value_type derivative(const value_type& t, const size_type& order) const {
73 ✗ return tp_->derivative(t + t_, order);
74 }
75 value_type impl_derivativeBound(const value_type& l,
76 const value_type& u) const {
77 return tp_->derivativeBound(l + t_, u + t_);
78 }
79
80 ✗ TimeParameterizationPtr_t copy() const { return create(tp_->copy(), t_, s_); }
81
82 TimeParameterizationPtr_t tp_;
83 value_type t_;
84 value_type s_;
85 };
86 } // namespace timeParameterization
87
88 // Constructor with constraints
89 1873086 Path::Path(const interval_t& interval, size_type outputSize,
90 size_type outputDerivativeSize,
91 1873086 const ConstraintSetPtr_t& constraints)
92 1873086 : paramRange_(interval),
93 1873086 timeRange_(interval),
94 1873086 outputSize_(outputSize),
95 1873086 outputDerivativeSize_(outputDerivativeSize),
96 1873086 constraints_() {
97
2/2
✓ Branch 1 taken 1853012 times.
✓ Branch 2 taken 20074 times.
 1873088 if (constraints) {
98
1/2
✓ Branch 2 taken 1853012 times.
✗ Branch 3 not taken.
 1853012 constraints_ = HPP_STATIC_PTR_CAST(ConstraintSet, constraints->copy());
99 }
100 1873086 }
101
102 // Constructor without constraints
103 6507 Path::Path(const interval_t& interval, size_type outputSize,
104 6507 size_type outputDerivativeSize)
105 6507 : paramRange_(interval),
106 6507 timeRange_(interval),
107 6507 outputSize_(outputSize),
108 6507 outputDerivativeSize_(outputDerivativeSize),
109 6507 constraints_() {}
110
111 // Copy constructor
112 1864014 Path::Path(const Path& path)
113 1864014 : paramRange_(path.paramRange_),
114 1864014 timeRange_(path.timeRange_),
115 1864014 outputSize_(path.outputSize_),
116 1864014 outputDerivativeSize_(path.outputDerivativeSize_),
117 1864014 constraints_(),
118 1864014 timeParam_() {
119
2/2
✓ Branch 1 taken 1852843 times.
✓ Branch 2 taken 11171 times.
 1864014 if (path.constraints_) {
120 constraints_ =
121
1/2
✓ Branch 2 taken 1852843 times.
✗ Branch 3 not taken.
 1852843 HPP_STATIC_PTR_CAST(ConstraintSet, path.constraints_->copy());
122 }
123
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 1864014 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 1864014 if (path.timeParam_) timeParam_ = path.timeParam_->copy();
124 1864014 }
125
126 24 Path::Path(const Path& path, const ConstraintSetPtr_t& constraints)
127 24 : paramRange_(path.paramRange_),
128 24 timeRange_(path.timeRange_),
129 24 outputSize_(path.outputSize_),
130 24 outputDerivativeSize_(path.outputDerivativeSize_),
131 24 constraints_(constraints),
132 24 timeParam_() {
133
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 24 times.
 24 assert(!path.constraints_);
134
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 24 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 24 if (path.timeParam_) timeParam_ = path.timeParam_->copy();
135 24 }
136
137 // Initialization after creation
138 3743631 void Path::init(const PathWkPtr_t& self) { weak_ = self; }
139
140 2090766 bool Path::applyConstraints(ConfigurationOut_t result,
141 const value_type& param) const {
142
2/2
✓ Branch 1 taken 231827 times.
✓ Branch 2 taken 1858937 times.
 2090766 if (!constraints_) return true;
143
2/2
✓ Branch 4 taken 1852680 times.
✓ Branch 5 taken 6258 times.
 1858937 if (constraints_->configProjector())
144
1/2
✓ Branch 4 taken 1852680 times.
✗ Branch 5 not taken.
 1852680 constraints_->configProjector()->rightHandSideAt(param);
145
1/2
✓ Branch 3 taken 1858938 times.
✗ Branch 4 not taken.
 1858938 return constraints_->apply(result);
146 }
147
148 929649 void Path::derivative(vectorOut_t result, const value_type& time,
149 size_type order) const {
150
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 929649 times.
 929649 if (timeParam_) {
151 ✗ switch (order) {
152 ✗ case 1:
153 ✗ impl_derivative(result, timeParam_->value(time), 1);
154 ✗ result *= timeParam_->derivative(time, 1);
155 ✗ break;
156 ✗ case 2: {
157 ✗ vector_t tmp(outputDerivativeSize());
158 ✗ impl_derivative(tmp, timeParam_->value(time), 2);
159 ✗ value_type der = timeParam_->derivative(time, 1);
160 ✗ result.noalias() = tmp * (der * der);
161
162 ✗ impl_derivative(tmp, timeParam_->value(time), 1);
163 ✗ result.noalias() += tmp * timeParam_->derivative(time, 2);
164 ✗ break;
165 }
166 ✗ default:
167 ✗ throw std::invalid_argument(
168 ✗ "Cannot compute the derivative of order greater than 2.");
169 }
170 } else {
171
1/2
✓ Branch 2 taken 929649 times.
✗ Branch 3 not taken.
 929649 impl_derivative(result, time, order);
172 }
173 929649 }
174
175 20288 PathPtr_t Path::extract(const interval_t& subInterval) const {
176 20288 PathPtr_t res;
177
2/2
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 20286 times.
 20288 if (timeParam_) {
178
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 interval_t paramInterval(timeParam_->value(subInterval.first),
179
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 4 timeParam_->value(subInterval.second));
180
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 res = this->impl_extract(paramInterval);
181 // TODO Child class that reimplement impl_extract may return
182 // a path whose paramRange has been shifted to 0. We must then shift
183 // the time parameterization.
184 value_type shift_t, shift_s;
185 2 interval_t timeInterval;
186
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1 times.
 2 if (subInterval.first > subInterval.second) {
187 1 shift_t = 0;
188 1 shift_s = res->paramRange().first - paramInterval.second;
189
190
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (shift_s != 0) {
191 ✗ shift_t = subInterval.second;
192 ✗ timeInterval.first = 0;
193 ✗ timeInterval.second = subInterval.first - subInterval.second;
194 } else {
195 1 shift_t = 0;
196 1 timeInterval = interval_t(subInterval.second, subInterval.first);
197 }
198 } else {
199 1 shift_t = 0;
200 1 shift_s = res->paramRange().first - paramInterval.first;
201
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (shift_s != 0) {
202 ✗ shift_t = subInterval.first;
203 ✗ timeInterval.first = 0;
204 ✗ timeInterval.second = subInterval.second - subInterval.first;
205 } else {
206
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 assert(res->paramRange() == paramInterval);
207 1 shift_t = 0;
208 1 timeInterval = subInterval;
209 }
210 }
211
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 timeParameterization::Shift::createWithCheck(timeParam_, shift_t, shift_s);
212 #ifndef NDEBUG
213 2 interval_t pr = res->paramRange();
214 #endif // NDEBUG
215
2/4
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
 2 res->timeParameterization(timeParam_->copy(), timeInterval);
216
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 2 times.
 2 assert(pr == res->paramRange());
217 } else {
218
1/2
✓ Branch 1 taken 20286 times.
✗ Branch 2 not taken.
 20286 res = this->impl_extract(subInterval);
219 }
220 20288 return res;
221 }
222
223 45 PathPtr_t Path::impl_extract(const interval_t& paramInterval) const {
224
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 45 times.
 45 if (paramInterval == paramRange_) return this->copy();
225
1/2
✓ Branch 2 taken 45 times.
✗ Branch 3 not taken.
 45 return ExtractedPath::create(weak_.lock(), paramInterval);
226 }
227
228 53 PathPtr_t Path::reverse() const {
229 53 interval_t interval;
230 53 interval.first = this->timeRange_.second;
231 53 interval.second = this->timeRange_.first;
232
1/2
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
 106 return this->extract(interval);
233 }
234
235 3824840 void Path::checkPath() const {
236 using pinocchio::displayConfig;
237
2/2
✓ Branch 2 taken 3706015 times.
✓ Branch 3 taken 118825 times.
 3824840 if (constraints()) {
238
2/2
✓ Branch 4 taken 3705410 times.
✓ Branch 5 taken 605 times.
 3706015 if (constraints_->configProjector())
239
1/2
✓ Branch 4 taken 3705410 times.
✗ Branch 5 not taken.
 3705410 constraints_->configProjector()->rightHandSideAt(paramRange_.first);
240
3/6
✓ Branch 4 taken 3706015 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3706015 times.
✗ Branch 8 not taken.
✗ Branch 11 not taken.
✓ Branch 12 taken 3706015 times.
 3706015 if (!constraints()->isSatisfied(initial())) {
241 ✗ std::stringstream oss;
242 hppDout(error, *constraints());
243 hppDout(error, initial().transpose());
244 oss << "Initial configuration of path does not satisfy the path "
245 "constraints: q="
246 ✗ << displayConfig(initial()) << "; error=";
247 ✗ vector_t error;
248 ✗ constraints()->isSatisfied(initial(), error);
249 ✗ oss << displayConfig(error) << ".";
250 ✗ throw projection_error(oss.str().c_str());
251 }
252
2/2
✓ Branch 4 taken 3705410 times.
✓ Branch 5 taken 605 times.
 3706015 if (constraints_->configProjector())
253
1/2
✓ Branch 4 taken 3705410 times.
✗ Branch 5 not taken.
 3705410 constraints_->configProjector()->rightHandSideAt(paramRange_.second);
254
8/20
✓ Branch 2 taken 3706015 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 3706015 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3706015 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3706015 times.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✓ Branch 16 taken 3706015 times.
✓ Branch 17 taken 3706015 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 3706015 times.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 3706015 times.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
 3706015 if (constraints() && !constraints()->isSatisfied(end())) {
255 ✗ std::stringstream oss;
256 hppDout(error, *constraints());
257 hppDout(error, displayConfig(end()));
258 oss << "End configuration of path does not satisfy the path "
259 "constraints: q="
260 ✗ << displayConfig(end()) << "; error=";
261 ✗ vector_t error;
262 ✗ constraints()->isSatisfied(end(), error);
263 ✗ Configuration_t q = end();
264 ✗ constraints()->apply(q);
265 ✗ oss << displayConfig(error) << "; qproj=" << displayConfig(q) << ".\n"
266 ✗ << *constraints();
267 ✗ throw projection_error(oss.str().c_str());
268 }
269 }
270 3824840 }
271
272 ✗ std::ostream& Path::print(std::ostream& os) const {
273 ✗ os << "time in [ " << timeRange().first << ", " << timeRange().second << " ]";
274 ✗ if (timeParam_)
275 ✗ os << ", param in [ " << paramRange().first << ", " << paramRange().second
276 ✗ << " ]";
277 ✗ os << iendl;
278 ✗ return os;
279 }
280
281 template <class Archive>
282 60 void Path::serialize(Archive& ar, const unsigned int version) {
283 (void)version;
284
1/2
✓ Branch 2 taken 30 times.
✗ Branch 3 not taken.
 60 ar& BOOST_SERIALIZATION_NVP(timeRange_);
285
1/2
✓ Branch 2 taken 30 times.
✗ Branch 3 not taken.
 60 ar& BOOST_SERIALIZATION_NVP(outputSize_);
286
1/2
✓ Branch 2 taken 30 times.
✗ Branch 3 not taken.
 60 ar& BOOST_SERIALIZATION_NVP(outputDerivativeSize_);
287
1/2
✓ Branch 2 taken 30 times.
✗ Branch 3 not taken.
 60 ar& BOOST_SERIALIZATION_NVP(constraints_);
288
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 15 times.
 30 if (Archive::is_saving::value && timeParam_)
289 ✗ throw std::logic_error(
290 "At the moment, it is not possible to serialize "
291 "a Path with time parameterization.");
292 // ar & BOOST_SERIALIZATION_NVP(timeParam_);
293 if (!Archive::is_saving::value) { // handles paramRange_
294 30 timeRange(timeRange_);
295 }
296
1/2
✓ Branch 2 taken 30 times.
✗ Branch 3 not taken.
 60 ar& BOOST_SERIALIZATION_NVP(weak_);
297 60 }
298
299 HPP_SERIALIZATION_IMPLEMENT(Path);
300 } // namespace core
301 } // namespace hpp
302