Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[multibody] Hotfix for PR 22649 duplicate frame name problem #22691

Conversation

sherm1
Copy link
Member

@sherm1 sherm1 commented Mar 3, 2025

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 Reviewable

@sherm1 sherm1 added priority: emergency release notes: none This pull request should not be mentioned in the release notes status: single reviewer ok https://drake.mit.edu/reviewable.html labels Mar 3, 2025
Copy link
Member Author

@sherm1 sherm1 left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

+@jwnimmer-tri for both reviews, please

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

Copy link
Contributor

@calderpg-tri calderpg-tri left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copy link
Member Author

@sherm1 sherm1 left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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!

Copy link
Contributor

@calderpg-tri calderpg-tri left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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 sherm1 closed this Mar 3, 2025
Copy link
Member Author

@sherm1 sherm1 left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copy link
Member Author

@sherm1 sherm1 left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.)

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
priority: emergency release notes: none This pull request should not be mentioned in the release notes status: do not review status: single reviewer ok https://drake.mit.edu/reviewable.html
Projects
None yet
Development

Successfully merging this pull request may close these issues.

3 participants