Skip to content

[multibody] Assign coordinate starts to all joints in topology - #24923

Open
sherm1 wants to merge 1 commit into
RobotLocomotion:masterfrom
sherm1:topology_assigns_qv_start_for_all_joints
Open

[multibody] Assign coordinate starts to all joints in topology#24923
sherm1 wants to merge 1 commit into
RobotLocomotion:masterfrom
sherm1:topology_assigns_qv_start_for_all_joints

Conversation

@sherm1

@sherm1 sherm1 commented Aug 27, 2026

Copy link
Copy Markdown
Member

This is a yak shave along the path to full support of welded-link fusing.

Lots of Drake code that iterates over joints assumes that every joint corresponds to a mobilizer that can answer questions about how the joint was modeled. Those loops can fail when some joints (welds within a fused composite) are not modeled at all. One of the common queries is to find the state segment corresponding to a joint. We already have a convention for weld joints -- they report a start location where their state variables would have gone if they had any, and then a length of zero. Everything works fine with that convention. For fused welds the query fails.

The policy we want is for fused (unmodeled) welds to report the start location for the mobod followed by both their parent and child links, and a size of zero. That convention eliminates many special cases. Since q's and v's are assigned by the topology code, this PR stores the q_start and v_start in the as-modeled LinkJointGraph::Joint, and modifies MultibodyPlant to use those values rather than trying to figure it out by looking at the mobilizer. In addition to putting the policy where it belongs, that allows us to nuke a bunch of now-unneeded virtuals on the joints.

We also get rid of the Joint NVI virtuals for num_positions() and num_velocities() using already-present Joint information.

Unit tests verify the policy in multibody/topology and in multibody/plant when fusing is enabled.

Release notes

There are no user-visible changes here, however the removal of the now-unneeded protected pure virtuals in Joint could be a breaking change for a downstream user who wrote their own joint. It is very unlikely that any such joint extensions exist, but if they did the fix would be simply to delete the implementations of these virtuals in Joint:::

  • do_get_velocity_start()
  • do_get_num_velocities()
  • do_get_position_start()
  • do_get_num_positions()

Reminder: the Joint class has a specific exception to our deprecation policy:

/// @note To developers: this is the base class for all concrete Joint types.
/// Extending this class to add a new Joint type necessarily requires working
/// with internal implementation classes for which we cannot guarantee API
/// stability due to the need for ongoing improvements to these
/// performance-critical classes. So while our usual stability guarantees
/// apply to the Joint `public` API, the `protected` API here is subject to
/// change when the underlying internal objects change. Our release notes will
/// say when we have made changes that might affect your Joint implementations,
/// but we won't necessarily be able to provide a deprecation period.

This change is Reviewable

@sherm1 sherm1 added priority: low release notes: none This pull request should not be mentioned in the release notes labels Aug 27, 2026
@sherm1
sherm1 force-pushed the topology_assigns_qv_start_for_all_joints branch from 25a3b1a to 9fb8bfe Compare August 28, 2026 00:07
@sherm1
sherm1 requested a balanced review from Copilot August 28, 2026 00:15

Copilot AI left a comment

Copy link
Copy Markdown

Choose a reason for hiding this comment

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

Pull request overview

Moves joint coordinate-start assignment into topology so fused, unmodeled welds have valid state offsets.

Changes:

  • Stores q/v starts for every topology joint.
  • Uses topology and limit metadata for Joint state queries.
  • Removes obsolete per-joint virtual implementations and adds topology tests.

Reviewed changes

Copilot reviewed 20 out of 20 changed files in this pull request and generated 1 comment.

Show a summary per file
File Description
bindings/generated_docstrings/multibody_tree.h Updates generated Joint documentation.
multibody/plant/multibody_plant.cc Handles zero-DOF joints in state-name generation.
multibody/plant/test/sap_driver_multidof_joints_test.cc Removes obsolete test-joint overrides.
multibody/topology/link_joint_graph.h Declares coordinate-start assignment.
multibody/topology/link_joint_graph_inlines.h Implements coordinate-start storage.
multibody/topology/link_joint_graph_joint.h Stores and exposes joint q/v starts.
multibody/topology/spanning_forest.cc Assigns starts to modeled and fused joints.
multibody/topology/test/spanning_forest_test.cc Tests fused and unfused topology assignments.
multibody/tree/ball_rpy_joint.h Removes obsolete state-query overrides.
multibody/tree/curvilinear_joint.h Removes obsolete state-query overrides.
multibody/tree/joint.h Uses topology starts and limit-vector dimensions.
multibody/tree/multibody_tree.cc Validates graph joint traits.
multibody/tree/planar_joint.h Removes obsolete state-query overrides.
multibody/tree/prismatic_joint.h Removes obsolete state-query overrides.
multibody/tree/quaternion_floating_joint.h Removes obsolete state-query overrides.
multibody/tree/revolute_joint.h Removes obsolete state-query overrides.
multibody/tree/rpy_floating_joint.h Removes obsolete state-query overrides.
multibody/tree/screw_joint.h Removes obsolete state-query overrides.
multibody/tree/universal_joint.h Removes obsolete state-query overrides.
multibody/tree/weld_joint.h Removes mobilizer-dependent state queries.

💡 Add a code-review agent skill or configure MCP servers for context-aware, tailored reviews. Learn more in the docs.

Comment thread multibody/tree/joint.h

@sherm1 sherm1 left a comment

Copy link
Copy Markdown
Member Author

Choose a reason for hiding this comment

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

+a:@rpoyner-tri for feature review, please

@sherm1 made 1 comment.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee rpoyner-tri(platform), needs at least two assigned reviewers (waiting on rpoyner-tri).

Store q_start and v_start for every LinkJointGraph joint during forest
modeling, including unmodeled welds in fused assemblies. Simplify
MultibodyPlant and MultibodyTree to use those assignments directly.

Derive Joint coordinate counts from its existing limit vectors instead of
virtual methods. Add an end-to-end MultibodyPlant regression test that
checks all modeled and fused welds, including a weld fused into World.
@sherm1
sherm1 force-pushed the topology_assigns_qv_start_for_all_joints branch from 9fb8bfe to 795899e Compare August 28, 2026 00:37

@sherm1 sherm1 left a comment

Copy link
Copy Markdown
Member Author

Choose a reason for hiding this comment

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

@sherm1 made 1 comment and resolved 1 discussion.
Reviewable status: LGTM missing from assignee rpoyner-tri(platform), needs at least two assigned reviewers (waiting on rpoyner-tri).

Comment thread multibody/tree/joint.h

Copilot AI left a comment

Copy link
Copy Markdown

Choose a reason for hiding this comment

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

Pull request overview

Copilot reviewed 21 out of 21 changed files in this pull request and generated 1 comment.

Comment thread multibody/tree/joint.h
@@ -803,31 +818,6 @@ class Joint : public MultibodyElement<T> {
// End of hidden Doxygen section.

protected:

Copy link
Copy Markdown
Member Author

Choose a reason for hiding this comment

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

Done. Release note added in PR description.

@rpoyner-tri rpoyner-tri left a comment

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

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

@rpoyner-tri reviewed 1 file.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee rpoyner-tri(platform), needs at least two assigned reviewers (waiting on sherm1).

@sherm1 sherm1 added release notes: breaking change This pull request contains breaking changes and removed release notes: none This pull request should not be mentioned in the release notes labels Aug 29, 2026

@sherm1 sherm1 left a comment

Copy link
Copy Markdown
Member Author

Choose a reason for hiding this comment

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

@sherm1 made 1 comment and resolved 1 discussion.
Reviewable status: LGTM missing from assignee rpoyner-tri(platform), needs at least two assigned reviewers (waiting on rpoyner-tri).

Comment thread multibody/tree/joint.h
@@ -803,31 +818,6 @@ class Joint : public MultibodyElement<T> {
// End of hidden Doxygen section.

protected:

Copy link
Copy Markdown
Member Author

Choose a reason for hiding this comment

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

Done. Release note added in PR description.

@sherm1 sherm1 left a comment

Copy link
Copy Markdown
Member Author

Choose a reason for hiding this comment

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

@rpoyner-tri I added a note to the PR description per Copilot's review comment.

@sherm1 made 1 comment.
Reviewable status: LGTM missing from assignee rpoyner-tri(platform), needs at least two assigned reviewers (waiting on rpoyner-tri).

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

priority: low release notes: breaking change This pull request contains breaking changes

Projects

None yet

Development

Successfully merging this pull request may close these issues.

3 participants