From b3dfdcf89fdfc74f7fc7db1c6c242b2c4247e139 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Wed, 8 Jan 2020 16:45:47 +0100
Subject: [PATCH] Update to usage of ObstacleUser.
---
.../hpp/manipulation/graph-path-validation.hh | 28 +++++++++++++++----
src/graph-path-validation.cc | 5 ----
src/graph/edge.cc | 15 +++++++---
src/problem.cc | 15 ++++++----
4 files changed, 43 insertions(+), 20 deletions(-)
diff --git a/include/hpp/manipulation/graph-path-validation.hh b/include/hpp/manipulation/graph-path-validation.hh
index a27fce9..0958119 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 080e208..a916811 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 ffda441..7964663 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 002e0b0..aa18b9b 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;
--
GitLab