body-pair-collision.hh

Go to the documentation of this file.

//
// Copyright (c) 2014,2015,2016,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_BODY_PAIR_COLLISION_HH
# define HPP_CORE_CONTINUOUS_VALIDATION_BODY_PAIR_COLLISION_HH

# include <boost/icl/continuous_interval.hpp>
# include <boost/icl/interval_set.hpp>

# include <hpp/fcl/collision_data.h>

# include <hpp/core/collision-validation-report.hh>
# include <hpp/core/continuous-validation/interval-validation.hh>

namespace hpp {
  namespace core {
    namespace continuousValidation {
      class BodyPairCollision : public IntervalValidation
      {
      public:
        typedef std::pair<CollisionObjectConstPtr_t, CollisionObjectConstPtr_t> CollisionPair_t;
        typedef std::vector<CollisionPair_t> CollisionPairs_t;

        bool validateConfiguration (const value_type& t, interval_t& interval,
                  ValidationReportPtr_t& report,
                  const pinocchio::DeviceData& data);

        // Get pairs checked for collision
        const CollisionPairs_t& pairs () const
        {
          return m_->pairs;
        }

        // Get pairs checked for collision
        CollisionPairs_t& pairs ()
        {
          return m_->pairs;
        }

        // Get maximal velocity
        value_type maximalVelocity () const
        {
          return maximalVelocity_;
        }

        virtual size_type indexJointA () const { return -1; }
        virtual size_type indexJointB () const { return -1; }

        virtual bool removeObjectTo_b (const CollisionObjectConstPtr_t& /*object*/) { return false;}
        virtual void addCollisionPair (const CollisionObjectConstPtr_t& /*left*/,
            const CollisionObjectConstPtr_t &/*right*/) {}

        virtual std::string name () const = 0;
        virtual std::ostream& print (std::ostream& os) const = 0;


        value_type securityMargin() const
        {
          return collisionRequest_.security_margin;
        }
        void securityMargin(const value_type& securityMargin)
        {
          collisionRequest_.security_margin = securityMargin;
        }
      protected:
        BodyPairCollision (value_type tolerance):
          IntervalValidation(tolerance), m_ (new Model),
          collisionRequest_(fcl::DISTANCE_LOWER_BOUND, 1), maximalVelocity_(0)
        {
        }

        BodyPairCollision (const BodyPairCollision& other):
          IntervalValidation(other), m_(other.m_),
          collisionRequest_(other.collisionRequest_),
          maximalVelocity_(other.maximalVelocity_)
        {}

        virtual void setReport (ValidationReportPtr_t& report,
                            fcl::CollisionResult result,
                            CollisionPair_t _pair) const
        {
          report = CollisionValidationReportPtr_t
            (new CollisionValidationReport (_pair, result));
        }

      private:
        struct Model {
          CollisionPairs_t pairs;
        };
        boost::shared_ptr<Model> m_;
        fcl::CollisionRequest collisionRequest_;

        mutable vector_t Vb_;
        value_type maximalVelocity_;

        virtual value_type computeMaximalVelocity(vector_t& Vb) const = 0;

        virtual void setupPath();

        value_type collisionFreeInterval(const value_type &t,
                                        const value_type &distanceLowerBound,
                                        value_type &maxVelocity) const;

        virtual bool computeDistanceLowerBound(value_type &distanceLowerBound,
          ValidationReportPtr_t& report,
          const pinocchio::DeviceData& data) const;

      }; // class BodyPairCollision

      inline std::ostream& operator<< (std::ostream& os,
                                       const BodyPairCollision& pair)
      {
        return pair.print (os);
      }
    } // namespace continuousValidation
  } // namespace core
} // namespace hpp
#endif // HPP_CORE_CONTINUOUS_VALIDATION_BODY_PAIR_COLLISION_HH