From 12853855f19f007cf3e5ea139cff7ef00fb5e509 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Wed, 11 May 2016 17:20:29 +0200
Subject: [PATCH] Add debug output and fix packaging issues
---
src/graph-path-validation.cc | 1 +
src/graph-steering-method.cc | 3 +++
src/graph/edge.cc | 36 ++++++++++++++++++++++++++++--------
src/problem-solver.cc | 4 +---
src/problem.cc | 4 ++++
5 files changed, 37 insertions(+), 11 deletions(-)
diff --git a/src/graph-path-validation.cc b/src/graph-path-validation.cc
index ffd704c..baa5183 100644
--- a/src/graph-path-validation.cc
+++ b/src/graph-path-validation.cc
@@ -91,6 +91,7 @@ namespace hpp {
PathPtr_t pathNoCollision;
ConstraintSetPtr_t c = HPP_DYNAMIC_PTR_CAST(ConstraintSet, path->constraints());
+ hppDout(info, (c?"Using edge path validation":"Using default path validation"));
PathValidationPtr_t validation (c
? c->edge()->pathValidation()
: pathValidation_);
diff --git a/src/graph-steering-method.cc b/src/graph-steering-method.cc
index 381d301..1094f82 100644
--- a/src/graph-steering-method.cc
+++ b/src/graph-steering-method.cc
@@ -74,6 +74,9 @@ namespace hpp {
return PathPtr_t ();
}
PathPtr_t path;
+ if (possibleEdges.empty()) {
+ hppDout (info, "No edge found.");
+ }
while (!possibleEdges.empty()) {
if (possibleEdges.back ()->build (path, q1, q2)) {
return path;
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index 43c142e..9e9ee77 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -278,11 +278,15 @@ namespace hpp {
{
ConstraintSetPtr_t constraints = pathConstraint ();
constraints->configProjector ()->rightHandSideFromConfig(q1);
- if (!constraints->isSatisfied (q1) || !constraints->isSatisfied (q2)) {
- return false;
+ if (constraints->isSatisfied (q1)) {
+ if (constraints->isSatisfied (q2)) {
+ path = (*steeringMethod_->get()) (q1, q2);
+ return true;
+ }
+ hppDout(info, "q2 does not satisfy the constraints");
}
- path = (*steeringMethod_->get()) (q1, q2);
- return true;
+ hppDout(info, "q1 does not satisfy the constraints");
+ return false;
}
bool Edge::applyConstraints (core::NodePtr_t nnear, ConfigurationOut_t q) const
@@ -346,19 +350,35 @@ namespace hpp {
if (!useCache) configs_.col (0) = q2;
assert (waypoints_[0].first);
- if (!waypoints_[0].first->applyConstraints (q1, configs_.col (0)))
+ if (!waypoints_[0].first->applyConstraints (q1, configs_.col (0))) {
+ hppDout (info, "Waypoint edge " << name() << ": applyConstraints failed at waypoint 0."
+ << "\nUse cache: " << useCache
+ );
return false;
- if (!waypoints_[0].first->build (p, q1, configs_.col (0)))
+ }
+ if (!waypoints_[0].first->build (p, q1, configs_.col (0))) {
+ hppDout (info, "Waypoint edge " << name() << ": build failed at waypoint 0."
+ << "\nUse cache: " << useCache
+ );
return false;
+ }
pv->appendPath (p);
for (std::size_t i = 1; i < waypoints_.size (); ++i) {
assert (waypoints_[i].first);
if (!useCache) configs_.col (i) = q2;
- if (!waypoints_[i].first->applyConstraints (configs_.col(i-1), configs_.col (i)))
+ if (!waypoints_[i].first->applyConstraints (configs_.col(i-1), configs_.col (i))) {
+ hppDout (info, "Waypoint edge " << name() << ": applyConstraints failed at waypoint " << i << "."
+ << "\nUse cache: " << useCache
+ );
return false;
- if (!waypoints_[i].first->build (p, configs_.col(i-1), configs_.col (i)))
+ }
+ if (!waypoints_[i].first->build (p, configs_.col(i-1), configs_.col (i))) {
+ hppDout (info, "Waypoint edge " << name() << ": build failed at waypoint " << i << "."
+ << "\nUse cache: " << useCache
+ );
return false;
+ }
pv->appendPath (p);
}
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index 8c3b434..e3dca54 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -27,8 +27,6 @@
#include <hpp/constraints/convex-shape-contact.hh>
#include <hpp/core/random-shortcut.hh>
-#include <hpp/core/discretized-collision-checking.hh>
-#include <hpp/core/continuous-collision-checking/progressive.hh>
#include <hpp/core/path-optimization/partial-shortcut.hh>
#include <hpp/core/roadmap.hh>
#include <hpp/core/steering-method-straight.hh>
@@ -49,7 +47,7 @@
#include "hpp/manipulation/graph-steering-method.hh"
#include "hpp/manipulation/path-optimization/config-optimization.hh"
-#ifdef HPP_MANIPULATION_HAS_WHOLEBODY_STEP
+#if HPP_MANIPULATION_HAS_WHOLEBODY_STEP
#include <hpp/wholebody-step/small-steps.hh>
#include "hpp/manipulation/path-optimization/small-steps.hh"
#endif
diff --git a/src/problem.cc b/src/problem.cc
index 155bb89..fba71b7 100644
--- a/src/problem.cc
+++ b/src/problem.cc
@@ -15,6 +15,9 @@
// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.
#include <hpp/manipulation/problem.hh>
+
+#include <hpp/core/discretized-collision-checking.hh>
+
#include <hpp/manipulation/weighed-distance.hh>
#include <hpp/manipulation/graph-steering-method.hh>
@@ -25,6 +28,7 @@ namespace hpp {
{
Parent::steeringMethod (GraphSteeringMethod::create (this));
distance (WeighedDistance::create (robot, graph_));
+ setPathValidationFactory(core::DiscretizedCollisionChecking::create, 0.05);
}
void Problem::constraintGraph (const graph::GraphPtr_t& graph)
--
GitLab