From 6301dfe1ec9aed30e1ac5baf82abfa736bc4c77c Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Wed, 29 Jun 2016 20:03:15 +0200
Subject: [PATCH] Use hpp-model function isApprox to compare 2 configurations
- in ManipulationPlanner::extend.
- the difference with eigen method isApprox is that quaternions and opposite
reprensent the same rotation.
---
src/manipulation-planner.cc | 4 +++-
1 file changed, 3 insertions(+), 1 deletion(-)
diff --git a/src/manipulation-planner.cc b/src/manipulation-planner.cc
index a110edc..e785b02 100644
--- a/src/manipulation-planner.cc
+++ b/src/manipulation-planner.cc
@@ -220,6 +220,8 @@ namespace hpp {
{
graph::GraphPtr_t graph = problem_.constraintGraph ();
PathProjectorPtr_t pathProjector = problem_.pathProjector ();
+ model::DevicePtr_t robot (problem_.robot ());
+ value_type eps (graph->errorThreshold ());
// Select next node in the constraint graph.
const ConfigurationPtr_t q_near = n_near->configuration ();
HPP_START_TIMECOUNTER (chooseEdge);
@@ -237,7 +239,7 @@ namespace hpp {
es.addFailure (reasons_[PROJECTION]);
return false;
}
- if (qProj_.isApprox (*q_near)) {
+ if (model::isApprox (robot, qProj_, *q_near, eps)) {
es.addFailure (reasons_[FAILURE]);
es.addFailure (reasons_[PATH_PROJECTION_ZERO]);
return false;
--
GitLab