obstacle-user.hh

Go to the documentation of this file.

// Copyright (c) 2019 CNRS
// Authors: Joseph Mirabel
//
 
// 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_OBSTACLE_USER_HH
#define HPP_CORE_OBSTACLE_USER_HH
 
#include <coal/collision_data.h>
 
#include <hpp/core/collision-pair.hh>
#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() = default;
 
  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;
 
  virtual void setSecurityMarginBetweenBodies(const std::string& body_a,
                                              const std::string& body_b,
                                              const value_type& margin) = 0;
};  // class ObstacleUserInterface
 
template <typename Derived>
class HPP_CORE_DLLAPI ObstacleUserVector : public ObstacleUserInterface {
 public:
  virtual ~ObstacleUserVector() = default;
 
  void addObstacle(const CollisionObjectConstPtr_t& object) {
    for (std::size_t i = 0; i < validations_.size(); ++i) {
      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) {
      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) {
      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) {
      shared_ptr<ObstacleUserInterface> oui =
          HPP_DYNAMIC_PTR_CAST(ObstacleUserInterface, validations_[i]);
      if (oui) oui->setSecurityMargins(securityMatrix);
    }
  }
 
  void setSecurityMarginBetweenBodies(const std::string& body_a,
                                      const std::string& body_b,
                                      const value_type& margin) {
    for (std::size_t i = 0; i < validations_.size(); ++i) {
      shared_ptr<ObstacleUserInterface> oui =
          HPP_DYNAMIC_PTR_CAST(ObstacleUserInterface, validations_[i]);
      if (oui) oui->setSecurityMarginBetweenBodies(body_a, body_b, margin);
    }
  }
 
  // Clear the vector of config validations
  void clear() { validations_.clear(); }
 
 protected:
  typedef Derived value_t;
  typedef std::vector<value_t> values_t;
 
  ObstacleUserVector() = default;
  ObstacleUserVector(std::initializer_list<value_t> validations)
      : validations_(validations) {};
 
  values_t validations_;
};  // class ObstacleUserVector
 
class HPP_CORE_DLLAPI ObstacleUser : public ObstacleUserInterface {
 public:
  virtual ~ObstacleUser() = default;
 
  static bool collide(const CollisionPairs_t& pairs, CollisionRequests_t& reqs,
                      coal::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_; }
 
  coal::CollisionRequest& defaultRequest() { return defaultRequest_; }
 
  void setRequests(const coal::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);
 
  virtual void setSecurityMarginBetweenBodies(const std::string& body_a,
                                              const std::string& body_b,
                                              const value_type& margin);
 
 protected:
  ObstacleUser(DevicePtr_t robot)
      : robot_(robot), defaultRequest_(coal::NO_REQUEST, 1) {
    defaultRequest_.enable_cached_gjk_guess = true;
  }
 
  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_;
  coal::CollisionRequest defaultRequest_;
 
  CollisionPairs_t cPairs_,        
      pPairs_,                     
      dPairs_;                     
  CollisionRequests_t cRequests_,  
      pRequests_,                  
      dRequests_;                  
};  // class ObstacleUser
}  // namespace core
}  // namespace hpp
#endif  // HPP_CORE_OBSTACLE_USER_HH