From 42575d2a8fa1fbbf632b260bd7f9aa429f576b37 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Wed, 25 May 2016 20:45:58 +0200
Subject: [PATCH] graph::Edge now stores the RelativeMotion matrix
---
include/hpp/manipulation/graph/edge.hh | 9 +++++++++
src/graph/edge.cc | 7 +++----
2 files changed, 12 insertions(+), 4 deletions(-)
diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh
index 159b1ade..9a25a670 100644
--- a/include/hpp/manipulation/graph/edge.hh
+++ b/include/hpp/manipulation/graph/edge.hh
@@ -19,6 +19,7 @@
#include <hpp/core/constraint-set.hh>
#include <hpp/core/steering-method.hh>
+#include <hpp/core/relative-motion.hh>
#include <hpp/core/path.hh>
#include "hpp/manipulation/config.hh"
@@ -59,6 +60,8 @@ namespace hpp {
class HPP_MANIPULATION_DLLAPI Edge : public GraphComponent
{
public:
+ typedef core::RelativeMotion RelativeMotion;
+
/// Destructor
virtual ~Edge ();
@@ -120,6 +123,11 @@ namespace hpp {
return pathValidation_->get();
}
+ const RelativeMotion::matrix_type& relativeMotion () const
+ {
+ return relMotion_;
+ }
+
/// Get direction of the path compare to the edge
/// \return true is reverse
virtual bool direction (const core::PathPtr_t& path) const;
@@ -187,6 +195,7 @@ namespace hpp {
SteeringMethod_t* steeringMethod_;
/// Path validation associated to the edge
+ mutable RelativeMotion::matrix_type relMotion_;
PathValidation_t* pathValidation_;
/// Weak pointer to itself.
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index 9e9ee772..4e8c30ab 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -254,10 +254,9 @@ namespace hpp {
// TODO this path validation will not contain obstacles added after
// its creation.
pathValidation_->set(problem->pathValidationFactory ());
- using core::RelativeMotion;
- RelativeMotion::matrix_type matrix (RelativeMotion::matrix (g->robot()));
- RelativeMotion::fromConstraint (matrix, g->robot(), constraint);
- pathValidation_->get()->filterCollisionPairs (matrix);
+ relMotion_ = RelativeMotion::matrix (g->robot());
+ RelativeMotion::fromConstraint (relMotion_, g->robot(), constraint);
+ pathValidation_->get()->filterCollisionPairs (relMotion_);
return constraint;
}
--
GitLab