From 99f54fee3fe0623e5d35656063d04d9a344c9707 Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Fri, 22 Nov 2024 17:01:31 -0600 Subject: [PATCH 1/4] Add ability to save graph as a dot graph --- .../solvers/ladder_graph/ladder_graph.h | 68 ++++++++++++++++++- 1 file changed, 66 insertions(+), 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 index 75182a83..8db2cbb1 100644 --- 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 @@ -26,6 +26,7 @@ DESCARTES_IGNORE_WARNINGS_PUSH #include #include #include +#include DESCARTES_IGNORE_WARNINGS_POP namespace descartes_core @@ -157,6 +158,68 @@ class LadderGraph */ void clear(); + void toDotGraph(std::string filepath, const std::vector& path = std::vector()) const + { + std::ofstream out(filepath); + if (!out) + return; + + out << "digraph ladder {\n"; + out << " rankdir=LR;\n"; + out << " node [shape=circle];\n"; + + // Create subgraphs for each rung + for (size_t rung_idx = 0; rung_idx < rungs_.size(); ++rung_idx) { + out << " subgraph cluster_" << rung_idx << " {\n"; + out << " label=\"Rung " << rung_idx << "\";\n"; + out << " style=dotted;\n"; + + const auto& rung = rungs_[rung_idx]; + for (size_t node_idx = 0; node_idx < rung.nodes.size(); ++node_idx) { + std::string node_name = "r" + std::to_string(rung_idx) + "n" + std::to_string(node_idx); + + // Fix: Properly escape quotes and handle label closing + out << " " << node_name << " [label=\"" << node_idx; + if (rung.nodes[node_idx].sample.cost >= 0) + out << "\\nCost: " << rung.nodes[node_idx].sample.cost; + + out << "\\nState: "; + if (rung.nodes[node_idx].sample.state) + for (Eigen::Index i = 0; i < rung.nodes[node_idx].sample.state->values.size(); ++i) + out << std::fixed << std::setprecision(3) << rung.nodes[node_idx].sample.state->values[i] << " "; + + if (!path.empty() && rung_idx < path.size() && path[rung_idx] == node_idx) + out << "\",color=green,style=filled];\n"; + else + out << "\"];\n"; + } + out << " }\n"; + + // Create edges to next rung + if (rung_idx < rungs_.size() - 1) { + for (size_t node_idx = 0; node_idx < rung.nodes.size(); ++node_idx) { + const auto& node = rung.nodes[node_idx]; + std::string from_node = "r" + std::to_string(rung_idx) + "n" + std::to_string(node_idx); + + for (const auto& edge : node.edges) { + std::string to_node = "r" + std::to_string(rung_idx + 1) + "n" + std::to_string(edge.idx); + // Fix: Properly format edge labels + if (!path.empty() && rung_idx + 1 < path.size() && + path[rung_idx] == node_idx && path[rung_idx + 1] == edge.idx) + out << " " << from_node << " -> " << to_node + << " [label=\"" << edge.cost << "\",color=green,penwidth=2.0];\n"; + else + out << " " << from_node << " -> " << to_node + << " [label=\"" << edge.cost << "\"];\n"; + } + } + } + } + + out << "}\n"; + out.close(); + } + friend std::ostream& operator<<(std::ostream& out, const LadderGraph& ladder_graph) { out << "\nRung\t(Nodes)\t|# Outgoing Edges|\n"; @@ -192,9 +255,10 @@ class LadderGraph out << "Rung # " << failed_id << "\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] - << "\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"; } } out << "\n"; From 878dfc1acb2fc0ce2678c1309119b3ada133eea3 Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Fri, 22 Nov 2024 17:19:30 -0600 Subject: [PATCH 2/4] Clean up failed edges terminal output --- .../descartes_light/solvers/ladder_graph/ladder_graph.h | 8 ++++++-- 1 file changed, 6 insertions(+), 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 index 8db2cbb1..722397e5 100644 --- 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 node2 = rung2.nodes.front(); typename State::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"; } From 696a59fbf9537bb44375b58425b9335c8c60d047 Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Fri, 22 Nov 2024 17:19:52 -0600 Subject: [PATCH 3/4] Switch to box for nodes to condense graph vertically --- .../include/descartes_light/solvers/ladder_graph/ladder_graph.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index 722397e5..75def2f6 100644 --- 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 @@ -166,7 +166,7 @@ class LadderGraph out << "digraph ladder {\n"; out << " rankdir=LR;\n"; - out << " node [shape=circle];\n"; + out << " node [shape=box];\n"; // Create subgraphs for each rung for (size_t rung_idx = 0; rung_idx < rungs_.size(); ++rung_idx) { From 61702693cfae912e45a9590d666c32db049fb467 Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Mon, 16 Dec 2024 12:08:17 -0600 Subject: [PATCH 4/4] Move dotgraph implementation to .hpp file --- .../ladder_graph/impl/ladder_graph.hpp | 69 +++++++++++++++++++ .../solvers/ladder_graph/ladder_graph.h | 64 +---------------- 2 files changed, 71 insertions(+), 62 deletions(-) diff --git a/descartes_light/core/include/descartes_light/solvers/ladder_graph/impl/ladder_graph.hpp b/descartes_light/core/include/descartes_light/solvers/ladder_graph/impl/ladder_graph.hpp index cb737b11..38e9a049 100644 --- a/descartes_light/core/include/descartes_light/solvers/ladder_graph/impl/ladder_graph.hpp +++ b/descartes_light/core/include/descartes_light/solvers/ladder_graph/impl/ladder_graph.hpp @@ -20,6 +20,7 @@ #include #include +#include namespace descartes_light { @@ -128,6 +129,74 @@ void LadderGraph::clear() rungs_.clear(); } +template +void LadderGraph::toDotGraph(std::string filepath, + const std::vector& path) const +{ + std::ofstream out(filepath); + if (!out) + return; + + out << "digraph ladder {\n"; + out << " rankdir=LR;\n"; + out << " node [shape=box];\n"; + + // Create subgraphs for each rung + for (size_t rung_idx = 0; rung_idx < rungs_.size(); ++rung_idx) + { + out << " subgraph cluster_" << rung_idx << " {\n"; + out << " label=\"Rung " << rung_idx << "\";\n"; + out << " style=dotted;\n"; + + const auto& rung = rungs_[rung_idx]; + for (size_t node_idx = 0; node_idx < rung.nodes.size(); ++node_idx) + { + std::string node_name = "r" + std::to_string(rung_idx) + "n" + std::to_string(node_idx); + + // Fix: Properly escape quotes and handle label closing + out << " " << node_name << " [label=\"" << node_idx; + if (rung.nodes[node_idx].sample.cost >= 0) + out << "\\nCost: " << rung.nodes[node_idx].sample.cost; + + out << "\\nState: "; + if (rung.nodes[node_idx].sample.state) + for (Eigen::Index i = 0; i < rung.nodes[node_idx].sample.state->values.size(); ++i) + out << std::fixed << std::setprecision(3) << rung.nodes[node_idx].sample.state->values[i] << " "; + + if (!path.empty() && rung_idx < path.size() && path[rung_idx] == node_idx) + out << "\",color=green,style=filled];\n"; + else + out << "\"];\n"; + } + out << " }\n"; + + // Create edges to next rung + if (rung_idx < rungs_.size() - 1) + { + for (size_t node_idx = 0; node_idx < rung.nodes.size(); ++node_idx) + { + const auto& node = rung.nodes[node_idx]; + std::string from_node = "r" + std::to_string(rung_idx) + "n" + std::to_string(node_idx); + + for (const auto& edge : node.edges) + { + std::string to_node = "r" + std::to_string(rung_idx + 1) + "n" + std::to_string(edge.idx); + // Fix: Properly format edge labels + if (!path.empty() && rung_idx + 1 < path.size() && path[rung_idx] == node_idx && + path[rung_idx + 1] == edge.idx) + out << " " << from_node << " -> " << to_node << " [label=\"" << edge.cost + << "\",color=green,penwidth=2.0];\n"; + else + out << " " << from_node << " -> " << to_node << " [label=\"" << edge.cost << "\"];\n"; + } + } + } + } + + out << "}\n"; + out.close(); +} + } // namespace descartes_light #endif // DESCARTES_LIGHT_IMPL_LADDER_GRAPH_HPP 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 index 75def2f6..cfef1a95 100644 --- 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 @@ -26,7 +26,7 @@ DESCARTES_IGNORE_WARNINGS_PUSH #include #include #include -#include +#include DESCARTES_IGNORE_WARNINGS_POP namespace descartes_core @@ -158,67 +158,7 @@ class LadderGraph */ void clear(); - void toDotGraph(std::string filepath, const std::vector& path = std::vector()) const - { - std::ofstream out(filepath); - if (!out) - return; - - out << "digraph ladder {\n"; - out << " rankdir=LR;\n"; - out << " node [shape=box];\n"; - - // Create subgraphs for each rung - for (size_t rung_idx = 0; rung_idx < rungs_.size(); ++rung_idx) { - out << " subgraph cluster_" << rung_idx << " {\n"; - out << " label=\"Rung " << rung_idx << "\";\n"; - out << " style=dotted;\n"; - - const auto& rung = rungs_[rung_idx]; - for (size_t node_idx = 0; node_idx < rung.nodes.size(); ++node_idx) { - std::string node_name = "r" + std::to_string(rung_idx) + "n" + std::to_string(node_idx); - - // Fix: Properly escape quotes and handle label closing - out << " " << node_name << " [label=\"" << node_idx; - if (rung.nodes[node_idx].sample.cost >= 0) - out << "\\nCost: " << rung.nodes[node_idx].sample.cost; - - out << "\\nState: "; - if (rung.nodes[node_idx].sample.state) - for (Eigen::Index i = 0; i < rung.nodes[node_idx].sample.state->values.size(); ++i) - out << std::fixed << std::setprecision(3) << rung.nodes[node_idx].sample.state->values[i] << " "; - - if (!path.empty() && rung_idx < path.size() && path[rung_idx] == node_idx) - out << "\",color=green,style=filled];\n"; - else - out << "\"];\n"; - } - out << " }\n"; - - // Create edges to next rung - if (rung_idx < rungs_.size() - 1) { - for (size_t node_idx = 0; node_idx < rung.nodes.size(); ++node_idx) { - const auto& node = rung.nodes[node_idx]; - std::string from_node = "r" + std::to_string(rung_idx) + "n" + std::to_string(node_idx); - - for (const auto& edge : node.edges) { - std::string to_node = "r" + std::to_string(rung_idx + 1) + "n" + std::to_string(edge.idx); - // Fix: Properly format edge labels - if (!path.empty() && rung_idx + 1 < path.size() && - path[rung_idx] == node_idx && path[rung_idx + 1] == edge.idx) - out << " " << from_node << " -> " << to_node - << " [label=\"" << edge.cost << "\",color=green,penwidth=2.0];\n"; - else - out << " " << from_node << " -> " << to_node - << " [label=\"" << edge.cost << "\"];\n"; - } - } - } - } - - out << "}\n"; - out.close(); - } + void toDotGraph(std::string filepath, const std::vector& path = std::vector()) const; friend std::ostream& operator<<(std::ostream& out, const LadderGraph& ladder_graph) {