Skip to content

Commit 89cc4ab

Browse files
committed
[fem] Refactor ForceDensityField
The goal of the commit is the remove the dependency of the fem library from the tree library so that later we create deformable multibody element (in the tree library) that depends on fem.
1 parent bd7834f commit 89cc4ab

23 files changed

+284
-94
lines changed

examples/multibody/deformable/point_source_force_field.cc

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -4,7 +4,7 @@ namespace drake {
44
namespace examples {
55
namespace deformable {
66

7-
using multibody::ForceDensityField;
7+
using multibody::ForceDensityFieldBase;
88
using multibody::MultibodyPlant;
99
using multibody::RigidBody;
1010
using systems::BasicVector;
@@ -38,7 +38,7 @@ Vector3<double> PointSourceForceField::DoEvaluateAt(
3838
return magnitude * p_QC_W / p_QC_W.norm();
3939
}
4040

41-
std::unique_ptr<ForceDensityField<double>> PointSourceForceField::DoClone()
41+
std::unique_ptr<ForceDensityFieldBase<double>> PointSourceForceField::DoClone()
4242
const {
4343
return std::make_unique<PointSourceForceField>(
4444
*plant_, plant_->get_body(body_), p_BC_, falloff_distance_);

examples/multibody/deformable/point_source_force_field.h

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -2,8 +2,8 @@
22

33
#include <memory>
44

5-
#include "drake/multibody/plant/force_density_field.h"
65
#include "drake/multibody/plant/multibody_plant.h"
6+
#include "drake/multibody/tree/force_density_field.h"
77

88
namespace drake {
99
namespace examples {
@@ -61,7 +61,7 @@ class PointSourceForceField final
6161
Vector3<double> DoEvaluateAt(const systems::Context<double>& context,
6262
const Vector3<double>& p_WQ) const final;
6363

64-
std::unique_ptr<ForceDensityField<double>> DoClone() const final;
64+
std::unique_ptr<ForceDensityFieldBase<double>> DoClone() const final;
6565

6666
void DoDeclareCacheEntries(multibody::MultibodyPlant<double>* plant) final;
6767

multibody/fem/BUILD.bazel

Lines changed: 25 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -32,6 +32,7 @@ drake_cc_package_library(
3232
":fem_solver",
3333
":fem_state",
3434
":fem_state_system",
35+
":force_density_field_base",
3536
":isoparametric_element",
3637
":linear_constitutive_model",
3738
":linear_constitutive_model_data",
@@ -251,9 +252,9 @@ drake_cc_library(
251252
"fem_plant_data.h",
252253
],
253254
deps = [
255+
":force_density_field_base",
254256
"//common:default_scalars",
255257
"//common:essential",
256-
"//multibody/plant:force_density_field",
257258
"//systems/framework:context",
258259
],
259260
)
@@ -308,6 +309,21 @@ drake_cc_library(
308309
],
309310
)
310311

312+
drake_cc_library(
313+
name = "force_density_field_base",
314+
srcs = [
315+
"force_density_field_base.cc",
316+
],
317+
hdrs = [
318+
"force_density_field_base.h",
319+
],
320+
deps = [
321+
"//common:default_scalars",
322+
"//common:essential",
323+
"//systems/framework:context",
324+
],
325+
)
326+
311327
drake_cc_library(
312328
name = "isoparametric_element",
313329
srcs = [
@@ -715,6 +731,14 @@ drake_cc_googletest(
715731
],
716732
)
717733

734+
drake_cc_googletest(
735+
name = "force_density_field_base_test",
736+
deps = [
737+
":force_density_field_base",
738+
"//multibody/plant",
739+
],
740+
)
741+
718742
drake_cc_googletest(
719743
name = "isoparametric_element_test",
720744
deps = [

multibody/fem/fem_plant_data.h

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@
33

44
#include "drake/common/default_scalars.h"
55
#include "drake/common/drake_copyable.h"
6-
#include "drake/multibody/plant/force_density_field.h"
6+
#include "drake/multibody/fem/force_density_field_base.h"
77
#include "drake/systems/framework/context.h"
88

99
namespace drake {
@@ -19,14 +19,14 @@ template <typename T>
1919
struct FemPlantData {
2020
public:
2121
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(FemPlantData);
22-
FemPlantData(
23-
const systems::Context<T>& plant_context_in,
24-
const std::vector<const ForceDensityField<T>*>& force_density_fields_in)
22+
FemPlantData(const systems::Context<T>& plant_context_in,
23+
const std::vector<const ForceDensityFieldBase<T>*>&
24+
force_density_fields_in)
2525
: plant_context(plant_context_in),
2626
force_density_fields(force_density_fields_in) {}
2727

2828
const systems::Context<T>& plant_context;
29-
std::vector<const ForceDensityField<T>*> force_density_fields;
29+
std::vector<const ForceDensityFieldBase<T>*> force_density_fields;
3030
};
3131

3232
} // namespace fem
Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,13 @@
1+
#include "drake/multibody/fem/force_density_field_base.h"
2+
3+
namespace drake {
4+
namespace multibody {
5+
6+
template <typename T>
7+
ForceDensityFieldBase<T>::~ForceDensityFieldBase() = default;
8+
9+
} // namespace multibody
10+
} // namespace drake
11+
12+
DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
13+
class ::drake::multibody::ForceDensityFieldBase);
Lines changed: 82 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,82 @@
1+
#pragma once
2+
3+
#include <memory>
4+
5+
#include "drake/common/default_scalars.h"
6+
#include "drake/common/drake_copyable.h"
7+
#include "drake/common/eigen_types.h"
8+
#include "drake/systems/framework/context.h"
9+
10+
namespace drake {
11+
namespace multibody {
12+
13+
template <typename T>
14+
class ForceDensityField;
15+
16+
/** (Advanced) Enum for the type of force density in ForceDensityFieldBase. */
17+
enum class ForceDensityType {
18+
/** ForceDensityFieldBase::EvaluateAt() returns the force per unit of
19+
_current_ (deformed) configuration volume. */
20+
kPerCurrentVolume,
21+
/** ForceDensityFieldBase::EvaluateAt() returns the force per unit of
22+
_reference_ configuration volume where the reference undeformed configuration
23+
is defined by the input mesh provided by the user. */
24+
kPerReferenceVolume,
25+
};
26+
27+
/** The %ForceDensityFieldBase class is an abstract base class that represents a
28+
force density field affecting deformable bodies in a MultibodyPlant. The
29+
force field is described by the member function EvaluateAt() which takes as
30+
input a position in the world frame and returns the force density from the
31+
force density field at the given location, with unit [N/m³]. To create a
32+
concrete %ForceDensityFieldBase class, inherit from ForceDensityField instead
33+
of directly inheriting from %ForceDensityFieldBase.
34+
@tparam_default_scalar */
35+
template <typename T>
36+
class ForceDensityFieldBase {
37+
public:
38+
virtual ~ForceDensityFieldBase() = 0;
39+
40+
/** Evaluates the force density [N/m³] with the given `context` of the
41+
owning MultibodyPlant and a position in world, `p_WQ`. */
42+
Vector3<T> EvaluateAt(const systems::Context<T>& context,
43+
const Vector3<T>& p_WQ) const {
44+
return DoEvaluateAt(context, p_WQ);
45+
}
46+
47+
/** Returns an identical copy of `this` ForceDensityFieldBase. */
48+
std::unique_ptr<ForceDensityFieldBase<T>> Clone() const { return DoClone(); }
49+
50+
/* (Advanced) Returns the force density type of `this` %ForceDensityField.
51+
*/
52+
ForceDensityType density_type() const { return density_type_; }
53+
54+
protected:
55+
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(ForceDensityFieldBase);
56+
57+
/** Derived classes must override this function to provide a threadsafe
58+
implemention to the NVI EvaluateAt(). */
59+
virtual Vector3<T> DoEvaluateAt(const systems::Context<T>& context,
60+
const Vector3<T>& p_WQ) const = 0;
61+
62+
/** Derived classes must override this function to implement the NVI
63+
Clone(). */
64+
virtual std::unique_ptr<ForceDensityFieldBase<T>> DoClone() const = 0;
65+
66+
private:
67+
friend class ForceDensityField<T>;
68+
69+
/* Private constructor exposed only to ForceDensityField. This prevents
70+
users from creating concrete force densities that directly inherit from
71+
ForceDensityFieldBase. */
72+
explicit ForceDensityFieldBase(ForceDensityType density_type)
73+
: density_type_(density_type) {}
74+
75+
ForceDensityType density_type_;
76+
};
77+
78+
} // namespace multibody
79+
} // namespace drake
80+
81+
DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
82+
class ::drake::multibody::ForceDensityFieldBase);

multibody/fem/test/dummy_element.h

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -151,7 +151,7 @@ class DummyElement final : public FemElement<DummyElement<is_linear>> {
151151
EigenPtr<Vector<T, kNumDofs>> result) const {
152152
for (int i = 0; i < this->num_nodes; ++i) {
153153
const auto node_q = data.element_q.template segment<3>(3 * i);
154-
for (const multibody::ForceDensityField<T>* force_density :
154+
for (const multibody::ForceDensityFieldBase<T>* force_density :
155155
plant_data.force_density_fields) {
156156
result->template segment<3>(3 * i) +=
157157
scale * force_density->EvaluateAt(plant_data.plant_context, node_q);

multibody/fem/test/fem_element_test.cc

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@
44

55
#include "drake/multibody/fem/test/dummy_element.h"
66
#include "drake/multibody/plant/multibody_plant.h"
7+
#include "drake/multibody/tree/force_density_field.h"
78

89
namespace drake {
910
namespace multibody {

multibody/fem/test/fem_model_test.cc

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@
1111
#include "drake/multibody/fem/volumetric_element.h"
1212
#include "drake/multibody/fem/volumetric_model.h"
1313
#include "drake/multibody/plant/multibody_plant.h"
14+
#include "drake/multibody/tree/force_density_field.h"
1415

1516
namespace drake {
1617
namespace multibody {
@@ -134,7 +135,7 @@ GTEST_TEST(FemModelTest, CalcResidualWithContextDependentExternalForce) {
134135
return context.get_time() * unit_vector_;
135136
};
136137

137-
std::unique_ptr<ForceDensityField<double>> DoClone() const final {
138+
std::unique_ptr<ForceDensityFieldBase<double>> DoClone() const final {
138139
return std::make_unique<TimeScaledForceDensityField>(*this);
139140
}
140141

Lines changed: 113 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,113 @@
1+
#include "drake/multibody/fem/force_density_field_base.h"
2+
3+
#include <gtest/gtest.h>
4+
5+
#include "drake/multibody/plant/multibody_plant.h"
6+
#include "drake/multibody/tree/force_density_field.h"
7+
8+
namespace drake {
9+
namespace multibody {
10+
namespace fem {
11+
namespace {
12+
13+
using drake::systems::BasicVector;
14+
using Eigen::Vector3d;
15+
16+
/* A concrete force density field for testing. */
17+
class ConstantForceDensityField final : public ForceDensityField<double> {
18+
public:
19+
/* Constructs an force density field that has the functional form given by
20+
input `field` which is then scaled by a scalar value via input port. */
21+
explicit ConstantForceDensityField(
22+
std::function<Vector3<double>(const Vector3<double>&)> field)
23+
: field_(std::move(field)) {}
24+
25+
/* Gets the double-valued input port -- the input port value scales the
26+
force density multiplicatively. */
27+
const systems::InputPort<double>& get_input_port() const {
28+
return parent_system_or_throw().get_input_port(scale_port_index_);
29+
}
30+
31+
private:
32+
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(ConstantForceDensityField);
33+
34+
Vector3<double> DoEvaluateAt(const systems::Context<double>& context,
35+
const Vector3<double>& p_WQ) const final {
36+
return get_input_port().Eval(context)(0) * field_(p_WQ);
37+
};
38+
39+
std::unique_ptr<ForceDensityFieldBase<double>> DoClone() const final {
40+
return std::unique_ptr<ConstantForceDensityField>(
41+
new ConstantForceDensityField(*this));
42+
}
43+
44+
void DoDeclareInputPorts(multibody::MultibodyPlant<double>* plant) final {
45+
scale_port_index_ =
46+
this->DeclareVectorInputPort(plant, "on/off signal for the force field",
47+
BasicVector<double>(1.0))
48+
.get_index();
49+
}
50+
51+
std::function<Vector3<double>(const Vector3<double>&)> field_;
52+
systems::InputPortIndex scale_port_index_;
53+
};
54+
55+
class ForceDensityFieldTest : public ::testing::Test {
56+
protected:
57+
void SetUp() override {
58+
/* Define an arbtrary force density field. */
59+
auto force_field = [](const Vector3d& x) {
60+
return Vector3d(std::sin(x[0]), std::cos(x[1]), x[2]);
61+
};
62+
dut_ = std::make_unique<ConstantForceDensityField>(force_field);
63+
dut_->DeclareSystemResources(&plant_);
64+
plant_.Finalize();
65+
}
66+
67+
std::unique_ptr<ConstantForceDensityField> dut_;
68+
MultibodyPlant<double> plant_{0.01};
69+
};
70+
71+
TEST_F(ForceDensityFieldTest, EvaluateAt) {
72+
auto plant_context = plant_.CreateDefaultContext();
73+
EXPECT_EQ(dut_->density_type(), ForceDensityType::kPerCurrentVolume);
74+
const double scale = 2.71;
75+
dut_->get_input_port().FixValue(plant_context.get(), Vector1d(scale));
76+
const Vector3d p_WQ(1, 2, 3);
77+
const Vector3d expected_force_density =
78+
scale * Vector3d(std::sin(p_WQ[0]), std::cos(p_WQ[1]), p_WQ[2]);
79+
EXPECT_EQ(dut_->EvaluateAt(*plant_context, p_WQ), expected_force_density);
80+
}
81+
82+
TEST_F(ForceDensityFieldTest, Clone) {
83+
auto clone = dut_->Clone();
84+
ASSERT_NE(clone, nullptr);
85+
EXPECT_EQ(clone->density_type(), dut_->density_type());
86+
87+
auto concrete_clone = dynamic_cast<ConstantForceDensityField*>(clone.get());
88+
ASSERT_NE(concrete_clone, nullptr);
89+
MultibodyPlant<double> plant_clone(0.01);
90+
concrete_clone->DeclareSystemResources(&plant_clone);
91+
plant_clone.Finalize();
92+
auto plant_context = plant_.CreateDefaultContext();
93+
auto clone_context = plant_clone.CreateDefaultContext();
94+
95+
const double scale = 2.71;
96+
dut_->get_input_port().FixValue(plant_context.get(), Vector1d(scale));
97+
concrete_clone->get_input_port().FixValue(clone_context.get(),
98+
Vector1d(scale));
99+
for (int i = 0; i < 3; ++i) {
100+
for (int j = 0; j < 3; ++j) {
101+
for (int k = 0; k < 3; ++k) {
102+
const Vector3d p_WQ(i, j, k);
103+
EXPECT_EQ(dut_->EvaluateAt(*plant_context, p_WQ),
104+
clone->EvaluateAt(*clone_context, p_WQ));
105+
}
106+
}
107+
}
108+
}
109+
110+
} // namespace
111+
} // namespace fem
112+
} // namespace multibody
113+
} // namespace drake

multibody/fem/test/volumetric_element_test.cc

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@
1111
#include "drake/multibody/fem/linear_simplex_element.h"
1212
#include "drake/multibody/fem/simplex_gaussian_quadrature.h"
1313
#include "drake/multibody/plant/multibody_plant.h"
14+
#include "drake/multibody/tree/force_density_field.h"
1415

1516
namespace drake {
1617
namespace multibody {
@@ -429,7 +430,7 @@ TEST_F(VolumetricElementTest, PerCurrentVolumeExternalForce) {
429430
return f_;
430431
};
431432

432-
std::unique_ptr<ForceDensityField<AD>> DoClone() const final {
433+
std::unique_ptr<ForceDensityFieldBase<AD>> DoClone() const final {
433434
return std::make_unique<ConstantForceDensityField>(*this);
434435
}
435436

multibody/fem/volumetric_element.h

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -441,7 +441,7 @@ class VolumetricElement
441441
const Matrix3<T>& deformation_gradient =
442442
data.deformation_gradient_data[q].deformation_gradient();
443443
Vector3<T> scaled_force = Vector3<T>::Zero();
444-
for (const multibody::ForceDensityField<T>* force_density :
444+
for (const multibody::ForceDensityFieldBase<T>* force_density :
445445
plant_data.force_density_fields) {
446446
DRAKE_ASSERT(force_density != nullptr);
447447
const T change_of_volume =

0 commit comments

Comments
 (0)