| Directory: | ./ |
|---|---|
| File: | tests/spline-path.cc |
| Date: | 2025-03-10 11:18:21 |
| 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 |