From 5a357d1ae5a383833bb4a5d684fa2268f9831408 Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Tue, 14 Jun 2016 08:53:42 +0200
Subject: [PATCH] in ManipulationPlanner::extend, test that qProj is different
from q_near
This case may occur.
---
src/graph/edge.cc | 1 +
src/manipulation-planner.cc | 4 ++++
2 files changed, 5 insertions(+)
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index c51297f0..da9f99d2 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -286,6 +286,7 @@ namespace hpp {
constraints->configProjector ()->rightHandSideFromConfig(q1);
if (constraints->isSatisfied (q1)) {
if (constraints->isSatisfied (q2)) {
+ if (q1.isApprox (q2)) return false;
path = (*steeringMethod_->get()) (q1, q2);
return path;
}
diff --git a/src/manipulation-planner.cc b/src/manipulation-planner.cc
index aed7a214..eaa52388 100644
--- a/src/manipulation-planner.cc
+++ b/src/manipulation-planner.cc
@@ -245,6 +245,10 @@ namespace hpp {
es.addFailure (reasons_[PROJECTION]);
return false;
}
+ if (qProj_.isApprox (*q_near)) {
+ es.addFailure (reasons_[PATH_PROJECTION_ZERO]);
+ return false;
+ }
HPP_STOP_TIMECOUNTER (applyConstraints);
core::PathPtr_t path;
HPP_START_TIMECOUNTER (buildPath);
--
GitLab