diff --git a/src/steering-method/cross-state-optimization.cc b/src/steering-method/cross-state-optimization.cc index 83bef2bb41a234f11902394442e538961b9bc2d1..2cc7e36e90ac9c9ec3fd461273c67e55e9e0a78c 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;