Skip to content

[multibody] Remove more ad hoc topology in favor of SpanningForest #23450

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.

Sign up for GitHub

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

Draft
wants to merge 7 commits into
base: master
Choose a base branch
from
Draft

[multibody] Remove more ad hoc topology in favor of SpanningForest #23450

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
8898774
Got rid of topology use in RigidBody.
sherm1 Aug 21, 2025
5998141
Nuke RigidBodyTopology.
sherm1 Sep 25, 2025
4c367f8
Nuked MobilizerTopology
sherm1 Sep 29, 2025
7c0b41e
Nuked FrameTopology
sherm1 Sep 29, 2025
be0b3e1
Nuked JointActuatorTopology.
sherm1 Sep 30, 2025
90abde6
Working on cutting down dependence on MultibodyTopology in prep for n…
sherm1 Sep 30, 2025
0d966a5
Tree field references gone except for one mbtree test.
sherm1 Oct 9, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 9 additions & 7 deletions bindings/generated_docstrings/multibody_tree.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4202,9 +4202,8 @@ with a call to set_controller_gains().
const char* doc =
R"""(Returns the index to the first element for this joint actuator /
within the vector of actuation inputs for the full multibody / system.

Raises:
RuntimeError if the MultibodyTree model is not finalized.)""";
Returns -1 if this JointActuator hasn't been added to a
MultibodyPlant.)""";
} input_start;
// Symbol: drake::multibody::JointActuator::joint
struct /* joint */ {
Expand All @@ -4225,10 +4224,8 @@ R"""(Returns a reference to the joint actuated by this JointActuator.
struct /* num_inputs */ {
// Source: drake/multibody/tree/joint_actuator.h
const char* doc =
R"""(Returns the number of inputs associated with this actuator.

Raises:
RuntimeError if the MultibodyTree model is not finalized.)""";
R"""(Returns the number of inputs associated with this actuator. This is
always the number of degrees of freedom of the actuated joint.)""";
} num_inputs;
// Symbol: drake::multibody::JointActuator::rotor_inertia
struct /* rotor_inertia */ {
Expand Down Expand Up @@ -4272,6 +4269,11 @@ Parameter ``u``:
RuntimeError if ``u.size() !=
this->GetParentPlant().num_actuated_dofs()``.)""";
} set_actuation_vector;
// Symbol: drake::multibody::JointActuator::set_actuator_dof_start
struct /* set_actuator_dof_start */ {
// Source: drake/multibody/tree/joint_actuator.h
const char* doc = R"""()""";
} set_actuator_dof_start;
// Symbol: drake::multibody::JointActuator::set_controller_gains
struct /* set_controller_gains */ {
// Source: drake/multibody/tree/joint_actuator.h
Expand Down
34 changes: 16 additions & 18 deletions multibody/plant/deformable_driver.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -318,8 +318,8 @@ DeformableDriver<T>::ComputeContactDataForDeformable(
}
scaled_Jv_v_WGc_C.SetFromTriplets(triplets);
result.v_WGc.emplace_back(v_WGc);
const TreeIndex tree_index(
manager_->internal_tree().get_topology().num_trees() + body_index);
const TreeIndex tree_index(manager_->internal_tree().forest().num_trees() +
body_index);
result.jacobian.emplace_back(tree_index,
MatrixBlock<T>(std::move(scaled_Jv_v_WGc_C)));
}
Expand All @@ -343,14 +343,15 @@ DeformableDriver<T>::ComputeContactDataForRigid(
manager_->plant()
.EvalBodyPoseInWorld(context, manager_->plant().get_body(body_index))
.translation();
const MultibodyTreeTopology& tree_topology =
manager_->internal_tree().get_topology();
const TreeIndex tree_index = tree_topology.body_to_tree_index(body_index);
if (body_index == world_index()) return result;

const SpanningForest& forest = manager_->internal_tree().forest();
const TreeIndex tree_index = forest.link_to_tree(body_index);
const SpanningForest::Tree& tree = forest.trees(tree_index);
const int tree_nv = tree.nv();
/* If the body is welded to world, then everything is trivially zero (as
indicated by empty jacobian and velocity vectors). */
if (!tree_topology.tree_has_dofs(tree_index)) {
return result;
}
if (tree_nv == 0) return result;

const Eigen::VectorBlock<const VectorX<T>> rigid_v0 =
manager_->plant().GetVelocities(context);
Expand All @@ -367,9 +368,7 @@ DeformableDriver<T>::ComputeContactDataForRigid(
p_WC, frame_W, frame_W, &Jv_v_WGc_W);
result.v_WGc.emplace_back(Jv_v_WGc_W * rigid_v0);
Matrix3X<T> J = surface.R_WCs()[i].matrix().transpose() *
Jv_v_WGc_W.middleCols(
tree_topology.tree_velocities_start_in_v(tree_index),
tree_topology.num_tree_velocities(tree_index));
Jv_v_WGc_W.middleCols(tree.v_start(), tree_nv);
result.jacobian.emplace_back(tree_index, MatrixBlock<T>(std::move(J)));
}
return result;
Expand Down Expand Up @@ -616,8 +615,7 @@ void DeformableDriver<T>::AppendDeformableRigidFixedConstraintKinematics(
and rigid bodies are mutually exclusive, and Jv_v_WAp = 0 for rigid dofs
and Jv_v_WBq = 0 for deformable dofs. */
const int nv = manager_->plant().num_velocities();
const MultibodyTreeTopology& tree_topology =
manager_->internal_tree().get_topology();
const SpanningForest& forest = manager_->internal_tree().forest();
const auto& configurations =
manager_->plant()
.get_deformable_body_configuration_output_port()
Expand All @@ -644,7 +642,7 @@ void DeformableDriver<T>::AppendDeformableRigidFixedConstraintKinematics(
EvalConstraintParticipation(context, index);
// Each deformable body forms its own clique and are indexed in inceasing
// DeformableBodyIndex order and placed after all rigid cliques.
const TreeIndex clique_index_A(tree_topology.num_trees() + index);
const TreeIndex clique_index_A(forest.num_trees() + index);
const GeometryId geometry_id = body.geometry_id();
const PartialPermutation& vertex_permutation =
EvalVertexPermutation(context, geometry_id);
Expand All @@ -660,7 +658,6 @@ void DeformableDriver<T>::AppendDeformableRigidFixedConstraintKinematics(
jacobian_triplets.reserve(num_vertices_in_constraint);
/* The Jacobian block for the rigid body B. */
const BodyIndex index_B = spec.body_B;
const TreeIndex tree_index = tree_topology.body_to_tree_index(index_B);
const RigidBody<T>& rigid_body = manager_->plant().get_body(index_B);
const math::RigidTransform<T>& X_WB =
manager_->plant().EvalBodyPoseInWorld(context, rigid_body);
Expand Down Expand Up @@ -698,6 +695,7 @@ void DeformableDriver<T>::AppendDeformableRigidFixedConstraintKinematics(
index + manager_->plant().num_bodies(); // Deformable body.
const int object_B = index_B; // Rigid body.

const TreeIndex tree_index = forest.link_to_tree(index_B);
if (tree_index.is_valid()) {
/* Rigid body is not welded to world. */
const int clique_index_B = tree_index;
Expand All @@ -707,9 +705,9 @@ void DeformableDriver<T>::AppendDeformableRigidFixedConstraintKinematics(
context, JacobianWrtVariable::kV, rigid_body.body_frame(), frame_W,
Eigen::Map<const Matrix3X<T>>(p_WQs.data(), 3, p_WQs.size() / 3),
frame_W, frame_W, &Jv_v_WBq);
MatrixBlock<T> jacobian_block_B(Jv_v_WBq.middleCols(
tree_topology.tree_velocities_start_in_v(tree_index),
tree_topology.num_tree_velocities(tree_index)));
const SpanningForest::Tree& tree = forest.trees(tree_index);
MatrixBlock<T> jacobian_block_B(
Jv_v_WBq.middleCols(tree.v_start(), tree.nv()));
SapConstraintJacobian<T> J(clique_index_A, std::move(jacobian_block_A),
clique_index_B, std::move(jacobian_block_B));
result->emplace_back(object_A, std::move(p_WPs), object_B,
Expand Down
77 changes: 35 additions & 42 deletions multibody/plant/discrete_update_manager.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,8 @@
#include <limits>
#include <utility>

#include "multibody/topology/spanning_forest.h"

#include "drake/multibody/plant/contact_properties.h"
#include "drake/multibody/plant/deformable_driver.h"
#include "drake/multibody/plant/deformable_model.h"
Expand All @@ -20,8 +22,6 @@ using drake::math::RigidTransform;
using drake::math::RotationMatrix;
using drake::multibody::contact_solvers::internal::ContactSolverResults;
using drake::multibody::contact_solvers::internal::MatrixBlock;
using drake::multibody::internal::DiscreteContactPair;
using drake::multibody::internal::MultibodyTreeTopology;
using drake::systems::Context;
using drake::systems::DependencyTicket;

Expand Down Expand Up @@ -212,11 +212,6 @@ const MultibodyPlant<T>& DiscreteUpdateManager<T>::plant() const {
return *plant_;
}

template <typename T>
const MultibodyTree<T>& DiscreteUpdateManager<T>::internal_tree() const {
return MultibodyPlantDiscreteUpdateManagerAttorney<T>::internal_tree(plant());
}

template <typename T>
void DiscreteUpdateManager<T>::CalcNonContactForces(
const drake::systems::Context<T>& context,
Expand Down Expand Up @@ -644,7 +639,7 @@ void DiscreteUpdateManager<T>::AppendDiscreteContactPairsForPointContact(
contact_pairs->Reserve(num_point_contacts, 0, 0);
const geometry::SceneGraphInspector<T>& inspector =
plant().EvalSceneGraphInspector(context);
const MultibodyTreeTopology& topology = internal_tree().get_topology();
const SpanningForest& forest = get_forest();
const Eigen::VectorBlock<const VectorX<T>> v = plant().GetVelocities(context);
const Frame<T>& frame_W = plant().world_frame();

Expand All @@ -663,10 +658,13 @@ void DiscreteUpdateManager<T>::AppendDiscreteContactPairsForPointContact(
const BodyIndex body_B_index = FindBodyByGeometryId(pair.id_B);
const RigidBody<T>& body_B = plant().get_body(body_B_index);

const TreeIndex treeA_index = topology.body_to_tree_index(body_A_index);
const TreeIndex treeB_index = topology.body_to_tree_index(body_B_index);
const bool treeA_has_dofs = topology.tree_has_dofs(treeA_index);
const bool treeB_has_dofs = topology.tree_has_dofs(treeB_index);
const TreeIndex& tree_A_index = forest.link_to_tree(body_A_index);
const bool tree_A_has_dofs =
tree_A_index.is_valid() && forest.trees(tree_A_index).nv() > 0;

const TreeIndex& tree_B_index = forest.link_to_tree(body_B_index);
const bool tree_B_has_dofs =
tree_B_index.is_valid() && forest.trees(tree_B_index).nv() > 0;

const T kA = GetPointContactStiffness(
pair.id_A, default_contact_stiffness(), inspector);
Expand Down Expand Up @@ -711,25 +709,21 @@ void DiscreteUpdateManager<T>::AppendDiscreteContactPairsForPointContact(
jacobian_blocks.reserve(2);

// Tree A contribution to contact Jacobian Jv_W_AcBc_C.
if (treeA_has_dofs) {
Matrix3X<T> J =
R_WC.matrix().transpose() *
Jv_AcBc_W.middleCols(
tree_topology().tree_velocities_start_in_v(treeA_index),
tree_topology().num_tree_velocities(treeA_index));
jacobian_blocks.emplace_back(treeA_index, MatrixBlock<T>(std::move(J)));
if (tree_A_has_dofs) {
const SpanningForest::Tree& tree_A = forest.trees(tree_A_index);
Matrix3X<T> J = R_WC.matrix().transpose() *
Jv_AcBc_W.middleCols(tree_A.v_start(), tree_A.nv());
jacobian_blocks.emplace_back(tree_A_index, MatrixBlock<T>(std::move(J)));
}

// Tree B contribution to contact Jacobian Jv_W_AcBc_C.
// This contribution must be added only if B is different from A.
if ((treeB_has_dofs && !treeA_has_dofs) ||
(treeB_has_dofs && treeB_index != treeA_index)) {
Matrix3X<T> J =
R_WC.matrix().transpose() *
Jv_AcBc_W.middleCols(
tree_topology().tree_velocities_start_in_v(treeB_index),
tree_topology().num_tree_velocities(treeB_index));
jacobian_blocks.emplace_back(treeB_index, MatrixBlock<T>(std::move(J)));
if ((tree_B_has_dofs && !tree_A_has_dofs) ||
(tree_B_has_dofs && tree_B_index != tree_A_index)) {
const SpanningForest::Tree& tree_B = forest.trees(tree_B_index);
Matrix3X<T> J = R_WC.matrix().transpose() *
Jv_AcBc_W.middleCols(tree_B.v_start(), tree_B.nv());
jacobian_blocks.emplace_back(tree_B_index, MatrixBlock<T>(std::move(J)));
}

// Contact stiffness and damping
Expand Down Expand Up @@ -812,7 +806,7 @@ void DiscreteUpdateManager<T>::AppendDiscreteContactPairsForHydroelasticContact(
contact_pairs->Reserve(0, num_hydro_contacts, 0);
const geometry::SceneGraphInspector<T>& inspector =
plant().EvalSceneGraphInspector(context);
const MultibodyTreeTopology& topology = internal_tree().get_topology();
const SpanningForest& forest = get_forest();
const Eigen::VectorBlock<const VectorX<T>> v = plant().GetVelocities(context);
const Frame<T>& frame_W = plant().world_frame();

Expand All @@ -837,10 +831,13 @@ void DiscreteUpdateManager<T>::AppendDiscreteContactPairsForHydroelasticContact(
const BodyIndex body_B_index = FindBodyByGeometryId(s.id_N());
const RigidBody<T>& body_B = plant().get_body(body_B_index);

const TreeIndex& tree_A_index = topology.body_to_tree_index(body_A_index);
const TreeIndex& tree_B_index = topology.body_to_tree_index(body_B_index);
const bool treeA_has_dofs = topology.tree_has_dofs(tree_A_index);
const bool treeB_has_dofs = topology.tree_has_dofs(tree_B_index);
const TreeIndex& tree_A_index = forest.link_to_tree(body_A_index);
const bool treeA_has_dofs =
tree_A_index.is_valid() && forest.trees(tree_A_index).nv() > 0;

const TreeIndex& tree_B_index = forest.link_to_tree(body_B_index);
const bool treeB_has_dofs =
tree_B_index.is_valid() && forest.trees(tree_B_index).nv() > 0;

// TODO(amcastro-tri): Consider making the modulus required, instead of
// a default infinite value.
Expand Down Expand Up @@ -951,11 +948,9 @@ void DiscreteUpdateManager<T>::AppendDiscreteContactPairsForHydroelasticContact(

// Tree A contribution to contact Jacobian Jv_W_AcBc_C.
if (treeA_has_dofs) {
Matrix3X<T> J =
R_WC.matrix().transpose() *
Jv_AcBc_W.middleCols(
tree_topology().tree_velocities_start_in_v(tree_A_index),
tree_topology().num_tree_velocities(tree_A_index));
const SpanningForest::Tree& tree_A = forest.trees(tree_A_index);
Matrix3X<T> J = R_WC.matrix().transpose() *
Jv_AcBc_W.middleCols(tree_A.v_start(), tree_A.nv());
jacobian_blocks.emplace_back(tree_A_index,
MatrixBlock<T>(std::move(J)));
}
Expand All @@ -964,11 +959,9 @@ void DiscreteUpdateManager<T>::AppendDiscreteContactPairsForHydroelasticContact(
// This contribution must be added only if B is different from A.
if ((treeB_has_dofs && !treeA_has_dofs) ||
(treeB_has_dofs && tree_B_index != tree_A_index)) {
Matrix3X<T> J =
R_WC.matrix().transpose() *
Jv_AcBc_W.middleCols(
tree_topology().tree_velocities_start_in_v(tree_B_index),
tree_topology().num_tree_velocities(tree_B_index));
const SpanningForest::Tree& tree_B = forest.trees(tree_B_index);
Matrix3X<T> J = R_WC.matrix().transpose() *
Jv_AcBc_W.middleCols(tree_B.v_start(), tree_B.nv());
jacobian_blocks.emplace_back(tree_B_index,
MatrixBlock<T>(std::move(J)));
}
Expand Down
11 changes: 4 additions & 7 deletions multibody/plant/discrete_update_manager.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -220,7 +220,9 @@ class DiscreteUpdateManager : public ScalarConvertibleComponent<T> {
/* N.B. Keep the spelling and order of declarations here identical to the
MultibodyPlantDiscreteUpdateManagerAttorney spelling and order of same. */

const MultibodyTree<T>& internal_tree() const;
const MultibodyTree<T>& internal_tree() const {
return GetInternalTree(this->plant());
}

systems::CacheEntry& DeclareCacheEntry(std::string description,
systems::ValueProducer,
Expand All @@ -236,13 +238,8 @@ class DiscreteUpdateManager : public ScalarConvertibleComponent<T> {

/* @} */

const MultibodyTreeTopology& tree_topology() const {
return internal::GetInternalTree(this->plant()).get_topology();
}
const SpanningForest& get_forest() const { return internal_tree().forest(); }

const internal::SpanningForest& forest() const {
return internal::GetInternalTree(this->plant()).forest();
}
/* Returns the pointer to the DeformableDriver owned by `this` manager if one
exists. Otherwise, returns nullptr. */
const DeformableDriver<double>* deformable_driver() const {
Expand Down
Loading