Clean up failed edges terminal output

swri-robotics · Nov 22, 2024 · 878dfc1 · 878dfc1
1 parent 99f54fe
commit 878dfc1
Showing 1 changed file with 6 additions and 2 deletions.
diff --git a/descartes_light/core/include/descartes_light/solvers/ladder_graph/ladder_graph.h b/descartes_light/core/include/descartes_light/solvers/ladder_graph/ladder_graph.h
@@ -253,10 +253,14 @@ class LadderGraph
         Node<FloatType> node2 = rung2.nodes.front();
         typename State<FloatType>::ConstPtr state2 = node2.sample.state;
         out << "Rung # " << failed_id << "\n";
+        out << "\tNode 1\t|\tNode 2\t|\tDiff (Node2 - Node1)\n";
+        out << "\t---------------------------------------\n";
         for (Eigen::Index i = 0; i < state1->values.rows(); i++)
         {
-            out << std::setprecision(4) << std::fixed << "\t" << state1->values[i] << "\t|\t" << state2->values[i] 
-              << "\t|\t" << state2->values[i] - state1->values[i] << "\n";
+            out << std::setprecision(4) << std::fixed 
+          << "\t" << state1->values[i] 
+          << "\t|\t" << state2->values[i] 
+          << "\t|\t" << state2->values[i] - state1->values[i] << "\n";
         }
         out << "\tTotal L2 Norm: " << (state2->values - state1->values).norm() << "\n";
       }