hpp-core  4.12.0
Implement basic classes for canonical path planning for kinematic chains.
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 // This file is part of hpp-core
6 // hpp-core is free software: you can redistribute it
7 // and/or modify it under the terms of the GNU Lesser General Public
8 // License as published by the Free Software Foundation, either version
9 // 3 of the License, or (at your option) any later version.
10 //
11 // hpp-core is distributed in the hope that it will be
12 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
13 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14 // General Lesser Public License for more details. You should have
15 // received a copy of the GNU Lesser General Public License along with
16 // hpp-core If not, see
17 // <http://www.gnu.org/licenses/>.
18 
19 #ifndef HPP_CORE_CONTINUOUS_VALIDATION_HH
20 # define HPP_CORE_CONTINUOUS_VALIDATION_HH
21 
22 # include <hpp/pinocchio/pool.hh>
23 
24 # include <hpp/core/fwd.hh>
25 # include <hpp/core/obstacle-user.hh>
26 # include <hpp/core/path.hh>
30 
31 namespace hpp {
32  namespace core {
33  using continuousValidation::IntervalValidation;
37 
87  public PathValidation,
89  {
90  public:
94  class Initialize
95  {
96  public:
103  virtual void doExecute() const;
104  ContinuousValidation & owner() const { return *owner_; }
105  virtual ~Initialize () {}
106  protected:
108  }; // class Initialize
113  {
114  public:
120  virtual void doExecute(const CollisionObjectConstPtr_t &object) const;
121  ContinuousValidation & owner() const { return *owner_; }
122  virtual ~AddObstacle () {}
123  protected:
126  }; // class AddObstacle
127 
137  virtual bool validate (const PathPtr_t& path, bool reverse,
138  PathPtr_t& validPart,
139  PathValidationReportPtr_t& report);
143  virtual void addObstacle (const CollisionObjectConstPtr_t& object);
144 
151  virtual void removeObstacleFromJoint
152  (const JointPtr_t& joint, const CollisionObjectConstPtr_t& obstacle);
153 
161  void filterCollisionPairs (const RelativeMotion::matrix_type& relMotion);
162 
164  virtual void setSecurityMargins(const matrix_t& securityMatrix);
165 
167  virtual void setSecurityMarginBetweenBodies(const std::string& body_a,
168  const std::string& body_b,
169  const value_type& margin);
170 
173 
191  template <class Delegate> void add(const Delegate& delegate);
192 
195  template <class Delegate> void reset();
196 
199  void addIntervalValidation
200  (const IntervalValidationPtr_t& intervalValidation);
203  {
204  return tolerance_;
205  }
206 
208  {
209  return robot_;
210  }
213  void initialize();
214 
215  virtual ~ContinuousValidation ();
216  protected:
218 
219  static void setPath(IntervalValidations_t& intervalValidations,
220  const PathPtr_t &path, bool reverse);
221 
225  ContinuousValidation (const DevicePtr_t& robot,
226  const value_type& tolerance);
227 
238  virtual bool validateConfiguration
239  (IntervalValidations_t& intervalValidations,
240  const Configuration_t& config,
241  const value_type& t,
242  interval_t& interval,
243  PathValidationReportPtr_t& report);
244 
257  bool validateIntervals
258  (IntervalValidations_t& validations, const value_type &t,
259  interval_t &interval, PathValidationReportPtr_t &pathReport,
260  typename IntervalValidations_t::iterator& smallestInterval,
261  pinocchio::DeviceData& data)
262  {
263  typename IntervalValidations_t::iterator itMin = validations.begin();
264  for (IntervalValidations_t::iterator itVal (validations.begin());
265  itVal != validations.end(); ++itVal)
266  {
267  ValidationReportPtr_t report;
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))
272  {
274  pathReport->configurationReport = report;
275  pathReport->parameter = t;
276  return false;
277  }
278  else
279  {
280  if (interval.second > tmpInt.second)
281  {
282  itMin = itVal;
283  smallestInterval = itVal;
284  }
285  interval.first = std::max(interval.first, tmpInt.first);
286  interval.second = std::min(interval.second, tmpInt.second);
287  assert((*itVal)->path()->length() == 0 || interval.second > interval.first);
288  assert(interval.first <= t);
289  assert(t <= interval.second);
290  }
291  }
292  return true;
293  }
294 
297 
299  void init (ContinuousValidationWkPtr_t weak);
300 
305 
306  pinocchio::Pool<IntervalValidations_t> bodyPairCollisionPool_;
307 
309  private:
310  // Weak pointer to itself
311  ContinuousValidationWkPtr_t weak_;
312 
313  virtual bool validateStraightPath
314  (IntervalValidations_t& intervalValidations,
315  const PathPtr_t& path, bool reverse, PathPtr_t& validPart,
316  PathValidationReportPtr_t& report) = 0;
317 
318  std::vector<Initialize> initialize_;
319  std::vector<AddObstacle> addObstacle_;
320  }; // class ContinuousValidation
322  template <>
323  void ContinuousValidation::add<ContinuousValidation::AddObstacle>
324  (const ContinuousValidation::AddObstacle& delegate);
325 
326  template <>
327  void ContinuousValidation::reset<ContinuousValidation::AddObstacle>();
328 
329  template <>
330  void ContinuousValidation::add<ContinuousValidation::Initialize>
331  (const ContinuousValidation::Initialize& delegate);
332 
333  template <>
334  void ContinuousValidation::reset<ContinuousValidation::Initialize>();
335 
336  template <class Delegate> void ContinuousValidation::add
337  (const Delegate& delegate)
338  {
339  assert (false &&
340  "No delegate of this type in class ContinuousValidation.");
341  }
342  template <class Delegate> void ContinuousValidation::reset()
343  {
344  assert (false &&
345  "No delegate of this type in class ContinuousValidation.");
346  }
347 
348  } // namespace core
349 } // namespace hpp
350 #endif // HPP_CORE_CONTINUOUS_VALIDATION_HH
value_type tolerance() const
Get tolerance value.
Definition: continuous-validation.hh:202
IntervalValidations_t intervalValidations_
All BodyPairValidation to validate.
Definition: continuous-validation.hh:302
DevicePtr_t robot_
Definition: continuous-validation.hh:295
Definition: continuous-validation.hh:94
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:114
void add(const Delegate &delegate)
Definition: continuous-validation.hh:337
Definition: bi-rrt-planner.hh:24
value_type tolerance_
Definition: continuous-validation.hh:296
pinocchio::Pool< IntervalValidations_t > bodyPairCollisionPool_
Definition: continuous-validation.hh:306
ContinuousValidation & owner() const
Definition: continuous-validation.hh:121
std::pair< value_type, value_type > interval_t
Definition: fwd.hh:158
pinocchio::DeviceWkPtr_t DeviceWkPtr_t
Definition: fwd.hh:115
Definition: continuous-validation.hh:86
std::vector< IntervalValidationPtr_t > IntervalValidations_t
Definition: fwd.hh:244
ContinuousValidation * owner_
Definition: continuous-validation.hh:107
pinocchio::JointPtr_t JointPtr_t
Definition: fwd.hh:133
DevicePtr_t robot() const
Definition: continuous-validation.hh:207
continuousValidation::IntervalValidations_t IntervalValidations_t
Definition: continuous-validation.hh:217
ContinuousValidation * owner_
Definition: continuous-validation.hh:124
Definition: continuous-validation.hh:112
ContinuousValidation & owner() const
Definition: continuous-validation.hh:104
shared_ptr< ValidationReport > ValidationReportPtr_t
Definition: fwd.hh:207
void reset()
Definition: continuous-validation.hh:342
pinocchio::matrix_t matrix_t
Definition: fwd.hh:145
Definition: path-validation.hh:35
shared_ptr< PathValidationReport > PathValidationReportPtr_t
Definition: fwd.hh:299
pinocchio::value_type value_type
Definition: fwd.hh:157
virtual ~AddObstacle()
Definition: continuous-validation.hh:122
Definition: path-validation-report.hh:34
Definition: obstacle-user.hh:36
virtual ~Initialize()
Definition: continuous-validation.hh:105
shared_ptr< IntervalValidation > IntervalValidationPtr_t
Definition: fwd.hh:243
IntervalValidations_t disabledBodyPairCollisions_
BodyPairCollision for which collision is disabled.
Definition: continuous-validation.hh:304
value_type stepSize_
Definition: continuous-validation.hh:308
#define HPP_CORE_DLLAPI
Definition: config.hh:64
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:96
Eigen::Matrix< RelativeMotionType, Eigen::Dynamic, Eigen::Dynamic > matrix_type
Definition: relative-motion.hh:51
pinocchio::CollisionObjectConstPtr_t CollisionObjectConstPtr_t
Definition: fwd.hh:90
shared_ptr< Path > PathPtr_t
Definition: fwd.hh:170
DeviceWkPtr_t robot_
Definition: continuous-validation.hh:125