continuous-validation.hh

Go to the documentation of this file.

//
// Copyright (c) 2014, 2015, 2016, 2017, 2018 CNRS
// Authors: Florent Lamiraux, Joseph Mirabel, Diane Bury
//
 
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
 
#ifndef HPP_CORE_CONTINUOUS_VALIDATION_HH
#define HPP_CORE_CONTINUOUS_VALIDATION_HH
 
#include <hpp/core/continuous-validation/interval-validation.hh>
#include <hpp/core/fwd.hh>
#include <hpp/core/obstacle-user.hh>
#include <hpp/core/path-validation-report.hh>
#include <hpp/core/path-validation.hh>
#include <hpp/core/path.hh>
#include <hpp/pinocchio/pool.hh>
 
namespace hpp {
namespace core {
using continuousValidation::IntervalValidation;
using continuousValidation::IntervalValidationPtr_t;
 
class HPP_CORE_DLLAPI ContinuousValidation : public PathValidation,
                                             public ObstacleUserInterface {
 public:
  class Initialize {
   public:
    Initialize(ContinuousValidation& owner);
    virtual void doExecute() const;
    ContinuousValidation& owner() const { return *owner_; }
    virtual ~Initialize() {}
 
   protected:
    ContinuousValidation* owner_;
  };  // class Initialize
  class AddObstacle {
   public:
    AddObstacle(ContinuousValidation& owner);
    virtual void doExecute(const CollisionObjectConstPtr_t& object) const;
    ContinuousValidation& owner() const { return *owner_; }
    virtual ~AddObstacle() {}
 
   protected:
    ContinuousValidation* owner_;
    DeviceWkPtr_t robot_;
  };  // class AddObstacle
 
  virtual bool validate(const PathPtr_t& path, bool reverse,
                        PathPtr_t& validPart,
                        PathValidationReportPtr_t& report);
  virtual void addObstacle(const CollisionObjectConstPtr_t& object);
 
  virtual void removeObstacleFromJoint(
      const JointPtr_t& joint, const CollisionObjectConstPtr_t& obstacle);
 
  void filterCollisionPairs(const RelativeMotion::matrix_type& relMotion);
 
  virtual void setSecurityMargins(const matrix_t& securityMatrix);
 
  virtual void setSecurityMarginBetweenBodies(const std::string& body_a,
                                              const std::string& body_b,
                                              const value_type& margin);
 
 
  template <class Delegate>
  void add(const Delegate& delegate);
 
  template <class Delegate>
  void reset();
 
  void addIntervalValidation(const IntervalValidationPtr_t& intervalValidation);
  value_type tolerance() const { return tolerance_; }
 
  value_type breakDistance() const { return breakDistance_; }
 
  void breakDistance(value_type distance);
 
  DevicePtr_t robot() const { return robot_; }
  void initialize();
 
  virtual ~ContinuousValidation();
 
 protected:
  typedef continuousValidation::IntervalValidations_t IntervalValidations_t;
 
  static void setPath(IntervalValidations_t& intervalValidations,
                      const PathPtr_t& path, bool reverse);
 
  ContinuousValidation(const DevicePtr_t& robot, const value_type& tolerance);
 
  virtual bool validateConfiguration(IntervalValidations_t& intervalValidations,
                                     const Configuration_t& config,
                                     const value_type& t, interval_t& interval,
                                     PathValidationReportPtr_t& report);
 
  bool validateIntervals(
      IntervalValidations_t& validations, const value_type& t,
      interval_t& interval, PathValidationReportPtr_t& pathReport,
      typename IntervalValidations_t::iterator& smallestInterval,
      pinocchio::DeviceData& data) {
    typename IntervalValidations_t::iterator itMin = validations.begin();
    for (IntervalValidations_t::iterator itVal(validations.begin());
         itVal != validations.end(); ++itVal) {
      ValidationReportPtr_t report;
      // the valid interval will not be greater than "interval" so we do not
      // need to perform validation on a greater interval.
      interval_t tmpInt = interval;
      if (!(*itVal)->validateConfiguration(t, tmpInt, report, data)) {
        pathReport = PathValidationReportPtr_t(new PathValidationReport);
        pathReport->configurationReport = report;
        pathReport->parameter = t;
        return false;
      } else {
        if (interval.second > tmpInt.second) {
          itMin = itVal;
          smallestInterval = itVal;
        }
        interval.first = std::max(interval.first, tmpInt.first);
        interval.second = std::min(interval.second, tmpInt.second);
        assert((*itVal)->path()->length() == 0 ||
               interval.second > interval.first);
        assert(interval.first <= t);
        assert(t <= interval.second);
      }
    }
    return true;
  }
 
  DevicePtr_t robot_;
  value_type tolerance_;
  value_type breakDistance_;
 
  void init(ContinuousValidationWkPtr_t weak);
 
  IntervalValidations_t intervalValidations_;
  IntervalValidations_t disabledBodyPairCollisions_;
 
  pinocchio::Pool<IntervalValidations_t> bodyPairCollisionPool_;
 
  value_type stepSize_;
 
 private:
  // Weak pointer to itself
  ContinuousValidationWkPtr_t weak_;
 
  virtual bool validateStraightPath(IntervalValidations_t& intervalValidations,
                                    const PathPtr_t& path, bool reverse,
                                    PathPtr_t& validPart,
                                    PathValidationReportPtr_t& report) = 0;
 
  std::vector<Initialize> initialize_;
  std::vector<AddObstacle> addObstacle_;
};  // class ContinuousValidation
template <>
void ContinuousValidation::add<ContinuousValidation::AddObstacle>(
    const ContinuousValidation::AddObstacle& delegate);
 
template <>
void ContinuousValidation::reset<ContinuousValidation::AddObstacle>();
 
template <>
void ContinuousValidation::add<ContinuousValidation::Initialize>(
    const ContinuousValidation::Initialize& delegate);
 
template <>
void ContinuousValidation::reset<ContinuousValidation::Initialize>();
 
template <class Delegate>
void ContinuousValidation::add(const Delegate& delegate) {
  assert(false && "No delegate of this type in class ContinuousValidation.");
}
template <class Delegate>
void ContinuousValidation::reset() {
  assert(false && "No delegate of this type in class ContinuousValidation.");
}
 
}  // namespace core
}  // namespace hpp
#endif  // HPP_CORE_CONTINUOUS_VALIDATION_HH