Skip to content

Commit 085db92

Browse files
committed
Add DeformableBody as a new MultibodyElement
Since deformable bodies have discrete states, allow MultibodyElement to declare discrete states in the tree system. Move the bookkeeping for deformable bodies from DeformableModel to DeformableBody. The deformable bodies are stored as an element collection in DeformableModel. Remove deformable_ids.h and move the definitions there into multibody_tree_indexes.h.
1 parent 35b44d5 commit 085db92

20 files changed

+1177
-476
lines changed

multibody/plant/BUILD.bazel

Lines changed: 0 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -19,7 +19,6 @@ drake_cc_package_library(
1919
":contact_jacobians",
2020
":contact_results",
2121
":coulomb_friction",
22-
":deformable_ids",
2322
":desired_state_input",
2423
":discrete_contact_data",
2524
":discrete_contact_pair",
@@ -43,7 +42,6 @@ drake_cc_library(
4342
"constraint_specs.h",
4443
],
4544
deps = [
46-
":deformable_ids",
4745
"//multibody/tree:multibody_tree_indexes",
4846
],
4947
)
@@ -105,7 +103,6 @@ drake_cc_library(
105103
hdrs = [
106104
"compliant_contact_manager.h",
107105
"deformable_driver.h",
108-
"deformable_ids.h",
109106
"deformable_model.h",
110107
"discrete_step_memory.h",
111108
"discrete_update_manager.h",
@@ -128,7 +125,6 @@ drake_cc_library(
128125
":contact_jacobians",
129126
":contact_results",
130127
":coulomb_friction",
131-
":deformable_ids",
132128
":desired_state_input",
133129
":discrete_contact_data",
134130
":discrete_contact_pair",
@@ -262,16 +258,6 @@ drake_cc_library(
262258
],
263259
)
264260

265-
drake_cc_library(
266-
name = "deformable_ids",
267-
hdrs = [
268-
"deformable_ids.h",
269-
],
270-
deps = [
271-
"//common:identifier",
272-
],
273-
)
274-
275261
drake_cc_library(
276262
name = "desired_state_input",
277263
srcs = [

multibody/plant/constraint_specs.h

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -10,7 +10,6 @@
1010
#include <vector>
1111

1212
#include "drake/math/rigid_transform.h"
13-
#include "drake/multibody/plant/deformable_ids.h"
1413
#include "drake/multibody/tree/multibody_tree_indexes.h"
1514

1615
namespace drake {

multibody/plant/deformable_driver.cc

Lines changed: 15 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -631,18 +631,17 @@ void DeformableDriver<T>::AppendDeformableRigidFixedConstraintKinematics(
631631
.template Eval<geometry::GeometryConfigurationVector<T>>(context);
632632
for (DeformableBodyIndex index(0); index < deformable_model_->num_bodies();
633633
++index) {
634-
DeformableBodyId body_id = deformable_model_->GetBodyId(index);
635-
if (!deformable_model_->is_enabled(body_id, context) ||
636-
!deformable_model_->HasConstraint(body_id)) {
634+
const DeformableBody<T>& body = deformable_model_->GetBody(index);
635+
if (!body.is_enabled(context) || !body.has_fixed_constraint()) {
637636
continue;
638637
}
639638

640-
const FemModel<T>& fem_model = deformable_model_->GetFemModel(body_id);
639+
const FemModel<T>& fem_model = body.fem_model();
641640
const DirichletBoundaryCondition<T>& bc =
642641
fem_model.dirichlet_boundary_condition();
643642

644-
/* Returns true iff for the deformable body with `body_id`, the given vertex
645-
index is under boundary condition. */
643+
/* Returns true iff for the deformable body, the given vertex index is under
644+
boundary condition. */
646645
auto is_under_bc = [&](int vertex_index) {
647646
return bc.index_to_boundary_state().count(
648647
multibody::fem::FemNodeIndex(vertex_index)) > 0;
@@ -653,15 +652,13 @@ void DeformableDriver<T>::AppendDeformableRigidFixedConstraintKinematics(
653652
// Each deformable body forms its own clique and are indexed in inceasing
654653
// DeformableBodyIndex order and placed after all rigid cliques.
655654
const TreeIndex clique_index_A(tree_topology.num_trees() + index);
656-
GeometryId geometry_id = deformable_model_->GetGeometryId(body_id);
655+
const GeometryId geometry_id = body.geometry_id();
657656
const PartialPermutation& vertex_permutation =
658657
EvalVertexPermutation(context, geometry_id);
659658
const VectorX<T>& p_WVs = configurations.value(geometry_id);
660659

661-
for (const MultibodyConstraintId& constraint_id :
662-
deformable_model_->fixed_constraint_ids(body_id)) {
663-
const DeformableRigidFixedConstraintSpec& spec =
664-
deformable_model_->fixed_constraint_spec(constraint_id);
660+
for (const DeformableRigidFixedConstraintSpec& spec :
661+
body.fixed_constraint_specs()) {
665662
const int num_vertices_in_constraint = ssize(spec.vertices);
666663
/* The Jacobian block for the deformable body A. */
667664
Block3x3SparseMatrix<T> negative_Jv_v_WAp(
@@ -1056,23 +1053,21 @@ void DeformableDriver<T>::CalcDeformableContact(
10561053
/* Complete the result with information on constraints other than contact. */
10571054
for (DeformableBodyIndex body_index(0);
10581055
body_index < deformable_model_->num_bodies(); ++body_index) {
1059-
DeformableBodyId body_id = deformable_model_->GetBodyId(body_index);
1056+
const DeformableBody<T>& body = deformable_model_->GetBody(body_index);
10601057
/* Add in constraints. */
1061-
if (deformable_model_->HasConstraint(body_id)) {
1058+
if (body.has_fixed_constraint()) {
10621059
std::unordered_set<int> fixed_vertices;
1063-
for (const MultibodyConstraintId& constraint_id :
1064-
deformable_model_->fixed_constraint_ids(body_id)) {
1065-
const DeformableRigidFixedConstraintSpec& spec =
1066-
deformable_model_->fixed_constraint_spec(constraint_id);
1060+
for (const DeformableRigidFixedConstraintSpec& spec :
1061+
body.fixed_constraint_specs()) {
10671062
fixed_vertices.insert(spec.vertices.begin(), spec.vertices.end());
10681063
}
10691064
/* Register the geometry in case it hasn't been registered because it's
10701065
not participating in contact but is participating in other types of
10711066
constraints. */
1072-
GeometryId geometry_id = deformable_model_->GetGeometryId(body_id);
1067+
GeometryId geometry_id = body.geometry_id();
10731068
if (!result->IsRegistered(geometry_id)) {
1074-
result->RegisterDeformableGeometry(
1075-
geometry_id, deformable_model_->GetFemModel(body_id).num_nodes());
1069+
result->RegisterDeformableGeometry(geometry_id,
1070+
body.fem_model().num_nodes());
10761071
}
10771072
result->Participate(geometry_id, fixed_vertices);
10781073
}

multibody/plant/deformable_ids.h

Lines changed: 0 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -6,13 +6,6 @@
66
namespace drake {
77
namespace multibody {
88

9-
/** Uniquely identifies a deformable body. It is valid before and after
10-
Finalize(). */
11-
using DeformableBodyId = Identifier<class DeformableBodyTag>;
12-
13-
/** (Internal use only) Indexes deformable bodies, only used after Finalize().
14-
*/
15-
using DeformableBodyIndex = TypeSafeIndex<class DeformableBodyTag>;
169

1710
} // namespace multibody
1811
} // namespace drake

0 commit comments

Comments
 (0)