hpp-core 4.15.1
Implement basic classes for canonical path planning for kinematic chains.
Loading...
Searching...
No Matches
continuous-validation.hh
Go to the documentation of this file.
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
34#include <hpp/core/fwd.hh>
38#include <hpp/core/path.hh>
39#include <hpp/pinocchio/pool.hh>
40
41namespace hpp {
42namespace core {
43using continuousValidation::IntervalValidation;
47
98 public:
103 public:
110 virtual void doExecute() const;
111 ContinuousValidation& owner() const { return *owner_; }
112 virtual ~Initialize() {}
113
114 protected:
116 }; // class Initialize
121 public:
127 virtual void doExecute(const CollisionObjectConstPtr_t& object) const;
128 ContinuousValidation& owner() const { return *owner_; }
129 virtual ~AddObstacle() {}
130
131 protected:
134 }; // class AddObstacle
135
145 virtual bool validate(const PathPtr_t& path, bool reverse,
146 PathPtr_t& validPart,
151 virtual void addObstacle(const CollisionObjectConstPtr_t& object);
152
160 const JointPtr_t& joint, const CollisionObjectConstPtr_t& obstacle);
161
171
173 virtual void setSecurityMargins(const matrix_t& securityMatrix);
174
176 virtual void setSecurityMarginBetweenBodies(const std::string& body_a,
177 const std::string& body_b,
178 const value_type& margin);
179
182
200 template <class Delegate>
201 void add(const Delegate& delegate);
202
205 template <class Delegate>
206 void reset();
207
210 void addIntervalValidation(const IntervalValidationPtr_t& intervalValidation);
212 value_type tolerance() const { return tolerance_; }
213
214 DevicePtr_t robot() const { return robot_; }
218
220
221 protected:
223
224 static void setPath(IntervalValidations_t& intervalValidations,
225 const PathPtr_t& path, bool reverse);
226
230 ContinuousValidation(const DevicePtr_t& robot, const value_type& tolerance);
231
242 virtual bool validateConfiguration(IntervalValidations_t& intervalValidations,
243 const Configuration_t& config,
244 const value_type& t, interval_t& interval,
246
260 IntervalValidations_t& validations, const value_type& t,
261 interval_t& interval, PathValidationReportPtr_t& pathReport,
262 typename IntervalValidations_t::iterator& smallestInterval,
263 pinocchio::DeviceData& data) {
264 typename IntervalValidations_t::iterator itMin = validations.begin();
265 for (IntervalValidations_t::iterator itVal(validations.begin());
266 itVal != validations.end(); ++itVal) {
268 // the valid interval will not be greater than "interval" so we do not
269 // need to perform validation on a greater interval.
270 interval_t tmpInt = interval;
271 if (!(*itVal)->validateConfiguration(t, tmpInt, report, data)) {
273 pathReport->configurationReport = report;
274 pathReport->parameter = t;
275 return false;
276 } else {
277 if (interval.second > tmpInt.second) {
278 itMin = itVal;
279 smallestInterval = itVal;
280 }
281 interval.first = std::max(interval.first, tmpInt.first);
282 interval.second = std::min(interval.second, tmpInt.second);
283 assert((*itVal)->path()->length() == 0 ||
284 interval.second > interval.first);
285 assert(interval.first <= t);
286 assert(t <= interval.second);
287 }
288 }
289 return true;
290 }
291
294
296 void init(ContinuousValidationWkPtr_t weak);
297
302
303 pinocchio::Pool<IntervalValidations_t> bodyPairCollisionPool_;
304
306
307 private:
308 // Weak pointer to itself
309 ContinuousValidationWkPtr_t weak_;
310
311 virtual bool validateStraightPath(IntervalValidations_t& intervalValidations,
312 const PathPtr_t& path, bool reverse,
313 PathPtr_t& validPart,
314 PathValidationReportPtr_t& report) = 0;
315
316 std::vector<Initialize> initialize_;
317 std::vector<AddObstacle> addObstacle_;
318}; // class ContinuousValidation
320template <>
321void ContinuousValidation::add<ContinuousValidation::AddObstacle>(
322 const ContinuousValidation::AddObstacle& delegate);
323
324template <>
325void ContinuousValidation::reset<ContinuousValidation::AddObstacle>();
326
327template <>
328void ContinuousValidation::add<ContinuousValidation::Initialize>(
329 const ContinuousValidation::Initialize& delegate);
330
331template <>
332void ContinuousValidation::reset<ContinuousValidation::Initialize>();
333
334template <class Delegate>
335void ContinuousValidation::add(const Delegate& delegate) {
336 assert(false && "No delegate of this type in class ContinuousValidation.");
337}
338template <class Delegate>
340 assert(false && "No delegate of this type in class ContinuousValidation.");
341}
342
343} // namespace core
344} // namespace hpp
345#endif // HPP_CORE_CONTINUOUS_VALIDATION_HH
Definition: continuous-validation.hh:120
ContinuousValidation * owner_
Definition: continuous-validation.hh:132
DeviceWkPtr_t robot_
Definition: continuous-validation.hh:133
AddObstacle(ContinuousValidation &owner)
virtual void doExecute(const CollisionObjectConstPtr_t &object) const
virtual ~AddObstacle()
Definition: continuous-validation.hh:129
ContinuousValidation & owner() const
Definition: continuous-validation.hh:128
Definition: continuous-validation.hh:102
virtual ~Initialize()
Definition: continuous-validation.hh:112
ContinuousValidation & owner() const
Definition: continuous-validation.hh:111
ContinuousValidation * owner_
Definition: continuous-validation.hh:115
Initialize(ContinuousValidation &owner)
Definition: continuous-validation.hh:97
void init(ContinuousValidationWkPtr_t weak)
Store weak pointer to itself.
static void setPath(IntervalValidations_t &intervalValidations, const PathPtr_t &path, bool reverse)
virtual bool validate(const PathPtr_t &path, bool reverse, PathPtr_t &validPart, PathValidationReportPtr_t &report)
virtual void addObstacle(const CollisionObjectConstPtr_t &object)
IntervalValidations_t intervalValidations_
All BodyPairValidation to validate.
Definition: continuous-validation.hh:299
continuousValidation::IntervalValidations_t IntervalValidations_t
Definition: continuous-validation.hh:222
virtual void setSecurityMargins(const matrix_t &securityMatrix)
bool validateIntervals(IntervalValidations_t &validations, const value_type &t, interval_t &interval, PathValidationReportPtr_t &pathReport, typename IntervalValidations_t::iterator &smallestInterval, pinocchio::DeviceData &data)
Definition: continuous-validation.hh:259
virtual bool validateConfiguration(IntervalValidations_t &intervalValidations, const Configuration_t &config, const value_type &t, interval_t &interval, PathValidationReportPtr_t &report)
ContinuousValidation(const DevicePtr_t &robot, const value_type &tolerance)
pinocchio::Pool< IntervalValidations_t > bodyPairCollisionPool_
Definition: continuous-validation.hh:303
void addIntervalValidation(const IntervalValidationPtr_t &intervalValidation)
void filterCollisionPairs(const RelativeMotion::matrix_type &relMotion)
void reset()
Definition: continuous-validation.hh:339
DevicePtr_t robot() const
Definition: continuous-validation.hh:214
IntervalValidations_t disabledBodyPairCollisions_
BodyPairCollision for which collision is disabled.
Definition: continuous-validation.hh:301
value_type tolerance_
Definition: continuous-validation.hh:293
virtual void setSecurityMarginBetweenBodies(const std::string &body_a, const std::string &body_b, const value_type &margin)
value_type tolerance() const
Get tolerance value.
Definition: continuous-validation.hh:212
value_type stepSize_
Definition: continuous-validation.hh:305
DevicePtr_t robot_
Definition: continuous-validation.hh:292
void add(const Delegate &delegate)
Definition: continuous-validation.hh:335
virtual void removeObstacleFromJoint(const JointPtr_t &joint, const CollisionObjectConstPtr_t &obstacle)
Definition: obstacle-user.hh:47
Definition: path-validation.hh:51
#define HPP_CORE_DLLAPI
Definition: config.hh:64
shared_ptr< IntervalValidation > IntervalValidationPtr_t
Definition: fwd.hh:261
std::vector< IntervalValidationPtr_t > IntervalValidations_t
Definition: fwd.hh:262
pinocchio::DeviceWkPtr_t DeviceWkPtr_t
Definition: fwd.hh:135
pinocchio::value_type value_type
Definition: fwd.hh:174
pinocchio::JointPtr_t JointPtr_t
Definition: fwd.hh:151
std::pair< value_type, value_type > interval_t
Definition: fwd.hh:175
shared_ptr< ValidationReport > ValidationReportPtr_t
Definition: fwd.hh:225
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:106
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:134
shared_ptr< Path > PathPtr_t
Definition: fwd.hh:187
shared_ptr< PathValidationReport > PathValidationReportPtr_t
Definition: fwd.hh:316
pinocchio::CollisionObjectConstPtr_t CollisionObjectConstPtr_t
Definition: fwd.hh:99
pinocchio::matrix_t matrix_t
Definition: fwd.hh:162
Definition: bi-rrt-planner.hh:35
Definition: path-validation-report.hh:45
Eigen::Matrix< RelativeMotionType, Eigen::Dynamic, Eigen::Dynamic > matrix_type
Definition: relative-motion.hh:62