| Directory: | ./ |
|---|---|
| File: | tests/util.hh |
| Date: | 2025-05-05 12:19:30 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 29 | 35 | 82.9% |
| Branches: | 37 | 92 | 40.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 | #ifndef TEST_UTIL_HH | ||
| 30 | #define TEST_UTIL_HH | ||
| 31 | |||
| 32 | #define EIGEN_VECTOR_IS_APPROX(Va, Vb) \ | ||
| 33 | BOOST_CHECK_MESSAGE((Va).isApprox(Vb, test_precision), \ | ||
| 34 | "check " #Va ".isApprox(" #Vb \ | ||
| 35 | ") failed " \ | ||
| 36 | "[\n" \ | ||
| 37 | << (Va).transpose() << "\n!=\n" \ | ||
| 38 | << (Vb).transpose() << "\n]") | ||
| 39 | |||
| 40 | #define SE3CONFIG_IS_APPROX(Va, Vb) \ | ||
| 41 | { \ | ||
| 42 | BOOST_CHECK_MESSAGE((Va.head<3>()).isApprox(Vb.head<3>(), test_precision), \ | ||
| 43 | "check " #Va ".isApprox(" #Vb \ | ||
| 44 | ") failed " \ | ||
| 45 | "[\n" \ | ||
| 46 | << (Va).transpose() << "\n!=\n" \ | ||
| 47 | << (Vb).transpose() << "\n]"); \ | ||
| 48 | if ((Va.tail<4>().array() * Vb.tail<4>().array() > 0.01).any()) { \ | ||
| 49 | BOOST_CHECK_MESSAGE( \ | ||
| 50 | (Va.tail<4>()).isApprox(Vb.tail<4>(), test_precision), \ | ||
| 51 | "check " #Va ".isApprox(" #Vb \ | ||
| 52 | ") failed " \ | ||
| 53 | "[\n" \ | ||
| 54 | << (Va).transpose() << "\n!=\n" \ | ||
| 55 | << (Vb).transpose() << "\n]"); \ | ||
| 56 | } else { \ | ||
| 57 | BOOST_CHECK_MESSAGE( \ | ||
| 58 | (Va.tail<4>()).isApprox(-Vb.tail<4>(), test_precision), \ | ||
| 59 | "check " #Va ".isApprox(" #Vb \ | ||
| 60 | ") failed " \ | ||
| 61 | "[\n" \ | ||
| 62 | << (Va).transpose() << "\n!=\n" \ | ||
| 63 | << (Vb).transpose() << "\n]"); \ | ||
| 64 | } \ | ||
| 65 | } | ||
| 66 | |||
| 67 | #define EIGEN_VECTOR_IS_NOT_APPROX(Va, Vb) \ | ||
| 68 | BOOST_CHECK_MESSAGE(!Va.isApprox(Vb, test_precision), \ | ||
| 69 | "check !" #Va ".isApprox(" #Vb \ | ||
| 70 | ") failed " \ | ||
| 71 | "[\n" \ | ||
| 72 | << (Va).transpose() << "\n==\n" \ | ||
| 73 | << (Vb).transpose() << "\n]") | ||
| 74 | |||
| 75 | #define EIGEN_IS_APPROX(matrixA, matrixB) \ | ||
| 76 | BOOST_CHECK_MESSAGE(matrixA.isApprox(matrixB, test_precision), \ | ||
| 77 | "check " #matrixA ".isApprox(" #matrixB \ | ||
| 78 | ") failed " \ | ||
| 79 | "[\n" \ | ||
| 80 | << matrixA << "\n!=\n" \ | ||
| 81 | << matrixB << "\n]") | ||
| 82 | |||
| 83 | #define EIGEN_IS_NOT_APPROX(matrixA, matrixB) \ | ||
| 84 | BOOST_CHECK_MESSAGE(!matrixA.isApprox(matrixB, test_precision), \ | ||
| 85 | "check !" #matrixA ".isApprox(" #matrixB \ | ||
| 86 | ") failed " \ | ||
| 87 | "[\n" \ | ||
| 88 | << matrixA << "\n==\n" \ | ||
| 89 | << matrixB << "\n]") | ||
| 90 | |||
| 91 | #define SOLVER_CHECK_SOLVE(expr, expected) \ | ||
| 92 | { \ | ||
| 93 | hpp::constraints::solver::HierarchicalIterative::Status __status = expr; \ | ||
| 94 | BOOST_CHECK_MESSAGE( \ | ||
| 95 | __status == hpp::constraints::solver::HierarchicalIterative::expected, \ | ||
| 96 | "check " #expr " == " #expected " failed [" \ | ||
| 97 | << __status << " != " \ | ||
| 98 | << hpp::constraints::solver::HierarchicalIterative::expected \ | ||
| 99 | << "]"); \ | ||
| 100 | } | ||
| 101 | |||
| 102 | #include <boost/test/included/unit_test.hpp> | ||
| 103 | #include <hpp/constraints/differentiable-function.hh> | ||
| 104 | #include <hpp/constraints/fwd.hh> | ||
| 105 | #include <hpp/constraints/matrix-view.hh> | ||
| 106 | #include <hpp/pinocchio/device.hh> | ||
| 107 | #include <hpp/pinocchio/extra-config-space.hh> | ||
| 108 | #include <hpp/pinocchio/joint-collection.hh> | ||
| 109 | #include <hpp/pinocchio/joint.hh> | ||
| 110 | #include <hpp/pinocchio/util.hh> | ||
| 111 | #include <pinocchio/algorithm/joint-configuration.hpp> | ||
| 112 | #include <pinocchio/multibody/model.hpp> | ||
| 113 | |||
| 114 | 211 | void randomConfig(const hpp::pinocchio::DevicePtr_t& d, | |
| 115 | hpp::pinocchio::Configuration_t& q) { | ||
| 116 | using namespace hpp::constraints; | ||
| 117 | 211 | size_type extraDim = d->extraConfigSpace().dimension(); | |
| 118 | 211 | size_type offset = d->configSize() - extraDim; | |
| 119 | |||
| 120 | 211 | q.resize(d->configSize()); | |
| 121 |
2/4✓ Branch 4 taken 211 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 211 times.
✗ Branch 8 not taken.
|
211 | q.head(offset) = ::pinocchio::randomConfiguration(d->model()); |
| 122 | |||
| 123 | // Shoot extra configuration variables | ||
| 124 |
2/2✓ Branch 0 taken 303 times.
✓ Branch 1 taken 211 times.
|
514 | for (size_type i = 0; i < extraDim; ++i) { |
| 125 |
1/2✓ Branch 3 taken 303 times.
✗ Branch 4 not taken.
|
303 | value_type lower = d->extraConfigSpace().lower(i); |
| 126 |
1/2✓ Branch 3 taken 303 times.
✗ Branch 4 not taken.
|
303 | value_type upper = d->extraConfigSpace().upper(i); |
| 127 | 303 | value_type range = upper - lower; | |
| 128 |
3/6✓ Branch 0 taken 303 times.
✗ Branch 1 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 303 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 303 times.
|
303 | if ((range < 0) || (range == std::numeric_limits<double>::infinity())) { |
| 129 | ✗ | std::ostringstream oss; | |
| 130 | ✗ | oss << "Cannot uniformy sample extra config variable " << i | |
| 131 | ✗ | << ". min = " << lower << ", max = " << upper << std::endl; | |
| 132 | ✗ | throw std::runtime_error(oss.str()); | |
| 133 | } | ||
| 134 |
1/2✓ Branch 2 taken 303 times.
✗ Branch 3 not taken.
|
303 | q[offset + i] = lower + (upper - lower) * rand() / RAND_MAX; |
| 135 | } | ||
| 136 | 211 | } | |
| 137 | |||
| 138 | // This is an ugly fix to make BOOST_CHECK_EQUAL able to print segments_t | ||
| 139 | // when they are not equal. | ||
| 140 | namespace std { | ||
| 141 | ✗ | std::ostream& operator<<(std::ostream& os, | |
| 142 | hpp::constraints::BlockIndex::segments_t b) { | ||
| 143 | ✗ | return os << hpp::pretty_print(b); | |
| 144 | } | ||
| 145 | } // namespace std | ||
| 146 | |||
| 147 | class Quadratic : public hpp::constraints::DifferentiableFunction { | ||
| 148 | public: | ||
| 149 | typedef hpp::shared_ptr<Quadratic> Ptr_t; | ||
| 150 | typedef hpp::constraints::DifferentiableFunction DifferentiableFunction; | ||
| 151 | typedef hpp::constraints::matrix_t matrix_t; | ||
| 152 | typedef hpp::constraints::matrixOut_t matrixOut_t; | ||
| 153 | typedef hpp::constraints::vector_t vector_t; | ||
| 154 | typedef hpp::constraints::vectorIn_t vectorIn_t; | ||
| 155 | typedef hpp::constraints::LiegroupElementRef LiegroupElementRef; | ||
| 156 | typedef hpp::constraints::value_type value_type; | ||
| 157 | |||
| 158 | Quadratic(const matrix_t& _A, const vector_t& _b, const value_type& _c) | ||
| 159 | : hpp::constraints::DifferentiableFunction(_A.cols(), _A.cols(), 1, | ||
| 160 | "Quadratic"), | ||
| 161 | A(_A), | ||
| 162 | b(_b), | ||
| 163 | c(_c) { | ||
| 164 | check(); | ||
| 165 | } | ||
| 166 | |||
| 167 | 12 | Quadratic(const matrix_t& _A, const value_type& _c = 0) | |
| 168 | 12 | : hpp::constraints::DifferentiableFunction(_A.cols(), _A.cols(), 1, | |
| 169 | "Quadratic"), | ||
| 170 | 12 | A(_A), | |
| 171 |
2/4✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
|
12 | b(vector_t::Zero(_A.rows())), |
| 172 |
3/6✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
✓ Branch 12 taken 12 times.
✗ Branch 13 not taken.
|
12 | c(_c) { |
| 173 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
12 | check(); |
| 174 | 12 | } | |
| 175 | |||
| 176 | 12 | void check() const { | |
| 177 |
5/10✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
✓ Branch 9 taken 12 times.
✗ Branch 10 not taken.
✓ Branch 14 taken 12 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 12 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 12 times.
|
12 | BOOST_REQUIRE(A.rows() == A.cols()); |
| 178 |
5/10✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
✓ Branch 9 taken 12 times.
✗ Branch 10 not taken.
✓ Branch 14 taken 12 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 12 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 12 times.
|
12 | BOOST_REQUIRE(A.rows() == b.rows()); |
| 179 | 12 | } | |
| 180 | |||
| 181 | 346 | void impl_compute(LiegroupElementRef y, vectorIn_t x) const { | |
| 182 |
5/10✓ Branch 2 taken 346 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 346 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 346 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 346 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 346 times.
✗ Branch 16 not taken.
|
346 | y.vector()[0] = x.transpose() * A * x + b.dot(x) + c; |
| 183 | 346 | } | |
| 184 | |||
| 185 | 273 | void impl_jacobian(matrixOut_t J, vectorIn_t x) const { | |
| 186 |
6/12✓ Branch 2 taken 273 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 273 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 273 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 273 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 273 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 273 times.
✗ Branch 18 not taken.
|
273 | J.noalias() = 2 * x.transpose() * A + b.transpose(); |
| 187 | 273 | } | |
| 188 | |||
| 189 | matrix_t A; | ||
| 190 | vector_t b; | ||
| 191 | value_type c; | ||
| 192 | }; | ||
| 193 | |||
| 194 | #endif // TEST_UTIL_HH | ||
| 195 |