Directory: | ./ |
---|---|
File: | include/hpp/core/continuous-validation.hh |
Date: | 2024-08-10 11:29:48 |
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 hpp::fcl::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 | 20379 | 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 | 20379 | typename IntervalValidations_t::iterator itMin = validations.begin(); | |
273 | 20379 | for (IntervalValidations_t::iterator itVal(validations.begin()); | |
274 |
2/2✓ Branch 3 taken 239702 times.
✓ Branch 4 taken 20283 times.
|
260000 | itVal != validations.end(); ++itVal) { |
275 | 239702 | 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 | 239702 | interval_t tmpInt = interval; | |
279 |
3/4✓ Branch 3 taken 239701 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 95 times.
✓ Branch 6 taken 239606 times.
|
239702 | 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 24289 times.
✓ Branch 1 taken 215317 times.
|
239606 | if (interval.second > tmpInt.second) { |
286 | 24289 | itMin = itVal; | |
287 | 24289 | smallestInterval = itVal; | |
288 | } | ||
289 | 239606 | interval.first = std::max(interval.first, tmpInt.first); | |
290 | 239606 | interval.second = std::min(interval.second, tmpInt.second); | |
291 |
5/10✓ Branch 5 taken 239609 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 237406 times.
✓ Branch 8 taken 2203 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 237406 times.
✓ Branch 12 taken 239609 times.
✗ Branch 13 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
|
239604 | assert((*itVal)->path()->length() == 0 || |
292 | interval.second > interval.first); | ||
293 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 239622 times.
|
239622 | assert(interval.first <= t); |
294 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 239622 times.
|
239622 | assert(t <= interval.second); |
295 | } | ||
296 |
2/2✓ Branch 1 taken 239621 times.
✓ Branch 2 taken 95 times.
|
239717 | } |
297 | 20283 | 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 |