Skip to content

Commit

Permalink
[fem] Add Rayleigh damping model (RobotLocomotion#16496)
Browse files Browse the repository at this point in the history
  • Loading branch information
xuchenhan-tri authored Feb 4, 2022
1 parent bb4d6f9 commit 8fdef42
Show file tree
Hide file tree
Showing 4 changed files with 125 additions and 0 deletions.
23 changes: 23 additions & 0 deletions multibody/fem/BUILD.bazel
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@ drake_cc_package_library(
deps = [
":calc_lame_parameters",
":constitutive_model",
":damping_model",
":deformation_gradient_data",
":isoparametric_element",
":linear_constitutive_model_data",
Expand Down Expand Up @@ -50,6 +51,20 @@ drake_cc_library(
],
)

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

drake_cc_library(
name = "deformation_gradient_data",
srcs = [
Expand Down Expand Up @@ -150,6 +165,14 @@ drake_cc_googletest(
],
)

drake_cc_googletest(
name = "damping_model_test",
deps = [
":damping_model",
"//common/test_utilities:expect_throws_message",
],
)

drake_cc_googletest(
name = "deformation_gradient_data_test",
deps = [
Expand Down
23 changes: 23 additions & 0 deletions multibody/fem/damping_model.cc
Original file line number Diff line number Diff line change
@@ -0,0 +1,23 @@
#include "drake/multibody/fem/damping_model.h"

#include "drake/common/drake_throw.h"

namespace drake {
namespace multibody {
namespace fem {

template <typename T>
DampingModel<T>::DampingModel(const T& mass_coeff_alpha,
const T& stiffness_coeff_beta)
: mass_coeff_alpha_(mass_coeff_alpha),
stiffness_coeff_beta_(stiffness_coeff_beta) {
DRAKE_THROW_UNLESS(mass_coeff_alpha >= 0.0);
DRAKE_THROW_UNLESS(stiffness_coeff_beta >= 0.0);
}

} // namespace fem
} // namespace multibody
} // namespace drake

DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
class ::drake::multibody::fem::DampingModel);
48 changes: 48 additions & 0 deletions multibody/fem/damping_model.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,48 @@
#pragma once

#include "drake/common/default_scalars.h"
#include "drake/common/drake_copyable.h"

namespace drake {
namespace multibody {
namespace fem {

/** A viscous Rayleigh damping model in which the damping matrix D is a linear
combination of mass and stiffness matrices, as, D = αM + βK where α and β are
nonnegative. The damping ratio ζ for a given natural frequency ωₙ of a mode of
vibration can be calculated as ζ = (α/ωₙ + βωₙ)/2. Notice the contribution of
the stiffness term βK to the damping matrix D causes ζ to be proportional to a
mode's natural frequency ωₙ whereas the contribution of the mass term αM to the
damping matrix D causes ζ to be inversely proportional to a mode's natural
frequency ωₙ. In the context of rigid body motion (ωₙ = 0), only the αM term
contributes to the damping matrix D, hence if rigid body motion (or low value
of ωₙ) is expected, α should be kept small. One way to determine numerical
values for α and β is to somehow obtain reasonable estimates of the range of
ωₙ, and then choose numerical values for ζ (e.g 0 ≤ ζ < 0.05). Thereafter
calculate the associated numerical values of α and β.
@tparam_nonsymbolic_scalar */
template <typename T>
class DampingModel {
public:
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(DampingModel);

/** Constructs a Rayleigh damping model by storing the mass coefficient α and
the stiffness coefficient β that appears in the damping matrix D = αM + βK.
@throw std::exception if either `mass_coeff_alpha` or `stiffness_coeff_beta`
is negative. */
DampingModel(const T& mass_coeff_alpha, const T& stiffness_coeff_beta);

const T& mass_coeff_alpha() const { return mass_coeff_alpha_; }
const T& stiffness_coeff_beta() const { return stiffness_coeff_beta_; }

private:
T mass_coeff_alpha_{};
T stiffness_coeff_beta_{};
};

} // namespace fem
} // namespace multibody
} // namespace drake

DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
class ::drake::multibody::fem::DampingModel);
31 changes: 31 additions & 0 deletions multibody/fem/test/damping_model_test.cc
Original file line number Diff line number Diff line change
@@ -0,0 +1,31 @@
#include "drake/multibody/fem/damping_model.h"

#include <gtest/gtest.h>

#include "drake/common/test_utilities/expect_throws_message.h"

namespace drake {
namespace multibody {
namespace fem {
namespace {

GTEST_TEST(DampingModelTest, Getters) {
const double mass_coeff_alpha = 1.0;
const double stiffness_coeff_beta = 2.0;
const DampingModel<double> model(mass_coeff_alpha, stiffness_coeff_beta);
EXPECT_EQ(model.mass_coeff_alpha(), mass_coeff_alpha);
EXPECT_EQ(model.stiffness_coeff_beta(), stiffness_coeff_beta);
}

GTEST_TEST(DampingModelTest, InvalidModel) {
/* Negative coefficients are not allowed. */
EXPECT_THROW(DampingModel<double>(1.0, -1.0), std::exception);
EXPECT_THROW(DampingModel<double>(1.0, -1.0), std::exception);
/* Zero coefficients are OK. */
EXPECT_NO_THROW(DampingModel<double>(0, 0));
}

} // namespace
} // namespace fem
} // namespace multibody
} // namespace drake

0 comments on commit 8fdef42

Please sign in to comment.