[multibody] Hotfix for PR 22649 duplicate frame name problem

[sherm1]

PR #22649 generated some internal frames with made-up names that turned out not always to be unique in practice. This hotfix ensures that the made-up names are always unique.

Once this merges we can close revert #22688

---

This change is 

[sherm1]

+@jwnimmer-tri for both reviews, please

Reviewable status: LGTM missing from assignee jwnimmer-tri(platform)

[calderpg-tri]

Reviewable status: 1 unresolved discussion, LGTM missing from assignee jwnimmer-tri(platform)

---

multibody/tree/weld_joint.cc line 89 at r1 (raw file):

  auto F_frame_name = [this, tree](const Frame<T>& frame) -> std::string {
    std::string F_name = fmt::format("{}_{}_F", this->name(), frame.name());
    while (tree->HasFrameNamed(F_name, this->model_instance())) F_name += "_";

I don't have a lot of context on #22649 that got us here, but adding frames that differ only in trailing underscores seems near-pathologically confusing to anyone who ever needs to look at frame names. If "internal use" frames are getting regularly added, wouldn't it make sense to put something like "internal" in the name to indicate why/how they were added?

Also, as an aside, how many frames are actually getting added here? There are enough places in Drake/drake-ros/Anzu where all frame poses are being computed that a significant increase in the number of frames might actually have meaningful consequences.

[sherm1]

Reviewable status: 1 unresolved discussion, needs platform reviewer assigned, needs at least one assigned reviewer (waiting on @sherm1)

---

multibody/tree/weld_joint.cc line 89 at r1 (raw file):

Previously, calderpg-tri wrote…

I don't have a lot of context on #22649 that got us here, but adding frames that differ only in trailing underscores seems near-pathologically confusing to anyone who ever needs to look at frame names. If "internal use" frames are getting regularly added, wouldn't it make sense to put something like "internal" in the name to indicate why/how they were added?

Also, as an aside, how many frames are actually getting added here? There are enough places in Drake/drake-ros/Anzu where all frame poses are being computed that a significant increase in the number of frames might actually have meaningful consequences.

Thanks, Calder. One new frame is added for any Weld joint for which there is a non-identity transform specified relating its parent (Jp) and child (Jc) frames. Effectively the parent frame is moved to be coincident with the child so that we don't have to repeatedly compose with X_JpJc when calculating kinematics. The new parent frame's pose with respect to the original Jp and the parent body P is fixed once parameters have been set. Drake doesn't have any further use for the original Jp so there is no additional expense there.

Are there use cases in Anzu where you loop through all frames and perform some calculations even if you don't know anything about those frames? There are lots of frames already and most are not particularly significant. I would be happy to stick "internal" or some other marker in the made-up names if that would help filter out junk you don't want to see.

Alejandro and I are on a vendetta to speed up kinematics by optimizing the internal representation of joints (that is, the underlying mobilizers), at least revolute, prismatic, and welds for which there is much low-hanging fruit and many use cases. That starts with creating the optimal frames in which to perform runtime computations. Drake is very generous in what we allow users to specify for joint Jp and Jc frames; those are not necessarily the best ones for computation.

Suggestions for making your life easier w.r.t. new frames greatly appreciated!

[calderpg-tri]

Reviewable status: 1 unresolved discussion, needs platform reviewer assigned, needs at least one assigned reviewer (waiting on @sherm1)

---

multibody/tree/weld_joint.cc line 89 at r1 (raw file):

Are there use cases in Anzu where you loop through all frames and perform some calculations even if you don't know anything about those frames? There are lots of frames already and most are not particularly significant. I would be happy to stick "internal" or some other marker in the made-up names if that would help filter out junk you don't want to see.

The primary uses that come to mind deal with visualization and TF publication, where we currently don't know which frames might matter to the user and thus all frame poses need to be computed. E.g.

std::map<std::string, Eigen::Isometry3d>
RvizGeometryVisualizer::MakePoseMap(
    const drake::multibody::MultibodyPlant<double>& plant,
    const drake::systems::Context<double>& plant_context) {
  // Get the pose of every frame in the plant.
  std::map<std::string, Eigen::Isometry3d> pose_map;
  for (drake::multibody::FrameIndex frame_index(0);
       frame_index < plant.num_frames(); frame_index++) {
    const auto& frame = plant.get_frame(frame_index);
    const std::string tf_frame_name = GetTfFrameNameForFrame(plant, frame);
    const Eigen::Isometry3d X_WF =
        plant.CalcRelativeTransform(
            plant_context, plant.world_frame(), frame).GetAsIsometry3();
    if (!pose_map.try_emplace(tf_frame_name, X_WF).second) {
      throw std::runtime_error(
          "Frame names within the provided MbP are not unique: frame name [" +
          tf_frame_name + "] is duplicated");
    }
  }
  return pose_map;
}

(IIRC drake-ros has a very similar method). Having an easy way to identify internal-only frames (probably a prefix would be fastest) would allow them to be skipped in these operations. Possibly we would also want to suppress them in some diagnostic messages (e.g. when a frame can't be found by name) or at least indicate they shouldn't be directly used.

[sherm1]

Reviewable status: 1 unresolved discussion, needs platform reviewer assigned, needs at least one assigned reviewer (waiting on @sherm1)

---

multibody/tree/weld_joint.cc line 89 at r1 (raw file):

Previously, calderpg-tri wrote…

Are there use cases in Anzu where you loop through all frames and perform some calculations even if you don't know anything about those frames? There are lots of frames already and most are not particularly significant. I would be happy to stick "internal" or some other marker in the made-up names if that would help filter out junk you don't want to see.

The primary uses that come to mind deal with visualization and TF publication, where we currently don't know which frames might matter to the user and thus all frame poses need to be computed. E.g.

std::map<std::string, Eigen::Isometry3d>
RvizGeometryVisualizer::MakePoseMap(
    const drake::multibody::MultibodyPlant<double>& plant,
    const drake::systems::Context<double>& plant_context) {
  // Get the pose of every frame in the plant.
  std::map<std::string, Eigen::Isometry3d> pose_map;
  for (drake::multibody::FrameIndex frame_index(0);
       frame_index < plant.num_frames(); frame_index++) {
    const auto& frame = plant.get_frame(frame_index);
    const std::string tf_frame_name = GetTfFrameNameForFrame(plant, frame);
    const Eigen::Isometry3d X_WF =
        plant.CalcRelativeTransform(
            plant_context, plant.world_frame(), frame).GetAsIsometry3();
    if (!pose_map.try_emplace(tf_frame_name, X_WF).second) {
      throw std::runtime_error(
          "Frame names within the provided MbP are not unique: frame name [" +
          tf_frame_name + "] is duplicated");
    }
  }
  return pose_map;
}

(IIRC drake-ros has a very similar method). Having an easy way to identify internal-only frames (probably a prefix would be fastest) would allow them to be skipped in these operations. Possibly we would also want to suppress them in some diagnostic messages (e.g. when a frame can't be found by name) or at least indicate they shouldn't be directly used.

A few random suggestions for the code above:

• Frame::CalcPoseInWorld() is better than CalcRelativeTransform() in this case.
• Even better, all the body frames are precalculated when RigidBody::EvalPoseInWorld() is called. Could be worthwhile to use Frame::is_body_frame() to get those freebies.

[sherm1]

Reviewable status: 1 unresolved discussion, needs platform reviewer assigned, needs at least one assigned reviewer (waiting on @sherm1)

---

multibody/tree/weld_joint.cc line 89 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

A few random suggestions for the code above:

• Frame::CalcPoseInWorld() is better than CalcRelativeTransform() in this case.
• Even better, all the body frames are precalculated when RigidBody::EvalPoseInWorld() is called. Could be worthwhile to use Frame::is_body_frame() to get those freebies.

To wrap this up @calderpg-tri: we ended up adding an API is_ephemeral() that can be applied to any MultibodyPlant element (joint, body, frame, etc.). Sorry about the name, but the meaning is that any element that got added automatically by the Finalize() call is marked is_ephemeral(). Anzu's multibody_extra.py now makes use of that to filter out the unwanted frames.

(The reason these are "ephemeral" is that if we ever implement Unfinalize() those elements would disappear; they aren't part of the user's model.)