Added Graphviz visualization

multibody/topology/link_joint_graph.h

@@ -4,6 +4,7 @@
 #error Do not include this file. Use "drake/multibody/topology/graph.h".
 #endif
 
+#include <filesystem>
 #include <map>
 #include <memory>
 #include <optional>
@@ -574,6 +575,33 @@ class LinkJointGraph {
   /** (Internal use only) For testing. */
   void DumpGraph(std::string title) const;
 
+  /** Generate a graphviz representation of this %LinkJointGraph, with the
+  given label at the top. Will include ephemeral elements if they are
+  available (that is, if the forest is valid)  unless suppressed explicitly.
+  The result is in the "dot" language, see https://graphviz.org. If you
+  write it to some file foo.dot, you can generate a viewable png (for
+  example) using the command `dot -Tpng foo.dot >foo.png`.
+  @see MakeGraphvizFiles() for an easier way to get pngs. */
+  std::string GenerateGraphvizString(
+      std::string_view label, bool include_ephemeral_elements = true) const;
+
+  /** This is a useful debugging and presentation utility for getting
+  viewable "dot diagrams" of the graph and forest. You provide a directory
+  and a base name for the results. This function will generate
+  `basename_graph.png` showing the graph as the user defined it. If the
+  forest has been built, it will also produce `basename_graph+.png` showing
+  the augmented graph with its ephemeral elements, and `basename_forest.png`
+  showing the spanning forest.
+  @param where The directory in which to put the files. If empty, the
+    current directory is used.
+  @param basename The base of the file names to be produced, see above.
+  @returns the absolute path of the directory into which the files were
+    created.
+  @throws std::exception if files can't be created, or the `dot` command
+    fails or isn't in /usr/bin, /usr/local/bin, or /bin. */
+  std::filesystem::path MakeGraphvizFiles(std::filesystem::path where,
+                                          std::string_view basename) const;
+
   // Forest building requires these joint types so they are predefined.
 
   /** The predefined index for the "weld" joint type's traits. */

multibody/topology/link_joint_graph_debug.cc

@@ -1,3 +1,6 @@
+#include <cstdlib>
+#include <filesystem>
+#include <fstream>
 #include <iostream>
 
 #include <fmt/format.h>
@@ -80,6 +83,197 @@ void LinkJointGraph::DumpGraph(std::string title) const {
   }
 }
 
+std::string LinkJointGraph::GenerateGraphvizString(
+    std::string_view label, bool include_ephemeral_elements) const {
+  const bool as_modeled = include_ephemeral_elements && forest_is_valid();
+  std::string graphviz = "digraph LinkJointGraph {\n";
+  graphviz += "rankdir=BT;\n";
+  graphviz += "labelloc=t;\n";
+  graphviz += fmt::format("label=\"{}\nLinkJointGraph{}\";\n", label,
+                          as_modeled ? "+" : "");
+
+  // Generate a legend.
+  graphviz += "legend [shape=none]\n";
+  graphviz +=
+      "[label=\""
+      "* = massless"
+      "\nL/J(i) link/joint(ordinal)"
+      "\nname:index";
+  if (as_modeled) graphviz += "\nred = ephemeral";
+  graphviz += "\"]\n";
+
+  // Link composites are discovered during forest building. If there
+  // is no forest, there will be no composites. But if there are composites
+  // we'd like to draw boxes around them so we'll process composited links
+  // first and then pick up the leftovers below.
+  for (const LinkComposite& composite : link_composites()) {
+    // Oddly, in order to get the box and label for a subgraph, the _name_
+    // of the subgraph must begin with "cluster"!
+    graphviz +=
+        fmt::format("subgraph cluster{}", &composite - &link_composites()[0]) +
+        " {\n";
+    graphviz += fmt::format("label=\"LinkComposite({}){}\";\n",
+                            &composite - &link_composites()[0],
+                            composite.is_massless ? "*" : "");
+    for (const BodyIndex& link_index : composite.links) {
+      const Link& link = link_by_index(link_index);
+      const bool ephemeral = link_is_ephemeral(link.index());
+      if (ephemeral && !include_ephemeral_elements) continue;
+      graphviz += fmt::format("link{} [label=\"L({}){} {}:{}\"]{};\n",
+                              link.ordinal(), link.ordinal(),
+                              link.is_massless() ? "*" : "", link.name(),
+                              link.index(), ephemeral ? "[color=red]" : "");
+    }
+    graphviz += "}\n";
+  }
+
+  // Now pick up the links that aren't in a composite.
+  for (const Link& link : links()) {
+    if (link.composite().has_value()) continue;
+    const bool ephemeral = link_is_ephemeral(link.index());
+    if (ephemeral && !include_ephemeral_elements) continue;
+    graphviz +=
+        fmt::format("link{} [label=\"L({}){} {}:{}\"]{};\n", link.ordinal(),
+                    link.ordinal(), link.is_massless() ? "*" : "", link.name(),
+                    link.index(), ephemeral ? "[color=red]" : "");
+  }
+
+  // Draw the joints as edges, with various representations. Note that a
+  // loop joint gets retargeted to a shadow body; we show that and also
+  // the original parent/child connection (as a dashed gray edge).
+  for (const Joint& joint : joints()) {
+    const JointTraits& traits = joint_traits(joint.traits_index());
+    const bool ephemeral = joint_is_ephemeral(joint.index());
+    if (ephemeral && !include_ephemeral_elements) continue;
+    const std::string color = ephemeral               ? "red"
+                              : traits.name == "weld" ? "black"
+                                                      : "green";
+
+    const std::string arrow_type = ephemeral               ? "empty"
+                                   : traits.name == "weld" ? "box"
+                                                           : "normal";
+    const std::string style = traits.name == "weld" ? "bold" : "solid";
+
+    const LinkOrdinal parent_ordinal =
+        link_by_index(joint.parent_link_index()).ordinal();
+    const LinkOrdinal child_ordinal =
+        link_by_index(joint.child_link_index()).ordinal();
+
+    // If this is a loop joint one of these links will be revised to
+    // be the shadow link (if we're generating the "as modeled" graph).
+    LinkOrdinal revised_parent_ordinal = parent_ordinal;
+    LinkOrdinal revised_child_ordinal = child_ordinal;
+    if (include_ephemeral_elements && joint.mobod_index().is_valid()) {
+      const SpanningForest::Mobod& mobod = forest().mobods(joint.mobod_index());
+      if (mobod.is_reversed())
+        revised_parent_ordinal = mobod.link_ordinal();
+      else
+        revised_child_ordinal = mobod.link_ordinal();
+    }
+
+    // Draw the effective joint connection.
+    graphviz += fmt::format(
+        "link{} -> link{} [arrowhead={}] [style={}] [fontsize=10] "
+        "[label=\"J({}) {}:{}\n{}\"] [color={}];\n",
+        revised_parent_ordinal, revised_child_ordinal, arrow_type, style,
+        joint.ordinal(), joint.name(), joint.index(),
+        joint_traits(joint.traits_index()).name, color);
+
+    // Draw the original joint connection if we retargeted it to a shadow.
+    if (revised_parent_ordinal != parent_ordinal ||
+        revised_child_ordinal != child_ordinal) {
+      graphviz += fmt::format(
+          "link{} -> link{} [arrowhead={}] [color=gray] [style=dashed] "
+          "[label=\"loop\nJ({})\"] [fontsize=10];\n",
+          parent_ordinal, child_ordinal, arrow_type, joint.ordinal());
+    }
+  }
+
+  // Draw the ephemeral weld constraints added to connect shadow to primary.
+  if (include_ephemeral_elements) {
+    for (const LoopConstraint& constraint : loop_constraints()) {
+      graphviz += fmt::format(
+          "link{} -> link{} [dir=both] [arrowhead=obox] [arrowtail=obox] "
+          "[style=bold] [fontsize=10] [label=\"WELD({})\n{}\"] [color=red]\n",
+          link_by_index(constraint.primary_link()).ordinal(),
+          link_by_index(constraint.shadow_link()).ordinal(), constraint.index(),
+          constraint.name());
+    }
+  }
+
+  graphviz += "}\n";
+  return graphviz;
+}
+
+std::filesystem::path LinkJointGraph::MakeGraphvizFiles(
+    std::filesystem::path where, const std::string_view basename) const {
+  // Find the "dot" command.
+  std::string dot;
+  if (std::filesystem::exists("/usr/bin/dot")) {
+    dot = "/usr/bin/dot";
+  } else if (std::filesystem::exists("/usr/local/bin/dot")) {
+    dot = "/usr/local/bin/dot";
+  } else if (std::filesystem::exists("/bin/dot")) {
+    dot = "/bin/dot";
+  } else {
+    throw std::runtime_error(fmt::format(
+        "{}(): Graphviz 'dot' is required but missing. It must be in "
+        "/usr/bin/dot, /usr/local/bin/dot, or /bin/dot. Install it in one "
+        "of those places if you want to use this function."
+        "See https://graphviz.org.",
+        __func__));
+  }
+
+  // Default to the current directory if none specified.
+  if (where.empty()) where = std::filesystem::current_path();
+
+  // This lambda creates one png given a name and generator. The dot file
+  // is removed after use.
+  auto MakePng = [&dot, &where, &basename](
+                     std::string_view suffix,
+                     std::function<std::string()> generate) {
+    // Create the dot file.
+    std::filesystem::path dotname(
+        std::filesystem::absolute(std::filesystem::path(where).append(
+            fmt::format("{}_{}.dot", basename, suffix))));
+    std::ofstream dotfile(dotname);
+    if (!dotfile.good()) {
+      throw std::runtime_error(fmt::format(
+          "MakeGraphvizFiles(): can't create file {}.", dotname.string()));
+    }
+    dotfile << generate();
+    dotfile.close();
+
+    // Convert the dot file to a png.
+    const std::string cmd = fmt::format(
+        "{} -Tpng {} >{}", dot, dotname.string(),
+        std::filesystem::path(dotname).replace_extension("png").string());
+    int status = system(cmd.c_str());
+    if (status != 0) {
+      throw std::runtime_error(
+          fmt::format("{}(): failed to execute command\n{}", __func__, cmd));
+    }
+
+    std::filesystem::remove(dotname);
+  };
+
+  MakePng("graph", [this, &basename]() {
+    return GenerateGraphvizString(basename, false);
+  });
+
+  if (forest_is_valid()) {
+    MakePng("graph+", [this, &basename]() {
+      return GenerateGraphvizString(basename, true);
+    });
+
+    MakePng("forest", [this, &basename]() {
+      return forest().GenerateGraphvizString(basename);
+    });
+  }
+
+  return std::filesystem::absolute(where);
+}
+
 }  // namespace internal
 }  // namespace multibody
 }  // namespace drake

multibody/topology/spanning_forest.cc

@@ -144,9 +144,10 @@ bool SpanningForest::BuildForest() {
   /* Model the World (Link 0) with mobilized body 0. This also starts the 0th
   LinkComposite in the graph and the 0th Welded Mobods group in the Forest.
   (1.1) */
-  data_.mobods.emplace_back(MobodIndex(0), LinkOrdinal(0));
+  Mobod& world = data_.mobods.emplace_back(MobodIndex(0), LinkOrdinal(0));
+  world.has_massful_follower_link_ = true;  // World is heavy!
   data_.welded_mobods.emplace_back(std::vector{MobodIndex(0)});
-  data_.mobods[MobodIndex(0)].welded_mobods_index_ = WeldedMobodsIndex(0);
+  world.welded_mobods_index_ = WeldedMobodsIndex(0);
   mutable_graph().set_primary_mobod_for_link(LinkOrdinal(0), MobodIndex(0),
                                              JointIndex{});
   mutable_graph().CreateWorldLinkComposite();

multibody/topology/spanning_forest.h

@@ -391,6 +391,8 @@ class SpanningForest {
   Forest is internally consistent and aborts if not. */
   void SanityCheckForest() const;
 
+  std::string GenerateGraphvizString(std::string_view label) const;
+
   /** (Debugging) Produces a human-readable summary of this Forest. */
   void DumpForest(std::string title) const;
 

multibody/topology/spanning_forest_debug.cc

@@ -254,6 +254,63 @@ void SpanningForest::DumpForestImpl(MobodIndex index, int level) const {
     DumpForestImpl(outboard, level + 2);
 }
 
+std::string SpanningForest::GenerateGraphvizString(
+    std::string_view label) const {
+  std::string graphviz = "digraph SpanningForest {\n";
+  graphviz += "rankdir=BT;\n";
+  graphviz += "labelloc=t;\n";
+  graphviz += fmt::format("label=\"{}\nSpanningForest\";\n", label);
+
+  // Generate a legend.
+  graphviz += "legend [shape=none]\n";
+  graphviz +=
+      "[label=\""
+      "* = massless"
+      "\nred = shadow"
+      "\npurple = reversed"
+      "\"]\n";
+
+  // For each Mobod, draw the body (as a node) and the mobilizer (as an edge).
+  for (const Mobod& mobod : mobods()) {
+    std::string followers;
+    for (LinkOrdinal ordinal : mobod.follower_link_ordinals())
+      followers += fmt::format("L({}) ", ordinal);
+    graphviz += fmt::format(
+        "mobod{} [color={}] [label=\"mobod({}){}\n{}\"];\n", mobod.index(),
+        links(mobod.link_ordinal()).is_shadow() ? "red" : "black",
+        mobod.index(), mobod.has_massful_follower_link() ? "" : "*", followers);
+    if (mobod.is_world()) continue;
+    const Mobod& inboard = mobods(mobod.inboard());
+
+    const std::string color = mobod.is_reversed() ? "purple"
+                              : mobod.is_weld()   ? "black"
+                                                  : "blue";
+    const std::string arrow_type = mobod.is_weld() ? "box" : "normal";
+    const std::string style = mobod.is_weld() ? "bold" : "solid";
+
+    const LinkJointGraph::Joint& joint = joints(mobod.joint_ordinal());
+    graphviz += fmt::format(
+        "mobod{} -> mobod{} [arrowhead={}] [fontsize=10] [style={}]"
+        "[label=\"mobilizer({})\nJ({}) {}{}\nq{} v{}\"] [color={}];\n",
+        inboard.index(), mobod.index(), arrow_type, style, mobod.index(),
+        joint.ordinal(), graph().joint_traits(joint.traits_index()).name,
+        mobod.is_reversed() ? "-R" : "", mobod.q_start(), mobod.v_start(),
+        color);
+  }
+
+  // Draw weld constraints that were added to close loops.
+  for (const LoopConstraint& constraint : loop_constraints()) {
+    graphviz += fmt::format(
+        "mobod{} -> mobod{} [dir=both] [arrowhead=box] [arrowtail=box] "
+        "[style=bold] [fontsize=10] [label=\"WELD({})\n\"] [color=red]\n",
+        constraint.primary_mobod(), constraint.shadow_mobod(),
+        constraint.index());
+  }
+
+  graphviz += "}\n";
+  return graphviz;
+}
+
 }  // namespace internal
 }  // namespace multibody
 }  // namespace drake

multibody/topology/spanning_forest_mobod.h

@@ -89,7 +89,7 @@ class SpanningForest::Mobod {
 
   /** Returns all the Links that are mobilized by this %Mobod. If this %Mobod
   represents a LinkComposite, the first Link returned is the "active" Link as
-  returned by link(). There is always at least one Link. */
+  returned by link_ordinal(). There is always at least one Link. */
   const std::vector<LinkOrdinal>& follower_link_ordinals() const {
     DRAKE_ASSERT(!follower_link_ordinals_.empty());
     return follower_link_ordinals_;