From b57faedda7f8f7141b07b685c58e22573068a3ef Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Wed, 24 Jan 2018 17:55:56 +0100
Subject: [PATCH] Add Graph::getConfigErrorForEdgeTarget
---
include/hpp/manipulation/graph/graph.hh | 14 ++++++++++++++
src/graph/graph.cc | 10 ++++++++++
2 files changed, 24 insertions(+)
diff --git a/include/hpp/manipulation/graph/graph.hh b/include/hpp/manipulation/graph/graph.hh
index a7314ea..9d1b67c 100644
--- a/include/hpp/manipulation/graph/graph.hh
+++ b/include/hpp/manipulation/graph/graph.hh
@@ -201,6 +201,20 @@ namespace hpp {
(ConfigurationIn_t leafConfig, ConfigurationIn_t config,
const EdgePtr_t& edge, vector_t& error) const;
+ /// Get error of a config with respect to the target of an edge foliation leaf
+ ///
+ /// \param leafConfig Configuration that determines the foliation leaf
+ /// \param config Configuration the error of which is computed
+ /// \retval error the error
+ /// \return whether config can be the end point of a path of the edge
+ /// starting at leafConfig
+ /// Call methods core::ConfigProjector::rightHandSideFromConfig with
+ /// leafConfig and then core::ConstraintSet::isSatisfied with config.
+ /// on the edge constraints.
+ bool getConfigErrorForEdgeTarget
+ (ConfigurationIn_t leafConfig, ConfigurationIn_t config,
+ const EdgePtr_t& edge, vector_t& error) const;
+
/// Constraint to project a path.
/// \param edge a list of edges defining the foliation.
/// \return The constraint.
diff --git a/src/graph/graph.cc b/src/graph/graph.cc
index 7d397a9..f50d5af 100644
--- a/src/graph/graph.cc
+++ b/src/graph/graph.cc
@@ -204,6 +204,16 @@ namespace hpp {
return cs->isSatisfied (config, error);
}
+ bool Graph::getConfigErrorForEdgeTarget
+ (ConfigurationIn_t leafConfig, ConfigurationIn_t config,
+ const EdgePtr_t& edge, vector_t& error) const
+ {
+ ConstraintSetPtr_t cs (configConstraint (edge));
+ ConfigProjectorPtr_t cp (cs->configProjector ());
+ if (cp) cp->rightHandSideFromConfig (leafConfig);
+ return cs->isSatisfied (config, error);
+ }
+
bool Graph::getConfigErrorForEdgeLeaf
(ConfigurationIn_t leafConfig, ConfigurationIn_t config,
const EdgePtr_t& edge, vector_t& error) const
--
GitLab