From 3553fb20d7674b8a9c8ca17e98194100a49df4e5 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Mon, 14 Dec 2015 17:13:53 +0100
Subject: [PATCH] Add Edge::canConnect
---
include/hpp/manipulation/graph/edge.hh | 4 ++++
src/graph/edge.cc | 16 ++++++++++++++++
2 files changed, 20 insertions(+)
diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh
index ace1836c..4b5f3286 100644
--- a/include/hpp/manipulation/graph/edge.hh
+++ b/include/hpp/manipulation/graph/edge.hh
@@ -73,6 +73,8 @@ namespace hpp {
virtual bool applyConstraints (ConfigurationIn_t qoffset, ConfigurationOut_t q) const;
+ virtual bool canConnect (ConfigurationIn_t q1, ConfigurationIn_t q2) const;
+
virtual bool build (core::PathPtr_t& path, ConfigurationIn_t q1,
ConfigurationIn_t q2) const;
@@ -204,6 +206,8 @@ namespace hpp {
virtual bool direction (const core::PathPtr_t& path) const;
+ virtual bool canConnect (ConfigurationIn_t q1, ConfigurationIn_t q2) const;
+
virtual bool build (core::PathPtr_t& path, ConfigurationIn_t q1, ConfigurationIn_t q2) const;
virtual bool applyConstraints (ConfigurationIn_t qoffset, ConfigurationOut_t q) const;
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index 402857b7..1c2089f9 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -256,6 +256,17 @@ namespace hpp {
return constraint;
}
+ bool Edge::canConnect (ConfigurationIn_t q1, ConfigurationIn_t q2)
+ const
+ {
+ ConstraintSetPtr_t constraints = pathConstraint ();
+ constraints->configProjector ()->rightHandSideFromConfig(q1);
+ if (!constraints->isSatisfied (q1) || !constraints->isSatisfied (q2)) {
+ return false;
+ }
+ return true;
+ }
+
bool Edge::build (core::PathPtr_t& path, ConfigurationIn_t q1,
ConfigurationIn_t q2)
const
@@ -311,6 +322,11 @@ namespace hpp {
wkPtr_ = weak;
}
+ bool WaypointEdge::canConnect (ConfigurationIn_t q1, ConfigurationIn_t q2) const
+ {
+ return waypoint_.first->canConnect (q1, q2) && Edge::canConnect (q1, q2);
+ }
+
bool WaypointEdge::build (core::PathPtr_t& path, ConfigurationIn_t q1,
ConfigurationIn_t q2) const
{
--
GitLab