Directory: | ./ |
---|---|
File: | tests/spline-path.cc |
Date: | 2024-08-10 11:29:48 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 183 | 189 | 96.8% |
Branches: | 743 | 1480 | 50.2% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // Copyright (c) 2017, Joseph Mirabel | ||
2 | // Authors: Joseph Mirabel (joseph.mirabel@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 spline_path | ||
30 | #include <../tests/util.hh> | ||
31 | #include <boost/test/included/unit_test.hpp> | ||
32 | #include <hpp/core/path/hermite.hh> | ||
33 | #include <hpp/core/path/spline.hh> | ||
34 | #include <hpp/core/problem.hh> | ||
35 | #include <hpp/core/steering-method/spline.hh> | ||
36 | #include <hpp/core/steering-method/straight.hh> | ||
37 | #include <hpp/pinocchio/configuration.hh> | ||
38 | #include <hpp/pinocchio/joint-collection.hh> | ||
39 | #include <hpp/pinocchio/joint.hh> | ||
40 | #include <hpp/pinocchio/liegroup.hh> | ||
41 | #include <hpp/pinocchio/simple-device.hh> | ||
42 | #include <hpp/pinocchio/urdf/util.hh> | ||
43 | #include <pinocchio/algorithm/joint-configuration.hpp> | ||
44 | #include <pinocchio/fwd.hpp> | ||
45 | |||
46 | #define TOSTR(x) \ | ||
47 | static_cast<std::ostringstream&>((std::ostringstream() << x)).str() | ||
48 | |||
49 | using namespace hpp::core; | ||
50 | using namespace hpp::pinocchio; | ||
51 | |||
52 | 5 | DevicePtr_t createNDoFRobot(int ndof) { | |
53 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | std::ostringstream oss; |
54 | oss << "<robot name='test'>" | ||
55 |
2/4✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
|
5 | << "<link name='link0'/>"; |
56 |
2/2✓ Branch 0 taken 5 times.
✓ Branch 1 taken 5 times.
|
10 | for (int i = 0; i < ndof; ++i) { |
57 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | oss << "<joint name='joint" << i << "' type='prismatic'>" |
58 |
3/6✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
|
5 | << "<parent link='link" << i << "'/>" |
59 |
3/6✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
|
5 | << "<child link='link" << i + 1 << "'/>" |
60 | << "<limit effort='30' velocity='1.0' lower='-4' upper='4'/>" | ||
61 | << "</joint>" | ||
62 |
7/14✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 5 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 5 times.
✗ Branch 20 not taken.
|
5 | << "<link name='link" << i + 1 << "'/>"; |
63 | } | ||
64 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | oss << "</robot>"; |
65 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | std::string urdf(oss.str()); |
66 | |||
67 |
2/4✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
|
10 | DevicePtr_t robot = Device::create("test"); |
68 |
5/10✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 5 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 5 times.
✗ Branch 17 not taken.
|
5 | urdf::loadModelFromString(robot, 0, "", "anchor", urdf, ""); |
69 | 10 | return robot; | |
70 | 5 | } | |
71 | |||
72 | 2 | DevicePtr_t createRobot() { | |
73 |
2/4✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
|
4 | DevicePtr_t robot = unittest::makeDevice(unittest::HumanoidRomeo); |
74 |
2/4✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
|
2 | robot->rootJoint()->lowerBound(0, -1); |
75 |
2/4✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
|
2 | robot->rootJoint()->lowerBound(1, -1); |
76 |
2/4✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
|
2 | robot->rootJoint()->lowerBound(2, -1); |
77 |
2/4✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
|
2 | robot->rootJoint()->upperBound(0, 1); |
78 |
2/4✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
|
2 | robot->rootJoint()->upperBound(1, 1); |
79 |
2/4✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
|
2 | robot->rootJoint()->upperBound(2, 1); |
80 | 2 | return robot; | |
81 | } | ||
82 | |||
83 | 4 | DevicePtr_t createRobotArm() { | |
84 |
2/4✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
|
8 | DevicePtr_t robot = unittest::makeDevice(unittest::ManipulatorArm2); |
85 | 4 | return robot; | |
86 | } | ||
87 | |||
88 | typedef std::pair<value_type, value_type> Pair_t; | ||
89 | |||
90 | ✗ | std::ostream& operator<<(std::ostream& os, const Pair_t& p) { | |
91 | ✗ | os << "Pair " << p.first << ", " << p.second; | |
92 | ✗ | return os; | |
93 | } | ||
94 | |||
95 | ✗ | void printAt(const PathPtr_t& p, ConfigurationOut_t q, value_type t) { | |
96 | ✗ | p->eval(q, t); | |
97 | ✗ | std::cout << t << ":\t" << q.transpose() << std::endl; | |
98 | } | ||
99 | 10 | void checkAt(const PathPtr_t orig, value_type to, const PathPtr_t extr, | |
100 | value_type te) { | ||
101 |
2/4✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
|
10 | Configuration_t q1(orig->outputSize()), q2(orig->outputSize()); |
102 |
2/4✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
|
10 | orig->eval(q1, to); |
103 |
2/4✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
|
10 | extr->eval(q2, te); |
104 |
14/28✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 10 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 10 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 10 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 10 times.
✗ Branch 32 not taken.
✓ Branch 35 taken 10 times.
✗ Branch 36 not taken.
✓ Branch 38 taken 10 times.
✗ Branch 39 not taken.
✓ Branch 41 taken 10 times.
✗ Branch 42 not taken.
✗ Branch 49 not taken.
✓ Branch 50 taken 10 times.
|
10 | BOOST_CHECK_MESSAGE(q2.isApprox(q1), |
105 | "\nPath 1: " << q1.head<10>().transpose() << "\nPath 2: " | ||
106 | << q2.head<10>().transpose()); | ||
107 | 10 | } | |
108 | |||
109 | template <int SplineType> | ||
110 | 1 | void compare_to_straight_path() { | |
111 | typedef path::Spline<SplineType, 1> path_t; | ||
112 | typedef steeringMethod::Spline<SplineType, 1> SM_t; | ||
113 | |||
114 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | DevicePtr_t dev = createRobot(); |
115 |
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.
|
1 | BOOST_REQUIRE(dev); |
116 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | ProblemPtr_t problem = Problem::create(dev); |
117 | |||
118 |
1/2✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
|
1 | Configuration_t q1(::pinocchio::randomConfiguration(dev->model())); |
119 |
1/2✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
|
1 | Configuration_t q2(::pinocchio::randomConfiguration(dev->model())); |
120 | |||
121 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
1 | vector_t v(dev->numberDof()); |
122 |
4/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
|
1 | difference<RnxSOnLieGroupMap>(dev, q2, q1, v); |
123 | |||
124 | // create StraightPath | ||
125 |
3/6✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
|
2 | PathPtr_t sp = (*problem->steeringMethod())(q1, q2); |
126 | // value_type length = sp->length(); | ||
127 | |||
128 | // Create linear spline | ||
129 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | typename SM_t::Ptr_t sm(SM_t::create(problem)); |
130 |
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 | PathPtr_t ls_abstract = (*sm)(q1, q2); |
131 | 1 | typename path_t::Ptr_t ls = HPP_DYNAMIC_PTR_CAST(path_t, ls_abstract); | |
132 | |||
133 | /* | ||
134 | typename path_t::Ptr_t ls = path_t::create (dev, interval_t(0, length), | ||
135 | ConstraintSetPtr_t()); ls->base (q1); typename path_t::ParameterMatrix_t | ||
136 | ls_param = ls->parameters(); ls_param.row(0).setZero(); ls_param.row(1) = v; | ||
137 | ls->parameters(ls_param); | ||
138 | */ | ||
139 | |||
140 |
19/38✓ 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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 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 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 40 taken 1 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 1 times.
✗ Branch 44 not taken.
✓ Branch 47 taken 1 times.
✗ Branch 48 not taken.
✓ Branch 50 taken 1 times.
✗ Branch 51 not taken.
✓ Branch 53 taken 1 times.
✗ Branch 54 not taken.
✓ Branch 56 taken 1 times.
✗ Branch 57 not taken.
✓ Branch 59 taken 1 times.
✗ Branch 60 not taken.
✗ Branch 74 not taken.
✓ Branch 75 taken 1 times.
|
1 | CONFIGURATION_VECTOR_IS_APPROX(dev, sp->initial(), ls->initial(), 1e-7); |
141 |
19/38✓ 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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 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 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 40 taken 1 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 1 times.
✗ Branch 44 not taken.
✓ Branch 47 taken 1 times.
✗ Branch 48 not taken.
✓ Branch 50 taken 1 times.
✗ Branch 51 not taken.
✓ Branch 53 taken 1 times.
✗ Branch 54 not taken.
✓ Branch 56 taken 1 times.
✗ Branch 57 not taken.
✓ Branch 59 taken 1 times.
✗ Branch 60 not taken.
✗ Branch 74 not taken.
✓ Branch 75 taken 1 times.
|
1 | CONFIGURATION_VECTOR_IS_APPROX(dev, sp->end(), ls->end(), 1e-7); |
142 | |||
143 | 1 | const size_type N = 10; | |
144 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | const value_type step1 = sp->length() / N; |
145 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | const value_type step2 = ls->length() / N; |
146 | |||
147 | // Check that straight path and linear spline return the same result. | ||
148 |
2/2✓ Branch 0 taken 10 times.
✓ Branch 1 taken 1 times.
|
11 | for (size_type i = 0; i < N; ++i) |
149 |
1/2✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
|
10 | checkAt(sp, value_type(i) * step1, ls, value_type(i) * step2); |
150 | |||
151 | // Check that the velocities are equals | ||
152 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
1 | vector_t v1(dev->numberDof()); |
153 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
1 | vector_t v2(dev->numberDof()); |
154 |
2/2✓ Branch 0 taken 10 times.
✓ Branch 1 taken 1 times.
|
11 | for (size_type i = 0; i < N; ++i) { |
155 |
2/4✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
|
10 | sp->derivative(v1, value_type(i) * step1, 1); |
156 |
2/4✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
|
10 | ls->derivative(v2, value_type(i) * step2, 1); |
157 |
9/18✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 10 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 10 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 10 times.
✗ Branch 18 not taken.
✓ Branch 22 taken 10 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 10 times.
✗ Branch 26 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 10 times.
|
10 | BOOST_CHECK_SMALL((v2 * step2 - v1 * step1).squaredNorm(), 1e-12); |
158 | } | ||
159 | |||
160 | // Check integral value | ||
161 |
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.
|
1 | BOOST_CHECK_EQUAL(ls->squaredNormIntegral(2), 0); |
162 |
9/18✓ 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 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 31 not taken.
✓ Branch 32 taken 1 times.
|
1 | BOOST_CHECK_CLOSE(ls->squaredNormIntegral(1), v.squaredNorm() / sp->length(), |
163 | 1e-12); | ||
164 | |||
165 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
1 | vector_t derivative(dev->numberDof() * 2); |
166 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
1 | ls->squaredNormIntegralDerivative(1, derivative); |
167 | switch (SplineType) { | ||
168 | case path::CanonicalPolynomeBasis: | ||
169 | BOOST_CHECK(derivative.head(dev->numberDof()).isZero()); | ||
170 | EIGEN_VECTOR_IS_APPROX(derivative.tail(dev->numberDof()), | ||
171 | (2. / sp->length()) * v, 1e-6); | ||
172 | break; | ||
173 | 1 | case path::BernsteinBasis: | |
174 |
21/42✓ 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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 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 39 taken 1 times.
✗ Branch 40 not taken.
✓ Branch 42 taken 1 times.
✗ Branch 43 not taken.
✓ Branch 46 taken 1 times.
✗ Branch 47 not taken.
✓ Branch 49 taken 1 times.
✗ Branch 50 not taken.
✓ Branch 53 taken 1 times.
✗ Branch 54 not taken.
✓ Branch 56 taken 1 times.
✗ Branch 57 not taken.
✓ Branch 59 taken 1 times.
✗ Branch 60 not taken.
✓ Branch 62 taken 1 times.
✗ Branch 63 not taken.
✓ Branch 65 taken 1 times.
✗ Branch 66 not taken.
✗ Branch 74 not taken.
✓ Branch 75 taken 1 times.
|
1 | EIGEN_VECTOR_IS_APPROX(derivative.head(dev->numberDof()), |
175 | (-2. / sp->length()) * v, 1e-6); | ||
176 |
21/42✓ 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 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 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 39 taken 1 times.
✗ Branch 40 not taken.
✓ Branch 42 taken 1 times.
✗ Branch 43 not taken.
✓ Branch 46 taken 1 times.
✗ Branch 47 not taken.
✓ Branch 49 taken 1 times.
✗ Branch 50 not taken.
✓ Branch 53 taken 1 times.
✗ Branch 54 not taken.
✓ Branch 56 taken 1 times.
✗ Branch 57 not taken.
✓ Branch 59 taken 1 times.
✗ Branch 60 not taken.
✓ Branch 62 taken 1 times.
✗ Branch 63 not taken.
✓ Branch 65 taken 1 times.
✗ Branch 66 not taken.
✗ Branch 74 not taken.
✓ Branch 75 taken 1 times.
|
1 | EIGEN_VECTOR_IS_APPROX(derivative.tail(dev->numberDof()), |
177 | (2. / sp->length()) * v, 1e-6); | ||
178 | 1 | break; | |
179 | } | ||
180 | 1 | } | |
181 | |||
182 | template <int SplineType, int Degree> | ||
183 | 1 | void check_velocity_bounds() { | |
184 | typedef steeringMethod::Spline<SplineType, Degree> SM_t; | ||
185 | |||
186 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | DevicePtr_t dev = createRobot(); |
187 |
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.
|
1 | BOOST_REQUIRE(dev); |
188 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | ProblemPtr_t problem = Problem::create(dev); |
189 | |||
190 |
1/2✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
|
1 | Configuration_t q1(::pinocchio::randomConfiguration(dev->model())); |
191 |
1/2✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
|
1 | Configuration_t q2(::pinocchio::randomConfiguration(dev->model())); |
192 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | std::vector<int> orders(1, 1); |
193 |
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.
|
1 | vector_t v1(vector_t::Random(dev->numberDof())), |
194 |
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.
|
1 | v2(vector_t::Random(dev->numberDof())); |
195 | |||
196 | // Create spline | ||
197 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | typename SM_t::Ptr_t sm(SM_t::create(problem)); |
198 |
7/14✓ 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.
✓ 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 20 taken 1 times.
✗ Branch 21 not taken.
|
2 | PathPtr_t spline = sm->steer(q1, orders, v1, q2, orders, v2); |
199 | |||
200 |
5/10✓ 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.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
|
1 | vector_t vb1(-vector_t::Ones(dev->numberDof())), vb2 = vb1; |
201 | 1 | value_type t0 = spline->timeRange().first, t1 = spline->timeRange().second; | |
202 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
1 | spline->velocityBound(vb1, t0, t1); |
203 | |||
204 |
9/18✓ 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 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 1 times.
|
1 | BOOST_CHECK((vb1.array() >= 0).all()); |
205 | |||
206 | 1 | std::size_t N = 1000; | |
207 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | value_type step = spline->length() / value_type(N); |
208 |
2/2✓ Branch 0 taken 1000 times.
✓ Branch 1 taken 1 times.
|
1001 | for (std::size_t i = 0; i < N; ++i) { |
209 |
1/2✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
|
1000 | vb2.setConstant(-1); |
210 |
2/4✓ Branch 2 taken 1000 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1000 times.
✗ Branch 6 not taken.
|
1000 | spline->velocityBound(vb2, t0, t1); |
211 |
9/18✓ 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 20 taken 1000 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1000 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1000 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 1000 times.
|
1000 | BOOST_CHECK((vb2.array() >= 0).all()); |
212 |
19/38✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1000 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 1000 times.
✗ Branch 11 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 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 34 taken 1000 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 1000 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 1000 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 1000 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 1000 times.
✗ Branch 47 not taken.
✓ Branch 49 taken 1000 times.
✗ Branch 50 not taken.
✓ Branch 52 taken 1000 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1000 times.
✗ Branch 56 not taken.
✗ Branch 67 not taken.
✓ Branch 68 taken 1000 times.
|
1000 | BOOST_CHECK_MESSAGE( |
213 | (vb2.array() <= vb1.array()).all(), | ||
214 | "i=" << i << " Velocity bound should have decreased. Interval is [" | ||
215 | << t0 << ", " << t1 << "]. Difference:\n" | ||
216 | << (vb1 - vb2).transpose()); | ||
217 |
1/2✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
|
1000 | vb1 = vb2; |
218 |
2/2✓ Branch 0 taken 500 times.
✓ Branch 1 taken 500 times.
|
1000 | if (i % 2) { |
219 | 500 | t0 += step; | |
220 | } else { | ||
221 | 500 | t1 -= step; | |
222 | } | ||
223 | } | ||
224 | 1 | } | |
225 | |||
226 | template <int SplineType, int Degree, int order> | ||
227 | 6 | void check_steering_method() { | |
228 | typedef steeringMethod::Spline<SplineType, Degree> SM_t; | ||
229 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
10 | std::vector<int> orders{1}; |
230 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | if (order == 2) orders.push_back(2); |
231 | |||
232 | // Use the manipulator arm and not Romeo since steering method does not give | ||
233 | // correct values for vel/acc when the robot configuration contains a | ||
234 | // freeflyer | ||
235 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
6 | DevicePtr_t dev = createRobotArm(); |
236 |
6/12✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 3 times.
|
6 | BOOST_REQUIRE(dev); |
237 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
6 | ProblemPtr_t problem = Problem::create(dev); |
238 | |||
239 | // Create random configurations and velocities/accelerations | ||
240 |
1/2✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
|
6 | Configuration_t q1(::pinocchio::randomConfiguration(dev->model())); |
241 |
1/2✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
|
6 | Configuration_t q2(::pinocchio::randomConfiguration(dev->model())); |
242 |
3/6✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
|
6 | matrix_t deriv1(matrix_t::Random(dev->numberDof(), order)), |
243 |
3/6✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
|
6 | deriv2(matrix_t::Random(dev->numberDof(), order)); |
244 | 6 | double length = static_cast<float>(rand()) / static_cast<float>(RAND_MAX); | |
245 | |||
246 | // Create spline | ||
247 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
6 | typename SM_t::Ptr_t sm(SM_t::create(problem)); |
248 |
7/14✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 3 times.
✗ Branch 21 not taken.
|
12 | PathPtr_t spline1 = sm->steer(q1, orders, deriv1, q2, orders, deriv2, length); |
249 | |||
250 | // Check length | ||
251 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
6 | double spline_length = spline1->length(); |
252 |
9/18✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 3 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 3 times.
✗ Branch 27 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 3 times.
|
6 | BOOST_CHECK_MESSAGE(abs(spline_length - length) < 0.0001, |
253 | "Path does not have desired length: " | ||
254 | << spline_length << " instead of " << length); | ||
255 | |||
256 | // Check configuration at start/end | ||
257 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
6 | Configuration_t spline_q1 = spline1->initial(); |
258 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
6 | Configuration_t spline_q2 = spline1->end(); |
259 |
13/26✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 3 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 3 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 3 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 3 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 3 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 3 times.
✗ Branch 38 not taken.
✗ Branch 46 not taken.
✓ Branch 47 taken 3 times.
|
6 | EIGEN_VECTOR_IS_APPROX(q1, spline_q1, 1e-6); |
260 |
13/26✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 3 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 3 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 3 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 3 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 3 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 3 times.
✗ Branch 38 not taken.
✗ Branch 46 not taken.
✓ Branch 47 taken 3 times.
|
6 | EIGEN_VECTOR_IS_APPROX(q2, spline_q2, 1e-6); |
261 | |||
262 | // Check derivatives at start/end | ||
263 |
2/2✓ Branch 2 taken 4 times.
✓ Branch 3 taken 3 times.
|
14 | for (int i = 1; i <= order; i++) { |
264 |
3/6✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 9 not taken.
|
8 | vector_t spline_v1(vector_t::Random(dev->numberDof())), |
265 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
8 | spline_v2 = spline_v1; |
266 |
2/4✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
|
8 | spline1->derivative(spline_v1, 0, i); |
267 |
2/4✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
|
8 | spline1->derivative(spline_v2, spline_length, i); |
268 |
15/30✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 4 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 4 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 4 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 4 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 4 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 4 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 4 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 4 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 4 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 4 times.
✗ Branch 44 not taken.
✗ Branch 52 not taken.
✓ Branch 53 taken 4 times.
|
8 | EIGEN_VECTOR_IS_APPROX(deriv1.col(i - 1), spline_v1, 1e-6); |
269 |
15/30✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 4 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 4 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 4 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 4 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 4 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 4 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 4 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 4 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 4 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 4 times.
✗ Branch 44 not taken.
✗ Branch 52 not taken.
✓ Branch 53 taken 4 times.
|
8 | EIGEN_VECTOR_IS_APPROX(deriv2.col(i - 1), spline_v2, 1e-6); |
270 | } | ||
271 | 6 | } | |
272 | |||
273 |
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(spline_bernstein) { |
274 | 2 | compare_to_straight_path<path::BernsteinBasis>(); | |
275 | 2 | } | |
276 | |||
277 | 5 | void check_bernstein_polynomial_3rd(value_type p0, value_type p1, value_type p2, | |
278 | value_type p3) { | ||
279 | // Degree 3 | ||
280 | typedef path::Spline<path::BernsteinBasis, 3> path_t; | ||
281 | |||
282 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | DevicePtr_t dev = createNDoFRobot(1); |
283 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | vector_t q0(1); |
284 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | q0 << 0.0; |
285 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | path_t::ParameterMatrix_t m(4, 1); |
286 |
4/8✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
|
5 | m << p0, p1, p2, p3; |
287 | bool ok; | ||
288 | |||
289 |
2/4✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
|
10 | ConstraintSetPtr_t constraint = ConstraintSet::create(dev, "empty"); |
290 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
5 | path_t::Ptr_t path = path_t::create(dev, interval_t(0, 1), constraint); |
291 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
5 | path->base(q0); |
292 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
5 | path->parameters(m); |
293 | |||
294 | // evaluation | ||
295 |
9/18✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 5 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 5 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 5 times.
✗ Branch 19 not taken.
✓ Branch 23 taken 5 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 5 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 5 times.
|
5 | BOOST_CHECK_CLOSE(path->eval(0.0, ok)[0], m(0, 0), 1e-8); |
296 |
12/24✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 5 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 5 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 5 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 5 times.
✗ Branch 21 not taken.
✓ Branch 24 taken 5 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 5 times.
✗ Branch 28 not taken.
✓ Branch 32 taken 5 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 5 times.
✗ Branch 36 not taken.
✗ Branch 40 not taken.
✓ Branch 41 taken 5 times.
|
5 | BOOST_CHECK_CLOSE(path->eval(0.5, ok)[0], |
297 | 0.125 * (m(0, 0) + 3 * m(1, 0) + 3 * m(2, 0) + m(3, 0)), | ||
298 | 1e-8); | ||
299 |
9/18✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 5 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 5 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 5 times.
✗ Branch 19 not taken.
✓ Branch 23 taken 5 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 5 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 5 times.
|
5 | BOOST_CHECK_CLOSE(path->eval(1.0, ok)[0], m(3, 0), 1e-8); |
300 | |||
301 | // derivative | ||
302 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | vector_t der(1); |
303 |
2/4✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
|
5 | path->derivative(der, 0.0, ok); |
304 |
9/18✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 5 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 5 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 5 times.
✗ Branch 18 not taken.
✓ Branch 22 taken 5 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 5 times.
✗ Branch 26 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 5 times.
|
5 | BOOST_CHECK_CLOSE(der[0], 3 * (-m(0, 0) + m(1, 0)), 1e-8); |
305 |
2/4✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
|
5 | path->derivative(der, 0.5, ok); |
306 |
11/22✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 5 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 5 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 5 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 5 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 5 times.
✗ Branch 24 not taken.
✓ Branch 28 taken 5 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 5 times.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✓ Branch 36 taken 5 times.
|
5 | BOOST_CHECK_CLOSE(der[0], 3 * (-m(0, 0) - m(1, 0) + m(2, 0) + m(3, 0)) / 4, |
307 | 1e-8); | ||
308 |
2/4✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
|
5 | path->derivative(der, 1.0, ok); |
309 |
9/18✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 5 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 5 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 5 times.
✗ Branch 18 not taken.
✓ Branch 22 taken 5 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 5 times.
✗ Branch 26 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 5 times.
|
5 | BOOST_CHECK_CLOSE(der[0], 3 * (-m(2, 0) + m(3, 0)), 1e-8); |
310 | |||
311 | // velocity bound | ||
312 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | vector_t vb(1); |
313 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | vector_t splineVbounds(4); |
314 |
2/4✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
|
5 | path->velocityBound(vb, 0.0, 1.0); |
315 |
4/8✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
|
5 | splineVbounds << 3, 3, 3, 3; |
316 |
11/22✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 5 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 5 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 5 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 5 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 5 times.
✗ Branch 24 not taken.
✓ Branch 28 taken 5 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 5 times.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✓ Branch 36 taken 5 times.
|
5 | BOOST_CHECK_CLOSE(vb[0], (m.cwiseAbs().transpose() * splineVbounds)[0], 1e-8); |
317 | |||
318 |
2/4✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
|
5 | path->velocityBound(vb, 0.0, 0.5); |
319 |
4/8✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
|
5 | splineVbounds << 3, 3, 1, 0.75; |
320 |
11/22✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 5 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 5 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 5 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 5 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 5 times.
✗ Branch 24 not taken.
✓ Branch 28 taken 5 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 5 times.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✓ Branch 36 taken 5 times.
|
5 | BOOST_CHECK_CLOSE(vb[0], (m.cwiseAbs().transpose() * splineVbounds)[0], 1e-8); |
321 | |||
322 |
2/4✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
|
5 | path->velocityBound(vb, 0.5, 1.0); |
323 |
4/8✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
|
5 | splineVbounds << 0.75, 1, 3, 3; |
324 |
11/22✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 5 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 5 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 5 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 5 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 5 times.
✗ Branch 24 not taken.
✓ Branch 28 taken 5 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 5 times.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✓ Branch 36 taken 5 times.
|
5 | BOOST_CHECK_CLOSE(vb[0], (m.cwiseAbs().transpose() * splineVbounds)[0], 1e-8); |
325 | |||
326 |
2/4✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
|
5 | path->velocityBound(vb, 0.5, 0.6); |
327 |
4/8✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
|
5 | splineVbounds << 0.75, 0.96, 0.75, 1.08; |
328 |
11/22✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 5 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 5 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 5 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 5 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 5 times.
✗ Branch 24 not taken.
✓ Branch 28 taken 5 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 5 times.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✓ Branch 36 taken 5 times.
|
5 | BOOST_CHECK_CLOSE(vb[0], (m.cwiseAbs().transpose() * splineVbounds)[0], 1e-8); |
329 | 5 | } | |
330 | |||
331 |
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(bernstein_polynomial) { |
332 | 2 | check_bernstein_polynomial_3rd(1.0, 0.0, 0.0, 0.0); | |
333 | 2 | check_bernstein_polynomial_3rd(0.0, 1.0, 0.0, 0.0); | |
334 | 2 | check_bernstein_polynomial_3rd(0.0, 0.0, 1.0, 0.0); | |
335 | 2 | check_bernstein_polynomial_3rd(0.0, 0.0, 0.0, 1.0); | |
336 | |||
337 | 2 | check_bernstein_polynomial_3rd(1.0, 0.3, 0.4, -0.5); | |
338 | 2 | } | |
339 | |||
340 |
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(spline_bernstein_velocity) { |
341 | 2 | check_velocity_bounds<path::BernsteinBasis, 3>(); | |
342 | 2 | } | |
343 | |||
344 |
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(steering_method) { |
345 | 2 | check_steering_method<path::BernsteinBasis, 3, 1>(); | |
346 | 2 | check_steering_method<path::BernsteinBasis, 5, 1>(); | |
347 | 2 | check_steering_method<path::BernsteinBasis, 5, 2>(); | |
348 | 2 | } | |
349 | |||
350 |
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(hermite_path) { |
351 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | DevicePtr_t dev = createRobotArm(); |
352 | ConstraintSetPtr_t constraint = | ||
353 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
4 | hpp::core::ConstraintSet::create(dev, "empty"); |
354 | |||
355 |
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 | vector_t q1(vector_t::Zero(dev->configSize())); |
356 |
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 | vector_t q2(vector_t::Ones(dev->configSize())); |
357 | |||
358 | path::HermitePtr_t path = | ||
359 |
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 | hpp::core::path::Hermite::create(dev, q1, q2, constraint); |
360 | bool ok; | ||
361 |
2/4✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
|
2 | vector_t evaluated = path->eval(path->length(), ok); |
362 |
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_CHECK(ok); |
363 |
17/34✓ 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 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 1 times.
✗ Branch 33 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 39 taken 1 times.
✗ Branch 40 not taken.
✓ Branch 42 taken 1 times.
✗ Branch 43 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 51 taken 1 times.
✗ Branch 52 not taken.
✗ Branch 64 not taken.
✓ Branch 65 taken 1 times.
|
2 | CONFIGURATION_VECTOR_IS_APPROX(dev, evaluated, path->end(), 1e-12); |
364 | 2 | } | |
365 |