adding support for model instances in our common utils functions

[yangwill]

This change is 

[Brian-Acosta]

BTW CI says there are build (linker) errors.

Reviewed 11 of 11 files at r1, all commit messages.
Reviewable status: all files reviewed, 16 unresolved discussions (waiting on @yangwill)

---

multibody/multibody_utils.h line 83 at r1 (raw file):

    const drake::multibody::MultibodyPlant<T>& plant);

/// Given a MultibodyPlant, builds a map from position name to position index

Can you use this comment to explain the difference between the two function signatures? Or better yet, can we make this one function with std::optional defaulting to empty?

---

multibody/multibody_utils.h line 95 at r1 (raw file):

/// Given a MultiBodyTree, builds a map from velocity name to velocity index

nit - comment should say MultibodyPlant

---

multibody/multibody_utils.cc line 155 at r1 (raw file):

  if (show_ground) {
    //    plant->RegisterVisualGeometry(plant->world_body(), X_WG, HalfSpace(),
    //                                  "visual");

un-comment this

---

multibody/multibody_utils.cc line 205 at r1 (raw file):

  }

  // TODO: once RBT fully deprecated, this block can likely be removed, using

nit - while we're here, this comment doesn't seem relevant anymore

---

multibody/multibody_utils.cc line 208 at r1 (raw file):

  // default coordinate names from Drake.
  auto floating_bodies = plant.GetFloatingBaseBodies();
  //  DRAKE_THROW_UNLESS(floating_bodies.size() <= 1);

Remove dead code

---

multibody/multibody_utils.cc line 213 at r1 (raw file):

    DRAKE_ASSERT(body.has_quaternion_dofs());
    int start = body.floating_positions_start();
    // should be body.name() once RBT is deprecated

nit - same with this comment

---

multibody/multibody_utils.cc line 274 at r1 (raw file):

  // TODO: once RBT fully deprecated, this block can likely be removed, using
  // default coordinate names from Drake.

nit - once again

---

multibody/multibody_utils.cc line 279 at r1 (raw file):

    DRAKE_ASSERT(body.has_quaternion_dofs());
    int start = body.floating_positions_start();
    // should be body.name() once RBT is deprecated

same

---

multibody/multibody_utils.cc line 340 at r1 (raw file):

  auto floating_bodies = plant.GetFloatingBaseBodies();
  // Remove throw once RBT deprecated
  //  DRAKE_THROW_UNLESS(floating_bodies.size() <= 1);

Remove dead code

---

multibody/multibody_utils.cc line 409 at r1 (raw file):

  }

  // Remove throw once RBT deprecated

remove comment

---

multibody/multibody_utils.cc line 414 at r1 (raw file):

    int start = body.floating_velocities_start() - plant.num_positions();
    std::string name =
        body.name();  // should be body.name() once RBT is deprecated

remove comment

---

multibody/multipose_visualizer.cc line 3 at r1 (raw file):

#include "multibody/multipose_visualizer.h"

#include <iostream>

is iostream necessary?

---

multibody/multipose_visualizer.cc line 6 at r1 (raw file):

#include "drake/geometry/drake_visualizer.h"
#include "drake/geometry/meshcat_visualizer_params.h"

do we need meshcat_visualizer_params or just meshcat_visualizer?

---

multibody/visualization_utils.cc line 7 at r1 (raw file):

#include "systems/primitives/subvector_pass_through.h"
#include "drake/geometry/drake_visualizer.h"
#include "drake/geometry/meshcat_visualizer.h"

Meshcat is included but not used?
@yangwill Since you're the only one who uses trajectory optimization currently, would you be willing to just remove drake visualizer from trajectory visualization tools in favor of meshcat?

---

systems/robot_lcm_systems.h line 34 at r1 (raw file):

  explicit RobotOutputReceiver(
      const drake::multibody::MultibodyPlant<double>& plant,
      drake::multibody::ModelInstanceIndex model_instance);

Similarly to above, can we get away with one constructor which takes a std::optional

---

systems/robot_lcm_systems.h line 169 at r1 (raw file):

    std::string state_channel, double publish_rate,
    drake::multibody::ModelInstanceIndex model_instance_index,
    bool publish_efforts = true, double actuator_delay = 0);

Same here re: one function signature

[yangwill]

Yep, probably missed some functions when copying over files, will fix in next revision

Reviewable status: all files reviewed, 16 unresolved discussions (waiting on @Brian-Acosta)

---

multibody/multibody_utils.h line 83 at r1 (raw file):

Previously, Brian-Acosta (Brian Acosta) wrote…

Can you use this comment to explain the difference between the two function signatures? Or better yet, can we make this one function with std::optional defaulting to empty?

this is because default_model_instance link to doc returns only the tree elements without an explicit model instance, whereas we sometimes want to use it to get all tree elements regardless of how they were added (calling MultibodyPlant functions with no model instance).

Drake also has this issue
i.e. num_positions() has no equivalent num_positions(ModelInstanceIndex)

If you have a solution, let me know - i agree it's annoying/ugly.

[Brian-Acosta]

Reviewable status: all files reviewed, 16 unresolved discussions (waiting on @yangwill)

---

multibody/multibody_utils.h line 83 at r1 (raw file):

Previously, yangwill (William Yang) wrote…

this is because default_model_instance link to doc returns only the tree elements without an explicit model instance, whereas we sometimes want to use it to get all tree elements regardless of how they were added (calling MultibodyPlant functions with no model instance).

Drake also has this issue
i.e. num_positions() has no equivalent num_positions(ModelInstanceIndex)

If you have a solution, let me know - i agree it's annoying/ugly.

Sorry I'm not sure my suggestion was clear. My suggestion was for the function signature to be something like:

Code snippet:

template <typename T>
std::map<std::string, int> MakeNameToPositionsMap(
    const drake::multibody::MultibodyPlant<T>& plant,
    std::optional<drake::multibody::ModelInstanceIndex> model_instance_index = std::nullopt);

[yangwill]

Reviewable status: all files reviewed, 12 unresolved discussions (waiting on @Brian-Acosta)

---

multibody/multibody_utils.h line 83 at r1 (raw file):

Previously, Brian-Acosta (Brian Acosta) wrote…

Sorry I'm not sure my suggestion was clear. My suggestion was for the function signature to be something like:

as mentioned in f2f, it's not trivial as drake doesn't have a default model instance index that behaves as is calling without a model instance index, so we would have to have an if statement accompanying any function calls that might use a model instance index

---

multibody/multibody_utils.cc line 155 at r1 (raw file):

Previously, Brian-Acosta (Brian Acosta) wrote…

un-comment this

Done.

---

multibody/multibody_utils.cc line 208 at r1 (raw file):

Previously, Brian-Acosta (Brian Acosta) wrote…

Remove dead code

Done.

---

multibody/multibody_utils.cc line 279 at r1 (raw file):

Previously, Brian-Acosta (Brian Acosta) wrote…

same

Done.

---

multibody/multibody_utils.cc line 340 at r1 (raw file):

Previously, Brian-Acosta (Brian Acosta) wrote…

Remove dead code

Done.

---

multibody/multibody_utils.cc line 409 at r1 (raw file):

Previously, Brian-Acosta (Brian Acosta) wrote…

remove comment

Done.

---

multibody/multibody_utils.cc line 414 at r1 (raw file):

Previously, Brian-Acosta (Brian Acosta) wrote…

remove comment

Done.

---

multibody/multipose_visualizer.cc line 3 at r1 (raw file):

Previously, Brian-Acosta (Brian Acosta) wrote…

is iostream necessary?

Done.

---

multibody/multipose_visualizer.cc line 6 at r1 (raw file):

Previously, Brian-Acosta (Brian Acosta) wrote…

do we need meshcat_visualizer_params or just meshcat_visualizer?

Done.

---

multibody/visualization_utils.cc line 7 at r1 (raw file):

Previously, Brian-Acosta (Brian Acosta) wrote…

Meshcat is included but not used?
@yangwill Since you're the only one who uses trajectory optimization currently, would you be willing to just remove drake visualizer from trajectory visualization tools in favor of meshcat?

yep, will do. actually will remove all references to drake visualizer and meshcat visualizer in this file as we can choose the visualizer by attaching the scenegraph outside of this function.

---

systems/robot_lcm_systems.h line 34 at r1 (raw file):

Previously, Brian-Acosta (Brian Acosta) wrote…

Similarly to above, can we get away with one constructor which takes a std::optional

i dont think it's trivial to condense the two functions with optional, drake multibody_plant.cc also has separate implementations for with and without a model instance index

---

systems/robot_lcm_systems.h line 169 at r1 (raw file):

Previously, Brian-Acosta (Brian Acosta) wrote…

Same here re: one function signature

Done.

[Brian-Acosta]

LGTM (assuming CI passes)

Reviewed 5 of 5 files at r2, all commit messages.
Reviewable status: complete! all files reviewed, all discussions resolved (waiting on @yangwill)

[yangwill]

Oops, will need another revision, have some compile errors and after fixing those also have some examples not working properly.

Reviewable status: complete! all files reviewed, all discussions resolved (waiting on @yangwill)