GCC Code Coverage Report


Directory: ./
File: tests/test-kinodynamic.cc
Date: 2024-12-13 16:14:03
Exec Total Coverage
Lines: 428 428 100.0%
Branches: 1946 3854 50.5%

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 #define BOOST_TEST_MODULE kinodynamic
30
31 #include <../tests/util.hh>
32 #include <boost/assign.hpp>
33 #include <boost/test/included/unit_test.hpp>
34 #include <fstream>
35 #include <hpp/core/configuration-shooter/uniform.hh>
36 #include <hpp/core/connected-component.hh>
37 #include <hpp/core/fwd.hh>
38 #include <hpp/core/joint-bound-validation.hh>
39 #include <hpp/core/kinodynamic-distance.hh>
40 #include <hpp/core/kinodynamic-oriented-path.hh>
41 #include <hpp/core/kinodynamic-path.hh>
42 #include <hpp/core/nearest-neighbor.hh>
43 #include <hpp/core/node.hh>
44 #include <hpp/core/path-validation/discretized-collision-checking.hh>
45 #include <hpp/core/problem.hh>
46 #include <hpp/core/roadmap.hh>
47 #include <hpp/core/steering-method/steering-kinodynamic.hh>
48 #include <hpp/core/weighed-distance.hh>
49 #include <hpp/pinocchio/configuration.hh>
50 #include <hpp/pinocchio/joint.hh>
51 #include <hpp/pinocchio/liegroup.hh>
52 #include <hpp/pinocchio/simple-device.hh>
53 #include <hpp/util/debug.hh>
54 #include <ostream>
55 #include <sstream>
56 #include <vector>
57
58 using namespace hpp::core;
59 using namespace hpp::pinocchio;
60
61 BOOST_AUTO_TEST_SUITE(test_hpp_core)
62
63
33/66
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
4 BOOST_AUTO_TEST_CASE(kinodynamic) {
64
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
4 DevicePtr_t robot = unittest::makeDevice(unittest::HumanoidSimple);
65
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
2 robot->rootJoint()->lowerBound(0, -10);
66
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
2 robot->rootJoint()->lowerBound(1, -10);
67
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
2 robot->rootJoint()->lowerBound(2, 0);
68
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
2 robot->rootJoint()->upperBound(0, 10);
69
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
2 robot->rootJoint()->upperBound(1, 10);
70
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
2 robot->rootJoint()->upperBound(2, 0);
71
72
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
2 robot->setDimensionExtraConfigSpace(6);
73 // define velocity and acceleration bounds
74 2 const double vMax = 2;
75 2 const double aMax = 0.5;
76
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 robot->extraConfigSpace().lower(0) = -vMax;
77
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 robot->extraConfigSpace().lower(1) = -vMax;
78
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 robot->extraConfigSpace().lower(2) = 0;
79
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 robot->extraConfigSpace().upper(0) = vMax;
80
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 robot->extraConfigSpace().upper(1) = vMax;
81
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 robot->extraConfigSpace().upper(2) = 0;
82
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 robot->extraConfigSpace().lower(3) = -aMax;
83
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 robot->extraConfigSpace().lower(4) = -aMax;
84
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 robot->extraConfigSpace().lower(5) = 0;
85
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 robot->extraConfigSpace().upper(3) = aMax;
86
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 robot->extraConfigSpace().upper(4) = aMax;
87
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 robot->extraConfigSpace().upper(5) = 0;
88
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 1 times.
2 BOOST_CHECK_MESSAGE(robot->extraConfigSpace().dimension() == 6,
89 "error during creation of the robot");
90
91 // Create steering method
92
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
2 ProblemPtr_t p = Problem::create(robot);
93
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
2 p->setParameter(std::string("Kinodynamic/velocityBound"), Parameter(vMax));
94
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
4 p->setParameter(std::string("Kinodynamic/accelerationBound"),
95
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
4 Parameter(aMax));
96
97
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
2 steeringMethod::KinodynamicPtr_t sm = steeringMethod::Kinodynamic::create(p);
98
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
2 KinodynamicDistancePtr_t dist = KinodynamicDistance::createFromProblem(p);
99
100 // try to connect several states : (notation : sx = (px, vx, ax)
101
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
2 Configuration_t q0(robot->currentConfiguration());
102
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
2 Configuration_t q1(robot->currentConfiguration());
103
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
2 size_t indexECS = robot->configSize() - 6;
104
105 JointBoundValidationPtr_t jointValidation =
106
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 JointBoundValidation::create(robot);
107 pathValidation::DiscretizedPtr_t pathVal =
108
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 pathValidation::createDiscretizedCollisionChecking(robot, 0.001);
109
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 pathVal->add(jointValidation);
110 2 PathValidationReportPtr_t validationReport;
111 2 PathPtr_t validPath;
112
113 // 1) simple case with null velocity, require a 2 segment trajectory
114 // px0 = (0,0,0) , px1 = (1,0,0)
115
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 q0[0] = 0;
116
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 q1[0] = 1;
117
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
4 PathPtr_t path = (*sm)(q0, q1);
118
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1 times.
2 BOOST_REQUIRE(path);
119 2 KinodynamicPathPtr_t pathKino = HPP_DYNAMIC_PTR_CAST(KinodynamicPath, path);
120
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1 times.
2 BOOST_REQUIRE(pathKino);
121
122 // length should be 2.82843
123
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 1 times.
2 BOOST_CHECK_CLOSE(path->length(), 2.82843, 1e-3);
124
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 1 times.
2 BOOST_CHECK_EQUAL(path->length(), (*dist)(q0, q1));
125 // check if 2 segment only
126
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
2 BOOST_CHECK_EQUAL(pathKino->getTv()[0], 0.);
127
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
2 BOOST_CHECK_EQUAL(pathKino->getT0()[0], 0.);
128
12/24
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 35 taken 1 times.
✗ Branch 36 not taken.
✓ Branch 38 taken 1 times.
✗ Branch 39 not taken.
✗ Branch 45 not taken.
✓ Branch 46 taken 1 times.
2 BOOST_CHECK_EQUAL(
129 pathKino->getT1()[0] + pathKino->getTv()[0] + pathKino->getT2()[0],
130 path->length());
131
132 // check if no unecessary motion :
133
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 1 times.
6 for (size_t i = 1; i < 3; i++) {
134
7/14
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 2 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 2 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 2 times.
4 BOOST_CHECK_EQUAL(pathKino->getT0()[i], 0.);
135
7/14
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 2 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 2 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 2 times.
4 BOOST_CHECK_EQUAL(pathKino->getT1()[i], 0.);
136
7/14
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 2 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 2 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 2 times.
4 BOOST_CHECK_EQUAL(pathKino->getT2()[i], 0.);
137
7/14
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 2 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 2 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 2 times.
4 BOOST_CHECK_EQUAL(pathKino->getA1()[i], 0.);
138
8/16
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2 times.
✗ Branch 17 not taken.
✓ Branch 21 taken 2 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 2 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 2 times.
4 BOOST_CHECK_EQUAL(pathKino->getTv()[i], path->length());
139 }
140 // check if the trajectory is really bang-bang :
141
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
2 BOOST_CHECK_EQUAL(pathKino->getA1()[0], aMax);
142
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 1 times.
2 BOOST_CHECK_EQUAL(pathKino->getT1()[0], pathKino->getT2()[0]);
143 // check if bounds are respected :
144
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
2 BOOST_CHECK(pathVal->validate(path, false, validPath, validationReport));
145
146 // 2) null velocity but hit velocity bounds, require 3 segments trajectory
147
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 q0[0] = -2;
148
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 q1[0] = 7;
149
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
2 path = (*sm)(q0, q1);
150
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1 times.
2 BOOST_REQUIRE(path);
151 2 pathKino = HPP_DYNAMIC_PTR_CAST(KinodynamicPath, path);
152
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1 times.
2 BOOST_REQUIRE(pathKino);
153
154 // check length
155
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 1 times.
2 BOOST_CHECK_CLOSE(path->length(), 8.5, 1e-3);
156
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 1 times.
2 BOOST_CHECK_EQUAL(path->length(), (*dist)(q0, q1));
157 // check if 3 segment
158
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
2 BOOST_CHECK_EQUAL(pathKino->getT0()[0], 0.);
159
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 1 times.
2 BOOST_CHECK(pathKino->getT1()[0] > 0);
160
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 1 times.
2 BOOST_CHECK(pathKino->getTv()[0] > 0);
161
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 1 times.
2 BOOST_CHECK(pathKino->getT2()[0] > 0);
162
12/24
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 35 taken 1 times.
✗ Branch 36 not taken.
✓ Branch 38 taken 1 times.
✗ Branch 39 not taken.
✗ Branch 45 not taken.
✓ Branch 46 taken 1 times.
2 BOOST_CHECK_EQUAL(
163 pathKino->getT1()[0] + pathKino->getTv()[0] + pathKino->getT2()[0],
164 path->length());
165
166 // check if no unecessary motion :
167
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 1 times.
6 for (size_t i = 1; i < 3; i++) {
168
7/14
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 2 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 2 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 2 times.
4 BOOST_CHECK_EQUAL(pathKino->getT0()[i], 0.);
169
7/14
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 2 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 2 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 2 times.
4 BOOST_CHECK_EQUAL(pathKino->getT1()[i], 0.);
170
7/14
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 2 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 2 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 2 times.
4 BOOST_CHECK_EQUAL(pathKino->getT2()[i], 0.);
171
7/14
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 2 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 2 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 2 times.
4 BOOST_CHECK_EQUAL(pathKino->getA1()[i], 0.);
172
8/16
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2 times.
✗ Branch 17 not taken.
✓ Branch 21 taken 2 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 2 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 2 times.
4 BOOST_CHECK_EQUAL(pathKino->getTv()[i], path->length());
173 }
174 // check if the trajectory is really bang-bang :
175
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
2 BOOST_CHECK_EQUAL(pathKino->getA1()[0], aMax);
176
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 1 times.
2 BOOST_CHECK_EQUAL(pathKino->getT1()[0], pathKino->getT2()[0]);
177 // check if bounds are respected :
178
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
2 BOOST_CHECK(pathVal->validate(path, false, validPath, validationReport));
179
180 // 3) only velocity change :
181 // px0 = (0,2,0) ; px1 = (0,-1,0)
182
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 q0[0] = 0;
183
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 q0[indexECS] = 2;
184
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 q1[0] = 0;
185
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 q1[indexECS] = -1;
186
187
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
2 path = (*sm)(q0, q1);
188
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1 times.
2 BOOST_REQUIRE(path);
189 2 pathKino = HPP_DYNAMIC_PTR_CAST(KinodynamicPath, path);
190
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1 times.
2 BOOST_REQUIRE(pathKino);
191
192 // check length
193
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 1 times.
2 BOOST_CHECK_CLOSE(path->length(), 8.32456, 1e-3);
194
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 1 times.
2 BOOST_CHECK_EQUAL(path->length(), (*dist)(q0, q1));
195 // check if 2 segment
196
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
2 BOOST_CHECK_EQUAL(pathKino->getT0()[0], 0.);
197
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 1 times.
2 BOOST_CHECK(pathKino->getT1()[0] > 0);
198
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
2 BOOST_CHECK_EQUAL(pathKino->getTv()[0], 0);
199
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 1 times.
2 BOOST_CHECK(pathKino->getT2()[0] > 0);
200
12/24
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 35 taken 1 times.
✗ Branch 36 not taken.
✓ Branch 38 taken 1 times.
✗ Branch 39 not taken.
✗ Branch 45 not taken.
✓ Branch 46 taken 1 times.
2 BOOST_CHECK_EQUAL(
201 pathKino->getT1()[0] + pathKino->getTv()[0] + pathKino->getT2()[0],
202 path->length());
203
204 // check if no unecessary motion :
205
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 1 times.
6 for (size_t i = 1; i < 3; i++) {
206
7/14
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 2 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 2 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 2 times.
4 BOOST_CHECK_EQUAL(pathKino->getT0()[i], 0.);
207
7/14
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 2 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 2 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 2 times.
4 BOOST_CHECK_EQUAL(pathKino->getT1()[i], 0.);
208
7/14
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 2 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 2 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 2 times.
4 BOOST_CHECK_EQUAL(pathKino->getT2()[i], 0.);
209
7/14
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 2 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 2 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 2 times.
4 BOOST_CHECK_EQUAL(pathKino->getA1()[i], 0.);
210
8/16
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2 times.
✗ Branch 17 not taken.
✓ Branch 21 taken 2 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 2 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 2 times.
4 BOOST_CHECK_EQUAL(pathKino->getTv()[i], path->length());
211 }
212 // check if the trajectory is really bang-bang :
213
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
2 BOOST_CHECK_EQUAL(pathKino->getA1()[0], -aMax);
214 // check if bounds are respected :
215
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
2 BOOST_CHECK(pathVal->validate(path, false, validPath, validationReport));
216
217 // 4) non null velocity (and sigma == 0)
218 // px0 = (0,1,0) ; px1 = (1,1,0)
219
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 q0[0] = 0;
220
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 q0[indexECS] = 1;
221
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 q1[0] = 1;
222
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 q1[indexECS] = 1;
223
224
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
2 path = (*sm)(q0, q1);
225
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1 times.
2 BOOST_REQUIRE(path);
226 2 pathKino = HPP_DYNAMIC_PTR_CAST(KinodynamicPath, path);
227
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1 times.
2 BOOST_REQUIRE(pathKino);
228
229 // check length
230
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 1 times.
2 BOOST_CHECK_CLOSE(path->length(), 0.89898, 1e-3);
231
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 1 times.
2 BOOST_CHECK_EQUAL(path->length(), (*dist)(q0, q1));
232 // check if 2 segment
233
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
2 BOOST_CHECK_EQUAL(pathKino->getT0()[0], 0.);
234
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 1 times.
2 BOOST_CHECK(pathKino->getT1()[0] > 0);
235
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
2 BOOST_CHECK_EQUAL(pathKino->getTv()[0], 0);
236
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 1 times.
2 BOOST_CHECK(pathKino->getT2()[0] > 0);
237
12/24
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 35 taken 1 times.
✗ Branch 36 not taken.
✓ Branch 38 taken 1 times.
✗ Branch 39 not taken.
✗ Branch 45 not taken.
✓ Branch 46 taken 1 times.
2 BOOST_CHECK_EQUAL(
238 pathKino->getT1()[0] + pathKino->getTv()[0] + pathKino->getT2()[0],
239 path->length());
240
241 // check if no unecessary motion :
242
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 1 times.
6 for (size_t i = 1; i < 3; i++) {
243
7/14
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 2 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 2 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 2 times.
4 BOOST_CHECK_EQUAL(pathKino->getT0()[i], 0.);
244
7/14
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 2 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 2 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 2 times.
4 BOOST_CHECK_EQUAL(pathKino->getT1()[i], 0.);
245
7/14
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 2 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 2 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 2 times.
4 BOOST_CHECK_EQUAL(pathKino->getT2()[i], 0.);
246
7/14
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 2 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 2 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 2 times.
4 BOOST_CHECK_EQUAL(pathKino->getA1()[i], 0.);
247
8/16
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2 times.
✗ Branch 17 not taken.
✓ Branch 21 taken 2 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 2 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 2 times.
4 BOOST_CHECK_EQUAL(pathKino->getTv()[i], path->length());
248 }
249 // check if the trajectory is really bang-bang :
250
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 1 times.
2 BOOST_CHECK_CLOSE(pathKino->getA1()[0], aMax, 1e-3);
251
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 1 times.
2 BOOST_CHECK_EQUAL(pathKino->getT1()[0], pathKino->getT2()[0]);
252 // check if bounds are respected :
253
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
2 BOOST_CHECK(pathVal->validate(path, false, validPath, validationReport));
254
255 // 5) case with infeasible interval
256 // infeasible interval on x axis is [1.172,6.828] , add a motion on y axis
257 // with a min time inside this interval : py0 = (-2,0,0) py1 = (1,0,0)
258
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 q0[1] = -2;
259
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 q0[indexECS + 1] = 0;
260
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 q1[1] = 1;
261
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 q1[indexECS + 1] = 0;
262
263
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
2 path = (*sm)(q0, q1);
264
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1 times.
2 BOOST_REQUIRE(path);
265 2 pathKino = HPP_DYNAMIC_PTR_CAST(KinodynamicPath, path);
266
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1 times.
2 BOOST_REQUIRE(pathKino);
267
268 // check length
269
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 1 times.
2 BOOST_CHECK_CLOSE(path->length(), 6.82843, 1e-3);
270 // don't check if distance == length here because the distance method don't
271 // consider infeasile interval It's a arbitrary choice made to speed up the
272 // computation of the distance
273
274 // check if 2 segment
275
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 1 times.
6 for (size_t i = 0; i < 2; i++) {
276
7/14
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 2 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 2 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 2 times.
4 BOOST_CHECK_EQUAL(pathKino->getT0()[0], 0.);
277
8/16
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 2 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 2 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 2 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 2 times.
✗ Branch 25 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 2 times.
4 BOOST_CHECK(pathKino->getT1()[i] > 0);
278
7/14
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 2 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 2 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 2 times.
4 BOOST_CHECK_EQUAL(pathKino->getTv()[i], 0);
279
8/16
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 2 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 2 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 2 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 2 times.
✗ Branch 25 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 2 times.
4 BOOST_CHECK(pathKino->getT2()[i] > 0);
280
12/24
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2 times.
✗ Branch 17 not taken.
✓ Branch 20 taken 2 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 2 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 2 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 2 times.
✗ Branch 31 not taken.
✓ Branch 35 taken 2 times.
✗ Branch 36 not taken.
✓ Branch 38 taken 2 times.
✗ Branch 39 not taken.
✗ Branch 45 not taken.
✓ Branch 46 taken 2 times.
4 BOOST_CHECK_EQUAL(
281 pathKino->getT1()[i] + pathKino->getTv()[i] + pathKino->getT2()[i],
282 path->length());
283 }
284
285 // check if no unecessary motion :
286
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
2 BOOST_CHECK_EQUAL(pathKino->getT0()[2], 0.);
287
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
2 BOOST_CHECK_EQUAL(pathKino->getT1()[2], 0.);
288
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
2 BOOST_CHECK_EQUAL(pathKino->getT2()[2], 0.);
289
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
2 BOOST_CHECK_EQUAL(pathKino->getA1()[2], 0.);
290
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
2 BOOST_CHECK_EQUAL(pathKino->getTv()[2], path->length());
291 // check if the trajectory is really bang-bang :
292
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
2 BOOST_CHECK_EQUAL(pathKino->getA1()[0], -aMax);
293
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 1 times.
2 BOOST_CHECK_EQUAL(pathKino->getT1()[0], pathKino->getT2()[0]);
294
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 1 times.
2 BOOST_CHECK_EQUAL(pathKino->getT1()[1], pathKino->getT2()[1]);
295
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 1 times.
2 BOOST_CHECK(pathKino->getA1()[1] < aMax);
296
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 1 times.
2 BOOST_CHECK(pathKino->getA1()[1] > 0);
297
298 // check if bounds are respected :
299
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
2 BOOST_CHECK(pathVal->validate(path, false, validPath, validationReport));
300
301 // ## check extract method
302
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 q0[0] = -2;
303
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 q0[indexECS] = 0;
304
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 q1[0] = 7;
305
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 q1[indexECS] = 0;
306
307
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
2 path = (*sm)(q0, q1);
308
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1 times.
2 BOOST_REQUIRE(path);
309 2 pathKino = HPP_DYNAMIC_PTR_CAST(KinodynamicPath, path);
310
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1 times.
2 BOOST_REQUIRE(pathKino);
311
312 2 PathPtr_t extractedPath;
313 2 KinodynamicPathPtr_t extractedPathKino;
314 bool success;
315 // take only beginning of the path (cut during second segment on x and third
316 // segment on y):
317 2 double begin = 0.;
318 2 double end = 4.3;
319
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
2 extractedPath = path->extract(std::make_pair(begin, end));
320
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1 times.
2 BOOST_REQUIRE(extractedPath);
321 2 extractedPathKino = HPP_DYNAMIC_PTR_CAST(KinodynamicPath, extractedPath);
322
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1 times.
2 BOOST_REQUIRE(extractedPathKino);
323
324
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 1 times.
2 BOOST_CHECK_EQUAL(extractedPath->length(), end - begin);
325
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 1 times.
2 BOOST_CHECK_EQUAL(extractedPath->initial(), path->initial());
326
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 1 times.
2 BOOST_CHECK_EQUAL(extractedPath->end().head(indexECS + 3),
327 path->eval(end, success).head(indexECS + 3));
328 // ignore last 3 extraDof because acceleration is set to 0 in initial/end
329 // configuration
330
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 1 times.
8 for (size_t i = 0; i < 3; i++) {
331
9/18
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 3 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 3 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 3 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 3 times.
6 BOOST_CHECK_EQUAL(extractedPathKino->getA1()[i], pathKino->getA1()[i]);
332
9/18
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 3 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 3 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 3 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 3 times.
6 BOOST_CHECK_EQUAL(extractedPathKino->getT0()[i], pathKino->getT0()[i]);
333
9/18
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 3 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 3 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 3 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 3 times.
6 BOOST_CHECK_EQUAL(extractedPathKino->getT1()[i], pathKino->getT1()[i]);
334 }
335
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
2 BOOST_CHECK_EQUAL(extractedPathKino->getTv()[2], extractedPath->length());
336
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
2 BOOST_CHECK_EQUAL(extractedPathKino->getT2()[2], 0);
337
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 1 times.
2 BOOST_CHECK_EQUAL(extractedPathKino->getTv()[0],
338 end - extractedPathKino->getT1()[0]);
339
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
2 BOOST_CHECK_EQUAL(extractedPathKino->getT2()[0], 0);
340
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
2 BOOST_CHECK_EQUAL(extractedPathKino->getTv()[1], 0);
341
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 1 times.
2 BOOST_CHECK_EQUAL(extractedPathKino->getT2()[1],
342 end - extractedPathKino->getT1()[1]);
343
344 // take only beginning of the path (cut during first segment on both axis):
345 2 begin = 0.;
346 2 end = 3.;
347
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
2 extractedPath = path->extract(std::make_pair(begin, end));
348
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1 times.
2 BOOST_REQUIRE(extractedPath);
349 2 extractedPathKino = HPP_DYNAMIC_PTR_CAST(KinodynamicPath, extractedPath);
350
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1 times.
2 BOOST_REQUIRE(extractedPathKino);
351
352
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 1 times.
2 BOOST_CHECK_EQUAL(extractedPath->length(), end - begin);
353
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 1 times.
2 BOOST_CHECK_EQUAL(extractedPath->initial(), path->initial());
354
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 1 times.
2 BOOST_CHECK_EQUAL(
355 extractedPath->end().head(indexECS + 3),
356 path->eval(end, success)
357 .head(indexECS + 3)); // ignore last 3 extraDof because acceleration
358 // is set to 0 in initial/end configuration
359
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 1 times.
8 for (size_t i = 0; i < 3; i++) {
360
9/18
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 3 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 3 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 3 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 3 times.
6 BOOST_CHECK_EQUAL(extractedPathKino->getA1()[i], pathKino->getA1()[i]);
361
9/18
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 3 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 3 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 3 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 3 times.
6 BOOST_CHECK_EQUAL(extractedPathKino->getT0()[i], pathKino->getT0()[i]);
362
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 3 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 3 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 3 times.
6 BOOST_CHECK_EQUAL(extractedPathKino->getT2()[i], 0.);
363 }
364
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
2 BOOST_CHECK_EQUAL(extractedPathKino->getT1()[2], 0);
365
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
2 BOOST_CHECK_EQUAL(extractedPathKino->getTv()[2], extractedPath->length());
366
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
2 BOOST_CHECK_EQUAL(extractedPathKino->getT2()[2], 0);
367
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
2 BOOST_CHECK_EQUAL(extractedPathKino->getT1()[0], extractedPath->length());
368
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
2 BOOST_CHECK_EQUAL(extractedPathKino->getTv()[0], 0);
369
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
2 BOOST_CHECK_EQUAL(extractedPathKino->getT1()[1], extractedPath->length());
370
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
2 BOOST_CHECK_EQUAL(extractedPathKino->getTv()[1], 0);
371
372 // take only end of the path (cut during first segment)
373 2 begin = 2.;
374
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
2 end = path->length();
375
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
2 extractedPath = path->extract(std::make_pair(begin, end));
376
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1 times.
2 BOOST_REQUIRE(extractedPath);
377 2 extractedPathKino = HPP_DYNAMIC_PTR_CAST(KinodynamicPath, extractedPath);
378
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1 times.
2 BOOST_REQUIRE(extractedPathKino);
379
380
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 1 times.
2 BOOST_CHECK_EQUAL(extractedPath->length(), end - begin);
381
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 1 times.
2 BOOST_CHECK_EQUAL(extractedPath->end(), path->end());
382
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 1 times.
2 BOOST_CHECK_EQUAL(
383 extractedPath->initial().head(indexECS + 3),
384 path->eval(begin, success)
385 .head(indexECS + 3)); // ignore last 3 extraDof because acceleration
386 // is set to 0 in initial/end configuration
387
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 1 times.
8 for (size_t i = 0; i < 3; i++) {
388
9/18
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 3 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 3 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 3 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 3 times.
6 BOOST_CHECK_EQUAL(extractedPathKino->getA1()[i], pathKino->getA1()[i]);
389
9/18
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 3 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 3 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 3 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 3 times.
6 BOOST_CHECK_EQUAL(extractedPathKino->getT0()[i], pathKino->getT0()[i]);
390
9/18
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 3 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 3 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 3 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 3 times.
6 BOOST_CHECK_EQUAL(extractedPathKino->getT2()[i], pathKino->getT2()[i]);
391 }
392
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
2 BOOST_CHECK_EQUAL(extractedPathKino->getT1()[2], 0);
393
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
2 BOOST_CHECK_EQUAL(extractedPathKino->getTv()[2], extractedPath->length());
394
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
2 BOOST_CHECK_EQUAL(extractedPathKino->getT2()[2], 0);
395
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 1 times.
2 BOOST_CHECK_EQUAL(extractedPathKino->getT1()[0],
396 pathKino->getT1()[0] - begin);
397
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 1 times.
2 BOOST_CHECK_EQUAL(extractedPathKino->getTv()[0], pathKino->getTv()[0]);
398
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 1 times.
2 BOOST_CHECK_EQUAL(extractedPathKino->getT1()[1],
399 pathKino->getT1()[1] - begin);
400
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
2 BOOST_CHECK_EQUAL(extractedPathKino->getTv()[1], 0);
401
402 // extract path in the middle :
403 2 begin = 1.5;
404 2 end = 4.4;
405
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
2 extractedPath = path->extract(std::make_pair(begin, end));
406
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1 times.
2 BOOST_REQUIRE(extractedPath);
407 2 extractedPathKino = HPP_DYNAMIC_PTR_CAST(KinodynamicPath, extractedPath);
408
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1 times.
2 BOOST_REQUIRE(extractedPathKino);
409
410
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 1 times.
2 BOOST_CHECK_EQUAL(extractedPath->length(), end - begin);
411
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 1 times.
2 BOOST_CHECK_EQUAL(extractedPath->end().head(indexECS + 3),
412 path->eval(end, success).head(indexECS + 3));
413
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 1 times.
2 BOOST_CHECK_EQUAL(
414 extractedPath->initial().head(indexECS + 3),
415 path->eval(begin, success)
416 .head(indexECS + 3)); // ignore last 3 extraDof because acceleration
417 // is set to 0 in initial/end configuration
418
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 1 times.
8 for (size_t i = 0; i < 3; i++) {
419
9/18
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 3 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 3 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 3 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 3 times.
6 BOOST_CHECK_EQUAL(extractedPathKino->getA1()[i], pathKino->getA1()[i]);
420
9/18
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 3 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 3 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 3 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 3 times.
6 BOOST_CHECK_EQUAL(extractedPathKino->getT0()[i], pathKino->getT0()[i]);
421 }
422
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
2 BOOST_CHECK_EQUAL(extractedPathKino->getT1()[2], 0);
423
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
2 BOOST_CHECK_EQUAL(extractedPathKino->getTv()[2], extractedPath->length());
424
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
2 BOOST_CHECK_EQUAL(extractedPathKino->getT2()[2], 0);
425 // x axis is cut during first and second segment :
426
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 1 times.
2 BOOST_CHECK_EQUAL(extractedPathKino->getT1()[0],
427 pathKino->getT1()[0] - begin);
428
10/20
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 1 times.
✗ Branch 32 not taken.
✗ Branch 37 not taken.
✓ Branch 38 taken 1 times.
2 BOOST_CHECK_EQUAL(extractedPathKino->getTv()[0],
429 extractedPath->length() - extractedPathKino->getT1()[0]);
430
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
2 BOOST_CHECK_EQUAL(extractedPathKino->getT2()[0], 0);
431 // y axis is cut during first and third segment
432
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 1 times.
2 BOOST_CHECK_EQUAL(extractedPathKino->getT1()[1],
433 pathKino->getT1()[1] - begin);
434
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
2 BOOST_CHECK_EQUAL(extractedPathKino->getTv()[1], 0);
435
10/20
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 1 times.
✗ Branch 32 not taken.
✗ Branch 37 not taken.
✓ Branch 38 taken 1 times.
2 BOOST_CHECK_EQUAL(extractedPathKino->getT2()[1],
436 extractedPath->length() - extractedPathKino->getT1()[1]);
437
438 // random tests with non null initial and final acceleration
439
440 ConfigurationShooterPtr_t shooter =
441
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 configurationShooter::Uniform::create(robot);
442
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 Configuration_t qr1, qr0;
443 value_type t1, t2;
444
2/2
✓ Branch 0 taken 1000 times.
✓ Branch 1 taken 1 times.
2002 for (size_t i = 0; i < 1000; i++) {
445
1/2
✓ Branch 2 taken 1000 times.
✗ Branch 3 not taken.
2000 qr0 = shooter->shoot();
446
1/2
✓ Branch 2 taken 1000 times.
✗ Branch 3 not taken.
2000 qr1 = shooter->shoot();
447
3/6
✓ Branch 2 taken 1000 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1000 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1000 times.
✗ Branch 9 not taken.
2000 path = (*sm)(qr0, qr1);
448
6/12
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1000 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1000 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1000 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1000 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1000 times.
2000 BOOST_REQUIRE(path);
449 2000 pathKino = HPP_DYNAMIC_PTR_CAST(KinodynamicPath, path);
450
6/12
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1000 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1000 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1000 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1000 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1000 times.
2000 BOOST_REQUIRE(pathKino);
451
10/20
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1000 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1000 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1000 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 1000 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1000 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1000 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1000 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 1000 times.
✗ Branch 32 not taken.
✗ Branch 38 not taken.
✓ Branch 39 taken 1000 times.
2000 BOOST_CHECK(path->length() >= (*dist)(qr0, qr1));
452
8/16
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1000 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1000 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 1000 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1000 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 1000 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1000 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 1000 times.
2000 BOOST_CHECK_EQUAL(path->end().head(indexECS + 3), (qr1).head(indexECS + 3));
453
8/16
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1000 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1000 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 1000 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1000 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 1000 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1000 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 1000 times.
2000 BOOST_CHECK_EQUAL(path->initial().head(indexECS + 3),
454 (qr0).head(indexECS + 3));
455
9/18
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1000 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1000 times.
✗ Branch 9 not taken.
✓ Branch 13 taken 1000 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1000 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1000 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 1000 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1000 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 1000 times.
2000 BOOST_CHECK_EQUAL(path->eval(path->length(), success).head(indexECS + 3),
456 (qr1).head(indexECS + 3));
457
8/16
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1000 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1000 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 1000 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1000 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 1000 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1000 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 1000 times.
2000 BOOST_CHECK_EQUAL(path->eval(0., success).head(indexECS + 3),
458 (qr0).head(indexECS + 3));
459
460
2/2
✓ Branch 0 taken 3000 times.
✓ Branch 1 taken 1000 times.
8000 for (size_t k = 0; k < 3; k++) {
461
7/14
✓ Branch 1 taken 3000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3000 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 3000 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 3000 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 3000 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 3000 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 3000 times.
6000 BOOST_CHECK_EQUAL(pathKino->getT0()[k], 0.);
462
8/16
✓ Branch 1 taken 3000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3000 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3000 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 3000 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 3000 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 3000 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 3000 times.
✗ Branch 25 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 3000 times.
6000 BOOST_CHECK(pathKino->getT1()[k] >= 0.);
463
8/16
✓ Branch 1 taken 3000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3000 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3000 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 3000 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 3000 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 3000 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 3000 times.
✗ Branch 25 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 3000 times.
6000 BOOST_CHECK(pathKino->getTv()[k] >= 0.);
464
8/16
✓ Branch 1 taken 3000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3000 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3000 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 3000 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 3000 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 3000 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 3000 times.
✗ Branch 25 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 3000 times.
6000 BOOST_CHECK(pathKino->getT2()[k] >= 0.);
465 }
466
467
2/2
✓ Branch 0 taken 10000 times.
✓ Branch 1 taken 1000 times.
22000 for (size_t j = 0; j < 10; j++) {
468 value_type a =
469
1/2
✓ Branch 3 taken 10000 times.
✗ Branch 4 not taken.
20000 ((value_type)rand() / (value_type)RAND_MAX) * path->length();
470 value_type b =
471
1/2
✓ Branch 3 taken 10000 times.
✗ Branch 4 not taken.
20000 ((value_type)rand() / (value_type)RAND_MAX) * path->length();
472
2/2
✓ Branch 0 taken 5021 times.
✓ Branch 1 taken 4979 times.
20000 if (a < b) {
473 10042 t1 = a;
474 10042 t2 = b;
475 } else {
476 9958 t1 = b;
477 9958 t2 = a;
478 }
479
2/4
✓ Branch 2 taken 10000 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10000 times.
✗ Branch 6 not taken.
20000 extractedPath = path->extract(std::make_pair(t1, t2));
480
6/12
✓ Branch 1 taken 10000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10000 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 10000 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 10000 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 10000 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 10000 times.
20000 BOOST_REQUIRE(extractedPath);
481
6/12
✓ Branch 1 taken 10000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10000 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 10000 times.
✗ Branch 10 not taken.
✓ Branch 14 taken 10000 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 10000 times.
✗ Branch 18 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 10000 times.
20000 BOOST_CHECK_EQUAL(extractedPath->length(), t2 - t1);
482
5/10
✓ Branch 1 taken 10000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10000 times.
✗ Branch 6 not taken.
✓ Branch 12 taken 10000 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 10000 times.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✓ Branch 20 taken 10000 times.
20000 BOOST_CHECK_EQUAL(extractedPath->timeRange().first, 0.);
483
5/10
✓ Branch 1 taken 10000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10000 times.
✗ Branch 6 not taken.
✓ Branch 12 taken 10000 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 10000 times.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✓ Branch 20 taken 10000 times.
20000 BOOST_CHECK_EQUAL(extractedPath->timeRange().second, t2 - t1);
484
9/18
✓ Branch 1 taken 10000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10000 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 10000 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 10000 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 10000 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10000 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 10000 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 10000 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 10000 times.
20000 BOOST_CHECK_EQUAL(extractedPath->initial().head(indexECS + 3),
485 path->eval(t1, success).head(indexECS + 3));
486
9/18
✓ Branch 1 taken 10000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10000 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 10000 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 10000 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 10000 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10000 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 10000 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 10000 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 10000 times.
20000 BOOST_CHECK_EQUAL(extractedPath->end().head(indexECS + 3),
487 path->eval(t2, success).head(indexECS + 3));
488
9/18
✓ Branch 1 taken 10000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10000 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 10000 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 10000 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 10000 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10000 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 10000 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 10000 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 10000 times.
20000 BOOST_CHECK_EQUAL(extractedPath->eval(0., success).head(indexECS + 3),
489 path->eval(t1, success).head(indexECS + 3));
490
9/18
✓ Branch 1 taken 10000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10000 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 10000 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 10000 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 10000 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10000 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 10000 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 10000 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 10000 times.
20000 BOOST_CHECK_EQUAL(
491 extractedPath->eval(t2 - t1, success).head(indexECS + 3),
492 path->eval(t2, success).head(indexECS + 3));
493
494
1/2
✓ Branch 0 taken 10000 times.
✗ Branch 1 not taken.
20000 if (t1 < t2) {
495 extractedPathKino =
496 20000 HPP_DYNAMIC_PTR_CAST(KinodynamicPath, extractedPath);
497
6/12
✓ Branch 1 taken 10000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10000 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 10000 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 10000 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 10000 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 10000 times.
20000 BOOST_REQUIRE(extractedPathKino);
498
2/2
✓ Branch 0 taken 30000 times.
✓ Branch 1 taken 10000 times.
80000 for (size_t k = 0; k < 3; k++) {
499
7/14
✓ Branch 1 taken 30000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 30000 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 30000 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 30000 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 30000 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 30000 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 30000 times.
60000 BOOST_CHECK_EQUAL(extractedPathKino->getT0()[k], 0.);
500
8/16
✓ Branch 1 taken 30000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 30000 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 30000 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 30000 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 30000 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 30000 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 30000 times.
✗ Branch 25 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 30000 times.
60000 BOOST_CHECK(extractedPathKino->getT1()[k] >= 0.);
501
8/16
✓ Branch 1 taken 30000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 30000 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 30000 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 30000 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 30000 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 30000 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 30000 times.
✗ Branch 25 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 30000 times.
60000 BOOST_CHECK(extractedPathKino->getTv()[k] >= 0.);
502
8/16
✓ Branch 1 taken 30000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 30000 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 30000 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 30000 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 30000 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 30000 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 30000 times.
✗ Branch 25 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 30000 times.
60000 BOOST_CHECK(extractedPathKino->getT2()[k] >= 0.);
503
12/24
✓ Branch 1 taken 30000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 30000 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 30000 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 30000 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 30000 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 30000 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 30000 times.
✗ Branch 23 not taken.
✓ Branch 26 taken 30000 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 30000 times.
✗ Branch 30 not taken.
✓ Branch 34 taken 30000 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 30000 times.
✗ Branch 38 not taken.
✗ Branch 44 not taken.
✓ Branch 45 taken 30000 times.
60000 BOOST_CHECK_CLOSE(extractedPathKino->getT1()[k] +
504 extractedPathKino->getTv()[k] +
505 extractedPathKino->getT2()[k],
506 t2 - t1, 1e-3);
507 }
508 }
509 }
510 }
511 2 }
512
513
33/66
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
4 BOOST_AUTO_TEST_CASE(kinodynamic_aMax1) {
514
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
4 DevicePtr_t robot = unittest::makeDevice(unittest::HumanoidSimple);
515
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
2 robot->rootJoint()->lowerBound(0, -10);
516
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
2 robot->rootJoint()->lowerBound(1, -10);
517
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
2 robot->rootJoint()->lowerBound(2, 0);
518
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
2 robot->rootJoint()->upperBound(0, 10);
519
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
2 robot->rootJoint()->upperBound(1, 10);
520
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
2 robot->rootJoint()->upperBound(2, 0);
521
522
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
2 robot->setDimensionExtraConfigSpace(6);
523 // define velocity and acceleration bounds
524 2 const double vMax = 0.3;
525 2 const double aMax = 1.;
526
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 robot->extraConfigSpace().lower(0) = -vMax;
527
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 robot->extraConfigSpace().lower(1) = -vMax;
528
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 robot->extraConfigSpace().lower(2) = 0;
529
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 robot->extraConfigSpace().upper(0) = vMax;
530
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 robot->extraConfigSpace().upper(1) = vMax;
531
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 robot->extraConfigSpace().upper(2) = 0;
532
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 robot->extraConfigSpace().lower(3) = -aMax;
533
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 robot->extraConfigSpace().lower(4) = -aMax;
534
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 robot->extraConfigSpace().lower(5) = 0;
535
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 robot->extraConfigSpace().upper(3) = aMax;
536
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 robot->extraConfigSpace().upper(4) = aMax;
537
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 robot->extraConfigSpace().upper(5) = 0;
538
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 1 times.
2 BOOST_CHECK_MESSAGE(robot->extraConfigSpace().dimension() == 6,
539 "error during creation of the robot");
540
541 // Create steering method
542
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
2 ProblemPtr_t p = Problem::create(robot);
543
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
2 p->setParameter(std::string("Kinodynamic/velocityBound"), Parameter(vMax));
544
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
4 p->setParameter(std::string("Kinodynamic/accelerationBound"),
545
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
4 Parameter(aMax));
546
547
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
2 steeringMethod::KinodynamicPtr_t sm = steeringMethod::Kinodynamic::create(p);
548
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
2 KinodynamicDistancePtr_t dist = KinodynamicDistance::createFromProblem(p);
549
550 // try to connect several states : (notation : sx = (px, vx, ax)
551
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
2 Configuration_t q0(robot->currentConfiguration());
552
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
2 Configuration_t q1(robot->currentConfiguration());
553
554 JointBoundValidationPtr_t jointValidation =
555
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 JointBoundValidation::create(robot);
556 pathValidation::DiscretizedPtr_t pathVal =
557
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 pathValidation::createDiscretizedCollisionChecking(robot, 0.001);
558
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 pathVal->add(jointValidation);
559 2 PathValidationReportPtr_t validationReport;
560 2 PathPtr_t validPath;
561
562
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 q0[0] = 0;
563
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 q1[0] = 1.5;
564
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
4 PathPtr_t path = (*sm)(q0, q1);
565
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1 times.
2 BOOST_REQUIRE(path);
566 2 KinodynamicPathPtr_t pathKino = HPP_DYNAMIC_PTR_CAST(KinodynamicPath, path);
567
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1 times.
2 BOOST_REQUIRE(pathKino);
568
569 // length should be 2.82843
570
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 1 times.
2 BOOST_CHECK_CLOSE(path->length(), 5.3, 1e-3);
571
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 1 times.
2 BOOST_CHECK_EQUAL(path->length(), (*dist)(q0, q1));
572 // check if 3 segments
573
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 1 times.
2 BOOST_CHECK(pathKino->getTv()[0] > 0.);
574
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
2 BOOST_CHECK_EQUAL(pathKino->getT0()[0], 0.);
575
12/24
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 35 taken 1 times.
✗ Branch 36 not taken.
✓ Branch 38 taken 1 times.
✗ Branch 39 not taken.
✗ Branch 45 not taken.
✓ Branch 46 taken 1 times.
2 BOOST_CHECK_EQUAL(
576 pathKino->getT1()[0] + pathKino->getTv()[0] + pathKino->getT2()[0],
577 path->length());
578
579 // check if no unecessary motion :
580
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 1 times.
6 for (size_t i = 1; i < 3; i++) {
581
7/14
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 2 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 2 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 2 times.
4 BOOST_CHECK_EQUAL(pathKino->getT0()[i], 0.);
582
7/14
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 2 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 2 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 2 times.
4 BOOST_CHECK_EQUAL(pathKino->getT1()[i], 0.);
583
7/14
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 2 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 2 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 2 times.
4 BOOST_CHECK_EQUAL(pathKino->getT2()[i], 0.);
584
7/14
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 2 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 2 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 2 times.
4 BOOST_CHECK_EQUAL(pathKino->getA1()[i], 0.);
585
8/16
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2 times.
✗ Branch 17 not taken.
✓ Branch 21 taken 2 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 2 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 2 times.
4 BOOST_CHECK_EQUAL(pathKino->getTv()[i], path->length());
586 }
587 // check if the trajectory is really bang-bang :
588
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
2 BOOST_CHECK_EQUAL(pathKino->getA1()[0], aMax);
589
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 1 times.
2 BOOST_CHECK_EQUAL(pathKino->getT1()[0], pathKino->getT2()[0]);
590 // check if bounds are respected :
591
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
2 BOOST_CHECK(pathVal->validate(path, false, validPath, validationReport));
592 2 }
593
594
33/66
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
4 BOOST_AUTO_TEST_CASE(kinodynamicOriented) {
595
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
4 DevicePtr_t robot = unittest::makeDevice(unittest::HumanoidSimple);
596
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
2 robot->rootJoint()->lowerBound(0, -10);
597
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
2 robot->rootJoint()->lowerBound(1, -10);
598
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
2 robot->rootJoint()->lowerBound(2, 0);
599
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
2 robot->rootJoint()->upperBound(0, 10);
600
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
2 robot->rootJoint()->upperBound(1, 10);
601
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
2 robot->rootJoint()->upperBound(2, 0);
602
603
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
2 robot->setDimensionExtraConfigSpace(6);
604 // define velocity and acceleration bounds
605 2 const double vMax = 2;
606 2 const double aMax = 0.5;
607
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 robot->extraConfigSpace().lower(0) = -vMax;
608
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 robot->extraConfigSpace().lower(1) = -vMax;
609
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 robot->extraConfigSpace().lower(2) = 0;
610
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 robot->extraConfigSpace().upper(0) = vMax;
611
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 robot->extraConfigSpace().upper(1) = vMax;
612
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 robot->extraConfigSpace().upper(2) = 0;
613
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 robot->extraConfigSpace().lower(3) = -aMax;
614
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 robot->extraConfigSpace().lower(4) = -aMax;
615
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 robot->extraConfigSpace().lower(5) = 0;
616
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 robot->extraConfigSpace().upper(3) = aMax;
617
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 robot->extraConfigSpace().upper(4) = aMax;
618
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 robot->extraConfigSpace().upper(5) = 0;
619
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 1 times.
2 BOOST_CHECK_MESSAGE(robot->extraConfigSpace().dimension() == 6,
620 "error during creation of the robot");
621
622 // Create steering method
623
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
2 ProblemPtr_t p = Problem::create(robot);
624
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
2 p->setParameter(std::string("Kinodynamic/velocityBound"), Parameter(vMax));
625
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
4 p->setParameter(std::string("Kinodynamic/accelerationBound"),
626
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
4 Parameter(aMax));
627
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
4 p->setParameter(std::string("Kinodynamic/forceAllOrientation"),
628
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
4 Parameter(true));
629
630
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
2 steeringMethod::KinodynamicPtr_t sm = steeringMethod::Kinodynamic::create(p);
631
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
2 KinodynamicDistancePtr_t dist = KinodynamicDistance::createFromProblem(p);
632 2 KinodynamicOrientedPathPtr_t pathKino, extractedPathKino;
633 2 PathPtr_t path, extractedPath;
634
635
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
2 size_t indexECS = robot->configSize() - 6;
636 bool success;
637
638 JointBoundValidationPtr_t jointValidation =
639
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 JointBoundValidation::create(robot);
640 pathValidation::DiscretizedPtr_t pathVal =
641
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 pathValidation::createDiscretizedCollisionChecking(robot, 0.001);
642
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 pathVal->add(jointValidation);
643 2 PathValidationReportPtr_t validationReport;
644 2 PathPtr_t validPath;
645
646 ConfigurationShooterPtr_t shooter =
647
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 configurationShooter::Uniform::create(robot);
648
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 Configuration_t qr1, qr0;
649 value_type t1, t2;
650
2/2
✓ Branch 0 taken 1000 times.
✓ Branch 1 taken 1 times.
2002 for (size_t i = 0; i < 1000; i++) {
651
1/2
✓ Branch 2 taken 1000 times.
✗ Branch 3 not taken.
2000 qr0 = shooter->shoot();
652
1/2
✓ Branch 2 taken 1000 times.
✗ Branch 3 not taken.
2000 qr1 = shooter->shoot();
653
3/6
✓ Branch 2 taken 1000 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1000 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1000 times.
✗ Branch 9 not taken.
2000 path = (*sm)(qr0, qr1);
654
6/12
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1000 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1000 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1000 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1000 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1000 times.
2000 BOOST_REQUIRE(path);
655 2000 pathKino = HPP_DYNAMIC_PTR_CAST(KinodynamicOrientedPath, path);
656
6/12
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1000 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1000 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1000 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1000 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 1000 times.
2000 BOOST_REQUIRE(pathKino);
657
10/20
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1000 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1000 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1000 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 1000 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1000 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1000 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1000 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 1000 times.
✗ Branch 32 not taken.
✗ Branch 38 not taken.
✓ Branch 39 taken 1000 times.
2000 BOOST_CHECK(path->length() >= (*dist)(qr0, qr1));
658
8/16
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1000 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1000 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 1000 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1000 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 1000 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1000 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 1000 times.
2000 BOOST_CHECK_EQUAL(path->end().head(3), (qr1).head(3));
659
8/16
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1000 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1000 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 1000 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1000 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 1000 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1000 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 1000 times.
2000 BOOST_CHECK_EQUAL(path->initial().head(3), (qr0).head(3));
660
9/18
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1000 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1000 times.
✗ Branch 9 not taken.
✓ Branch 13 taken 1000 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1000 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1000 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 1000 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1000 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 1000 times.
2000 BOOST_CHECK_EQUAL(path->eval(path->length(), success).head(3),
661 (qr1).head(3));
662
8/16
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1000 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1000 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 1000 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1000 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 1000 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1000 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 1000 times.
2000 BOOST_CHECK_EQUAL(path->eval(0., success).head(3), (qr0).head(3));
663
2/2
✓ Branch 0 taken 10000 times.
✓ Branch 1 taken 1000 times.
22000 for (size_t j = 0; j < 10; j++) {
664 value_type a =
665
1/2
✓ Branch 3 taken 10000 times.
✗ Branch 4 not taken.
20000 ((value_type)rand() / (value_type)RAND_MAX) * path->length();
666 value_type b =
667
1/2
✓ Branch 3 taken 10000 times.
✗ Branch 4 not taken.
20000 ((value_type)rand() / (value_type)RAND_MAX) * path->length();
668
2/2
✓ Branch 0 taken 4991 times.
✓ Branch 1 taken 5009 times.
20000 if (a < b) {
669 9982 t1 = a;
670 9982 t2 = b;
671 } else {
672 10018 t1 = b;
673 10018 t2 = a;
674 }
675
2/4
✓ Branch 2 taken 10000 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10000 times.
✗ Branch 6 not taken.
20000 extractedPath = path->extract(std::make_pair(t1, t2));
676
6/12
✓ Branch 1 taken 10000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10000 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 10000 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 10000 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 10000 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 10000 times.
20000 BOOST_REQUIRE(extractedPath);
677
6/12
✓ Branch 1 taken 10000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10000 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 10000 times.
✗ Branch 10 not taken.
✓ Branch 14 taken 10000 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 10000 times.
✗ Branch 18 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 10000 times.
20000 BOOST_CHECK_EQUAL(extractedPath->length(), t2 - t1);
678
5/10
✓ Branch 1 taken 10000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10000 times.
✗ Branch 6 not taken.
✓ Branch 12 taken 10000 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 10000 times.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✓ Branch 20 taken 10000 times.
20000 BOOST_CHECK_EQUAL(extractedPath->timeRange().first, 0.);
679
5/10
✓ Branch 1 taken 10000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10000 times.
✗ Branch 6 not taken.
✓ Branch 12 taken 10000 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 10000 times.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✓ Branch 20 taken 10000 times.
20000 BOOST_CHECK_EQUAL(extractedPath->timeRange().second, t2 - t1);
680
9/18
✓ Branch 1 taken 10000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10000 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 10000 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 10000 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 10000 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10000 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 10000 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 10000 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 10000 times.
20000 BOOST_CHECK_EQUAL(extractedPath->initial().head(indexECS + 3),
681 path->eval(t1, success).head(indexECS + 3));
682
9/18
✓ Branch 1 taken 10000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10000 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 10000 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 10000 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 10000 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10000 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 10000 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 10000 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 10000 times.
20000 BOOST_CHECK_EQUAL(extractedPath->end().head(indexECS + 3),
683 path->eval(t2, success).head(indexECS + 3));
684
9/18
✓ Branch 1 taken 10000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10000 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 10000 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 10000 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 10000 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10000 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 10000 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 10000 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 10000 times.
20000 BOOST_CHECK_EQUAL(extractedPath->eval(0., success).head(indexECS + 3),
685 path->eval(t1, success).head(indexECS + 3));
686
9/18
✓ Branch 1 taken 10000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10000 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 10000 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 10000 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 10000 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10000 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 10000 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 10000 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 10000 times.
20000 BOOST_CHECK_EQUAL(
687 extractedPath->eval(t2 - t1, success).head(indexECS + 3),
688 path->eval(t2, success).head(indexECS + 3));
689
690
1/2
✓ Branch 0 taken 10000 times.
✗ Branch 1 not taken.
20000 if (t1 < t2) {
691 extractedPathKino =
692 20000 HPP_DYNAMIC_PTR_CAST(KinodynamicOrientedPath, extractedPath);
693
6/12
✓ Branch 1 taken 10000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10000 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 10000 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 10000 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 10000 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 10000 times.
20000 BOOST_REQUIRE(extractedPathKino);
694
2/2
✓ Branch 0 taken 30000 times.
✓ Branch 1 taken 10000 times.
80000 for (size_t k = 0; k < 3; k++) {
695
7/14
✓ Branch 1 taken 30000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 30000 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 30000 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 30000 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 30000 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 30000 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 30000 times.
60000 BOOST_CHECK_EQUAL(extractedPathKino->getT0()[k], 0.);
696
8/16
✓ Branch 1 taken 30000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 30000 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 30000 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 30000 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 30000 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 30000 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 30000 times.
✗ Branch 25 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 30000 times.
60000 BOOST_CHECK(extractedPathKino->getT1()[k] >= 0.);
697
8/16
✓ Branch 1 taken 30000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 30000 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 30000 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 30000 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 30000 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 30000 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 30000 times.
✗ Branch 25 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 30000 times.
60000 BOOST_CHECK(extractedPathKino->getTv()[k] >= 0.);
698
8/16
✓ Branch 1 taken 30000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 30000 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 30000 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 30000 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 30000 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 30000 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 30000 times.
✗ Branch 25 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 30000 times.
60000 BOOST_CHECK(extractedPathKino->getT2()[k] >= 0.);
699
12/24
✓ Branch 1 taken 30000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 30000 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 30000 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 30000 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 30000 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 30000 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 30000 times.
✗ Branch 23 not taken.
✓ Branch 26 taken 30000 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 30000 times.
✗ Branch 30 not taken.
✓ Branch 34 taken 30000 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 30000 times.
✗ Branch 38 not taken.
✗ Branch 44 not taken.
✓ Branch 45 taken 30000 times.
60000 BOOST_CHECK_CLOSE(extractedPathKino->getT1()[k] +
700 extractedPathKino->getTv()[k] +
701 extractedPathKino->getT2()[k],
702 t2 - t1, 1e-3);
703 }
704 }
705 }
706 }
707 2 }
708
709 BOOST_AUTO_TEST_SUITE_END()
710