| Directory: | ./ |
|---|---|
| File: | include/hpp/core/continuous-validation.hh |
| Date: | 2024-12-13 16:14:03 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 28 | 28 | 100.0% |
| Branches: | 18 | 28 | 64.3% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2014, 2015, 2016, 2017, 2018 CNRS | ||
| 3 | // Authors: Florent Lamiraux, Joseph Mirabel, Diane Bury | ||
| 4 | // | ||
| 5 | |||
| 6 | // Redistribution and use in source and binary forms, with or without | ||
| 7 | // modification, are permitted provided that the following conditions are | ||
| 8 | // met: | ||
| 9 | // | ||
| 10 | // 1. Redistributions of source code must retain the above copyright | ||
| 11 | // notice, this list of conditions and the following disclaimer. | ||
| 12 | // | ||
| 13 | // 2. Redistributions in binary form must reproduce the above copyright | ||
| 14 | // notice, this list of conditions and the following disclaimer in the | ||
| 15 | // documentation and/or other materials provided with the distribution. | ||
| 16 | // | ||
| 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
| 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
| 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR | ||
| 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT | ||
| 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, | ||
| 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT | ||
| 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, | ||
| 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY | ||
| 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT | ||
| 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE | ||
| 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH | ||
| 28 | // DAMAGE. | ||
| 29 | |||
| 30 | #ifndef HPP_CORE_CONTINUOUS_VALIDATION_HH | ||
| 31 | #define HPP_CORE_CONTINUOUS_VALIDATION_HH | ||
| 32 | |||
| 33 | #include <hpp/core/continuous-validation/interval-validation.hh> | ||
| 34 | #include <hpp/core/fwd.hh> | ||
| 35 | #include <hpp/core/obstacle-user.hh> | ||
| 36 | #include <hpp/core/path-validation-report.hh> | ||
| 37 | #include <hpp/core/path-validation.hh> | ||
| 38 | #include <hpp/core/path.hh> | ||
| 39 | #include <hpp/pinocchio/pool.hh> | ||
| 40 | |||
| 41 | namespace hpp { | ||
| 42 | namespace core { | ||
| 43 | using continuousValidation::IntervalValidation; | ||
| 44 | using continuousValidation::IntervalValidationPtr_t; | ||
| 45 | /// \addtogroup validation | ||
| 46 | /// \{ | ||
| 47 | |||
| 48 | /// Continuous validation of a path | ||
| 49 | /// | ||
| 50 | /// This class valides a path for various criteria. | ||
| 51 | /// The default validation criterion is the absence of collisions | ||
| 52 | /// between bodies of a robot and the | ||
| 53 | /// environment. | ||
| 54 | /// | ||
| 55 | /// Validation of PathVector instances is performed path by path. | ||
| 56 | /// | ||
| 57 | /// A path is valid if and only if each interval validation element | ||
| 58 | /// is valid along the whole interval of definition (class | ||
| 59 | /// continuousValidation::IntervalValidation). | ||
| 60 | /// | ||
| 61 | /// In order to validate other criteria, users can add their own | ||
| 62 | /// derivation of class continuousValidation::IntervalValidation using | ||
| 63 | /// method \link ContinuousValidation::addIntervalValidation | ||
| 64 | /// addIntervalValidation\endlink. They can also make use of two types | ||
| 65 | /// of delegates: | ||
| 66 | /// \li \link ContinuousValidation::Initialize Initialize\endlink | ||
| 67 | /// and user defined derived classes. Instances of these classes | ||
| 68 | /// may be added to the list of initializers by calling method | ||
| 69 | /// \link ContinuousValidation::add <ContinuousValidation::Initialize> | ||
| 70 | /// add <Initialize> \endlink. Upon calling method | ||
| 71 | /// \link ContinuousValidation::initialize initialize\endlink, methods | ||
| 72 | /// \c doExecute of these instances are called sucessively. | ||
| 73 | /// \li \link ContinuousValidation::AddObstacle AddObstacle\endlink | ||
| 74 | /// and user defined derived classes. Instances of these classes | ||
| 75 | /// may be added to the list of obstacle adders by calling method | ||
| 76 | /// \link ContinuousValidation::add <ContinuousValidation::AddObstacle> | ||
| 77 | /// add <AddObstacle> \endlink. Upon calling method | ||
| 78 | /// \link ContinuousValidation::addObstacle addObstacle\endlink, method | ||
| 79 | /// \c doExecute of these instances are called sucessively. | ||
| 80 | /// | ||
| 81 | /// Base class ContinuousValidation::Initialize initializes | ||
| 82 | /// collision pairs between bodies of the robot. | ||
| 83 | /// | ||
| 84 | /// Validation of a collision pair is based | ||
| 85 | /// on the computation of an upper-bound of the relative velocity of | ||
| 86 | /// objects of one joint (or of the environment) in the reference frame | ||
| 87 | /// of the other joint. This is implemented in | ||
| 88 | /// continuousValidation::BodyPairCollision and | ||
| 89 | /// continuousValidation::SolidSolidCollision. | ||
| 90 | /// | ||
| 91 | /// See <a href="continuous-collision-checking.pdf"> this document </a> | ||
| 92 | /// for details on the continuous collision checking. | ||
| 93 | /// | ||
| 94 | /// See <a href="continuous-validation.pdf"> this document </a> | ||
| 95 | /// for details on the architecture of the code. | ||
| 96 | class HPP_CORE_DLLAPI ContinuousValidation : public PathValidation, | ||
| 97 | public ObstacleUserInterface { | ||
| 98 | public: | ||
| 99 | /// Delegate class to initialize ContinuousValidation instance | ||
| 100 | /// | ||
| 101 | /// See method ContinuousValidation::initialize for details | ||
| 102 | class Initialize { | ||
| 103 | public: | ||
| 104 | Initialize(ContinuousValidation& owner); | ||
| 105 | /// Initialize collision pairs between bodies of the robot | ||
| 106 | /// Iterate over all collision pairs of the robot, and for each one, | ||
| 107 | /// \li create an instance of continuousValidation::SolidSolidCollision, | ||
| 108 | /// \li add it to class ContinuousValidation using method | ||
| 109 | /// ContinuousValidation::addIntervalValidation. | ||
| 110 | virtual void doExecute() const; | ||
| 111 | 116 | ContinuousValidation& owner() const { return *owner_; } | |
| 112 | 24 | virtual ~Initialize() {} | |
| 113 | |||
| 114 | protected: | ||
| 115 | ContinuousValidation* owner_; | ||
| 116 | }; // class Initialize | ||
| 117 | /// Delegate class to add obstacle to ContinuousValidation instance | ||
| 118 | /// | ||
| 119 | /// See method ContinuousValidation::addObstacle for details | ||
| 120 | class AddObstacle { | ||
| 121 | public: | ||
| 122 | AddObstacle(ContinuousValidation& owner); | ||
| 123 | /// Add an obstacle | ||
| 124 | /// \param object obstacle added | ||
| 125 | /// Add the object to each collision pair a body of which is the | ||
| 126 | /// environment. | ||
| 127 | virtual void doExecute(const CollisionObjectConstPtr_t& object) const; | ||
| 128 | 27 | ContinuousValidation& owner() const { return *owner_; } | |
| 129 | 24 | virtual ~AddObstacle() {} | |
| 130 | |||
| 131 | protected: | ||
| 132 | ContinuousValidation* owner_; | ||
| 133 | DeviceWkPtr_t robot_; | ||
| 134 | }; // class AddObstacle | ||
| 135 | |||
| 136 | /// Compute the largest valid interval starting from the path beginning | ||
| 137 | /// | ||
| 138 | /// \param path the path to check for validity, | ||
| 139 | /// \param reverse if true check from the end, | ||
| 140 | /// \retval validPart the extracted valid part of the path, pointer to path if | ||
| 141 | /// path is valid. | ||
| 142 | /// \retval report information about the validation process. A report | ||
| 143 | /// is allocated if the path is not valid. | ||
| 144 | /// \return true if the whole path is valid. | ||
| 145 | virtual bool validate(const PathPtr_t& path, bool reverse, | ||
| 146 | PathPtr_t& validPart, | ||
| 147 | PathValidationReportPtr_t& report); | ||
| 148 | /// Iteratively call method doExecute of delegate classes AddObstacle | ||
| 149 | /// \param object new obstacle. | ||
| 150 | /// \sa ContinuousValidation::add, ContinuousValidation::AddObstacle. | ||
| 151 | virtual void addObstacle(const CollisionObjectConstPtr_t& object); | ||
| 152 | |||
| 153 | /// Remove a collision pair between a joint and an obstacle | ||
| 154 | /// \param joint the joint that holds the inner objects, | ||
| 155 | /// \param obstacle the obstacle to remove. | ||
| 156 | /// \note collision configuration validation needs to know about | ||
| 157 | /// obstacles. This virtual method does nothing for configuration | ||
| 158 | /// validation methods that do not care about obstacles. | ||
| 159 | virtual void removeObstacleFromJoint( | ||
| 160 | const JointPtr_t& joint, const CollisionObjectConstPtr_t& obstacle); | ||
| 161 | |||
| 162 | /// Filter collision pairs. | ||
| 163 | /// | ||
| 164 | /// Remove pairs of object that cannot be in collision. | ||
| 165 | /// This effectively disables collision detection between objects that | ||
| 166 | /// have no possible relative motion due to the constraints. | ||
| 167 | /// | ||
| 168 | /// \param relMotion square symmetric matrix of RelativeMotionType of size | ||
| 169 | /// numberDof x numberDof | ||
| 170 | void filterCollisionPairs(const RelativeMotion::matrix_type& relMotion); | ||
| 171 | |||
| 172 | /// \copydoc ObstacleUserInterface::setSecurityMargins | ||
| 173 | virtual void setSecurityMargins(const matrix_t& securityMatrix); | ||
| 174 | |||
| 175 | /// \copydoc ObstacleUserInterface::setSecurityMarginBetweenBodies | ||
| 176 | virtual void setSecurityMarginBetweenBodies(const std::string& body_a, | ||
| 177 | const std::string& body_b, | ||
| 178 | const value_type& margin); | ||
| 179 | |||
| 180 | /// \name Delegate | ||
| 181 | /// \{ | ||
| 182 | |||
| 183 | /// Add a delegate | ||
| 184 | /// \tparam Delegate type of delegate. | ||
| 185 | /// \param instance of delegate class. | ||
| 186 | /// | ||
| 187 | /// Delegates are used to enable users to specialize some actions of the | ||
| 188 | /// class without deriving the class. This class supports two types of | ||
| 189 | /// delegates: | ||
| 190 | /// \li Initialize: method | ||
| 191 | /// \link ContinuousValidation::Initialize::doExecute doExecute | ||
| 192 | /// \endlink of instances of this class and of user defined derived | ||
| 193 | /// classes are called successively upon call of method | ||
| 194 | /// \link ContinuousValidation::initialize\endlink, | ||
| 195 | /// \li AddObstacle: method | ||
| 196 | /// \link ContinuousValidation::AddObstacle::doExecute doExecute | ||
| 197 | /// \endlink of instances of this class and of user defined derived | ||
| 198 | /// classes are called successively upon call of method | ||
| 199 | /// \link ContinuousValidation::addObstacle\endlink, | ||
| 200 | template <class Delegate> | ||
| 201 | void add(const Delegate& delegate); | ||
| 202 | |||
| 203 | /// Reset delegates of a type | ||
| 204 | /// \tparam delegate type of delegate | ||
| 205 | template <class Delegate> | ||
| 206 | void reset(); | ||
| 207 | |||
| 208 | /// \} | ||
| 209 | /// Add interval validation instance | ||
| 210 | void addIntervalValidation(const IntervalValidationPtr_t& intervalValidation); | ||
| 211 | /// Get tolerance value | ||
| 212 | 9 | value_type tolerance() const { return tolerance_; } | |
| 213 | |||
| 214 | /// Get the break distance passed to collision checking. | ||
| 215 | /// \sa coal::CollisionRequest::break_distance | ||
| 216 | 45 | value_type breakDistance() const { return breakDistance_; } | |
| 217 | |||
| 218 | /// Set the break distance passed to collision checking. | ||
| 219 | /// \sa value_type breakDistance() const | ||
| 220 | void breakDistance(value_type distance); | ||
| 221 | |||
| 222 | 16 | DevicePtr_t robot() const { return robot_; } | |
| 223 | /// Iteratively call method doExecute of delegate classes Initialize | ||
| 224 | /// \sa ContinuousValidation::add, ContinuousValidation::Initialize. | ||
| 225 | void initialize(); | ||
| 226 | |||
| 227 | virtual ~ContinuousValidation(); | ||
| 228 | |||
| 229 | protected: | ||
| 230 | typedef continuousValidation::IntervalValidations_t IntervalValidations_t; | ||
| 231 | |||
| 232 | static void setPath(IntervalValidations_t& intervalValidations, | ||
| 233 | const PathPtr_t& path, bool reverse); | ||
| 234 | |||
| 235 | /// Constructor | ||
| 236 | /// \param robot the robot for which validation is performed, | ||
| 237 | /// \param tolerance maximal penetration allowed. | ||
| 238 | ContinuousValidation(const DevicePtr_t& robot, const value_type& tolerance); | ||
| 239 | |||
| 240 | /// Validate interval centered on a path parameter | ||
| 241 | /// \param intervalValidations collision to consider | ||
| 242 | /// \param config Configuration at abscissa t on the path. | ||
| 243 | /// \param t parameter value in the path interval of definition | ||
| 244 | /// \retval interval interval over which the path is collision-free, | ||
| 245 | /// not necessarily included in definition interval | ||
| 246 | /// \return true if the body pair is collision free for this parameter | ||
| 247 | /// value, false if the body pair is in collision. | ||
| 248 | /// \note object should be in the positions defined by the configuration | ||
| 249 | /// of parameter t on the path. | ||
| 250 | virtual bool validateConfiguration(IntervalValidations_t& intervalValidations, | ||
| 251 | const Configuration_t& config, | ||
| 252 | const value_type& t, interval_t& interval, | ||
| 253 | PathValidationReportPtr_t& report); | ||
| 254 | |||
| 255 | /// Validate a set of intervals for a given parameter along a path | ||
| 256 | /// | ||
| 257 | /// \tparam IntervalValidation type of container of validation elements | ||
| 258 | /// (for instance validation for collision between a pair of | ||
| 259 | /// bodies), | ||
| 260 | /// \tparam ValidationReportTypePtr_t type of validation report produced | ||
| 261 | /// in case non validation. Should derive from ValidationReport. | ||
| 262 | /// \param objects able to validate an interval for collision, | ||
| 263 | /// \param t center of the interval to be validated, | ||
| 264 | /// \retval interval interval validated for all objects, | ||
| 265 | /// \retval smallestInterval iterator to the validation element that | ||
| 266 | /// returned the smallest interval. | ||
| 267 | 21200 | bool validateIntervals( | |
| 268 | IntervalValidations_t& validations, const value_type& t, | ||
| 269 | interval_t& interval, PathValidationReportPtr_t& pathReport, | ||
| 270 | typename IntervalValidations_t::iterator& smallestInterval, | ||
| 271 | pinocchio::DeviceData& data) { | ||
| 272 | 21200 | typename IntervalValidations_t::iterator itMin = validations.begin(); | |
| 273 | 21201 | for (IntervalValidations_t::iterator itVal(validations.begin()); | |
| 274 |
2/2✓ Branch 3 taken 249330 times.
✓ Branch 4 taken 21106 times.
|
270524 | itVal != validations.end(); ++itVal) { |
| 275 | 249330 | ValidationReportPtr_t report; | |
| 276 | // the valid interval will not be greater than "interval" so we do not | ||
| 277 | // need to perform validation on a greater interval. | ||
| 278 | 249330 | interval_t tmpInt = interval; | |
| 279 |
3/4✓ Branch 3 taken 249312 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 95 times.
✓ Branch 6 taken 249217 times.
|
249330 | if (!(*itVal)->validateConfiguration(t, tmpInt, report, data)) { |
| 280 |
2/4✓ Branch 1 taken 95 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 95 times.
✗ Branch 6 not taken.
|
95 | pathReport = PathValidationReportPtr_t(new PathValidationReport); |
| 281 | 95 | pathReport->configurationReport = report; | |
| 282 | 95 | pathReport->parameter = t; | |
| 283 | 95 | return false; | |
| 284 | } else { | ||
| 285 |
2/2✓ Branch 0 taken 25117 times.
✓ Branch 1 taken 224100 times.
|
249217 | if (interval.second > tmpInt.second) { |
| 286 | 25117 | itMin = itVal; | |
| 287 | 25117 | smallestInterval = itVal; | |
| 288 | } | ||
| 289 | 249217 | interval.first = std::max(interval.first, tmpInt.first); | |
| 290 | 249216 | interval.second = std::min(interval.second, tmpInt.second); | |
| 291 |
5/10✓ Branch 5 taken 249240 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 247056 times.
✓ Branch 8 taken 2184 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 247056 times.
✓ Branch 12 taken 249269 times.
✗ Branch 13 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
|
249231 | assert((*itVal)->path()->length() == 0 || |
| 292 | interval.second > interval.first); | ||
| 293 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 249324 times.
|
249324 | assert(interval.first <= t); |
| 294 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 249324 times.
|
249324 | assert(t <= interval.second); |
| 295 | } | ||
| 296 |
2/2✓ Branch 1 taken 249322 times.
✓ Branch 2 taken 92 times.
|
249419 | } |
| 297 | 21106 | return true; | |
| 298 | } | ||
| 299 | |||
| 300 | DevicePtr_t robot_; | ||
| 301 | value_type tolerance_; | ||
| 302 | value_type breakDistance_; | ||
| 303 | |||
| 304 | /// Store weak pointer to itself. | ||
| 305 | void init(ContinuousValidationWkPtr_t weak); | ||
| 306 | |||
| 307 | /// All BodyPairValidation to validate | ||
| 308 | IntervalValidations_t intervalValidations_; | ||
| 309 | /// BodyPairCollision for which collision is disabled | ||
| 310 | IntervalValidations_t disabledBodyPairCollisions_; | ||
| 311 | |||
| 312 | pinocchio::Pool<IntervalValidations_t> bodyPairCollisionPool_; | ||
| 313 | |||
| 314 | value_type stepSize_; | ||
| 315 | |||
| 316 | private: | ||
| 317 | // Weak pointer to itself | ||
| 318 | ContinuousValidationWkPtr_t weak_; | ||
| 319 | |||
| 320 | virtual bool validateStraightPath(IntervalValidations_t& intervalValidations, | ||
| 321 | const PathPtr_t& path, bool reverse, | ||
| 322 | PathPtr_t& validPart, | ||
| 323 | PathValidationReportPtr_t& report) = 0; | ||
| 324 | |||
| 325 | std::vector<Initialize> initialize_; | ||
| 326 | std::vector<AddObstacle> addObstacle_; | ||
| 327 | }; // class ContinuousValidation | ||
| 328 | /// \} | ||
| 329 | template <> | ||
| 330 | void ContinuousValidation::add<ContinuousValidation::AddObstacle>( | ||
| 331 | const ContinuousValidation::AddObstacle& delegate); | ||
| 332 | |||
| 333 | template <> | ||
| 334 | void ContinuousValidation::reset<ContinuousValidation::AddObstacle>(); | ||
| 335 | |||
| 336 | template <> | ||
| 337 | void ContinuousValidation::add<ContinuousValidation::Initialize>( | ||
| 338 | const ContinuousValidation::Initialize& delegate); | ||
| 339 | |||
| 340 | template <> | ||
| 341 | void ContinuousValidation::reset<ContinuousValidation::Initialize>(); | ||
| 342 | |||
| 343 | template <class Delegate> | ||
| 344 | void ContinuousValidation::add(const Delegate& delegate) { | ||
| 345 | assert(false && "No delegate of this type in class ContinuousValidation."); | ||
| 346 | } | ||
| 347 | template <class Delegate> | ||
| 348 | void ContinuousValidation::reset() { | ||
| 349 | assert(false && "No delegate of this type in class ContinuousValidation."); | ||
| 350 | } | ||
| 351 | |||
| 352 | } // namespace core | ||
| 353 | } // namespace hpp | ||
| 354 | #endif // HPP_CORE_CONTINUOUS_VALIDATION_HH | ||
| 355 |