hpp-core  4.10.1
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 
168 
186  template <class Delegate> void add(const Delegate& delegate);
187 
190  template <class Delegate> void reset();
191 
194  void addIntervalValidation
195  (const IntervalValidationPtr_t& intervalValidation);
198  {
199  return tolerance_;
200  }
201 
203  {
204  return robot_;
205  }
208  void initialize();
209 
210  virtual ~ContinuousValidation ();
211  protected:
213 
214  static void setPath(IntervalValidations_t& intervalValidations,
215  const PathPtr_t &path, bool reverse);
216 
220  ContinuousValidation (const DevicePtr_t& robot,
221  const value_type& tolerance);
222 
233  virtual bool validateConfiguration
234  (IntervalValidations_t& intervalValidations,
235  const Configuration_t& config,
236  const value_type& t,
237  interval_t& interval,
238  PathValidationReportPtr_t& report);
239 
252  bool validateIntervals
253  (IntervalValidations_t& validations, const value_type &t,
254  interval_t &interval, PathValidationReportPtr_t &pathReport,
255  typename IntervalValidations_t::iterator& smallestInterval,
256  pinocchio::DeviceData& data)
257  {
258  typename IntervalValidations_t::iterator itMin = validations.begin();
259  for (IntervalValidations_t::iterator itVal (validations.begin());
260  itVal != validations.end(); ++itVal)
261  {
262  ValidationReportPtr_t report;
263  // the valid interval will not be greater than "interval" so we do not
264  // need to perform validation on a greater interval.
265  interval_t tmpInt = interval;
266  if (!(*itVal)->validateConfiguration(t, tmpInt, report, data))
267  {
269  pathReport->configurationReport = report;
270  pathReport->parameter = t;
271  return false;
272  }
273  else
274  {
275  if (interval.second > tmpInt.second)
276  {
277  itMin = itVal;
278  smallestInterval = itVal;
279  }
280  interval.first = std::max(interval.first, tmpInt.first);
281  interval.second = std::min(interval.second, tmpInt.second);
282  assert((*itVal)->path()->length() == 0 || interval.second > interval.first);
283  assert(interval.first <= t);
284  assert(t <= interval.second);
285  }
286  }
287  return true;
288  }
289 
292 
294  void init (ContinuousValidationWkPtr_t weak);
295 
297  IntervalValidations_t intervalValidations_;
299  IntervalValidations_t disabledBodyPairCollisions_;
300 
301  pinocchio::Pool<IntervalValidations_t> bodyPairCollisionPool_;
302 
304  private:
305  // Weak pointer to itself
306  ContinuousValidationWkPtr_t weak_;
307 
308  virtual bool validateStraightPath
309  (IntervalValidations_t& intervalValidations,
310  const PathPtr_t& path, bool reverse, PathPtr_t& validPart,
311  PathValidationReportPtr_t& report) = 0;
312 
313  std::vector<Initialize> initialize_;
314  std::vector<AddObstacle> addObstacle_;
315  }; // class ContinuousValidation
317  template <>
318  void ContinuousValidation::add<ContinuousValidation::AddObstacle>
319  (const ContinuousValidation::AddObstacle& delegate);
320 
321  template <>
322  void ContinuousValidation::reset<ContinuousValidation::AddObstacle>();
323 
324  template <>
325  void ContinuousValidation::add<ContinuousValidation::Initialize>
326  (const ContinuousValidation::Initialize& delegate);
327 
328  template <>
329  void ContinuousValidation::reset<ContinuousValidation::Initialize>();
330 
331  template <class Delegate> void ContinuousValidation::add
332  (const Delegate& delegate)
333  {
334  assert (false &&
335  "No delegate of this type in class ContinuousValidation.");
336  }
337  template <class Delegate> void ContinuousValidation::reset()
338  {
339  assert (false &&
340  "No delegate of this type in class ContinuousValidation.");
341  }
342 
343  } // namespace core
344 } // namespace hpp
345 #endif // HPP_CORE_CONTINUOUS_VALIDATION_HH
value_type tolerance() const
Get tolerance value.
Definition: continuous-validation.hh:197
boost::shared_ptr< Path > PathPtr_t
Definition: fwd.hh:170
IntervalValidations_t intervalValidations_
All BodyPairValidation to validate.
Definition: continuous-validation.hh:297
DevicePtr_t robot_
Definition: continuous-validation.hh:290
boost::shared_ptr< ValidationReport > ValidationReportPtr_t
Definition: fwd.hh:206
Definition: continuous-validation.hh:94
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:114
void add(const Delegate &delegate)
Definition: continuous-validation.hh:332
boost::shared_ptr< IntervalValidation > IntervalValidationPtr_t
Definition: fwd.hh:242
Definition: basic-configuration-shooter.hh:26
value_type tolerance_
Definition: continuous-validation.hh:291
pinocchio::Pool< IntervalValidations_t > bodyPairCollisionPool_
Definition: continuous-validation.hh:301
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:243
ContinuousValidation * owner_
Definition: continuous-validation.hh:107
pinocchio::JointPtr_t JointPtr_t
Definition: fwd.hh:133
DevicePtr_t robot() const
Definition: continuous-validation.hh:202
continuousValidation::IntervalValidations_t IntervalValidations_t
Definition: continuous-validation.hh:212
ContinuousValidation * owner_
Definition: continuous-validation.hh:124
Definition: continuous-validation.hh:112
ContinuousValidation & owner() const
Definition: continuous-validation.hh:104
void reset()
Definition: continuous-validation.hh:337
pinocchio::matrix_t matrix_t
Definition: fwd.hh:145
Definition: path-validation.hh:35
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:35
boost::shared_ptr< PathValidationReport > PathValidationReportPtr_t
Definition: fwd.hh:298
virtual ~Initialize()
Definition: continuous-validation.hh:105
IntervalValidations_t disabledBodyPairCollisions_
BodyPairCollision for which collision is disabled.
Definition: continuous-validation.hh:299
value_type stepSize_
Definition: continuous-validation.hh:303
#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
DeviceWkPtr_t robot_
Definition: continuous-validation.hh:125