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
//
// This file is part of hpp-core
// hpp-core is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core  If not, see
// <http://www.gnu.org/licenses/>.

#ifndef HPP_CORE_CONTINUOUS_VALIDATION_HH
# define HPP_CORE_CONTINUOUS_VALIDATION_HH

# include <hpp/pinocchio/pool.hh>

# include <hpp/core/fwd.hh>
# include <hpp/core/obstacle-user.hh>
# include <hpp/core/path.hh>
# include <hpp/core/path-validation.hh>
# include <hpp/core/path-validation-report.hh>
# include <hpp/core/continuous-validation/interval-validation.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);


      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_;
      }

      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_;

      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