diff --git a/include/hpp/manipulation/graph-path-validation.hh b/include/hpp/manipulation/graph-path-validation.hh index a27fce93cb0ed7676558a10f8877c9c717c8ab16..0958119e1b753c815c44c241765c1c4efdac5246 100644 --- a/include/hpp/manipulation/graph-path-validation.hh +++ b/include/hpp/manipulation/graph-path-validation.hh @@ -19,6 +19,7 @@ # define HPP_MANIPULATION_GRAPHPATHVALIDATOR_HH # include <hpp/core/path-validation.hh> +# include <hpp/core/obstacle-user.hh> # include <hpp/manipulation/fwd.hh> # include <hpp/manipulation/config.hh> @@ -42,7 +43,9 @@ namespace hpp { /// The encapsulated path validation is responsible for collision /// checking, whereas this class checks if a path is valid regarding /// the constraint graph. - class HPP_MANIPULATION_DLLAPI GraphPathValidation : public PathValidation + class HPP_MANIPULATION_DLLAPI GraphPathValidation : + public PathValidation, + public core::ObstacleUserInterface { public: /// Check that path is valid regarding the constraint graph. @@ -95,7 +98,12 @@ namespace hpp { static GraphPathValidationPtr_t create ( const pinocchio::DevicePtr_t& robot, const value_type& stepSize); - void addObstacle (const hpp::core::CollisionObjectPtr_t&); + void addObstacle (const hpp::core::CollisionObjectConstPtr_t& object) + { + boost::shared_ptr<core::ObstacleUserInterface> oui = + HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_); + if (oui) oui->addObstacle (object); + } /// Remove a collision pair between a joint and an obstacle /// \param joint the joint that holds the inner objects, @@ -103,11 +111,19 @@ namespace hpp { /// \notice collision configuration validation needs to know about /// obstacles. This virtual method does nothing for configuration /// validation methods that do not care about obstacles. - virtual void removeObstacleFromJoint (const JointPtr_t& joint, - const pinocchio::CollisionObjectPtr_t& obstacle) + void removeObstacleFromJoint (const JointPtr_t& joint, + const pinocchio::CollisionObjectConstPtr_t& obstacle) + { + boost::shared_ptr<core::ObstacleUserInterface> oui = + HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_); + if (oui) oui->removeObstacleFromJoint (joint, obstacle); + } + + void filterCollisionPairs (const core::RelativeMotion::matrix_type& relMotion) { - assert (pathValidation_); - pathValidation_->removeObstacleFromJoint (joint, obstacle); + boost::shared_ptr<core::ObstacleUserInterface> oui = + HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_); + if (oui) oui->filterCollisionPairs (relMotion); } protected: diff --git a/src/graph-path-validation.cc b/src/graph-path-validation.cc index 080e2087f5e6031a91d9ff1d169dbb868d638545..a916811c940341df21619b2bc619ad5707b8c468 100644 --- a/src/graph-path-validation.cc +++ b/src/graph-path-validation.cc @@ -193,10 +193,5 @@ namespace hpp { validPart = pathNoCollision; return false; } - - void GraphPathValidation::addObstacle (const hpp::core::CollisionObjectPtr_t& collisionObject) - { - pathValidation_->addObstacle (collisionObject); - } } // namespace manipulation } // namespace hpp diff --git a/src/graph/edge.cc b/src/graph/edge.cc index ffda441963529454c9dead496a8906796400865e..79646637707607ab48588ad08a330e37b88cda1a 100644 --- a/src/graph/edge.cc +++ b/src/graph/edge.cc @@ -22,6 +22,7 @@ #include <hpp/util/exception-factory.hh> #include <hpp/pinocchio/configuration.hh> +#include <hpp/core/obstacle-user.hh> #include <hpp/core/path-vector.hh> #include <hpp/core/path-validation.hh> @@ -61,7 +62,9 @@ namespace hpp { void Edge::relativeMotion(const RelativeMotion::matrix_type & m) { if(!isInit_) throw std::logic_error("The graph must be initialized before changing the relative motion matrix."); - pathValidation_->filterCollisionPairs(m); + boost::shared_ptr<core::ObstacleUserInterface> oui = + HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_); + if (oui) oui->filterCollisionPairs(m); relMotion_ = m; } @@ -285,9 +288,13 @@ namespace hpp { // TODO this path validation will not contain obstacles added after // its creation. pathValidation_ = problem->pathValidationFactory (); - relMotion_ = RelativeMotion::matrix (g->robot()); - RelativeMotion::fromConstraint (relMotion_, g->robot(), constraint); - pathValidation_->filterCollisionPairs (relMotion_); + boost::shared_ptr<core::ObstacleUserInterface> oui = + HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_); + if (oui) { + relMotion_ = RelativeMotion::matrix (g->robot()); + RelativeMotion::fromConstraint (relMotion_, g->robot(), constraint); + oui->filterCollisionPairs (relMotion_); + } return constraint; } diff --git a/src/problem.cc b/src/problem.cc index 002e0b0a468ec00acb51612d95b98279f0a6d097..aa18b9b8799822bc1d5ab19c44122e1ad26a6c8e 100644 --- a/src/problem.cc +++ b/src/problem.cc @@ -74,11 +74,16 @@ namespace hpp { PathValidationPtr_t Problem::pathValidationFactory () const { PathValidationPtr_t pv (pvFactory_ (robot(), pvTol_)); - const core::ObjectStdVector_t& obstacles (collisionObstacles ()); - // Insert obstacles in path validation object - for (core::ObjectStdVector_t::const_iterator _obs = obstacles.begin (); - _obs != obstacles.end (); ++_obs) - pv->addObstacle (*_obs); + + boost::shared_ptr<core::ObstacleUserInterface> oui = + HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pv); + if (oui) { + const core::ObjectStdVector_t& obstacles (collisionObstacles ()); + // Insert obstacles in path validation object + for (core::ObjectStdVector_t::const_iterator _obs = obstacles.begin (); + _obs != obstacles.end (); ++_obs) + oui->addObstacle (*_obs); + } GraphPathValidationPtr_t gpv = HPP_DYNAMIC_PTR_CAST(GraphPathValidation, pv); if (gpv) return gpv->innerValidation(); return pv;