GCC Code Coverage Report

Directory: ./
File: src/kinodynamic-path.cc
Date: 2024-12-13 16:14:03
Exec Total Coverage
Lines: 159 198 80.3%
Branches: 262 600 43.7%
Line Branch Exec Source
1 // Copyright (c) 2016, LAAS-CNRS
2 // Authors: Pierre Fernbach (pierre.fernbach@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/core/config-projector.hh>
30 #include <hpp/core/kinodynamic-path.hh>
31 #include <hpp/core/projection-error.hh>
32 #include <hpp/pinocchio/configuration.hh>
33 #include <hpp/pinocchio/device.hh>
34 #include <hpp/pinocchio/joint.hh>
35 #include <hpp/util/debug.hh>
36
37 namespace hpp {
38 namespace core {
39 2007 KinodynamicPath::KinodynamicPath(const DevicePtr_t& device,
40 ConfigurationIn_t init, ConfigurationIn_t end,
41 value_type length, ConfigurationIn_t a1,
42 ConfigurationIn_t t0, ConfigurationIn_t t1,
43 ConfigurationIn_t tv, ConfigurationIn_t t2,
44 2007 ConfigurationIn_t vLim)
45 2007 : parent_t(device->RnxSOnConfigSpace(), init, end, interval_t(0, length)),
46 2007 device_(device),
47
1/2
✓ Branch 1 taken 2007 times.
✗ Branch 2 not taken.
 2007 a1_(a1),
48
1/2
✓ Branch 1 taken 2007 times.
✗ Branch 2 not taken.
 2007 t0_(t0),
49
1/2
✓ Branch 1 taken 2007 times.
✗ Branch 2 not taken.
 2007 t1_(t1),
50
1/2
✓ Branch 1 taken 2007 times.
✗ Branch 2 not taken.
 2007 tv_(tv),
51
1/2
✓ Branch 1 taken 2007 times.
✗ Branch 2 not taken.
 2007 t2_(t2),
52
4/8
✓ Branch 2 taken 2007 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2007 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 2007 times.
✗ Branch 10 not taken.
✓ Branch 16 taken 2007 times.
✗ Branch 17 not taken.
 6021 vLim_(vLim) {
53
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2007 times.
 2007 assert(device);
54
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2007 times.
 2007 assert(length >= 0);
55
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 2007 times.
 2007 assert(!constraints());
56 hppDout(notice, "Create kinodynamic path with values : ");
57 hppDout(notice, "a1 = " << pinocchio::displayConfig(a1_));
58 hppDout(notice, "t0 = " << pinocchio::displayConfig(t0_));
59 hppDout(notice, "t1 = " << pinocchio::displayConfig(t1_));
60 hppDout(notice, "tv = " << pinocchio::displayConfig(tv_));
61 hppDout(notice, "t2 = " << pinocchio::displayConfig(t2_));
62 hppDout(notice, "length = " << length);
63 hppDout(notice, "vLim = " << pinocchio::displayConfig(vLim_));
64
65 // for now, this class only deal with the translation part of a freeflyer :
66
6/12
✓ Branch 1 taken 2007 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2007 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2007 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2007 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2007 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2007 times.
✗ Branch 17 not taken.
 2007 assert(a1.size() == 3 && t0.size() == 3 && t1.size() == 3 && tv.size() == 3 &&
67 t2.size() == 3 && vLim.size() == 3 &&
68 "Inputs vector of kinodynamicPath are not of size 3");
69
2/2
✓ Branch 0 taken 6021 times.
✓ Branch 1 taken 2007 times.
 8028 for (size_t i = 0; i < 3; i++) {
70
5/10
✓ Branch 1 taken 6021 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6021 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6021 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6021 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 6021 times.
✗ Branch 14 not taken.
 6021 assert(fabs(length - (t0[i] + t1[i] + tv[i] + t2[i])) <
71 std::numeric_limits<float>::epsilon() &&
72 "Kinodynamic path : length is not coherent with switch times at "
73 "index " +
74 i);
75 }
76 2007 }
77
78 20004 KinodynamicPath::KinodynamicPath(const DevicePtr_t& device,
79 ConfigurationIn_t init, ConfigurationIn_t end,
80 value_type length, ConfigurationIn_t a1,
81 ConfigurationIn_t t0, ConfigurationIn_t t1,
82 ConfigurationIn_t tv, ConfigurationIn_t t2,
83 ConfigurationIn_t vLim,
84 20004 ConstraintSetPtr_t constraints)
85 20004 : parent_t(device->RnxSOnConfigSpace(), init, end, interval_t(0, length),
86 constraints),
87 20004 device_(device),
88
1/2
✓ Branch 1 taken 20004 times.
✗ Branch 2 not taken.
 20004 a1_(a1),
89
1/2
✓ Branch 1 taken 20004 times.
✗ Branch 2 not taken.
 20004 t0_(t0),
90
1/2
✓ Branch 1 taken 20004 times.
✗ Branch 2 not taken.
 20004 t1_(t1),
91
1/2
✓ Branch 1 taken 20004 times.
✗ Branch 2 not taken.
 20004 tv_(tv),
92
1/2
✓ Branch 1 taken 20004 times.
✗ Branch 2 not taken.
 20004 t2_(t2),
93
4/8
✓ Branch 3 taken 20004 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 20004 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 20004 times.
✗ Branch 11 not taken.
✓ Branch 18 taken 20004 times.
✗ Branch 19 not taken.
 60012 vLim_(vLim) {
94
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 20004 times.
 20004 assert(device);
95
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 20004 times.
 20004 assert(length >= 0);
96 hppDout(notice, "Create kinodynamic path with constraints, with values : ");
97 hppDout(notice, "a1 = " << pinocchio::displayConfig(a1_));
98 hppDout(notice, "t0 = " << pinocchio::displayConfig(t0_));
99 hppDout(notice, "t1 = " << pinocchio::displayConfig(t1_));
100 hppDout(notice, "tv = " << pinocchio::displayConfig(tv_));
101 hppDout(notice, "t2 = " << pinocchio::displayConfig(t2_));
102 hppDout(notice, "length = " << length);
103 hppDout(notice, "vLim = " << pinocchio::displayConfig(vLim_));
104
105 // for now, this class only deal with the translation part of a freeflyer :
106
6/12
✓ Branch 1 taken 20004 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20004 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20004 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 20004 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 20004 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 20004 times.
✗ Branch 17 not taken.
 20004 assert(a1.size() == 3 && t0.size() == 3 && t1.size() == 3 && tv.size() == 3 &&
107 t2.size() == 3 && vLim.size() == 3 &&
108 "Inputs vector of kinodynamicPath are not of size 3");
109
2/2
✓ Branch 0 taken 60012 times.
✓ Branch 1 taken 20004 times.
 80016 for (size_t i = 0; i < 3; i++) {
110
5/10
✓ Branch 1 taken 60012 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 60012 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 60012 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 60012 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 60012 times.
✗ Branch 14 not taken.
 60012 assert(fabs(length - (t0[i] + t1[i] + tv[i] + t2[i])) <
111 Eigen::NumTraits<value_type>::dummy_precision() &&
112 "Kinodynamic path : length is not coherent with switch times");
113
2/4
✓ Branch 1 taken 60012 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 60012 times.
✗ Branch 4 not taken.
 60012 assert(t0[i] >= 0 &&
114 "Duration of the phases in kinodynamicPath must be positives.");
115
2/4
✓ Branch 1 taken 60012 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 60012 times.
✗ Branch 4 not taken.
 60012 assert(t1[i] >= 0 &&
116 "Duration of the phases in kinodynamicPath must be positives.");
117
2/4
✓ Branch 1 taken 60012 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 60012 times.
✗ Branch 4 not taken.
 60012 assert(tv[i] >= 0 &&
118 "Duration of the phases in kinodynamicPath must be positives.");
119
2/4
✓ Branch 1 taken 60012 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 60012 times.
✗ Branch 4 not taken.
 60012 assert(t2[i] >= 0 &&
120 "Duration of the phases in kinodynamicPath must be positives.");
121 }
122 // check if given parameters correctly reach end config after the given time :
123 value_type pf, v2;
124 size_type indexVel;
125 size_type configSize =
126
1/2
✓ Branch 2 taken 20004 times.
✗ Branch 3 not taken.
 20004 device->configSize() - device->extraConfigSpace().dimension();
127
2/2
✓ Branch 0 taken 60012 times.
✓ Branch 1 taken 20004 times.
 80016 for (size_t id = 0; id < 3; id++) {
128 60012 indexVel = id + configSize;
129
3/4
✓ Branch 1 taken 60012 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 29769 times.
✓ Branch 4 taken 30243 times.
 60012 if (tv[id] > 0) // v2 : velocity at the beginning of t2 phase
130
1/2
✓ Branch 1 taken 29769 times.
✗ Branch 2 not taken.
 29769 v2 = vLim[id];
131 else
132
3/6
✓ Branch 1 taken 30243 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30243 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 30243 times.
✗ Branch 8 not taken.
 30243 v2 = t1[id] * a1[id] + init[indexVel];
133
4/8
✓ Branch 1 taken 60012 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 60012 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 60012 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 60012 times.
✗ Branch 11 not taken.
 60012 pf = init[id] + init[indexVel] * (t0[id] + t1[id]) +
134
6/12
✓ Branch 1 taken 60012 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 60012 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 60012 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 60012 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 60012 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 60012 times.
✗ Branch 17 not taken.
 60012 0.5 * t1[id] * t1[id] * a1[id] + tv[id] * vLim[id] + v2 * t2[id] -
135
3/6
✓ Branch 1 taken 60012 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 60012 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 60012 times.
✗ Branch 8 not taken.
 60012 0.5 * t2[id] * t2[id] * a1[id];
136
1/2
✓ Branch 1 taken 60012 times.
✗ Branch 2 not taken.
 60012 if (fabs(pf - end[id]) > 1e-6) {
137 hppDout(error,
138 "ERROR : kinodynamic path do not connect to end config for index "
139 << id << " , pf = " << pf << " should be : " << end[id]);
140 }
141
2/4
✓ Branch 1 taken 60012 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 60012 times.
✗ Branch 4 not taken.
 60012 assert(
142 fabs(pf - end[id]) < 1e-6 &&
143 "Kinodynamic path do not connect to end config with given parameters.");
144 }
145 20004 }
146
147 11000 KinodynamicPath::KinodynamicPath(const KinodynamicPath& path)
148 : parent_t(path),
149 11000 device_(path.device_),
150
1/2
✓ Branch 1 taken 11000 times.
✗ Branch 2 not taken.
 11000 a1_(path.a1_),
151
1/2
✓ Branch 1 taken 11000 times.
✗ Branch 2 not taken.
 11000 t0_(path.t0_),
152
1/2
✓ Branch 1 taken 11000 times.
✗ Branch 2 not taken.
 11000 t1_(path.t1_),
153
1/2
✓ Branch 1 taken 11000 times.
✗ Branch 2 not taken.
 11000 tv_(path.tv_),
154
1/2
✓ Branch 1 taken 11000 times.
✗ Branch 2 not taken.
 11000 t2_(path.t2_),
155
1/2
✓ Branch 3 taken 11000 times.
✗ Branch 4 not taken.
 22000 vLim_(path.vLim_) {}
156
157 ✗ KinodynamicPath::KinodynamicPath(const KinodynamicPath& path,
158 ✗ const ConstraintSetPtr_t& constraints)
159 : parent_t(path, constraints),
160 ✗ device_(path.device_),
161 ✗ a1_(path.a1_),
162 ✗ t0_(path.t0_),
163 ✗ t1_(path.t1_),
164 ✗ tv_(path.tv_),
165 ✗ t2_(path.t2_),
166 ✗ vLim_(path.vLim_) {
167 ✗ assert(constraints->apply(initial_));
168 ✗ assert(constraints->apply(end_));
169 ✗ assert(constraints->isSatisfied(initial_));
170 ✗ assert(constraints->isSatisfied(end_));
171 }
172
173 196701 bool KinodynamicPath::impl_compute(ConfigurationOut_t result,
174 value_type t) const {
175
2/4
✓ Branch 4 taken 196701 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 196701 times.
 196701 assert(result.size() == device()->configSize());
176
177
5/6
✓ Branch 1 taken 174693 times.
✓ Branch 2 taken 22008 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 174693 times.
✓ Branch 6 taken 22008 times.
✓ Branch 7 taken 174693 times.
 196701 if (t == timeRange().first || timeRange().second == 0) {
178
1/2
✓ Branch 1 taken 22008 times.
✗ Branch 2 not taken.
 22008 result = initial_;
179 22008 return true;
180 }
181
2/2
✓ Branch 1 taken 22007 times.
✓ Branch 2 taken 152686 times.
 174693 if (t == timeRange().second) {
182
1/2
✓ Branch 1 taken 22007 times.
✗ Branch 2 not taken.
 22007 result = end_;
183 22007 return true;
184 }
185
186 size_type configSize =
187
1/2
✓ Branch 3 taken 152686 times.
✗ Branch 4 not taken.
 152686 device()->configSize() - device()->extraConfigSpace().dimension();
188 double v2, t2, t1, tv;
189 size_type indexVel;
190 size_type indexAcc;
191 // straight path for all the joints, except the translations of the base :
192 152686 value_type u = t / timeRange().second;
193
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 152686 times.
 152686 if (timeRange().second == 0) u = 0;
194
195
5/10
✓ Branch 1 taken 152686 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 152686 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 152686 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 152686 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 152686 times.
✗ Branch 14 not taken.
 305372 pinocchio::interpolate(device_, initial_.head(configSize),
196
1/2
✓ Branch 1 taken 152686 times.
✗ Branch 2 not taken.
 152686 end_.head(configSize), u, result);
197
198
2/2
✓ Branch 0 taken 458058 times.
✓ Branch 1 taken 152686 times.
 610744 for (int id = 0; id < 3;
199 id++) { // FIX ME : only work for freeflyer (translation part)
200
201 458058 indexVel = id + configSize;
202 458058 indexAcc = id + configSize + 3;
203
204
4/8
✓ Branch 3 taken 458058 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 458058 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 458058 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 458058 times.
✗ Branch 14 not taken.
 458058 if (device()->getJointAtConfigRank(id)->name() != "base_joint_SO3") {
205 // 3 case (each segment of the trajectory) :
206
2/4
✓ Branch 1 taken 458058 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 458058 times.
 458058 if (t <= t0_[id]) { // before first segment
207 // hppDout(info,"before 1° segment");
208 ✗ result[id] = initial_[id] + t * initial_[indexVel];
209 ✗ result[indexVel] = initial_[indexVel];
210 ✗ result[indexAcc] = 0;
211
4/6
✓ Branch 1 taken 458058 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 458058 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 127300 times.
✓ Branch 7 taken 330758 times.
 458058 } else if (t <= (t0_[id] + t1_[id])) {
212 // hppDout(info,"on 1° segment");
213
1/2
✓ Branch 1 taken 127300 times.
✗ Branch 2 not taken.
 127300 t1 = t - t0_[id];
214
2/4
✓ Branch 1 taken 127300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 127300 times.
✗ Branch 5 not taken.
 127300 result[id] = 0.5 * t1 * t1 * a1_[id] + t1 * initial_[indexVel] +
215
4/8
✓ Branch 1 taken 127300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 127300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 127300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 127300 times.
✗ Branch 11 not taken.
 127300 initial_[id] + t0_[id] * initial_[indexVel];
216
3/6
✓ Branch 1 taken 127300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 127300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 127300 times.
✗ Branch 8 not taken.
 127300 result[indexVel] = t1 * a1_[id] + initial_[indexVel];
217
2/4
✓ Branch 1 taken 127300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 127300 times.
✗ Branch 5 not taken.
 127300 result[indexAcc] = a1_[id];
218
5/8
✓ Branch 1 taken 330758 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 330758 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 330758 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 205134 times.
✓ Branch 10 taken 125624 times.
 330758 } else if (t <= (t0_[id] + t1_[id] + tv_[id])) {
219 // hppDout(info,"on constant velocity segment");
220
2/4
✓ Branch 1 taken 205134 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 205134 times.
✗ Branch 5 not taken.
 205134 tv = t - t0_[id] - t1_[id];
221
3/6
✓ Branch 1 taken 205134 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 205134 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 205134 times.
✗ Branch 8 not taken.
 205134 result[id] = 0.5 * t1_[id] * t1_[id] * a1_[id] +
222
3/6
✓ Branch 1 taken 205134 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 205134 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 205134 times.
✗ Branch 8 not taken.
 205134 t1_[id] * initial_[indexVel] + initial_[id] +
223
4/8
✓ Branch 1 taken 205134 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 205134 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 205134 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 205134 times.
✗ Branch 11 not taken.
 205134 t0_[id] * initial_[indexVel] + (tv)*vLim_[id];
224
2/4
✓ Branch 1 taken 205134 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 205134 times.
✗ Branch 5 not taken.
 205134 result[indexVel] = vLim_[id];
225
1/2
✓ Branch 1 taken 205134 times.
✗ Branch 2 not taken.
 205134 result[indexAcc] = 0.;
226
227
5/10
✓ Branch 1 taken 125624 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 125624 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 125624 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 125624 times.
✗ Branch 11 not taken.
✓ Branch 12 taken 125624 times.
✗ Branch 13 not taken.
 125624 } else if (t <= (t0_[id] + t1_[id] + tv_[id] + t2_[id])) {
228 // hppDout(info,"on 3° segment");
229
3/6
✓ Branch 1 taken 125624 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 125624 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 125624 times.
✗ Branch 8 not taken.
 125624 t2 = t - tv_[id] - t1_[id] - t0_[id];
230
3/4
✓ Branch 1 taken 125624 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 41283 times.
✓ Branch 4 taken 84341 times.
 125624 if (tv_[id] > 0)
231
1/2
✓ Branch 1 taken 41283 times.
✗ Branch 2 not taken.
 41283 v2 = vLim_[id];
232 else
233
3/6
✓ Branch 1 taken 84341 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 84341 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 84341 times.
✗ Branch 8 not taken.
 84341 v2 = t1_[id] * a1_[id] + initial_[indexVel];
234
3/6
✓ Branch 1 taken 125624 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 125624 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 125624 times.
✗ Branch 8 not taken.
 125624 result[id] = 0.5 * t1_[id] * t1_[id] * a1_[id] +
235
3/6
✓ Branch 1 taken 125624 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 125624 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 125624 times.
✗ Branch 8 not taken.
 125624 t1_[id] * initial_[indexVel] + initial_[id] +
236
4/8
✓ Branch 1 taken 125624 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 125624 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 125624 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 125624 times.
✗ Branch 11 not taken.
 125624 t0_[id] * initial_[indexVel] + tv_[id] * vLim_[id] -
237
2/4
✓ Branch 1 taken 125624 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 125624 times.
✗ Branch 5 not taken.
 125624 0.5 * t2 * t2 * a1_[id] + t2 * v2;
238
2/4
✓ Branch 1 taken 125624 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 125624 times.
✗ Branch 5 not taken.
 125624 result[indexVel] = v2 - t2 * a1_[id];
239
2/4
✓ Branch 1 taken 125624 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 125624 times.
✗ Branch 5 not taken.
 125624 result[indexAcc] = -a1_[id];
240
241 } else { // after last segment : constant velocity (null ?)
242 ✗ if (end_[indexVel] != 0 &&
243 ✗ (t - (t0_[id] + t1_[id] + tv_[id] + t2_[id])) >
244 0.00015) { // epsilon from rbprm-steering-kinodynamic
245 hppDout(notice,
246 "Should not happen : You should not have a constant velocity "
247 "segment at the end if the velocity is not null");
248 hppDout(notice, "index Joint = " << id);
249 hppDout(notice, "t = " << t << " ; length = "
250 << t0_[id] + t1_[id] + tv_[id] + t2_[id]);
251 }
252 ✗ result[id] = end_[id];
253 ✗ result[indexVel] = end_[indexVel];
254 ✗ result[indexAcc] = 0;
255 }
256 } // if not quaternion joint
257 } // for all joints
258
259 152686 return true;
260 }
261
262 20004 PathPtr_t KinodynamicPath::impl_extract(const interval_t& subInterval) const {
263
2/4
✓ Branch 0 taken 20004 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 20004 times.
✗ Branch 3 not taken.
 20004 assert(subInterval.first >= 0 && subInterval.second >= 0 &&
264 "Negative time values in extract path");
265
2/4
✓ Branch 1 taken 20004 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20004 times.
✗ Branch 5 not taken.
 20004 assert(subInterval.first >= timeRange().first &&
266 subInterval.second <= timeRange().second &&
267 "Given interval not inside path interval");
268 // Length is assumed to be proportional to interval range
269
3/4
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 20002 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 20004 times.
 20006 if (subInterval.first == timeRange().first &&
270
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 2 subInterval.second == timeRange().second) {
271 hppDout(notice, "Call extract with same interval");
272 ✗ return weak_.lock();
273 }
274 bool success;
275 20004 value_type l = fabs(subInterval.second - subInterval.first);
276
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 20004 times.
 20004 if (l <= 0) {
277 hppDout(notice, "Call Extract with a length null");
278 ✗ return StraightPath::create(device_, (eval(subInterval.first, success)),
279 ✗ (eval(subInterval.first, success)), 0.);
280 }
281
282 hppDout(notice, "%% EXTRACT PATH : path interval : "
283 << timeRange().first << " ; " << timeRange().second);
284 hppDout(notice, "%% EXTRACT PATH : sub interval : "
285 << subInterval.first << " ; " << subInterval.second);
286
287
1/2
✓ Branch 1 taken 20004 times.
✗ Branch 2 not taken.
 20004 Configuration_t q1(eval(subInterval.first, success));
288
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 20004 times.
 20004 if (!success)
289 ✗ throw projection_error(
290 ✗ "Failed to apply constraints in KinodynamicPath::extract");
291
1/2
✓ Branch 1 taken 20004 times.
✗ Branch 2 not taken.
 20004 Configuration_t q2(eval(subInterval.second, success));
292
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 20004 times.
 20004 if (!success)
293 ✗ throw projection_error(
294 ✗ "Failed to apply constraints in KinodynamicPath::extract");
295
296 // set acceleration to 0 for initial and end config :
297 size_type configSize =
298
1/2
✓ Branch 3 taken 20004 times.
✗ Branch 4 not taken.
 20004 device()->configSize() - device()->extraConfigSpace().dimension();
299
1/2
✓ Branch 1 taken 20004 times.
✗ Branch 2 not taken.
 20004 q1[configSize + 3] = 0.0;
300
1/2
✓ Branch 1 taken 20004 times.
✗ Branch 2 not taken.
 20004 q1[configSize + 4] = 0.0;
301
1/2
✓ Branch 1 taken 20004 times.
✗ Branch 2 not taken.
 20004 q1[configSize + 5] = 0.0;
302
1/2
✓ Branch 1 taken 20004 times.
✗ Branch 2 not taken.
 20004 q2[configSize + 3] = 0.0;
303
1/2
✓ Branch 1 taken 20004 times.
✗ Branch 2 not taken.
 20004 q2[configSize + 4] = 0.0;
304
1/2
✓ Branch 1 taken 20004 times.
✗ Branch 2 not taken.
 20004 q2[configSize + 5] = 0.0;
305 hppDout(info, "from : ");
306 hppDout(info, "q1 = " << pinocchio::displayConfig(initial_));
307 hppDout(info, "q2 = " << pinocchio::displayConfig(end_));
308 hppDout(info, "to : ");
309 hppDout(info, "q1 = " << pinocchio::displayConfig(q1));
310 hppDout(info, "q2 = " << pinocchio::displayConfig(q2));
311
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 20004 times.
 20004 if (!success)
312 ✗ throw projection_error(
313 ✗ "Failed to apply constraints in KinodynamicPath::extract");
314
315
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 20004 times.
 20004 if (subInterval.first > subInterval.second) { // reversed path
316 hppDout(notice, "%% REVERSE PATH, not implemented yet !");
317 ✗ std::cout << "ERROR, you shouldn't call reverse() on a kinodynamic path"
318 ✗ << std::endl;
319 ✗ return PathPtr_t();
320 }
321 double ti, tf, oldT0, oldT2, oldT1, oldTv;
322 hppDout(notice, "%% subinterval PATH");
323 // new timebounds
324
1/2
✓ Branch 1 taken 20004 times.
✗ Branch 2 not taken.
 20004 Configuration_t t0(t0_);
325
1/2
✓ Branch 1 taken 20004 times.
✗ Branch 2 not taken.
 20004 Configuration_t t1(t1_);
326
1/2
✓ Branch 1 taken 20004 times.
✗ Branch 2 not taken.
 20004 Configuration_t t2(t2_);
327
1/2
✓ Branch 1 taken 20004 times.
✗ Branch 2 not taken.
 20004 Configuration_t tv(tv_);
328
1/2
✓ Branch 1 taken 20004 times.
✗ Branch 2 not taken.
 20004 Configuration_t a1(a1_);
329
330
2/2
✓ Branch 1 taken 60012 times.
✓ Branch 2 taken 20004 times.
 80016 for (int i = 0; i < a1_.size(); ++i) { // adjust times bounds
331 60012 ti = subInterval.first - timeRange().first;
332 60012 tf = timeRange().second - subInterval.second;
333
2/4
✓ Branch 1 taken 60012 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 60012 times.
✗ Branch 5 not taken.
 60012 t0[i] = t0_[i] - ti;
334
2/4
✓ Branch 1 taken 60012 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 60012 times.
✗ Branch 4 not taken.
 60012 if (t0[i] <= 0) {
335
1/2
✓ Branch 1 taken 60012 times.
✗ Branch 2 not taken.
 60012 t0[i] = 0;
336
1/2
✓ Branch 1 taken 60012 times.
✗ Branch 2 not taken.
 60012 ti = ti - t0_[i];
337
2/4
✓ Branch 1 taken 60012 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 60012 times.
✗ Branch 5 not taken.
 60012 t1[i] = t1_[i] - ti;
338
3/4
✓ Branch 1 taken 60012 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 34626 times.
✓ Branch 4 taken 25386 times.
 60012 if (t1[i] <= 0) {
339
1/2
✓ Branch 1 taken 34626 times.
✗ Branch 2 not taken.
 34626 t1[i] = 0;
340
1/2
✓ Branch 1 taken 34626 times.
✗ Branch 2 not taken.
 34626 ti = ti - t1_[i];
341
2/4
✓ Branch 1 taken 34626 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34626 times.
✗ Branch 5 not taken.
 34626 tv[i] = tv_[i] - ti;
342
3/4
✓ Branch 1 taken 34626 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 11108 times.
✓ Branch 4 taken 23518 times.
 34626 if (tv[i] <= 0) {
343
1/2
✓ Branch 1 taken 11108 times.
✗ Branch 2 not taken.
 11108 tv[i] = 0;
344
1/2
✓ Branch 1 taken 11108 times.
✗ Branch 2 not taken.
 11108 ti = ti - tv_[i];
345
2/4
✓ Branch 1 taken 11108 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11108 times.
✗ Branch 5 not taken.
 11108 t2[i] = t2_[i] - ti;
346
2/4
✓ Branch 1 taken 11108 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 11108 times.
 11108 if (t2[i] < 0) {
347 ✗ t2[i] = 0;
348 hppDout(notice, "Should not happen !");
349 }
350 }
351 }
352 }
353
1/2
✓ Branch 1 taken 60012 times.
✗ Branch 2 not taken.
 60012 oldT0 = t0[i];
354
1/2
✓ Branch 1 taken 60012 times.
✗ Branch 2 not taken.
 60012 oldT1 = t1[i];
355
1/2
✓ Branch 1 taken 60012 times.
✗ Branch 2 not taken.
 60012 oldT2 = t2[i];
356
1/2
✓ Branch 1 taken 60012 times.
✗ Branch 2 not taken.
 60012 oldTv = tv[i];
357
1/2
✓ Branch 1 taken 60012 times.
✗ Branch 2 not taken.
 60012 t2[i] = oldT2 - tf;
358
3/4
✓ Branch 1 taken 60012 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 33960 times.
✓ Branch 4 taken 26052 times.
 60012 if (t2[i] <= 0) {
359
1/2
✓ Branch 1 taken 33960 times.
✗ Branch 2 not taken.
 33960 t2[i] = 0;
360 33960 tf = tf - oldT2;
361
1/2
✓ Branch 1 taken 33960 times.
✗ Branch 2 not taken.
 33960 tv[i] = oldTv - tf;
362
3/4
✓ Branch 1 taken 33960 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 10336 times.
✓ Branch 4 taken 23624 times.
 33960 if (tv[i] <= 0) {
363
1/2
✓ Branch 1 taken 10336 times.
✗ Branch 2 not taken.
 10336 tv[i] = 0;
364 10336 tf = tf - oldTv;
365
1/2
✓ Branch 1 taken 10336 times.
✗ Branch 2 not taken.
 10336 t1[i] = oldT1 - tf;
366
2/4
✓ Branch 1 taken 10336 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 10336 times.
 10336 if (t1[i] <= 0) {
367 ✗ t1[i] = 0;
368 ✗ tf = tf - oldT1;
369 ✗ t0[i] = oldT0 - tf;
370 ✗ if (t0[i] < 0) {
371 ✗ t0[i] = 0;
372 hppDout(notice, "Should not happen !");
373 }
374 }
375 }
376 }
377
378 } // for all joints
379
8/16
✓ Branch 2 taken 20004 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 20004 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 20004 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 20004 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 20004 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 20004 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 20004 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 20004 times.
✗ Branch 24 not taken.
 60012 PathPtr_t result = KinodynamicPath::create(device_, q1, q2, l, a1, t0, t1, tv,
380
1/2
✓ Branch 2 taken 20004 times.
✗ Branch 3 not taken.
 60012 t2, vLim_, constraints());
381 20004 return result;
382 20004 }
383
384 } // namespace core
385 } // namespace hpp
386