| Directory: | ./ |
|---|---|
| File: | src/obstacle-user.cc |
| Date: | 2024-08-10 11:29:48 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 30 | 123 | 24.4% |
| Branches: | 31 | 226 | 13.7% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // Copyright (c) 2019 CNRS | ||
| 2 | // Authors: Joseph Mirabel | ||
| 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 | #include <hpp/fcl/collision.h> | ||
| 30 | |||
| 31 | #include <hpp/core/obstacle-user.hh> | ||
| 32 | #include <hpp/pinocchio/body.hh> | ||
| 33 | #include <hpp/pinocchio/collision-object.hh> | ||
| 34 | #include <hpp/pinocchio/device.hh> | ||
| 35 | #include <hpp/util/exception-factory.hh> | ||
| 36 | #include <pinocchio/multibody/geometry.hpp> | ||
| 37 | #include <pinocchio/spatial/fcl-pinocchio-conversions.hpp> | ||
| 38 | |||
| 39 | namespace hpp { | ||
| 40 | namespace core { | ||
| 41 | using ::pinocchio::toFclTransform3f; | ||
| 42 | 49385 | bool ObstacleUser::collide(const CollisionPairs_t& pairs, | |
| 43 | CollisionRequests_t& reqs, fcl::CollisionResult& res, | ||
| 44 | std::size_t& i, pinocchio::DeviceData& data) { | ||
| 45 |
2/2✓ Branch 1 taken 181811 times.
✓ Branch 2 taken 49278 times.
|
231087 | for (i = 0; i < pairs.size(); ++i) { |
| 46 | 181811 | res.clear(); | |
| 47 |
2/2✓ Branch 3 taken 109 times.
✓ Branch 4 taken 181702 times.
|
181807 | if (pairs[i].collide(data, reqs[i], res) != 0) return true; |
| 48 | } | ||
| 49 | 49278 | return false; | |
| 50 | } | ||
| 51 | |||
| 52 | 10 | void ObstacleUser::addObstacle(const CollisionObjectConstPtr_t& object) { | |
| 53 |
3/4✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 22 times.
✓ Branch 5 taken 10 times.
|
32 | for (size_type j = 0; j < robot_->nbJoints(); ++j) { |
| 54 |
1/2✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
|
22 | JointPtr_t joint = robot_->jointAt(j); |
| 55 |
1/2✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
|
22 | addObstacleToJoint(object, joint, false); |
| 56 | 22 | } | |
| 57 | 10 | } | |
| 58 | |||
| 59 | 22 | void ObstacleUser::addObstacleToJoint(const CollisionObjectConstPtr_t& object, | |
| 60 | const JointPtr_t& joint, | ||
| 61 | const bool includeChildren) { | ||
| 62 |
1/2✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
|
22 | BodyPtr_t body = joint->linkedBody(); |
| 63 |
1/2✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
|
22 | if (body) { |
| 64 |
3/4✓ Branch 2 taken 44 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 22 times.
✓ Branch 5 taken 22 times.
|
44 | for (size_type o = 0; o < body->nbInnerObjects(); ++o) { |
| 65 | // TODO: check the objects are not in same joint | ||
| 66 |
3/6✓ Branch 3 taken 22 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 22 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 22 times.
✗ Branch 11 not taken.
|
22 | cPairs_.push_back(CollisionPair_t(body->innerObjectAt(o), object)); |
| 67 |
1/2✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
|
22 | cRequests_.push_back(defaultRequest_); |
| 68 | } | ||
| 69 | } | ||
| 70 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 22 times.
|
22 | if (includeChildren) { |
| 71 | ✗ | for (std::size_t i = 0; i < joint->numberChildJoints(); ++i) { | |
| 72 | ✗ | addObstacleToJoint(object, joint->childJoint(i), includeChildren); | |
| 73 | } | ||
| 74 | } | ||
| 75 | 22 | } | |
| 76 | |||
| 77 | struct CollisionPairComparision { | ||
| 78 | CollisionPair_t a; | ||
| 79 | ✗ | CollisionPairComparision(const CollisionPair_t& p) : a(p) {} | |
| 80 | ✗ | bool operator()(const CollisionPair_t& b) { | |
| 81 | ✗ | return (&(a.first->pinocchio()) == &(b.first->pinocchio())) && | |
| 82 | ✗ | (&(a.second->pinocchio()) == &(b.second->pinocchio())); | |
| 83 | } | ||
| 84 | }; | ||
| 85 | |||
| 86 | ✗ | void ObstacleUser::removeObstacleFromJoint( | |
| 87 | const JointPtr_t& joint, const CollisionObjectConstPtr_t& obstacle) { | ||
| 88 | ✗ | BodyPtr_t body = joint->linkedBody(); | |
| 89 | ✗ | if (body) { | |
| 90 | ✗ | for (size_type o = 0; o < body->nbInnerObjects(); ++o) { | |
| 91 | ✗ | CollisionPair_t colPair(body->innerObjectAt(o), obstacle); | |
| 92 | ✗ | CollisionPairComparision compare(colPair); | |
| 93 | ✗ | std::size_t nbDelPairs = 0; | |
| 94 | ✗ | for (std::size_t i = 0; i < cPairs_.size();) { | |
| 95 | ✗ | if (compare(cPairs_[i])) { | |
| 96 | ✗ | cPairs_.erase(cPairs_.begin() + i); | |
| 97 | ✗ | cRequests_.erase(cRequests_.begin() + i); | |
| 98 | ✗ | ++nbDelPairs; | |
| 99 | } else | ||
| 100 | ✗ | ++i; | |
| 101 | } | ||
| 102 | ✗ | if (nbDelPairs == 0) { | |
| 103 | ✗ | std::ostringstream oss; | |
| 104 | oss << "ObstacleUser::removeObstacleFromJoint: obstacle \"" | ||
| 105 | ✗ | << obstacle->name() | |
| 106 | ✗ | << "\" is not registered as obstacle for joint \"" << joint->name() | |
| 107 | ✗ | << "\"."; | |
| 108 | ✗ | throw std::runtime_error(oss.str()); | |
| 109 | ✗ | } else if (nbDelPairs >= 2) { | |
| 110 | hppDout(error, "obstacle " << obstacle->name() << " was registered " | ||
| 111 | << nbDelPairs | ||
| 112 | << " times as obstacle for joint " | ||
| 113 | << joint->name() << "."); | ||
| 114 | } | ||
| 115 | } | ||
| 116 | } | ||
| 117 | } | ||
| 118 | |||
| 119 | ✗ | void ObstacleUser::filterCollisionPairs( | |
| 120 | const RelativeMotion::matrix_type& matrix) { | ||
| 121 | // Loop over collision pairs and remove disabled ones. | ||
| 122 | pinocchio::JointIndex j1, j2; | ||
| 123 | ✗ | fcl::CollisionResult unused; | |
| 124 | ✗ | for (std::size_t i = 0; i < cPairs_.size();) { | |
| 125 | ✗ | const CollisionPair_t& pair = cPairs_[i]; | |
| 126 | |||
| 127 | ✗ | j1 = pair.first->jointIndex(); | |
| 128 | ✗ | j2 = pair.second->jointIndex(); | |
| 129 | |||
| 130 | ✗ | switch (matrix(j1, j2)) { | |
| 131 | ✗ | case RelativeMotion::Parameterized: | |
| 132 | hppDout(info, "Parameterized collision pairs between " | ||
| 133 | << pair.first->name() << " and " | ||
| 134 | << pair.second->name()); | ||
| 135 | ✗ | pPairs_.push_back(pair); | |
| 136 | ✗ | pRequests_.push_back(cRequests_[i]); | |
| 137 | ✗ | cPairs_.erase(cPairs_.begin() + i); | |
| 138 | ✗ | cRequests_.erase(cRequests_.begin() + i); | |
| 139 | ✗ | break; | |
| 140 | ✗ | case RelativeMotion::Constrained: | |
| 141 | hppDout(info, "Disabling collision between " << pair.first->name() | ||
| 142 | << " and " | ||
| 143 | << pair.second->name()); | ||
| 144 | ✗ | if (pair.collide(cRequests_[i], unused) != 0) { | |
| 145 | hppDout(warning, | ||
| 146 | "Disabling collision detection between two " | ||
| 147 | "bodies in collision."); | ||
| 148 | } | ||
| 149 | ✗ | dPairs_.push_back(pair); | |
| 150 | ✗ | dRequests_.push_back(cRequests_[i]); | |
| 151 | ✗ | cPairs_.erase(cPairs_.begin() + i); | |
| 152 | ✗ | cRequests_.erase(cRequests_.begin() + i); | |
| 153 | ✗ | break; | |
| 154 | ✗ | case RelativeMotion::Unconstrained: | |
| 155 | ✗ | ++i; | |
| 156 | ✗ | break; | |
| 157 | ✗ | default: | |
| 158 | hppDout(warning, "RelativeMotionType not understood"); | ||
| 159 | ✗ | ++i; | |
| 160 | ✗ | break; | |
| 161 | } | ||
| 162 | } | ||
| 163 | } | ||
| 164 | |||
| 165 | ✗ | void ObstacleUser::setSecurityMargins(const matrix_t& securityMatrix) { | |
| 166 | ✗ | if (securityMatrix.rows() != robot_->nbJoints() + 1 || | |
| 167 | ✗ | securityMatrix.cols() != robot_->nbJoints() + 1) { | |
| 168 | ✗ | HPP_THROW(std::invalid_argument, | |
| 169 | "Wrong size of security margin matrix." | ||
| 170 | " Expected " | ||
| 171 | << robot_->nbJoints() + 1 << 'x' << robot_->nbJoints() + 1 | ||
| 172 | << ". Got " << securityMatrix.rows() << 'x' | ||
| 173 | << securityMatrix.cols()); | ||
| 174 | } | ||
| 175 | pinocchio::JointIndex j1, j2; | ||
| 176 | ✗ | fcl::CollisionResult unused; | |
| 177 | ✗ | for (std::size_t i = 0; i < cPairs_.size(); ++i) { | |
| 178 | ✗ | const CollisionPair_t& pair = cPairs_[i]; | |
| 179 | ✗ | j1 = pair.first->jointIndex(); | |
| 180 | ✗ | j2 = pair.second->jointIndex(); | |
| 181 | ✗ | cRequests_[i].security_margin = securityMatrix(j1, j2); | |
| 182 | } | ||
| 183 | ✗ | for (std::size_t i = 0; i < pPairs_.size(); ++i) { | |
| 184 | ✗ | const CollisionPair_t& pair = pPairs_[i]; | |
| 185 | ✗ | j1 = pair.first->jointIndex(); | |
| 186 | ✗ | j2 = pair.second->jointIndex(); | |
| 187 | ✗ | pRequests_[i].security_margin = securityMatrix(j1, j2); | |
| 188 | } | ||
| 189 | ✗ | for (std::size_t i = 0; i < dPairs_.size(); ++i) { | |
| 190 | ✗ | const CollisionPair_t& pair = dPairs_[i]; | |
| 191 | ✗ | j1 = pair.first->jointIndex(); | |
| 192 | ✗ | j2 = pair.second->jointIndex(); | |
| 193 | ✗ | dRequests_[i].security_margin = securityMatrix(j1, j2); | |
| 194 | } | ||
| 195 | } | ||
| 196 | |||
| 197 | ✗ | void ObstacleUser::setSecurityMarginBetweenBodies(const std::string& body_a, | |
| 198 | const std::string& body_b, | ||
| 199 | const value_type& margin) { | ||
| 200 | ✗ | auto setMargin = [&body_a, &body_b, &margin](const CollisionPairs_t& pairs, | |
| 201 | ✗ | CollisionRequests_t& requests) { | |
| 202 | ✗ | for (std::size_t i = 0; i < pairs.size(); ++i) { | |
| 203 | ✗ | const CollisionPair_t& pair = pairs[i]; | |
| 204 | ✗ | if ((pair.first->name() == body_a && pair.second->name() == body_b) || | |
| 205 | ✗ | (pair.first->name() == body_b && pair.second->name() == body_a)) { | |
| 206 | ✗ | requests[i].security_margin = margin; | |
| 207 | ✗ | return true; | |
| 208 | } | ||
| 209 | } | ||
| 210 | ✗ | return false; | |
| 211 | ✗ | }; | |
| 212 | ✗ | if (setMargin(cPairs_, cRequests_) || setMargin(pPairs_, pRequests_) || | |
| 213 | ✗ | setMargin(dPairs_, dRequests_)) | |
| 214 | ✗ | return; | |
| 215 | ✗ | throw std::invalid_argument( | |
| 216 | "Could not find a collision pair between body" | ||
| 217 | ✗ | " " + | |
| 218 | ✗ | body_a + " and " + body_b); | |
| 219 | } | ||
| 220 | |||
| 221 | 75 | void ObstacleUser::addRobotCollisionPairs() { | |
| 222 | 75 | const pinocchio::GeomModel& model = robot_->geomModel(); | |
| 223 | 75 | const pinocchio::GeomData& data = robot_->geomData(); | |
| 224 | |||
| 225 |
2/2✓ Branch 1 taken 5192 times.
✓ Branch 2 taken 75 times.
|
5267 | for (std::size_t i = 0; i < model.collisionPairs.size(); ++i) |
| 226 |
1/2✓ Branch 1 taken 5192 times.
✗ Branch 2 not taken.
|
5192 | if (data.activeCollisionPairs[i]) { |
| 227 | CollisionObjectConstPtr_t o1(new pinocchio::CollisionObject( | ||
| 228 |
3/6✓ Branch 3 taken 5192 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 5192 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 5192 times.
✗ Branch 10 not taken.
|
5192 | robot_, model.collisionPairs[i].first)); |
| 229 | CollisionObjectConstPtr_t o2(new pinocchio::CollisionObject( | ||
| 230 |
3/6✓ Branch 3 taken 5192 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 5192 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 5192 times.
✗ Branch 10 not taken.
|
5192 | robot_, model.collisionPairs[i].second)); |
| 231 |
2/4✓ Branch 3 taken 5192 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 5192 times.
✗ Branch 7 not taken.
|
5192 | cPairs_.push_back(CollisionPair_t(o1, o2)); |
| 232 |
1/2✓ Branch 1 taken 5192 times.
✗ Branch 2 not taken.
|
5192 | cRequests_.push_back(defaultRequest_); |
| 233 | 5192 | } | |
| 234 | 75 | } | |
| 235 | |||
| 236 | ✗ | void ObstacleUser::setRequests(const fcl::CollisionRequest& r) { | |
| 237 | ✗ | for (std::size_t i = 0; i < cRequests_.size(); ++i) cRequests_[i] = r; | |
| 238 | ✗ | for (std::size_t i = 0; i < pRequests_.size(); ++i) pRequests_[i] = r; | |
| 239 | ✗ | for (std::size_t i = 0; i < dRequests_.size(); ++i) dRequests_[i] = r; | |
| 240 | } | ||
| 241 | } // namespace core | ||
| 242 | } // namespace hpp | ||
| 243 |