| Directory: | ./ |
|---|---|
| File: | tests/convex-shape-contact.cc |
| Date: | 2025-05-05 12:19:30 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 139 | 139 | 100.0% |
| Branches: | 374 | 736 | 50.8% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // Copyright (c) 2020, LAAS-CNRS | ||
| 2 | // Authors: Florent Lamiraux | ||
| 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 hpp_constraints | ||
| 30 | #include <../tests/convex-shape-contact-function.hh> | ||
| 31 | #include <../tests/util.hh> | ||
| 32 | #include <hpp/constraints/convex-shape-contact.hh> | ||
| 33 | #include <hpp/constraints/explicit/convex-shape-contact.hh> | ||
| 34 | #include <hpp/constraints/solver/by-substitution.hh> | ||
| 35 | #include <hpp/pinocchio/liegroup-space.hh> | ||
| 36 | #include <hpp/pinocchio/urdf/util.hh> | ||
| 37 | #include <pinocchio/algorithm/joint-configuration.hpp> | ||
| 38 | |||
| 39 | using hpp::pinocchio::BodyPtr_t; | ||
| 40 | using hpp::pinocchio::Configuration_t; | ||
| 41 | using hpp::pinocchio::ConfigurationPtr_t; | ||
| 42 | using hpp::pinocchio::Device; | ||
| 43 | using hpp::pinocchio::DevicePtr_t; | ||
| 44 | using hpp::pinocchio::JointPtr_t; | ||
| 45 | using hpp::pinocchio::JointVector_t; | ||
| 46 | using hpp::pinocchio::LiegroupElement; | ||
| 47 | using hpp::pinocchio::LiegroupSpace; | ||
| 48 | using hpp::pinocchio::urdf::loadModelFromString; | ||
| 49 | using namespace hpp::constraints; | ||
| 50 | |||
| 51 | LiegroupSpacePtr_t SE3(LiegroupSpace::SE3()); | ||
| 52 | // This test builds a robot with two freeflyer joints j1 and j2 | ||
| 53 | // To each joint, a box of size (2,2,2) is attached: box1 and box2. | ||
| 54 | // Two floor contact surfaces are attached to box 1 | ||
| 55 | // - one on face z=1, | ||
| 56 | // - one on face z=-1 | ||
| 57 | // Two object contact surfaces are attached to box2 | ||
| 58 | // - one on face z=1, | ||
| 59 | // - one on face z=-1 | ||
| 60 | // 4 configurations q_init, each one corresponding to a type of contact are | ||
| 61 | // produced. | ||
| 62 | // A solver of type BySubstitution is built with only one explicit contact | ||
| 63 | // constraint of type explicit_::ConvexShapeContact | ||
| 64 | // | ||
| 65 | // N random configurations are sampled. For each of them, and for each q_init, | ||
| 66 | // the solver right hand side is initialized with q_init, the solver solves from | ||
| 67 | // the random configuration. | ||
| 68 | // the relative position of box2 with respect to box1 is checked to be the same | ||
| 69 | // as in configuration q_init. | ||
| 70 |
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(convexShapeContact) { |
| 71 | const std::string model( | ||
| 72 | "<robot name=\"box\">" | ||
| 73 | " <link name=\"baselink\">" | ||
| 74 | " <collision>" | ||
| 75 | " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>" | ||
| 76 | " <geometry>" | ||
| 77 | " <box size=\"2 2 2\"/>" | ||
| 78 | " </geometry>" | ||
| 79 | " </collision>" | ||
| 80 | " </link>" | ||
| 81 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | "</robot>"); |
| 82 | |||
| 83 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
4 | DevicePtr_t robot(Device::create("two-boxes")); |
| 84 | // Create two freeflying boxes | ||
| 85 |
5/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
|
2 | loadModelFromString(robot, 0, "1/", "freeflyer", model, ""); |
| 86 |
5/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
|
2 | loadModelFromString(robot, 0, "2/", "freeflyer", model, ""); |
| 87 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
|
2 | assert(robot->nbJoints() == 2); |
| 88 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | JointPtr_t j1(robot->jointAt(0)); |
| 89 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | JointPtr_t j2(robot->jointAt(1)); |
| 90 | // Set joint bounds | ||
| 91 |
2/2✓ Branch 0 taken 2 times.
✓ Branch 1 taken 1 times.
|
6 | for (std::size_t i = 0; i < 2; ++i) { |
| 92 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
4 | vector_t l(7); |
| 93 |
7/14✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 2 times.
✗ Branch 20 not taken.
|
4 | l << -2, -2, -2, -1, -1, -1, -1; |
| 94 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
4 | vector_t u(7); |
| 95 |
7/14✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 2 times.
✗ Branch 20 not taken.
|
4 | u << 2, 2, 2, 1, 1, 1, 1; |
| 96 |
3/6✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
|
4 | robot->jointAt(i)->lowerBounds(l); |
| 97 |
3/6✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
|
4 | robot->jointAt(i)->upperBounds(u); |
| 98 | 4 | } | |
| 99 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | vector3_t v; |
| 100 | 2 | JointAndShape_t surface; | |
| 101 | 2 | JointAndShapes_t surfacesObject1, surfacesObject2; | |
| 102 | // Upper face of box1 | ||
| 103 | 2 | surface.first = j1; | |
| 104 |
3/6✓ 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.
|
2 | v << -1., -1., 1; |
| 105 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | surface.second.push_back(v); |
| 106 |
3/6✓ 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.
|
2 | v << 1., -1., 1.; |
| 107 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | surface.second.push_back(v); |
| 108 |
3/6✓ 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.
|
2 | v << 1., 1., 1.; |
| 109 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | surface.second.push_back(v); |
| 110 |
3/6✓ 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.
|
2 | v << -1., 1., 1.; |
| 111 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | surface.second.push_back(v); |
| 112 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | surfacesObject1.push_back(surface); |
| 113 | // Lower face of box1 | ||
| 114 | 2 | surface.second.clear(); | |
| 115 |
3/6✓ 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.
|
2 | v << -1., 1., -1.; |
| 116 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | surface.second.push_back(v); |
| 117 |
3/6✓ 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.
|
2 | v << 1., 1., -1.; |
| 118 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | surface.second.push_back(v); |
| 119 |
3/6✓ 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.
|
2 | v << 1., -1., -1.; |
| 120 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | surface.second.push_back(v); |
| 121 |
3/6✓ 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.
|
2 | v << -1., -1., -1; |
| 122 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | surface.second.push_back(v); |
| 123 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | surfacesObject1.push_back(surface); |
| 124 | // Upper face of box2 | ||
| 125 | 2 | surface.second.clear(); | |
| 126 | 2 | surface.first = j2; | |
| 127 |
3/6✓ 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.
|
2 | v << -1., -1., 1; |
| 128 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | surface.second.push_back(v); |
| 129 |
3/6✓ 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.
|
2 | v << 1., -1., 1.; |
| 130 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | surface.second.push_back(v); |
| 131 |
3/6✓ 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.
|
2 | v << 1., 1., 1.; |
| 132 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | surface.second.push_back(v); |
| 133 |
3/6✓ 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.
|
2 | v << -1., 1., 1.; |
| 134 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | surface.second.push_back(v); |
| 135 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | surfacesObject2.push_back(surface); |
| 136 | // Lower face of box2 | ||
| 137 | 2 | surface.second.clear(); | |
| 138 |
3/6✓ 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.
|
2 | v << -1., 1., -1.; |
| 139 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | surface.second.push_back(v); |
| 140 |
3/6✓ 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.
|
2 | v << 1., 1., -1.; |
| 141 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | surface.second.push_back(v); |
| 142 |
3/6✓ 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.
|
2 | v << 1., -1., -1.; |
| 143 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | surface.second.push_back(v); |
| 144 |
3/6✓ 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.
|
2 | v << -1., -1., -1; |
| 145 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | surface.second.push_back(v); |
| 146 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | surfacesObject2.push_back(surface); |
| 147 |
2/4✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
|
4 | ExplicitPtr_t contactConstraint(explicit_::ConvexShapeContact::create( |
| 148 | 2 | "box1/box2", robot, surfacesObject1, surfacesObject2, 0)); | |
| 149 | 2 | ConvexShapeContactHoldPtr_t f(HPP_DYNAMIC_PTR_CAST( | |
| 150 | 2 | ConvexShapeContactHold, contactConstraint->functionPtr())); | |
| 151 | 2 | value_type M(f->contactConstraint()->radius()); | |
| 152 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
2 | BOOST_CHECK(M > sqrt(2)); |
| 153 | // check that the two joints involved can be retrieved correctly | ||
| 154 | std::pair<JointConstPtr_t, JointConstPtr_t> joints = | ||
| 155 |
1/2✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
|
4 | f->dependsOnRelPoseBetween(nullptr); |
| 156 |
5/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 1 times.
|
2 | BOOST_CHECK_EQUAL(Joint::index(joints.first), Joint::index(j1)); |
| 157 |
5/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 1 times.
|
2 | BOOST_CHECK_EQUAL(Joint::index(joints.second), Joint::index(j2)); |
| 158 | |||
| 159 |
1/2✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | joints = f->contactConstraint()->dependsOnRelPoseBetween(nullptr); |
| 160 |
5/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 1 times.
|
2 | BOOST_CHECK_EQUAL(Joint::index(joints.first), Joint::index(j1)); |
| 161 |
5/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 1 times.
|
2 | BOOST_CHECK_EQUAL(Joint::index(joints.second), Joint::index(j2)); |
| 162 | |||
| 163 |
1/2✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | joints = f->complement()->dependsOnRelPoseBetween(nullptr); |
| 164 |
5/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 1 times.
|
2 | BOOST_CHECK_EQUAL(Joint::index(joints.first), Joint::index(j1)); |
| 165 |
5/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 1 times.
|
2 | BOOST_CHECK_EQUAL(Joint::index(joints.second), Joint::index(j2)); |
| 166 | // box 1 above box 2: surfaces in contact are | ||
| 167 | // - floor surface 1 for box 1, | ||
| 168 | // - object surface 0 for box 2. | ||
| 169 | 2 | std::vector<Configuration_t> q_init; | |
| 170 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Configuration_t q(14); |
| 171 | // box 1 above box 2: surfaces in contact are | ||
| 172 | // - floor surface 1 for box 1, | ||
| 173 | // - object surface 0 for box 2. | ||
| 174 |
14/28✓ 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.
✓ 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 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 1 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 1 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 1 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 1 times.
✗ Branch 41 not taken.
|
2 | q << 0, 0, 0, 0, 0, 0, 1, .5, .8, 2, 0, 0, 0, 1; |
| 175 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | q_init.push_back(q); |
| 176 | // box 1 above box 2: surfaces in contact are | ||
| 177 | // - floor surface 1 for box 1, | ||
| 178 | // - object surface 1 for box 2. | ||
| 179 |
14/28✓ 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.
✓ 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 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 1 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 1 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 1 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 1 times.
✗ Branch 41 not taken.
|
2 | q << 0, 0, 0, 0, 0, 0, 1, -.25, .2, 2, 1, 0, 0, 0; |
| 180 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | q_init.push_back(q); |
| 181 | // box 1 below box 2: surfaces in contact are | ||
| 182 | // - floor surface 0 for box 1, | ||
| 183 | // - object surface 1 for box 2. | ||
| 184 |
14/28✓ 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.
✓ 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 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 1 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 1 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 1 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 1 times.
✗ Branch 41 not taken.
|
2 | q << 0, 0, 0, 0, 0, 0, 1, .2, .35, -2, 0, 0, 0, 1; |
| 185 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | q_init.push_back(q); |
| 186 | // box 1 below box 2: surfaces in contact are | ||
| 187 | // - floor surface 0 for box 1, | ||
| 188 | // - object surface 0 for box 2. | ||
| 189 |
14/28✓ 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.
✓ 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 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 1 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 1 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 1 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 1 times.
✗ Branch 41 not taken.
|
2 | q << 0, 0, 0, 0, 0, 0, 1, -.8, .6, -2, 1, 0, 0, 0; |
| 190 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | q_init.push_back(q); |
| 191 | 2 | std::vector<LiegroupElement> q1Invq2Exp; | |
| 192 | // Create Solver by substitution with one explicit constraint. | ||
| 193 | 2 | const value_type epsilon(1e-7); | |
| 194 |
2/4✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
|
4 | solver::BySubstitution solver1(SE3 * SE3); |
| 195 |
2/4✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
|
4 | solver::HierarchicalIterative solver2(SE3 * SE3); |
| 196 | 2 | solver1.errorThreshold(epsilon); | |
| 197 | 2 | solver2.errorThreshold(epsilon); | |
| 198 | 2 | solver2.maxIterations(30); | |
| 199 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | solver1.add(contactConstraint); |
| 200 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | solver2.add(contactConstraint, 0); |
| 201 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | vector_t error(8); |
| 202 | // Compute expected box relative positions for each q_init | ||
| 203 |
2/2✓ Branch 0 taken 4 times.
✓ Branch 1 taken 1 times.
|
10 | for (std::size_t i = 0; i < 4; ++i) { |
| 204 |
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 | LiegroupElement q1(q_init[i].head<7>(), SE3); |
| 205 |
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 | LiegroupElement q2(q_init[i].tail<7>(), SE3); |
| 206 | // log of position of box2 with respect to box1. | ||
| 207 |
4/8✓ 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.
✓ Branch 11 taken 4 times.
✗ Branch 12 not taken.
|
8 | q1Invq2Exp.push_back(SE3->exp(q2 - q1)); |
| 208 | // Check that q_init satisfies the constraint | ||
| 209 |
2/4✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
|
16 | vector_t rhs(solver1.rightHandSideFromConfig(q_init[i])); |
| 210 |
9/18✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 4 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 4 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 4 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 4 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 4 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 4 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 4 times.
|
8 | BOOST_CHECK(solver1.isSatisfied(q_init[i], error)); |
| 211 |
2/4✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
|
8 | rhs = solver2.rightHandSideFromConfig(q_init[i]); |
| 212 |
8/16✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 4 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 4 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 4 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 4 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 4 times.
✗ Branch 25 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 4 times.
|
8 | BOOST_CHECK(solver2.isSatisfied(q_init[i])); |
| 213 | 8 | } | |
| 214 | 2 | const std::size_t N(100); | |
| 215 | // Generate random configurations | ||
| 216 | 2 | std::vector<Configuration_t> q_rand; | |
| 217 |
2/2✓ Branch 0 taken 100 times.
✓ Branch 1 taken 1 times.
|
202 | for (std::size_t i = 0; i < N; ++i) { |
| 218 |
2/4✓ Branch 3 taken 100 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 100 times.
✗ Branch 7 not taken.
|
200 | q_rand.push_back(::pinocchio::randomConfiguration(robot->model())); |
| 219 | } | ||
| 220 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | q_rand[0].head<6>().setZero(); |
| 221 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | q_rand[0][6] = 1; |
| 222 | |||
| 223 | 2 | std::size_t nSuccesses(0); | |
| 224 |
2/2✓ Branch 0 taken 100 times.
✓ Branch 1 taken 1 times.
|
202 | for (std::size_t i = 0; i < N; ++i) { |
| 225 |
1/2✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
|
200 | Configuration_t q(q_rand[i]); |
| 226 |
2/2✓ Branch 0 taken 400 times.
✓ Branch 1 taken 100 times.
|
1000 | for (std::size_t j = 0; j < 4; ++j) { |
| 227 |
2/4✓ Branch 2 taken 400 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 400 times.
✗ Branch 6 not taken.
|
800 | solver1.rightHandSideFromConfig(q_init[j]); |
| 228 |
2/4✓ Branch 2 taken 400 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 400 times.
✗ Branch 6 not taken.
|
800 | solver2.rightHandSideFromConfig(q_init[j]); |
| 229 |
8/16✓ Branch 1 taken 400 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 400 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 400 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 400 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 400 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 400 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 400 times.
✗ Branch 25 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 400 times.
|
800 | BOOST_CHECK(solver2.isSatisfied(q_init[j])); |
| 230 |
1/2✓ Branch 2 taken 400 times.
✗ Branch 3 not taken.
|
800 | q = q_rand[i]; |
| 231 |
2/4✓ Branch 1 taken 400 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 400 times.
✗ Branch 5 not taken.
|
800 | solver::HierarchicalIterative::Status status(solver1.solve(q)); |
| 232 |
6/12✓ Branch 1 taken 400 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 400 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 400 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 400 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 400 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 400 times.
|
800 | BOOST_CHECK(status == solver::HierarchicalIterative::SUCCESS); |
| 233 | // Check that box 1 did not move. | ||
| 234 |
9/18✓ Branch 1 taken 400 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 400 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 400 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 400 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 400 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 400 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 400 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 400 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 400 times.
|
800 | BOOST_CHECK(q.head<7>() == q_rand[i].head<7>()); |
| 235 |
3/6✓ Branch 1 taken 400 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 400 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 400 times.
✗ Branch 8 not taken.
|
800 | LiegroupElement q1(q.head<7>(), SE3); |
| 236 |
3/6✓ Branch 1 taken 400 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 400 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 400 times.
✗ Branch 8 not taken.
|
800 | LiegroupElement q2(q.tail<7>(), SE3); |
| 237 |
3/6✓ Branch 2 taken 400 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 400 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 400 times.
✗ Branch 9 not taken.
|
1600 | LiegroupElement q1Invq2(SE3->exp(q2 - q1)); |
| 238 | // Check that relative position of box 2 with respect to box 1 | ||
| 239 | // is the same as defined by q_init. | ||
| 240 |
8/16✓ Branch 1 taken 400 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 400 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 400 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 400 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 400 times.
✗ Branch 19 not taken.
✓ Branch 22 taken 400 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 400 times.
✗ Branch 26 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 400 times.
|
800 | BOOST_CHECK((q1Invq2 - q1Invq2Exp[j]).norm() <= |
| 241 | 10 * solver1.errorThreshold()); | ||
| 242 |
1/2✓ Branch 2 taken 400 times.
✗ Branch 3 not taken.
|
800 | q = q_rand[i]; |
| 243 |
2/4✓ Branch 1 taken 400 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 400 times.
✗ Branch 5 not taken.
|
800 | status = solver2.solve(q); |
| 244 |
2/2✓ Branch 0 taken 14 times.
✓ Branch 1 taken 386 times.
|
800 | if (status == solver::HierarchicalIterative::SUCCESS) { |
| 245 | 28 | ++nSuccesses; | |
| 246 |
2/4✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 14 times.
✗ Branch 6 not taken.
|
28 | q1.vector() = q.head<7>(); |
| 247 |
2/4✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 14 times.
✗ Branch 6 not taken.
|
28 | q2.vector() = q.tail<7>(); |
| 248 |
3/6✓ Branch 2 taken 14 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 14 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 14 times.
✗ Branch 9 not taken.
|
28 | q1Invq2 = SE3->exp(q2 - q1); |
| 249 | // Note that error of constraint is not equivalent to relative | ||
| 250 | // position between objects, but between contact surface frames. | ||
| 251 |
8/16✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 14 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 14 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 14 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 14 times.
✗ Branch 19 not taken.
✓ Branch 22 taken 14 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 14 times.
✗ Branch 26 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 14 times.
|
28 | BOOST_CHECK((q1Invq2 - q1Invq2Exp[j]).norm() <= |
| 252 | 10 * solver2.errorThreshold()); | ||
| 253 | } else { | ||
| 254 |
2/4✓ Branch 1 taken 386 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 386 times.
✗ Branch 5 not taken.
|
772 | solver2.isSatisfied(q); |
| 255 |
2/4✓ Branch 1 taken 386 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 386 times.
✗ Branch 5 not taken.
|
772 | solver2.residualError(error); |
| 256 | } | ||
| 257 | 800 | } | |
| 258 | 200 | } | |
| 259 | // Check that success rate is not too low. N/10 is an arbitrary value. | ||
| 260 |
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(nSuccesses >= N / 10); |
| 261 | 2 | } | |
| 262 |