obstacle-user.hh

Go to the documentation of this file.

// Copyright (c) 2019 CNRS
// Authors: Joseph Mirabel
//
// 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_OBSTACLE_USER_HH
# define HPP_CORE_OBSTACLE_USER_HH

# include <hpp/fcl/collision_data.h>

# include <hpp/core/config.hh>
# include <hpp/core/fwd.hh>
# include <hpp/core/relative-motion.hh>

namespace hpp {
  namespace core {
    class HPP_CORE_DLLAPI ObstacleUserInterface
    {
      public:
        virtual ~ObstacleUserInterface () {}

        virtual void addObstacle (const CollisionObjectConstPtr_t& object) = 0;

        virtual void removeObstacleFromJoint(const JointPtr_t& joint,
            const CollisionObjectConstPtr_t& object) = 0;

        virtual void filterCollisionPairs (const RelativeMotion::matrix_type& relMotion) = 0;
        virtual void setSecurityMargins(const matrix_t& securityMatrix) = 0;
    }; // class ObstacleUserInterface

    template<typename Derived>
    class HPP_CORE_DLLAPI ObstacleUserVector : public ObstacleUserInterface
    {
      public:
        virtual ~ObstacleUserVector () {}

        void addObstacle (const CollisionObjectConstPtr_t& object)
        {
          for (std::size_t i = 0; i < validations_.size(); ++i) {
            boost::shared_ptr<ObstacleUserInterface> oui =
              HPP_DYNAMIC_PTR_CAST(ObstacleUserInterface, validations_[i]);
            if (oui) oui->addObstacle (object);
          }
        }

        void removeObstacleFromJoint(const JointPtr_t& joint,
            const CollisionObjectConstPtr_t& object)
        {
          for (std::size_t i = 0; i < validations_.size(); ++i) {
            boost::shared_ptr<ObstacleUserInterface> oui =
              HPP_DYNAMIC_PTR_CAST(ObstacleUserInterface, validations_[i]);
            if (oui) oui->removeObstacleFromJoint (joint, object);
          }
        }

        void filterCollisionPairs (const RelativeMotion::matrix_type& relMotion)
        {
          for (std::size_t i = 0; i < validations_.size(); ++i) {
            boost::shared_ptr<ObstacleUserInterface> oui =
              HPP_DYNAMIC_PTR_CAST(ObstacleUserInterface, validations_[i]);
            if (oui) oui->filterCollisionPairs (relMotion);
          }
        }

        void setSecurityMargins(const matrix_t& securityMatrix)
        {
          for (std::size_t i = 0; i < validations_.size(); ++i) {
            boost::shared_ptr<ObstacleUserInterface> oui =
              HPP_DYNAMIC_PTR_CAST(ObstacleUserInterface, validations_[i]);
            if (oui) oui->setSecurityMargins (securityMatrix);
          }
        }

      protected:
        typedef Derived value_t;
        typedef std::vector<value_t> values_t;

        values_t validations_;
    }; // class ObstacleUserVector

    class HPP_CORE_DLLAPI ObstacleUser : public ObstacleUserInterface
    {
      public:
        virtual ~ObstacleUser () {}

        typedef std::pair<CollisionObjectConstPtr_t, CollisionObjectConstPtr_t> CollisionPair_t;
        typedef std::vector<CollisionPair_t> CollisionPairs_t;
        typedef std::vector<fcl::CollisionRequest> CollisionRequests_t;

        static bool collide (const CollisionPairs_t& pairs,
            const CollisionRequests_t& reqs,
            fcl::CollisionResult& res,
            std::size_t& i,
            pinocchio::DeviceData& data);

        // Get pairs checked for collision
        const CollisionPairs_t& pairs () const
        {
          return cPairs_;
        }

        // Get pairs checked for collision
        CollisionPairs_t& pairs ()
        {
          return cPairs_;
        }

        // Get requests of collision pairs
        const CollisionRequests_t& requests () const
        {
          return cRequests_;
        }

        // Get requests of collision pairs
        CollisionRequests_t& requests ()
        {
          return cRequests_;
        }

        fcl::CollisionRequest& defaultRequest()
        {
          return defaultRequest_;
        }

        void setRequests (const fcl::CollisionRequest& r);

        virtual void addObstacle (const CollisionObjectConstPtr_t& object);

        virtual void addObstacleToJoint (const CollisionObjectConstPtr_t& object,
                                         const JointPtr_t& joint, const bool includeChildren);

        virtual void removeObstacleFromJoint(const JointPtr_t& joint,
            const CollisionObjectConstPtr_t& object);

        virtual void filterCollisionPairs (const RelativeMotion::matrix_type& relMotion);

        virtual void setSecurityMargins(const matrix_t& securityMatrix);
      protected:
        ObstacleUser (DevicePtr_t robot)
          : robot_ (robot), defaultRequest_ (fcl::NO_REQUEST,1)
        {}

        ObstacleUser (const ObstacleUser& other)
          : robot_ (other.robot_),
            defaultRequest_ (other.defaultRequest_),
            cPairs_ (other.cPairs_),
            pPairs_ (other.pPairs_),
            dPairs_ (other.dPairs_),
            cRequests_ (other.cRequests_),
            pRequests_ (other.pRequests_),
            dRequests_ (other.dRequests_)
        {}

        void addRobotCollisionPairs ();

        DevicePtr_t robot_;
        fcl::CollisionRequest defaultRequest_;

        CollisionPairs_t cPairs_, 
                         pPairs_, 
                         dPairs_; 
        CollisionRequests_t cRequests_, 
                            pRequests_, 
                            dRequests_; 
    }; // class ObstacleUser
  } // namespace core
} // namespace hpp
#endif // HPP_CORE_OBSTACLE_USER_HH