Directory: | ./ |
---|---|
File: | src/straight-path.cc |
Date: | 2024-08-10 11:29:48 |
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 | 11204 | StraightPath::StraightPath(const StraightPath& path) | |
76 | : parent_t(path), | ||
77 | 11204 | space_(path.space_), | |
78 |
1/2✓ Branch 1 taken 11204 times.
✗ Branch 2 not taken.
|
11204 | initial_(path.initial_), |
79 |
1/2✓ Branch 3 taken 11204 times.
✗ Branch 4 not taken.
|
22408 | 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 | 37737 | bool StraightPath::impl_compute(ConfigurationOut_t result, | |
92 | value_type param) const { | ||
93 | 37737 | const value_type L = paramLength(); | |
94 |
5/6✓ Branch 1 taken 36797 times.
✓ Branch 2 taken 940 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 36797 times.
✓ Branch 5 taken 940 times.
✓ Branch 6 taken 36797 times.
|
37737 | 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 36262 times.
|
36797 | if (param == paramRange().second) { |
99 | 535 | result = end_; | |
100 | 535 | return true; | |
101 | } | ||
102 | 36262 | value_type u = (param - paramRange().first) / L; | |
103 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 36262 times.
|
36262 | if (L == 0) u = 0; |
104 |
3/6✓ Branch 1 taken 36262 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 36262 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 36262 times.
|
36262 | assert(hpp::pinocchio::checkNormalized( |
105 | hpp::pinocchio::LiegroupElement(initial_, space_))); | ||
106 |
3/6✓ Branch 1 taken 36262 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 36262 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 36262 times.
|
36262 | assert(hpp::pinocchio::checkNormalized( |
107 | hpp::pinocchio::LiegroupElement(end_, space_))); | ||
108 |
3/6✓ Branch 3 taken 36262 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 36262 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 36262 times.
✗ Branch 10 not taken.
|
36262 | space_->interpolate(initial_, end_, u, result); |
109 | 36262 | 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 |