Skip to content

[fem] Refactor ForceDensityField #23033

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

Merged
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
4 changes: 2 additions & 2 deletions examples/multibody/deformable/point_source_force_field.cc
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@ namespace drake {
namespace examples {
namespace deformable {

using multibody::ForceDensityField;
using multibody::ForceDensityFieldBase;
using multibody::MultibodyPlant;
using multibody::RigidBody;
using systems::BasicVector;
Expand Down Expand Up @@ -38,7 +38,7 @@ Vector3<double> PointSourceForceField::DoEvaluateAt(
return magnitude * p_QC_W / p_QC_W.norm();
}

std::unique_ptr<ForceDensityField<double>> PointSourceForceField::DoClone()
std::unique_ptr<ForceDensityFieldBase<double>> PointSourceForceField::DoClone()
const {
return std::make_unique<PointSourceForceField>(
*plant_, plant_->get_body(body_), p_BC_, falloff_distance_);
Expand Down
4 changes: 2 additions & 2 deletions examples/multibody/deformable/point_source_force_field.h
Original file line number Diff line number Diff line change
Expand Up @@ -2,8 +2,8 @@

#include <memory>

#include "drake/multibody/plant/force_density_field.h"
#include "drake/multibody/plant/multibody_plant.h"
#include "drake/multibody/tree/force_density_field.h"

namespace drake {
namespace examples {
Expand Down Expand Up @@ -61,7 +61,7 @@ class PointSourceForceField final
Vector3<double> DoEvaluateAt(const systems::Context<double>& context,
const Vector3<double>& p_WQ) const final;

std::unique_ptr<ForceDensityField<double>> DoClone() const final;
std::unique_ptr<ForceDensityFieldBase<double>> DoClone() const final;

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

Expand Down
26 changes: 25 additions & 1 deletion multibody/fem/BUILD.bazel
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@ drake_cc_package_library(
":fem_solver",
":fem_state",
":fem_state_system",
":force_density_field_base",
":isoparametric_element",
":linear_constitutive_model",
":linear_constitutive_model_data",
Expand Down Expand Up @@ -251,9 +252,9 @@ drake_cc_library(
"fem_plant_data.h",
],
deps = [
":force_density_field_base",
"//common:default_scalars",
"//common:essential",
"//multibody/plant:force_density_field",
"//systems/framework:context",
],
)
Expand Down Expand Up @@ -308,6 +309,21 @@ drake_cc_library(
],
)

drake_cc_library(
name = "force_density_field_base",
srcs = [
"force_density_field_base.cc",
],
hdrs = [
"force_density_field_base.h",
],
deps = [
"//common:default_scalars",
"//common:essential",
"//systems/framework:context",
],
)

drake_cc_library(
name = "isoparametric_element",
srcs = [
Expand Down Expand Up @@ -715,6 +731,14 @@ drake_cc_googletest(
],
)

drake_cc_googletest(
name = "force_density_field_base_test",
deps = [
":force_density_field_base",
"//multibody/plant",
],
)

drake_cc_googletest(
name = "isoparametric_element_test",
deps = [
Expand Down
10 changes: 5 additions & 5 deletions multibody/fem/fem_plant_data.h
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@

#include "drake/common/default_scalars.h"
#include "drake/common/drake_copyable.h"
#include "drake/multibody/plant/force_density_field.h"
#include "drake/multibody/fem/force_density_field_base.h"
#include "drake/systems/framework/context.h"

namespace drake {
Expand All @@ -19,14 +19,14 @@ template <typename T>
struct FemPlantData {
public:
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(FemPlantData);
FemPlantData(
const systems::Context<T>& plant_context_in,
const std::vector<const ForceDensityField<T>*>& force_density_fields_in)
FemPlantData(const systems::Context<T>& plant_context_in,
const std::vector<const ForceDensityFieldBase<T>*>&
force_density_fields_in)
: plant_context(plant_context_in),
force_density_fields(force_density_fields_in) {}

const systems::Context<T>& plant_context;
std::vector<const ForceDensityField<T>*> force_density_fields;
std::vector<const ForceDensityFieldBase<T>*> force_density_fields;
};

} // namespace fem
Expand Down
13 changes: 13 additions & 0 deletions multibody/fem/force_density_field_base.cc
Original file line number Diff line number Diff line change
@@ -0,0 +1,13 @@
#include "drake/multibody/fem/force_density_field_base.h"

namespace drake {
namespace multibody {

template <typename T>
ForceDensityFieldBase<T>::~ForceDensityFieldBase() = default;

} // namespace multibody
} // namespace drake

DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
class ::drake::multibody::ForceDensityFieldBase);
82 changes: 82 additions & 0 deletions multibody/fem/force_density_field_base.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,82 @@
#pragma once

#include <memory>

#include "drake/common/default_scalars.h"
#include "drake/common/drake_copyable.h"
#include "drake/common/eigen_types.h"
#include "drake/systems/framework/context.h"

namespace drake {
namespace multibody {

template <typename T>
class ForceDensityField;

/** (Advanced) Enum for the type of force density in ForceDensityFieldBase. */
enum class ForceDensityType {
/** ForceDensityFieldBase::EvaluateAt() returns the force per unit of
_current_ (deformed) configuration volume. */
kPerCurrentVolume,
/** ForceDensityFieldBase::EvaluateAt() returns the force per unit of
_reference_ configuration volume where the reference undeformed configuration
is defined by the input mesh provided by the user. */
kPerReferenceVolume,
};

/** The %ForceDensityFieldBase class is an abstract base class that represents a
force density field affecting deformable bodies in a MultibodyPlant. The
force field is described by the member function EvaluateAt() which takes as
input a position in the world frame and returns the force density from the
force density field at the given location, with unit [N/m³]. To create a
concrete %ForceDensityFieldBase class, inherit from ForceDensityField instead
of directly inheriting from %ForceDensityFieldBase.
@tparam_default_scalar */
template <typename T>
class ForceDensityFieldBase {
public:
virtual ~ForceDensityFieldBase() = 0;

/** Evaluates the force density [N/m³] with the given `context` of the
owning MultibodyPlant and a position in world, `p_WQ`. */
Vector3<T> EvaluateAt(const systems::Context<T>& context,
const Vector3<T>& p_WQ) const {
return DoEvaluateAt(context, p_WQ);
}

/** Returns an identical copy of `this` ForceDensityFieldBase. */
std::unique_ptr<ForceDensityFieldBase<T>> Clone() const { return DoClone(); }

/* (Advanced) Returns the force density type of `this` %ForceDensityField.
*/
ForceDensityType density_type() const { return density_type_; }

protected:
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(ForceDensityFieldBase);

/** Derived classes must override this function to provide a threadsafe
implemention to the NVI EvaluateAt(). */
virtual Vector3<T> DoEvaluateAt(const systems::Context<T>& context,
const Vector3<T>& p_WQ) const = 0;

/** Derived classes must override this function to implement the NVI
Clone(). */
virtual std::unique_ptr<ForceDensityFieldBase<T>> DoClone() const = 0;

private:
friend class ForceDensityField<T>;

/* Private constructor exposed only to ForceDensityField. This prevents
users from creating concrete force densities that directly inherit from
ForceDensityFieldBase. */
explicit ForceDensityFieldBase(ForceDensityType density_type)
: density_type_(density_type) {}

ForceDensityType density_type_;
};

} // namespace multibody
} // namespace drake

DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
class ::drake::multibody::ForceDensityFieldBase);
2 changes: 1 addition & 1 deletion multibody/fem/test/dummy_element.h
Original file line number Diff line number Diff line change
Expand Up @@ -151,7 +151,7 @@ class DummyElement final : public FemElement<DummyElement<is_linear>> {
EigenPtr<Vector<T, kNumDofs>> result) const {
for (int i = 0; i < this->num_nodes; ++i) {
const auto node_q = data.element_q.template segment<3>(3 * i);
for (const multibody::ForceDensityField<T>* force_density :
for (const multibody::ForceDensityFieldBase<T>* force_density :
plant_data.force_density_fields) {
result->template segment<3>(3 * i) +=
scale * force_density->EvaluateAt(plant_data.plant_context, node_q);
Expand Down
1 change: 1 addition & 0 deletions multibody/fem/test/fem_element_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@

#include "drake/multibody/fem/test/dummy_element.h"
#include "drake/multibody/plant/multibody_plant.h"
#include "drake/multibody/tree/force_density_field.h"

namespace drake {
namespace multibody {
Expand Down
3 changes: 2 additions & 1 deletion multibody/fem/test/fem_model_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@
#include "drake/multibody/fem/volumetric_element.h"
#include "drake/multibody/fem/volumetric_model.h"
#include "drake/multibody/plant/multibody_plant.h"
#include "drake/multibody/tree/force_density_field.h"

namespace drake {
namespace multibody {
Expand Down Expand Up @@ -134,7 +135,7 @@ GTEST_TEST(FemModelTest, CalcResidualWithContextDependentExternalForce) {
return context.get_time() * unit_vector_;
};

std::unique_ptr<ForceDensityField<double>> DoClone() const final {
std::unique_ptr<ForceDensityFieldBase<double>> DoClone() const final {
return std::make_unique<TimeScaledForceDensityField>(*this);
}

Expand Down
113 changes: 113 additions & 0 deletions multibody/fem/test/force_density_field_base_test.cc
Original file line number Diff line number Diff line change
@@ -0,0 +1,113 @@
#include "drake/multibody/fem/force_density_field_base.h"

#include <gtest/gtest.h>

#include "drake/multibody/plant/multibody_plant.h"
#include "drake/multibody/tree/force_density_field.h"

namespace drake {
namespace multibody {
namespace fem {
namespace {

using drake::systems::BasicVector;
using Eigen::Vector3d;

/* A concrete force density field for testing. */
class ConstantForceDensityField final : public ForceDensityField<double> {
public:
/* Constructs an force density field that has the functional form given by
input `field` which is then scaled by a scalar value via input port. */
explicit ConstantForceDensityField(
std::function<Vector3<double>(const Vector3<double>&)> field)
: field_(std::move(field)) {}

/* Gets the double-valued input port -- the input port value scales the
force density multiplicatively. */
const systems::InputPort<double>& get_input_port() const {
return parent_system_or_throw().get_input_port(scale_port_index_);
}

private:
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(ConstantForceDensityField);

Vector3<double> DoEvaluateAt(const systems::Context<double>& context,
const Vector3<double>& p_WQ) const final {
return get_input_port().Eval(context)(0) * field_(p_WQ);
};

std::unique_ptr<ForceDensityFieldBase<double>> DoClone() const final {
return std::unique_ptr<ConstantForceDensityField>(
new ConstantForceDensityField(*this));
}

void DoDeclareInputPorts(multibody::MultibodyPlant<double>* plant) final {
scale_port_index_ =
this->DeclareVectorInputPort(plant, "on/off signal for the force field",
BasicVector<double>(1.0))
.get_index();
}

std::function<Vector3<double>(const Vector3<double>&)> field_;
systems::InputPortIndex scale_port_index_;
};

class ForceDensityFieldTest : public ::testing::Test {
protected:
void SetUp() override {
/* Define an arbtrary force density field. */
auto force_field = [](const Vector3d& x) {
return Vector3d(std::sin(x[0]), std::cos(x[1]), x[2]);
};
dut_ = std::make_unique<ConstantForceDensityField>(force_field);
dut_->DeclareSystemResources(&plant_);
plant_.Finalize();
}

std::unique_ptr<ConstantForceDensityField> dut_;
MultibodyPlant<double> plant_{0.01};
};

TEST_F(ForceDensityFieldTest, EvaluateAt) {
auto plant_context = plant_.CreateDefaultContext();
EXPECT_EQ(dut_->density_type(), ForceDensityType::kPerCurrentVolume);
const double scale = 2.71;
dut_->get_input_port().FixValue(plant_context.get(), Vector1d(scale));
const Vector3d p_WQ(1, 2, 3);
const Vector3d expected_force_density =
scale * Vector3d(std::sin(p_WQ[0]), std::cos(p_WQ[1]), p_WQ[2]);
EXPECT_EQ(dut_->EvaluateAt(*plant_context, p_WQ), expected_force_density);
}

TEST_F(ForceDensityFieldTest, Clone) {
auto clone = dut_->Clone();
ASSERT_NE(clone, nullptr);
EXPECT_EQ(clone->density_type(), dut_->density_type());

auto concrete_clone = dynamic_cast<ConstantForceDensityField*>(clone.get());
ASSERT_NE(concrete_clone, nullptr);
MultibodyPlant<double> plant_clone(0.01);
concrete_clone->DeclareSystemResources(&plant_clone);
plant_clone.Finalize();
auto plant_context = plant_.CreateDefaultContext();
auto clone_context = plant_clone.CreateDefaultContext();

const double scale = 2.71;
dut_->get_input_port().FixValue(plant_context.get(), Vector1d(scale));
concrete_clone->get_input_port().FixValue(clone_context.get(),
Vector1d(scale));
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
for (int k = 0; k < 3; ++k) {
const Vector3d p_WQ(i, j, k);
EXPECT_EQ(dut_->EvaluateAt(*plant_context, p_WQ),
clone->EvaluateAt(*clone_context, p_WQ));
}
}
}
}

} // namespace
} // namespace fem
} // namespace multibody
} // namespace drake
3 changes: 2 additions & 1 deletion multibody/fem/test/volumetric_element_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@
#include "drake/multibody/fem/linear_simplex_element.h"
#include "drake/multibody/fem/simplex_gaussian_quadrature.h"
#include "drake/multibody/plant/multibody_plant.h"
#include "drake/multibody/tree/force_density_field.h"

namespace drake {
namespace multibody {
Expand Down Expand Up @@ -429,7 +430,7 @@ TEST_F(VolumetricElementTest, PerCurrentVolumeExternalForce) {
return f_;
};

std::unique_ptr<ForceDensityField<AD>> DoClone() const final {
std::unique_ptr<ForceDensityFieldBase<AD>> DoClone() const final {
return std::make_unique<ConstantForceDensityField>(*this);
}

Expand Down
2 changes: 1 addition & 1 deletion multibody/fem/volumetric_element.h
Original file line number Diff line number Diff line change
Expand Up @@ -441,7 +441,7 @@ class VolumetricElement
const Matrix3<T>& deformation_gradient =
data.deformation_gradient_data[q].deformation_gradient();
Vector3<T> scaled_force = Vector3<T>::Zero();
for (const multibody::ForceDensityField<T>* force_density :
for (const multibody::ForceDensityFieldBase<T>* force_density :
plant_data.force_density_fields) {
DRAKE_ASSERT(force_density != nullptr);
const T change_of_volume =
Expand Down
Loading