From 2fe99cf9324a66e7a726571593156638f3f8375a Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Mon, 22 Aug 2016 10:04:05 +0200
Subject: [PATCH] Add a method that computes the error of a config wrt a leaf.
- Graph::getConfigErrorForEdgeLeaf.
---
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 6169b16..dd8b006 100644
--- a/include/hpp/manipulation/graph/graph.hh
+++ b/include/hpp/manipulation/graph/graph.hh
@@ -112,6 +112,20 @@ namespace hpp {
bool getConfigErrorForEdge (ConfigurationIn_t config,
const EdgePtr_t& edge, vector_t& error);
+ /// Get error of a config with respect to 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 getConfigErrorForEdgeLeaf
+ (ConfigurationIn_t leafConfig, ConfigurationIn_t config,
+ const EdgePtr_t& edge, vector_t& error);
+
/// 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 d76e518..a2c3ed1 100644
--- a/src/graph/graph.cc
+++ b/src/graph/graph.cc
@@ -131,6 +131,16 @@ namespace hpp {
return cs->isSatisfied (config, error);
}
+ bool Graph::getConfigErrorForEdgeLeaf
+ (ConfigurationIn_t leafConfig, ConfigurationIn_t config,
+ const EdgePtr_t& edge, vector_t& error)
+ {
+ ConstraintSetPtr_t cs (pathConstraint (edge));
+ ConfigProjectorPtr_t cp (cs->configProjector ());
+ if (cp) cp->rightHandSideFromConfig (leafConfig);
+ return cs->isSatisfied (config, error);
+ }
+
ConstraintSetPtr_t Graph::configConstraint (const EdgePtr_t& edge)
{
return edge->configConstraint ();
--
GitLab