GCC Code Coverage Report

Directory: ./
File: src/straight-path.cc
Date: 2024-12-13 16:14:03
Exec Total Coverage
Lines: 63 88 71.6%
Branches: 63 158 39.9%
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/weak_ptr.hpp>
31 #include <hpp/core/config-projector.hh>
32 #include <hpp/core/projection-error.hh>
33 #include <hpp/core/straight-path.hh>
34 #include <hpp/pinocchio/configuration.hh>
35 #include <hpp/pinocchio/device.hh>
36 #include <hpp/pinocchio/liegroup.hh>
37 #include <hpp/pinocchio/serialization.hh>
38 #include <hpp/pinocchio/util.hh>
39 #include <hpp/util/debug.hh>
40 #include <hpp/util/exception.hh>
41 #include <hpp/util/serialization.hh>
42 #include <pinocchio/serialization/eigen.hpp>
43
44 namespace hpp {
45 namespace core {
46 4129 StraightPathPtr_t StraightPath::create(const DevicePtr_t& device,
47 ConfigurationIn_t init,
48 ConfigurationIn_t end,
49 interval_t interval,
50 ConstraintSetPtr_t constraints) {
51
3/6
✓ Branch 2 taken 4129 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4129 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 4129 times.
✗ Branch 12 not taken.
 4129 return create(device->RnxSOnConfigSpace(), init, end, interval, constraints);
52 }
53
54 6287 StraightPath::StraightPath(LiegroupSpacePtr_t space, vectorIn_t init,
55 6287 vectorIn_t end, interval_t interval)
56 : parent_t(interval, space->nq(), space->nv()),
57 6287 space_(space),
58
1/2
✓ Branch 1 taken 6287 times.
✗ Branch 2 not taken.
 6287 initial_(init),
59
1/2
✓ Branch 7 taken 6287 times.
✗ Branch 8 not taken.
 12574 end_(end) {
60
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 6287 times.
 6287 assert(interval.second >= interval.first);
61
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 6287 times.
 6287 assert(!constraints());
62 6287 }
63
64 20282 StraightPath::StraightPath(LiegroupSpacePtr_t space, vectorIn_t init,
65 vectorIn_t end, interval_t interval,
66 20282 ConstraintSetPtr_t constraints)
67 : parent_t(interval, space->nq(), space->nv(), constraints),
68 20282 space_(space),
69
1/2
✓ Branch 1 taken 20282 times.
✗ Branch 2 not taken.
 20282 initial_(init),
70
1/2
✓ Branch 7 taken 20282 times.
✗ Branch 8 not taken.
 40564 end_(end) {
71
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 20282 times.
 20282 assert(interval.second >= interval.first);
72
4/6
✓ Branch 0 taken 28 times.
✓ Branch 1 taken 20254 times.
✓ Branch 3 taken 28 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 28 times.
 20282 assert(interval.second > interval.first || init == end);
73 20282 }
74
75 11212 StraightPath::StraightPath(const StraightPath& path)
76 : parent_t(path),
77 11212 space_(path.space_),
78
1/2
✓ Branch 1 taken 11212 times.
✗ Branch 2 not taken.
 11212 initial_(path.initial_),
79
1/2
✓ Branch 3 taken 11212 times.
✗ Branch 4 not taken.
 22424 end_(path.end_) {}
80
81 ✗ StraightPath::StraightPath(const StraightPath& path,
82 ✗ const ConstraintSetPtr_t& constraints)
83 : parent_t(path, constraints),
84 ✗ space_(path.space_),
85 ✗ initial_(path.initial_),
86 ✗ end_(path.end_) {
87 ✗ assert(constraints->isSatisfied(initial_));
88 ✗ assert(constraints->isSatisfied(end_));
89 }
90
91 37742 bool StraightPath::impl_compute(ConfigurationOut_t result,
92 value_type param) const {
93 37742 const value_type L = paramLength();
94
5/6
✓ Branch 1 taken 36802 times.
✓ Branch 2 taken 940 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 36802 times.
✓ Branch 5 taken 940 times.
✓ Branch 6 taken 36802 times.
 37742 if (param == paramRange().first || L == 0) {
95 940 result = initial_;
96 940 return true;
97 }
98
2/2
✓ Branch 1 taken 535 times.
✓ Branch 2 taken 36267 times.
 36802 if (param == paramRange().second) {
99 535 result = end_;
100 535 return true;
101 }
102 36267 value_type u = (param - paramRange().first) / L;
103
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 36267 times.
 36267 if (L == 0) u = 0;
104
3/6
✓ Branch 1 taken 36267 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 36267 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 36267 times.
 36267 assert(hpp::pinocchio::checkNormalized(
105 hpp::pinocchio::LiegroupElement(initial_, space_)));
106
3/6
✓ Branch 1 taken 36267 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 36267 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 36267 times.
 36267 assert(hpp::pinocchio::checkNormalized(
107 hpp::pinocchio::LiegroupElement(end_, space_)));
108
3/6
✓ Branch 3 taken 36267 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 36267 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 36267 times.
✗ Branch 10 not taken.
 36267 space_->interpolate(initial_, end_, u, result);
109 36267 return true;
110 }
111
112 10 void StraightPath::impl_derivative(vectorOut_t result, const value_type&,
113 size_type order) const {
114
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 10 times.
 10 if (order > 1) {
115 ✗ result.setZero();
116 ✗ return;
117 }
118
1/2
✓ Branch 0 taken 10 times.
✗ Branch 1 not taken.
 10 if (order == 1) {
119
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 10 times.
 10 if (paramRange().first == paramRange().second) {
120 ✗ result.setZero();
121 ✗ return;
122 }
123
6/12
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 10 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 10 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 10 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 10 times.
✗ Branch 19 not taken.
 10 result = space_->elementConstRef(end_) - space_->elementConstRef(initial_);
124
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 10 result /= paramLength();
125 10 return;
126 }
127 ✗ std::ostringstream oss;
128 ✗ oss << "order of derivative (" << order << ") should be positive.";
129 ✗ HPP_THROW_EXCEPTION(hpp::Exception, oss.str());
130 }
131
132 4247 void StraightPath::impl_velocityBound(vectorOut_t result, const value_type&,
133 const value_type&) const {
134
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 4247 times.
 4247 if (paramRange().first == paramRange().second) {
135 ✗ result.setZero();
136 ✗ return;
137 }
138
5/10
✓ Branch 3 taken 4247 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 4247 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4247 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4247 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 4247 times.
✗ Branch 17 not taken.
 4247 result = space_->elementConstRef(end_) - space_->elementConstRef(initial_);
139
4/8
✓ Branch 2 taken 4247 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4247 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4247 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 4247 times.
✗ Branch 12 not taken.
 4247 result.noalias() = result.cwiseAbs() / paramLength();
140 }
141
142 229 PathPtr_t StraightPath::impl_extract(const interval_t& subInterval) const {
143 // Length is assumed to be proportional to interval range
144 229 value_type l = fabs(subInterval.second - subInterval.first);
145
146 bool success;
147
1/2
✓ Branch 1 taken 229 times.
✗ Branch 2 not taken.
 229 Configuration_t q1(configAtParam(subInterval.first, success));
148
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 229 times.
 229 if (!success)
149 ✗ throw projection_error(
150 ✗ "Failed to apply constraints in StraightPath::extract");
151
1/2
✓ Branch 1 taken 229 times.
✗ Branch 2 not taken.
 229 Configuration_t q2(configAtParam(subInterval.second, success));
152
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 229 times.
 229 if (!success)
153 ✗ throw projection_error(
154 ✗ "Failed to apply constraints in StraightPath::extract");
155 StraightPathPtr_t result =
156
3/6
✓ Branch 4 taken 229 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 229 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 229 times.
✗ Branch 12 not taken.
 458 StraightPath::create(space_, q1, q2, interval_t(0, l), constraints());
157 458 return result;
158 229 }
159
160 ✗ std::ostream& StraightPath::print(std::ostream& os) const {
161 ✗ Path::print(os << "StraightPath:")
162 ✗ << incendl << "initial configuration: " << one_line(initial_) << iendl
163 ✗ << "final configuration: " << one_line(end_) << decendl;
164 ✗ return os;
165 }
166
167 template <class Archive>
168 60 void StraightPath::serialize(Archive& ar, const unsigned int version) {
169 using namespace boost::serialization;
170 (void)version;
171
1/2
✓ Branch 3 taken 30 times.
✗ Branch 4 not taken.
 60 ar& make_nvp("base", base_object<Path>(*this));
172
1/2
✓ Branch 2 taken 30 times.
✗ Branch 3 not taken.
 60 ar& BOOST_SERIALIZATION_NVP(space_);
173 60 serialization::remove_duplicate::serialize_vector(ar, "initial", initial_,
174 version);
175 60 serialization::remove_duplicate::serialize_vector(ar, "end", end_, version);
176
1/2
✓ Branch 2 taken 30 times.
✗ Branch 3 not taken.
 60 ar& BOOST_SERIALIZATION_NVP(weak_);
177 60 }
178
179 HPP_SERIALIZATION_IMPLEMENT(StraightPath);
180 } // namespace core
181 } // namespace hpp
182
183 BOOST_CLASS_EXPORT_IMPLEMENT(hpp::core::StraightPath)
184