hpp-core: include/hpp/core/continuous-validation.hh Source File

hpp-core 9.0.2 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
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
41namespace hpp {
42namespace core {
43using continuousValidation::IntervalValidation;
44using continuousValidation::IntervalValidationPtr_t;
47
96 class HPP_CORE_DLLAPI ContinuousValidation : public PathValidation,
97 public ObstacleUserInterface {
98 public:
102 class Initialize {
103 public:
104 Initialize(ContinuousValidation& owner);
110 virtual void doExecute() const;
111 ContinuousValidation& owner() const { return *owner_; }
112 virtual ~Initialize() {}
113
114 protected:
115 ContinuousValidation* owner_;
116 }; // class Initialize
120 class AddObstacle {
121 public:
122 AddObstacle(ContinuousValidation& owner);
127 virtual void doExecute(const CollisionObjectConstPtr_t& object) const;
128 ContinuousValidation& owner() const { return *owner_; }
129 virtual ~AddObstacle() {}
130
131 protected:
132 ContinuousValidation* owner_;
133 DeviceWkPtr_t robot_;
134 }; // class AddObstacle
135
145 virtual bool validate(const PathPtr_t& path, bool reverse,
146 PathPtr_t& validPart,
147 PathValidationReportPtr_t& report);
151 virtual void addObstacle(const CollisionObjectConstPtr_t& object);
152
159 virtual void removeObstacleFromJoint(
160 const JointPtr_t& joint, const CollisionObjectConstPtr_t& obstacle);
161
170 void filterCollisionPairs(const RelativeMotion::matrix_type& relMotion);
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
216 value_type breakDistance() const { return breakDistance_; }
217
220 void breakDistance(value_type distance);
221
222 DevicePtr_t robot() const { return robot_; }
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
238 ContinuousValidation(const DevicePtr_t& robot, const value_type& tolerance);
239
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
267 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 typename IntervalValidations_t::iterator itMin = validations.begin();
273 for (IntervalValidations_t::iterator itVal(validations.begin());
274 itVal != validations.end(); ++itVal) {
275 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 interval_t tmpInt = interval;
279 if (!(*itVal)->validateConfiguration(t, tmpInt, report, data)) {
280 pathReport = PathValidationReportPtr_t(new PathValidationReport);
281 pathReport->configurationReport = report;
282 pathReport->parameter = t;
283 return false;
284 } else {
285 if (interval.second > tmpInt.second) {
286 itMin = itVal;
287 smallestInterval = itVal;
288 }
289 interval.first = std::max(interval.first, tmpInt.first);
290 interval.second = std::min(interval.second, tmpInt.second);
291 assert((*itVal)->path()->length() == 0 ||
292 interval.second > interval.first);
293 assert(interval.first <= t);
294 assert(t <= interval.second);
295 }
296 }
297 return true;
298 }
299
300 DevicePtr_t robot_;
301 value_type tolerance_;
302 value_type breakDistance_;
303
305 void init(ContinuousValidationWkPtr_t weak);
306
308 IntervalValidations_t intervalValidations_;
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
329template <>
330 void ContinuousValidation::add<ContinuousValidation::AddObstacle>(
331 const ContinuousValidation::AddObstacle& delegate);
332
333template <>
334 void ContinuousValidation::reset<ContinuousValidation::AddObstacle>();
335
336template <>
337 void ContinuousValidation::add<ContinuousValidation::Initialize>(
338 const ContinuousValidation::Initialize& delegate);
339
340template <>
341 void ContinuousValidation::reset<ContinuousValidation::Initialize>();
342
343template <class Delegate>
344 void ContinuousValidation::add(const Delegate& delegate) {
345 assert(false && "No delegate of this type in class ContinuousValidation.");
346}
347template <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
hpp::core::ContinuousValidation::AddObstacle
Definition continuous-validation.hh:120
hpp::core::ContinuousValidation::AddObstacle::owner_
ContinuousValidation * owner_
Definition continuous-validation.hh:132
hpp::core::ContinuousValidation::AddObstacle::robot_
DeviceWkPtr_t robot_
Definition continuous-validation.hh:133
hpp::core::ContinuousValidation::AddObstacle::AddObstacle
AddObstacle(ContinuousValidation &owner)
hpp::core::ContinuousValidation::AddObstacle::doExecute
virtual void doExecute(const CollisionObjectConstPtr_t &object) const
hpp::core::ContinuousValidation::AddObstacle::~AddObstacle
virtual ~AddObstacle()
Definition continuous-validation.hh:129
hpp::core::ContinuousValidation::AddObstacle::owner
ContinuousValidation & owner() const
Definition continuous-validation.hh:128
hpp::core::ContinuousValidation::Initialize
Definition continuous-validation.hh:102
hpp::core::ContinuousValidation::Initialize::~Initialize
virtual ~Initialize()
Definition continuous-validation.hh:112
hpp::core::ContinuousValidation::Initialize::owner
ContinuousValidation & owner() const
Definition continuous-validation.hh:111
hpp::core::ContinuousValidation::Initialize::owner_
ContinuousValidation * owner_
Definition continuous-validation.hh:115
hpp::core::ContinuousValidation::Initialize::Initialize
Initialize(ContinuousValidation &owner)
hpp::core::ContinuousValidation::Initialize::doExecute
virtual void doExecute() const
hpp::core::ContinuousValidation
Definition continuous-validation.hh:97
hpp::core::ContinuousValidation::init
void init(ContinuousValidationWkPtr_t weak)
Store weak pointer to itself.
hpp::core::ContinuousValidation::setPath
static void setPath(IntervalValidations_t &intervalValidations, const PathPtr_t &path, bool reverse)
hpp::core::ContinuousValidation::validate
virtual bool validate(const PathPtr_t &path, bool reverse, PathPtr_t &validPart, PathValidationReportPtr_t &report)
hpp::core::ContinuousValidation::addObstacle
virtual void addObstacle(const CollisionObjectConstPtr_t &object)
hpp::core::ContinuousValidation::intervalValidations_
IntervalValidations_t intervalValidations_
All BodyPairValidation to validate.
Definition continuous-validation.hh:308
hpp::core::ContinuousValidation::IntervalValidations_t
continuousValidation::IntervalValidations_t IntervalValidations_t
Definition continuous-validation.hh:230
hpp::core::ContinuousValidation::breakDistance
void breakDistance(value_type distance)
hpp::core::ContinuousValidation::setSecurityMargins
virtual void setSecurityMargins(const matrix_t &securityMatrix)
hpp::core::ContinuousValidation::validateIntervals
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:267
hpp::core::ContinuousValidation::validateConfiguration
virtual bool validateConfiguration(IntervalValidations_t &intervalValidations, const Configuration_t &config, const value_type &t, interval_t &interval, PathValidationReportPtr_t &report)
hpp::core::ContinuousValidation::~ContinuousValidation
virtual ~ContinuousValidation()
hpp::core::ContinuousValidation::ContinuousValidation
ContinuousValidation(const DevicePtr_t &robot, const value_type &tolerance)
hpp::core::ContinuousValidation::bodyPairCollisionPool_
pinocchio::Pool< IntervalValidations_t > bodyPairCollisionPool_
Definition continuous-validation.hh:312
hpp::core::ContinuousValidation::addIntervalValidation
void addIntervalValidation(const IntervalValidationPtr_t &intervalValidation)
hpp::core::ContinuousValidation::filterCollisionPairs
void filterCollisionPairs(const RelativeMotion::matrix_type &relMotion)
hpp::core::ContinuousValidation::reset
void reset()
Definition continuous-validation.hh:348
hpp::core::ContinuousValidation::robot
DevicePtr_t robot() const
Definition continuous-validation.hh:222
hpp::core::ContinuousValidation::disabledBodyPairCollisions_
IntervalValidations_t disabledBodyPairCollisions_
BodyPairCollision for which collision is disabled.
Definition continuous-validation.hh:310
hpp::core::ContinuousValidation::tolerance_
value_type tolerance_
Definition continuous-validation.hh:301
hpp::core::ContinuousValidation::setSecurityMarginBetweenBodies
virtual void setSecurityMarginBetweenBodies(const std::string &body_a, const std::string &body_b, const value_type &margin)
hpp::core::ContinuousValidation::tolerance
value_type tolerance() const
Get tolerance value.
Definition continuous-validation.hh:212
hpp::core::ContinuousValidation::breakDistance_
value_type breakDistance_
Definition continuous-validation.hh:302
hpp::core::ContinuousValidation::initialize
void initialize()
hpp::core::ContinuousValidation::stepSize_
value_type stepSize_
Definition continuous-validation.hh:314
hpp::core::ContinuousValidation::breakDistance
value_type breakDistance() const
Definition continuous-validation.hh:216
hpp::core::ContinuousValidation::robot_
DevicePtr_t robot_
Definition continuous-validation.hh:300
hpp::core::ContinuousValidation::add
void add(const Delegate &delegate)
Definition continuous-validation.hh:344
hpp::core::ContinuousValidation::removeObstacleFromJoint
virtual void removeObstacleFromJoint(const JointPtr_t &joint, const CollisionObjectConstPtr_t &obstacle)
hpp::core::ObstacleUserInterface
Definition obstacle-user.hh:47
hpp::core::PathValidation
Definition path-validation.hh:51
hpp::pinocchio::Pool
HPP_CORE_DLLAPI
#define HPP_CORE_DLLAPI
Definition config.hh:88
fwd.hh
interval-validation.hh
hpp
hpp::core::continuousValidation::IntervalValidationPtr_t
shared_ptr< IntervalValidation > IntervalValidationPtr_t
Definition fwd.hh:269
hpp::core::continuousValidation::IntervalValidations_t
std::vector< IntervalValidationPtr_t > IntervalValidations_t
Definition fwd.hh:270
hpp::core::DeviceWkPtr_t
pinocchio::DeviceWkPtr_t DeviceWkPtr_t
Definition fwd.hh:135
hpp::core::value_type
pinocchio::value_type value_type
Definition fwd.hh:174
hpp::core::JointPtr_t
pinocchio::JointPtr_t JointPtr_t
Definition fwd.hh:151
hpp::core::interval_t
std::pair< value_type, value_type > interval_t
Definition fwd.hh:175
hpp::core::ValidationReportPtr_t
shared_ptr< ValidationReport > ValidationReportPtr_t
Definition fwd.hh:225
hpp::core::Configuration_t
pinocchio::Configuration_t Configuration_t
Definition fwd.hh:107
hpp::core::DevicePtr_t
pinocchio::DevicePtr_t DevicePtr_t
Definition fwd.hh:134
hpp::core::PathPtr_t
shared_ptr< Path > PathPtr_t
Definition fwd.hh:187
hpp::core::PathValidationReportPtr_t
shared_ptr< PathValidationReport > PathValidationReportPtr_t
Definition fwd.hh:329
hpp::core::CollisionObjectConstPtr_t
pinocchio::CollisionObjectConstPtr_t CollisionObjectConstPtr_t
Definition fwd.hh:100
hpp::core::matrix_t
pinocchio::matrix_t matrix_t
Definition fwd.hh:162
obstacle-user.hh
path-validation-report.hh
path-validation.hh
path.hh
pool.hh
hpp::core::PathValidationReport
Definition path-validation-report.hh:45
hpp::core::RelativeMotion::matrix_type
Eigen::Matrix< RelativeMotionType, Eigen::Dynamic, Eigen::Dynamic > matrix_type
Definition relative-motion.hh:62
hpp::pinocchio::DeviceData