[WIP] Imported MultibodyGraphMaker from Simbody

[sherm1]

We need to be able to analyze links & joints input to determine how it would be modeled as a spanning tree plus constraints, without actually building a multibody tree. Simbody has a utility for that which is used by Gazebo to analyze sdf files. This PR imports the Simbody utility with style and minor substantive changes for use in Drake.

Relates to #9747 -- this is needed to discover where the disconnected base body would be.

This is a big PR since it moved over a substantial block of existing code.

Category            added  modified  removed  
----------------------------------------------
code                1958   0         0        
comments            1116   1         0        
blank               514    0         0        
----------------------------------------------
TOTAL               3588   1         0        

About half the code is unit tests (and it likely needs more).

---

This change is 

[sherm1]

@drake-jenkins-bot retest this please

[sherm1]

Here is a basic usage example, assuming the given input is:

       link1 <-- link2 <-- link3 --> link4 --> link5

where the arrows represent revolute joints and point from the "parent" link to the "child" link. Note that no explicit connection to World (link 0) has been given, so we're expecting the MultibodyGraphModeler to add one.

MultibodyGraphModeler graph;
// "world" is link 0
graph.AddLink("link1");
graph.AddLink("link2");
graph.AddLink("link3");
graph.AddLink("link4");
graph.AddLink("link5");
graph.AddJoint("joint0", "revolute", "link2", "link1");  // parent, child
graph.AddJoint("joint1", "revolute", "link3", "link2");
graph.AddJoint("joint2", "revolute", "link3", "link4");
graph.AddJoint("joint3", "revolute", "link4", "link5");

MultibodyTreeModel tree(graph);

for (MobilizerNum i(0); i < tree.num_mobilizers(); ++i) {
  auto& mobilizer = tree.get_mobilizer(i);
  auto& inb = tree.mobod(mobilizer.inboard_mobod_num());
  auto& outb = tree.mobod(mobilizer.outboard_mobod_num());
  std::cout << i << ": " << inb.name() << " --> " << outb.name() << "\n";
}

The above outputs

0: world --> link3
1: link3 --> link2
2: link3 --> link4
3: link2 --> link1
4: link4 --> link5

showing that the algorithm picked link3 as the base body and was able to preserve parent->child ordering for every joint. In actual use, the contents of the mobilizer loop above would of course build the multibody system rather than print it!

This is much more useful when there are loops in the input topology. See the unit tests for examples.

[sherm1]

+@amcastro-tri for feature review, please. This is a port of a chunk of code from Simbody so is oversize. If you think it is too big to review, I'd appreciate some ideas as to how to make it reviewable.

Reviewable status: all discussions resolved, platform LGTM missing

[amcastro-tri]

This is excellent @sherm1! thank you!. A first pass.

BTW, I'd place these files in drake/multibody/graph_maker so that we can use them from wherever in multibody we might need.

Reviewable status: 30 unresolved discussions, LGTM missing from assignee amcastro-tri, platform LGTM missing

a discussion (no related file):
lets see... so you are saying that graph maker adds a "FreeJoint" to all base bodies that ended up not connected to the world? I am trying to understand when the graph maker adds extra joints, my preference is that the number of joints (physical stuff) does not change when building the graph.

---

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 88 at r1 (raw file):

and containing a mobilizer for every body. You can also think of this graph as a
forest of trees each with a "base body" as its root, where a base body is a
body directly connected to World (often by a "free" or "floating" joint).

nit: it could also be a "weld" joint.

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 100 at r1 (raw file):

Heuristics here attempt to balance a number of competing goals:
  - choose sensible base bodies for trees that don't have a connection to World

"sensible", this might be later on but at this point I wonder what "sensible" means.

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 105 at r1 (raw file):

    in the input,
  - cut loops such that branch lengths are balanced (minimizes the maximum
    length of any branch),

could you expand on why this is important numerically? is this to increase the sparsity in the resulting system?

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 150 at r1 (raw file):

<h3>Loop joints</h3>
Normally every joint produces a corresponding mobilizer. A joint that would
form a loop is marked as such, and then its child link is split to form a new

is it always its child link that gets split?

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 221 at r1 (raw file):

  /** Construct an empty %MultibodyGraphMaker object and set the default
  names for Weld and Free joints to "weld" and "free". Also sets up Drake's

joints -> mobilizers?

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 221 at r1 (raw file):

  /** Construct an empty %MultibodyGraphMaker object and set the default
  names for Weld and Free joints to "weld" and "free". Also sets up Drake's

I wonder if we'd like unique names here? For instance, maybe we could concatenate the inboard/outboard bodies like so "inboard-body-name_weld_outboard-body-name".

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 224 at r1 (raw file):

  World body (named "world") and supported joint types, unless
  `use_drake_defaults` is set `false`. In that case the first added link (body
  number 0) is the World body and its name is used to represent World. */

with use_drake_defaults=true, does the world body also have index 0?

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 247 at r1 (raw file):

      on the contents of `name`. If this %MultibodyGraphMaker was constructed
      without Drake defaults, the first link you add is considered to be
      World, and its name is remembered and recognized when used in joints.

does this method throw if name already exists? does this method throw at all?

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 259 at r1 (raw file):

      to World by a Free joint before attempting to build the rest of the
      tree. Alternatively, provide a joint that connects this link directly
      to World, in which case you should _not_ set this flag.

what would happen if I set must_be_base_body = true when either: 1) the body already has a joint or when 2) I set this flag at two different places in the model?

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 282 at r1 (raw file):

  generated, calling this method clears the graph.
  @param[in]      name
      A string uniquely identifying this joint. There are no other

does this throw if name already exists?

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 286 at r1 (raw file):

  @param[in]      type
      A string designating the type of this joint, such as "revolute" or
      "ball". This must be chosen from the set of joint types previously

previously registered? maybe explain how to do so here?

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 290 at r1 (raw file):

  @param[in]      parent_body_name
      This must be the name of a link that was already specified in an earlier
      AddLink() call, or it must be the designated name for the World body.

maybe add link to method call to specify the default world body name?

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 296 at r1 (raw file):

      This must be the name of a link that was already specified in an earlier
      AddLink() call, or it must be the designated name for the World body.
      It must be distinct from `parent_body_name`. If possible, this will be

ditto comments for parent

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 298 at r1 (raw file):

      It must be distinct from `parent_body_name`. If possible, this will be
      used as the outboard body for the corresponding mobilizer.
  @param[in]      must_be_loop_joint

I don't understand what you mean with "loop joint" here. Maybe this flag should actually be called must_be_constraint? what am I missing?

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 301 at r1 (raw file):

      If you feel strongly that this joint should be chosen as a loop joint,
      set this flag. In that case the joint will not appear in the list of
      joints that are candidates for mobilizers (tree joints). Only after the

I'd remove "(tree joints)"

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 332 at r1 (raw file):

  change. In any case this method will clear the generated graph. This method
  exists primarily for testing purposes; normally the mass provided when the
  link is added remains unchanged. */

does it throw if name doesnt exist?

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 382 at r1 (raw file):

  by cutting a body to make a slave body, and those where the joint itself
  was implemented using a constraint rather than a mobilizer plus a slave.
  The latter occurs only if were told there is a perfectly good loop joint

"if we were told"?

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 397 at r1 (raw file):

  /** Returns the number of bodies, including all input bodies, a World body,
  and any slave bodies. */
  int num_bodies() const { return static_cast<int>(bodies_.size()); }

can I find these out separately? that is, the number of input bodies and slave bodies.

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 420 at r1 (raw file):

then we add additional joints to World as needed.

what do you mean? do you mean "mobilizers"? shouldn't joints only be specified by an external user with calls to AddJoint()?

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 425 at r1 (raw file):

  /** Returns the joint number assigned to the input joint with the given name.
  Returns -1 if the joint name is not recognized. You can't look up by name
  extra joints that were added by the graph-making algorithm. */

related to the above question. I do not understand why/when the graph maker would add joints

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 440 at r1 (raw file):

  If `body_num` is a base body it is returned. If `body_num` is World we
  return -1. */
  int FindBaseBody(int body_num) const;

do any of these methods taking an input index throw when passed with an invalid index?

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 457 at r1 (raw file):

  /** Specify relevant properties of a joint type as implemented in your
  multibody code. The joint type name must be unique. Weld and Free types are
  predefined and their names are reserved (though you can change the names to be

where did you define the predefined types? is this sentence to be it? it'd seem you have defaults specific to Drake and if not...?
also, are these type names case sensitive?

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 474 at r1 (raw file):

      and can be used by the caller to map back to their own data
      structure containing more joint type information.
  @returns Small integer joint type number used for referencing. */

referencing where?

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 480 at r1 (raw file):

  /** Change the name to be used to identify the weld joint type (0 dof) and
  weld loop constraint type (6 constraints). The default is "weld". Changing

nit: 6 dofs constraint.

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 499 at r1 (raw file):

  /** Get a JointType object by its assigned number. */
  const JointType& get_joint_type(int joint_type_num) const {

document range for joint_type_num? (even if apparently obvious from code right below)

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 512 at r1 (raw file):

  /** Return the name currently being used to identify the weld joint type
  and weld loop constraint type. */
  const std::string& get_weld_joint_type_name() const {

nit: add @see SetWeldJointTypeName()

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 518 at r1 (raw file):

  /** Returns the name currently being used to identify the free joint type
  and free loop constraint type. */
  const std::string& get_free_joint_type_name() const {

nit: add @see SetFreeJointTypeName()

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 574 at r1 (raw file):

generated. */
class MultibodyGraphMaker::Body {
 public:

should we add our DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN() macro? here and in all other classes?

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 935 at r1 (raw file):

/** Local class that represents one of the constraints that were added to close
topological loops that were cut to form the spanning tree. */
class MultibodyGraphMaker::LoopConstraint {

I don't think this violates any GSG but, wouldn't be easier for the programmer to find these docs in a header file? this entire directory could then be namespaces to be within multibody::graph_maker (which would suggest also to move this into drake/multibody/graph_maker.

[amcastro-tri]

checkpoint.

Reviewed 1 of 4 files at r1.
Reviewable status: 44 unresolved discussions, LGTM missing from assignee amcastro-tri, platform LGTM missing

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 609 at r1 (raw file):

  be broken into multiple bodies. If so you can find the generated slave
  bodies using slaves(). */
  bool is_master() const { return num_slaves() > 0; }

it'd seem like a body is always its own master. Maybe call this method has_slaves()?

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 627 at r1 (raw file):

  /** Returns the unique mobilizer (by number) for which this body is the
  outboard body, or -1 if the multibody graph has not yet be generated. */
  int mobilizer_num() const { return mobilizer_num_; }

what does it return for the world?

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 646 at r1 (raw file):

  /** Changes the mass (>= 0) of this body. */
  void set_mass(double mass) { DRAKE_DEMAND(mass >= 0); mass_ = mass; }

make private? since the graph maker is a friend anyway. That way graph invalidation is properly controlled by the maker from within ChangeLinkMass().

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 652 at r1 (raw file):

  friend class MultibodyGraphMaker;

  Body(const std::string& name_in, double mass_in, bool must_be_base_body_in,

nit: _in no longer needed to disambiguate args from member fields.

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 729 at r1 (raw file):

  /** Returns `true` if this joint was implemented with a loop joint. */
  bool has_loop_constraint() const { return loop_constraint_num_ >= 0; }

or just has_constraint()?

Along the same train of thought. Method must_be_loop_joint() could probably be named must_be_constraint()? for instance in case I'd like to build a maximal coordinates model of a simple double pendulum for which there are no kinematic loops.

I guess my question here is, are you using "loop joint" to mean "constraint", or are they somehow different?

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 737 at r1 (raw file):

  /** Returns the associated loop constraint if this joint is implemented with
  a loop joint, otherwise -1. */
  int loop_constraint_num() const { return loop_constraint_num_; }

ditto: just constraint_num()?

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 756 at r1 (raw file):

  Joint(const std::string& name, int joint_type_num, int parent_body_num,
        int child_body_num, bool must_be_loop_joint, void* user_ref)

ditto: must_be_constraint?

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 791 at r1 (raw file):

  // Mapping of strings to indices for fast lookup. These may have to be
  // adjusted later due to deletions.

nit: is this comment true?

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 810 at r1 (raw file):

  const std::string& name() const { return name_; }
  int num_mobilities() const { return num_mobilities_; }
  bool have_good_loop_joint_available() const {

nit: maybe just has_constraint_model()? or something of the like

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 863 at r1 (raw file):

  /** Return true if this mobilizer does not represent one of the input
  joints, but is instead a joint we added connecting a base body to World.

for an added base mobilizer, does it always correspond to a "free" joint or could it be a "weld" joint?
if so, are there any parameters for the graph maker controlling this?

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 873 at r1 (raw file):

  /** Get the joint type name of the joint that this mobilizer represents. */
  const std::string& get_joint_type_name() const {
    return mgm_->get_joint_type(mgm_->get_joint(joint_num_).joint_type_num())

should we DRAKE_ASSERT() this mgm_ dereferences?

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 927 at r1 (raw file):

  bool is_reversed_{false};    // if reversed, inboard=child, outboard=parent

  MultibodyGraphMaker* mgm_{nullptr};  // just a reference to container

const MultibodyGraphMaker* mgm_?

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 927 at r1 (raw file):

  bool is_reversed_{false};    // if reversed, inboard=child, outboard=parent

  MultibodyGraphMaker* mgm_{nullptr};  // just a reference to container

nit: Maybe per GSG just call this member maker_, or graph_maker_ if you like the longer version

---

multibody/multibody_tree/multibody_plant/multibody_graph_maker.h, line 983 at r1 (raw file):

  int child_body_num_{-1};   // child from the joint, or slave body

  MultibodyGraphMaker* mgm_{nullptr};  // just a reference to container

const MultibodyGraphMaker* mgm_?
ditto GSG comment above

[amcastro-tri]

Reviewable status: 45 unresolved discussions, LGTM missing from assignee amcastro-tri, platform LGTM missing

a discussion (no related file):
Per f2f:

• We agreed that links+joints are "inputs" and the graph maker will not add more of those when creating the graph. E.g. it will add free mobilizers but not physical joints (inputs).
• We think it will be useful to split into a very simple class (or struct?) MultibodyGraph and have the actual maker separately in MultibodyGraphMaker.
• We think this split will help us expose a "tree" structure for those needing an additional level of introspection beyond the physical bodies+joints (as for instance supplied through SDF).

---

[sherm1]

@amcastro-tri I reorganized this so MBGraphModeler stands alone with no dependencies or references to MultibodyTreeModel. Then when you want a tree you write MultibodyTreeModel tree(graph);. Everything compiles and the tree-building tests work fine. Plenty more to do to test more and add more interesting queries, but I'm going to stop here for now.

Reviewable status: 59 unresolved discussions, LGTM missing from assignee amcastro-tri, needs platform reviewer assigned (waiting on @amcastro-tri and @sherm1)

[EricCousineau-TRI]

Per discussion in #11257 and #11266, is there an overarching issue or design goal that this is directed for?
Thus far, the most concrete issue is for some types of tree queries, but I still feel like there's a decoupling goal hidden in there.

[sherm1]

One of the original goals (I thought) was to address trouble Russ was having building models. He wanted to read in a model, find a free base body, then weld it to ground, but ended up in pre/post-Finalize hell. The idea was to provide complete introspection of either the input graph or the generated tree model that would be lightweight, could be used to investigate, extract, restructure, etc. the system prior to actually committing to a computational model. Any user could work with the graph (using the original link/joint abstractions), and more advanced users could also work with the consequent tree (using body/mobilizer/constraint abstractions). [Subject to whatever names we come up with.]

However, recent discussion with @amcastro-tri makes me uncertain as to whether everyone agrees that is still a desirable goal. I still think it is.

The other goal, less negotiable, is to be able to generate tree/mobilizer/constraint models for input graphs that have loops, massless bodies, welded-together groupings, static and free bodies, etc. That can all be internal if we don't think the graph and trees should be exposed. But the graph-mangling algorithm to transform arbitrary input graphs into nice computational models is tricky both to write and to document and needs to use disciplined terminology where the terms have specific meanings. (See multibody_tree_model.h docs in this PR, for example.)

[jwnimmer-tri]

(We are closing PRs that have been dormant for 6+ months. Note that even though this PR does not have any discussion newer than 6 months, there have been pushes to the branch as of August 2019, so it's still about 6 weeks away until we close it for dormancy.)

[sherm1]

Freshened up a bit -- this is still useful.

[jwnimmer-tri]

This PR is twelve (six?) months stale. If it's no longer being worked, please close it.

[amcastro-tri]

The MultibodyTreeModel in this PR would be a great improvement over our current MultibodyTreeTopology. It'd pair up nicely with our current MultibodyGraph and would eventually allow us to even expose them publicly if people need that information. Towards #11307.

This PR essentially relates to every graph/tree query people would like to perform. E.g. #15496.
Personally I can even use this to extract sparsity information.