From 7fb0fdb9a20df5114b90c528fa0a9ba7c786a748 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Tue, 6 Mar 2018 16:58:19 +0100
Subject: [PATCH] CrossStateOptimization handles WaypointEdge
---
src/steering-method/cross-state-optimization.cc | 15 ++++++++++++---
1 file changed, 12 insertions(+), 3 deletions(-)
diff --git a/src/steering-method/cross-state-optimization.cc b/src/steering-method/cross-state-optimization.cc
index 83bef2b..2cc7e36 100644
--- a/src/steering-method/cross-state-optimization.cc
+++ b/src/steering-method/cross-state-optimization.cc
@@ -185,12 +185,20 @@ namespace hpp {
if (i >= roots.size()) return transitions;
const GraphSearchData::state_with_depth* current = &roots[i];
- transitions.resize (current->l);
+ transitions.reserve (current->l);
+ graph::WaypointEdgePtr_t we;
while (current->e) {
assert (current->l > 0);
- transitions[current->l-1] = current->e;
+ we = HPP_DYNAMIC_PTR_CAST(graph::WaypointEdge, current->e);
+ if (we) {
+ for (int i = (int)we->nbWaypoints(); i >= 0; --i)
+ transitions.push_back(we->waypoint(i));
+ } else {
+ transitions.push_back(current->e);
+ }
current = &d.parent1[current->s][current->i];
}
+ std::reverse (transitions.begin(), transitions.end());
return transitions;
}
@@ -394,7 +402,8 @@ namespace hpp {
solver.solve <constraints::lineSearch::FixedSequence> (q);
bool success = (status == constraints::HierarchicalIterativeSolver::SUCCESS);
if (!success) {
- hppDout (warning, "Solution from projection "
+ hppDout (warning, "Projection failed with status " << status <<
+ ". Configuration after projection is\n"
<< pinocchio::displayConfig(q));
}
return success;
--
GitLab