GCC Code Coverage Report

Directory: ./
File: src/steering-method/constant-curvature.cc
Date: 2025-03-10 11:18:21
Exec Total Coverage
Lines: 142 309 46.0%
Branches: 108 443 24.4%
Line Branch Exec Source
1 //
2 // Copyright (c) 2017 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/vector.hpp>
31 #include <boost/serialization/weak_ptr.hpp>
32 #include <hpp/core/steering-method/constant-curvature.hh>
33 #include <hpp/pinocchio/configuration.hh>
34 #include <hpp/pinocchio/device.hh>
35 #include <hpp/pinocchio/joint.hh>
36 #include <hpp/pinocchio/liegroup.hh>
37 #include <hpp/pinocchio/serialization.hh>
38 #include <hpp/util/serialization.hh>
39 #include <pinocchio/serialization/eigen.hpp>
40 #include <pinocchio/spatial/se3.hpp>
41
42 namespace hpp {
43 namespace core {
44 namespace steeringMethod {
45 namespace details {
46 using std::max;
47
48 /// \param t0, t1 angles between \f$ 0 \f$ and \f$ 2 \pi \f$.
49 ✗ value_type absCosineMax(const value_type& t0, const value_type& t1) {
50 ✗ assert(0 <= t0 && t0 <= t1 && t1 <= 2 * M_PI);
51 ✗ if (t1 <= M_PI) return max(fabs(cos(t0)), fabs(cos(t1)));
52 // M_PI < t1 <= 2*M_PI
53 ✗ if (t0 <= M_PI) return 1.;
54 // M_PI < t0 <= t1 <= 2*M_PI
55 ✗ return max(fabs(cos(t0)), fabs(cos(t1)));
56 }
57
58 /// \param t0, t1 angles between \f$ 0 \f$ and \f$ 2 \pi \f$.
59 ✗ value_type absSineMax(const value_type& t0, const value_type& t1) {
60 ✗ assert(0 <= t0 && t0 <= t1 && t1 <= 2 * M_PI);
61 ✗ if (t1 <= M_PI_2) return sin(t1);
62 ✗ if (t0 <= M_PI_2) return 1.;
63 ✗ if (t0 >= 3 * M_PI_2) return -sin(t0);
64 // M_PI_2 < t0 < 3*M_PI_2
65 ✗ if (t1 >= 3 * M_PI_2) return 1.;
66 ✗ return max(sin(t0), sin(t1));
67 }
68 } // namespace details
69
70 17 ConstantCurvaturePtr_t ConstantCurvature::create(
71 const DevicePtr_t& robot, ConfigurationIn_t init, ConfigurationIn_t end,
72 value_type curveLength, value_type pathLength, value_type curvature,
73 size_type xyId, size_type rzId, const JointPtr_t rz,
74 const std::vector<JointPtr_t> wheels,
75 const ConstraintSetPtr_t& constraints) {
76 ConstantCurvature* ptr;
77
2/2
✓ Branch 1 taken 15 times.
✓ Branch 2 taken 2 times.
 17 if (constraints)
78
2/4
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 15 times.
✗ Branch 7 not taken.
 30 ptr = new ConstantCurvature(robot, init, end, curveLength, pathLength,
79
2/4
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
 15 curvature, xyId, rzId, rz, wheels, constraints);
80 else
81
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
 4 ptr = new ConstantCurvature(robot, init, end, curveLength, pathLength,
82
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 curvature, xyId, rzId, rz, wheels);
83 17 ConstantCurvaturePtr_t shPtr(ptr);
84
1/2
✓ Branch 2 taken 17 times.
✗ Branch 3 not taken.
 17 ptr->init(shPtr);
85 17 return shPtr;
86 }
87
88 12 ConstantCurvaturePtr_t ConstantCurvature::createCopy(
89 const ConstantCurvaturePtr_t& other) {
90
1/2
✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
 12 ConstantCurvature* ptr(new ConstantCurvature(*other));
91 12 ConstantCurvaturePtr_t shPtr(ptr);
92
1/2
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
 12 ptr->init(shPtr);
93 12 return shPtr;
94 }
95
96 ✗ ConstantCurvaturePtr_t ConstantCurvature::createCopy(
97 const ConstantCurvaturePtr_t& other,
98 const ConstraintSetPtr_t& constraints) {
99 ✗ ConstantCurvature* ptr = new ConstantCurvature(*other, constraints);
100 ✗ ConstantCurvaturePtr_t shPtr(ptr);
101 ✗ ptr->init(shPtr);
102 ✗ return shPtr;
103 }
104
105 /// Return a shared pointer to a copy of this
106 ✗ PathPtr_t ConstantCurvature::copy() const { return createCopy(weak_.lock()); }
107
108 ✗ std::ostream& ConstantCurvature::print(std::ostream& os) const {
109 ✗ os << "-- ConstantCurvature" << std::endl;
110 ✗ os << "from " << initial_.transpose() << std::endl;
111 ✗ os << " length: " << forward_ * paramLength() << std::endl;
112 ✗ os << " curvature: " << curvature_ << std::endl;
113 ✗ return os;
114 }
115
116 15 ConstantCurvature::ConstantCurvature(
117 const DevicePtr_t& robot, ConfigurationIn_t init, ConfigurationIn_t end,
118 value_type curveLength, value_type pathLength, value_type curvature,
119 size_type xyId, size_type rzId, const JointPtr_t rz,
120 15 const std::vector<JointPtr_t> wheels, ConstraintSetPtr_t constraints)
121 ✗ : Path(std::make_pair(0., fabs(pathLength)), robot->configSize(),
122 robot->numberDof(), constraints),
123 15 robot_(robot),
124
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 initial_(init),
125
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 end_(end),
126 15 curveLength_(curveLength),
127 15 curvature_(curvature),
128 15 xyId_(xyId),
129 15 rzId_(rzId),
130
4/6
✓ Branch 5 taken 15 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 15 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 5 times.
✓ Branch 12 taken 10 times.
 30 forward_(curveLength > 0 ? 1 : -1) {
131 // Find rank of translation and rotation in velocity vectors
132 // Hypothesis: degrees of freedom all belong to a planar joint or
133 // xyId_ belong to a tranlation joint, rzId_ belongs to a SO2 joint.
134
1/2
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
 15 JointPtr_t joint(robot_->getJointAtConfigRank(xyId_));
135
1/2
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
 15 size_type offset(xyId_ - joint->rankInConfiguration());
136
1/2
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
 15 dxyId_ = joint->rankInVelocity() + offset;
137
1/2
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
 15 joint = robot_->getJointAtConfigRank(rzId_);
138
1/2
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
 15 offset = rzId_ - joint->rankInConfiguration();
139
1/2
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
 15 drzId_ = joint->rankInVelocity() + offset;
140
2/4
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 15 times.
✗ Branch 6 not taken.
 15 setWheelJoints(rz, wheels);
141
2/4
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 15 times.
✗ Branch 6 not taken.
 15 impl_compute(end_, paramRange().second);
142 15 }
143
144 2 ConstantCurvature::ConstantCurvature(
145 const DevicePtr_t& robot, ConfigurationIn_t init, ConfigurationIn_t end,
146 value_type curveLength, value_type pathLength, value_type curvature,
147 size_type xyId, size_type rzId, const JointPtr_t rz,
148 2 const std::vector<JointPtr_t> wheels)
149 ✗ : Path(std::make_pair(0., fabs(pathLength)), robot->configSize(),
150 robot->numberDof()),
151 2 robot_(robot),
152
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 initial_(init),
153
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 end_(end),
154 2 curveLength_(curveLength),
155 2 curvature_(curvature),
156 2 xyId_(xyId),
157 2 rzId_(rzId),
158
3/6
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✓ Branch 12 taken 2 times.
 4 forward_(curveLength > 0 ? 1 : -1) {
159 // Find rank of translation and rotation in velocity vectors
160 // Hypothesis: degrees of freedom all belong to a planar joint or
161 // xyId_ belong to a tranlation joint, rzId_ belongs to a SO2 joint.
162
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 JointPtr_t joint(robot_->getJointAtConfigRank(xyId_));
163
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 size_type offset(xyId_ - joint->rankInConfiguration());
164
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 dxyId_ = joint->rankInVelocity() + offset;
165
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 joint = robot_->getJointAtConfigRank(rzId_);
166
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 offset = rzId_ - joint->rankInConfiguration();
167
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 drzId_ = joint->rankInVelocity() + offset;
168
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 2 setWheelJoints(rz, wheels);
169
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 2 impl_compute(end_, paramRange().second);
170 2 }
171
172 12 ConstantCurvature::ConstantCurvature(const ConstantCurvature& other)
173 : Path(other),
174 12 robot_(other.robot_),
175
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 initial_(other.initial_),
176
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 end_(other.end_),
177 12 curveLength_(other.curveLength_),
178 12 curvature_(other.curvature_),
179 12 xyId_(other.xyId_),
180 12 rzId_(other.rzId_),
181 12 dxyId_(other.dxyId_),
182 12 drzId_(other.drzId_),
183 12 forward_(other.forward_),
184
1/2
✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
 24 wheels_(other.wheels_) {}
185
186 ✗ ConstantCurvature::ConstantCurvature(const ConstantCurvature& other,
187 ✗ const ConstraintSetPtr_t& constraints)
188 : parent_t(other, constraints),
189 ✗ robot_(other.robot_),
190 ✗ initial_(other.initial_),
191 ✗ end_(other.end_),
192 ✗ curveLength_(other.curveLength_),
193 ✗ curvature_(other.curvature_),
194 ✗ xyId_(other.xyId_),
195 ✗ rzId_(other.rzId_),
196 ✗ dxyId_(other.dxyId_),
197 ✗ drzId_(other.drzId_),
198 ✗ forward_(other.forward_),
199 ✗ wheels_(other.wheels_) {}
200
201 379 bool ConstantCurvature::impl_compute(ConfigurationOut_t result,
202 value_type param) const {
203 379 const value_type L = paramLength();
204 // Does a linear interpolation on all the joints.
205
2/2
✓ Branch 0 taken 376 times.
✓ Branch 1 taken 3 times.
 379 const value_type u = (L == 0) ? 0 : ((param - paramRange().first) / L);
206
4/8
✓ Branch 1 taken 379 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 379 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 379 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 379 times.
✗ Branch 11 not taken.
 379 pinocchio::interpolate<pinocchio::RnxSOnLieGroupMap>(robot_, initial_, end_,
207 u, result);
208
209 379 value_type t(u * curveLength_);
210
2/4
✓ Branch 1 taken 379 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 379 times.
✗ Branch 5 not taken.
 379 value_type x0(initial_[xyId_ + 0]), y0(initial_[xyId_ + 1]);
211
2/4
✓ Branch 1 taken 379 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 379 times.
✗ Branch 5 not taken.
 379 value_type c0(initial_[rzId_ + 0]), s0(initial_[rzId_ + 1]);
212 value_type x, y;
213 379 value_type c(cos(curvature_ * t)), s(sin(curvature_ * t));
214
215
2/2
✓ Branch 0 taken 271 times.
✓ Branch 1 taken 108 times.
 379 if (curvature_ == 0) {
216 271 x = x0 + t * c0;
217 271 y = y0 + t * s0;
218 } else {
219 108 value_type r(1. / curvature_);
220 108 x = x0 + r * (s0 * (c - 1) + c0 * s);
221 108 y = y0 + r * (c0 * (1 - c) + s0 * s);
222 }
223
1/2
✓ Branch 1 taken 379 times.
✗ Branch 2 not taken.
 379 result[xyId_ + 0] = x;
224
1/2
✓ Branch 1 taken 379 times.
✗ Branch 2 not taken.
 379 result[xyId_ + 1] = y;
225
1/2
✓ Branch 1 taken 379 times.
✗ Branch 2 not taken.
 379 result[rzId_ + 0] = c0 * c - s0 * s;
226
1/2
✓ Branch 1 taken 379 times.
✗ Branch 2 not taken.
 379 result[rzId_ + 1] = c0 * s + s0 * c;
227
228 // Set wheel joint positions
229 379 for (std::vector<Wheels_t>::const_iterator w = wheels_.begin();
230
2/2
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 379 times.
 381 w < wheels_.end(); ++w) {
231
2/4
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 result[w->j->rankInConfiguration()] = w->value;
232 }
233 379 return true;
234 }
235
236 ✗ void ConstantCurvature::impl_derivative(vectorOut_t result,
237 const value_type& param,
238 size_type order) const {
239 ✗ const value_type L = paramLength();
240 // Does a linear interpolation on all the joints.
241 ✗ const value_type u = (L == 0) ? 0 : ((param - paramRange().first) / L);
242 ✗ if (order == 1) {
243 ✗ pinocchio::difference<pinocchio::RnxSOnLieGroupMap>(robot_, end_, initial_,
244 result);
245 ✗ result /= L;
246 ✗ } else if (order > 1) {
247 ✗ result.setZero();
248 }
249
250 ✗ value_type t(u * curveLength_);
251 ✗ value_type beta(curveLength_ / L);
252 ✗ if (L == 0) beta = 1;
253 ✗ value_type alpha(fabs(beta));
254 ✗ if (forward_ == -1 && order % 2 == 1) alpha *= -1;
255 ✗ value_type c0(initial_[rzId_ + 0]), s0(initial_[rzId_ + 1]);
256 ✗ value_type dx, dy, dtheta = 0;
257 ✗ value_type c(cos(curvature_ * t)), s(sin(curvature_ * t));
258
259 ✗ if (order <= 0) {
260 ✗ std::ostringstream oss;
261 ✗ oss << "order of derivative (" << order << ") should be positive.";
262 ✗ throw std::runtime_error(oss.str().c_str());
263 }
264 ✗ if (order == 1) {
265 ✗ dx = alpha * (c0 * c - s0 * s);
266 ✗ dy = alpha * (c0 * s + s0 * c);
267 ✗ dtheta = alpha * curvature_;
268 } else {
269 ✗ switch (order % 4) {
270 ✗ case 2:
271 ✗ dx = alpha * pow(curvature_ * beta, (value_type)(order - 1)) *
272 ✗ (c0 * s + s0 * c);
273 ✗ dy = -alpha * pow(curvature_ * beta, (value_type)(order - 1)) *
274 ✗ (c0 * c - s0 * s);
275 ✗ break;
276 ✗ case 3:
277 ✗ dx = -alpha * pow(curvature_ * beta, (value_type)(order - 1)) *
278 ✗ (c0 * c - s0 * s);
279 ✗ dy = -alpha * pow(curvature_ * beta, (value_type)(order - 1)) *
280 ✗ (c0 * s + s0 * c);
281 ✗ break;
282 ✗ case 0:
283 ✗ dx = alpha * pow(curvature_ * beta, (value_type)(order - 1)) *
284 ✗ (c0 * s + s0 * c);
285 ✗ dy = -alpha * pow(curvature_ * beta, (value_type)(order - 1)) *
286 ✗ (c0 * c - s0 * s);
287 ✗ break;
288 ✗ case 1:
289 ✗ dx = alpha * pow(curvature_ * beta, (value_type)(order - 1)) *
290 ✗ (c0 * c - s0 * s);
291 ✗ dy = alpha * pow(curvature_ * beta, (value_type)(order - 1)) *
292 ✗ (c0 * s + s0 * c);
293 ✗ break;
294 }
295 }
296
297 ✗ result[dxyId_ + 0] = dx;
298 ✗ result[dxyId_ + 1] = dy;
299 ✗ result[drzId_] = dtheta;
300 // Express velocity in local frame
301 ✗ Eigen::Matrix<value_type, 2, 2> R;
302 ✗ R.col(0) << c0 * c - s0 * s, c0 * s + s0 * c;
303 ✗ R.col(1) << -R(1, 0), R(0, 0);
304 ✗ result.segment<2>(dxyId_) = R.transpose() * result.segment<2>(dxyId_);
305
306 // Set wheel joint velocities
307 ✗ for (std::vector<Wheels_t>::const_iterator w = wheels_.begin();
308 ✗ w < wheels_.end(); ++w) {
309 ✗ result[w->j->rankInVelocity()] = 0;
310 }
311 }
312
313 ✗ void ConstantCurvature::impl_velocityBound(vectorOut_t bound,
314 const value_type& param0,
315 const value_type& param1) const {
316 // Does a linear interpolation on all the joints.
317 ✗ if (paramRange().first == paramRange().second) {
318 ✗ bound.setZero();
319 ✗ return;
320 }
321 ✗ const value_type L = paramLength();
322 ✗ assert(L > 0);
323 ✗ pinocchio::difference<pinocchio::RnxSOnLieGroupMap>(robot_, end_, initial_,
324 bound);
325 ✗ bound.noalias() = bound.cwiseAbs() / L;
326
327 ✗ value_type alpha(fabs(curveLength_) / L);
328
329 // Compute max of
330 // dx = alpha * (c0 * c - s0 * s);
331 // dy = alpha * (c0 * s + s0 * c);
332 // dtheta = alpha * curvature_;
333 // onto [ param0, param1 ]
334 ✗ value_type Mx = std::numeric_limits<value_type>::quiet_NaN(),
335 ✗ My = std::numeric_limits<value_type>::quiet_NaN();
336
337 ✗ if (curvature_ != 0) {
338 ✗ value_type t0 = curvature_ * curveLength_ * (param0 - paramRange().first) /
339 ✗ L,
340 ✗ t1 = curvature_ * curveLength_ * (param1 - paramRange().first) /
341 ✗ L;
342 ✗ if (t0 > t1) std::swap(t0, t1);
343 ✗ if (t1 - t0 >= M_PI)
344 ✗ Mx = My = 1;
345 else {
346 // Compute thetaI
347 ✗ const value_type c0(initial_[rzId_ + 0]), s0(initial_[rzId_ + 1]);
348 ✗ const value_type tI(atan2(s0, c0));
349 ✗ t0 += tI;
350 ✗ t1 += tI;
351 // max(|c0 * c - s0 * s|) = max(|cos(t)|) for t in [t0, t1]
352 // max(|c0 * s + s0 * c|) = max(|sin(t)|) for t in [t0, t1]
353
354 ✗ value_type kd = std::floor(t0 * M_1_PI / 2) * 2 * M_PI;
355 ✗ t0 -= kd;
356 ✗ t1 -= kd;
357 ✗ assert(0 <= t0 && t0 <= t1 && t1 <= 2 * M_PI);
358
359 ✗ Mx = details::absCosineMax(t0, t1);
360 ✗ My = details::absSineMax(t0, t1);
361 }
362 } else {
363 ✗ Mx = 1.;
364 ✗ My = 0.;
365 }
366 ✗ assert(Mx >= 0. && My >= 0.);
367 ✗ assert(Mx <= 1. && My <= 1.);
368
369 ✗ bound[dxyId_ + 0] = alpha * Mx;
370 ✗ bound[dxyId_ + 1] = alpha * My;
371 ✗ bound[drzId_] = alpha * std::fabs(curvature_);
372
373 // Set wheel joint velocities
374 ✗ for (std::vector<Wheels_t>::const_iterator w = wheels_.begin();
375 ✗ w < wheels_.end(); ++w) {
376 ✗ bound[w->j->rankInVelocity()] = 0;
377 }
378 }
379
380 4 PathPtr_t ConstantCurvature::impl_extract(
381 const interval_t& paramInterval) const {
382
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
 4 assert(!timeParameterization());
383
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
 4 assert(timeRange().second - timeRange().first >= 0);
384 4 value_type L(timeRange().second - timeRange().first);
385 4 value_type tmin(paramInterval.first), tmax(paramInterval.second);
386 4 value_type curveLength = 0, pathLength = 0;
387
1/2
✓ Branch 0 taken 4 times.
✗ Branch 1 not taken.
 4 if (L != 0) {
388 4 curveLength = ((tmax - tmin) / L * curveLength_);
389
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 pathLength = fabs(tmax - tmin) / L * length();
390 }
391
4/8
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 4 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 4 times.
✗ Branch 13 not taken.
 4 Configuration_t init(robot_->configSize()), end(robot_->configSize());
392 4 auto tr = timeRange();
393 4 bool res(true);
394
1/2
✓ Branch 0 taken 4 times.
✗ Branch 1 not taken.
 4 if (tmin == tr.first)
395
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 init = initial_;
396 ✗ else if (tmin == tr.second)
397 ✗ init = end_;
398 else
399 ✗ res = impl_compute(init, tmin);
400
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
 4 assert(res);
401 (void)res;
402
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
 4 if (tmax == tr.first)
403 ✗ end = initial_;
404
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 2 times.
 4 else if (tmax == tr.second)
405
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 end = end_;
406 else
407
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 res = impl_compute(end, tmax);
408
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
 4 assert(res);
409
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 4 ConstantCurvaturePtr_t result(createCopy(weak_.lock()));
410 // swap to avoid memory allocation.
411
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 4 result->initial_.swap(init);
412
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 4 result->end_.swap(end);
413 4 result->curveLength_ = curveLength;
414
1/2
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
 4 result->timeRange(interval_t(0, pathLength));
415
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
 4 result->forward_ = curveLength > 0 ? 1 : -1;
416 8 return result;
417 4 }
418
419 8 PathPtr_t ConstantCurvature::reverse() const {
420
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 8 times.
 8 assert(!timeParameterization());
421
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 8 times.
 8 assert(timeRange().second - timeRange().first >= 0);
422
423
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 8 ConstantCurvaturePtr_t result(createCopy(weak_.lock()));
424
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 8 result->initial_ = end_;
425
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 8 result->end_ = initial_;
426 8 result->curveLength_ = -curveLength_;
427
2/4
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 8 times.
✗ Branch 7 not taken.
 8 result->timeRange(interval_t(0, length()));
428
2/2
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 4 times.
 8 result->forward_ = curveLength_ < 0 ? 1 : -1;
429 16 return result;
430 8 }
431
432 2 inline value_type meanBounds(const JointPtr_t& j, const size_type& i) {
433 2 return (j->upperBound(i) + j->lowerBound(i)) / 2;
434 }
435
436 ✗ inline value_type saturate(const value_type& v, const JointPtr_t& j,
437 const size_type& i) {
438 ✗ return std::min(j->upperBound(i), std::max(j->lowerBound(i), v));
439 }
440
441 17 void ConstantCurvature::setWheelJoints(const JointPtr_t rz,
442 const std::vector<JointPtr_t> wheels) {
443
2/4
✓ Branch 2 taken 17 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 17 times.
✗ Branch 6 not taken.
 17 Transform3s zt(rz->currentTransformation().inverse());
444
1/2
✓ Branch 2 taken 17 times.
✗ Branch 3 not taken.
 17 wheels_.resize(wheels.size());
445 17 std::size_t rk = 0;
446
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 11 times.
 17 if (curvature_ == 0) {
447 6 for (std::vector<JointPtr_t>::const_iterator _wheels = wheels.begin();
448
2/2
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 6 times.
 8 _wheels != wheels.end(); ++_wheels) {
449 2 wheels_[rk].j = *_wheels;
450
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 wheels_[rk].value = meanBounds(wheels_[rk].j, 0);
451 2 ++rk;
452 }
453 } else {
454 11 value_type rho(1. / curvature_);
455 11 for (std::vector<JointPtr_t>::const_iterator _wheels = wheels.begin();
456
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 11 times.
 11 _wheels != wheels.end(); ++_wheels) {
457 ✗ wheels_[rk].j = *_wheels;
458 ✗ wheels_[rk].value = meanBounds(wheels_[rk].j, 0);
459 const vector3_t wheelPos =
460 ✗ zt.act(wheels_[rk].j->currentTransformation().translation());
461 ✗ const value_type value(std::atan(wheelPos[0] / (rho - wheelPos[1])));
462 ✗ wheels_[rk].value =
463 ✗ saturate(meanBounds(wheels_[rk].j, 0) + value, *_wheels, 0);
464 ✗ ++rk;
465 }
466 }
467 17 }
468
469 template <class Archive>
470 ✗ void ConstantCurvature::serialize(Archive& ar, const unsigned int version) {
471 using namespace boost::serialization;
472 (void)version;
473 ✗ ar& make_nvp("base", base_object<Path>(*this));
474 ✗ ar& BOOST_SERIALIZATION_NVP(robot_);
475 ✗ serialization::remove_duplicate::serialize_vector(ar, "initial", initial_,
476 version);
477 ✗ serialization::remove_duplicate::serialize_vector(ar, "end", end_, version);
478 ✗ ar& BOOST_SERIALIZATION_NVP(curveLength_);
479 ✗ ar& make_nvp("curvature_", const_cast<value_type&>(curvature_));
480 ✗ ar& make_nvp("xyId_", const_cast<size_type&>(xyId_));
481 ✗ ar& make_nvp("rzId_", const_cast<size_type&>(rzId_));
482 ✗ ar& BOOST_SERIALIZATION_NVP(dxyId_);
483 ✗ ar& BOOST_SERIALIZATION_NVP(drzId_);
484 ✗ ar& BOOST_SERIALIZATION_NVP(forward_);
485 ✗ ar& BOOST_SERIALIZATION_NVP(wheels_);
486 ✗ ar& BOOST_SERIALIZATION_NVP(weak_);
487 }
488
489 HPP_SERIALIZATION_IMPLEMENT(ConstantCurvature);
490 } // namespace steeringMethod
491 } // namespace core
492 } // namespace hpp
493
494 namespace boost {
495 namespace serialization {
496 template <class Archive>
497 ✗ void serialize(Archive& ar,
498 hpp::core::steeringMethod::ConstantCurvature::Wheels_t& c,
499 const unsigned int version) {
500 (void)version;
501 ✗ ar& make_nvp("value", c.value);
502 ✗ ar& make_nvp("joint", c.j);
503 }
504 } // namespace serialization
505 } // namespace boost
506
507 18 BOOST_CLASS_EXPORT(hpp::core::steeringMethod::ConstantCurvature)
508