diff --git a/multibody/fixed_fem/dev/BUILD.bazel b/multibody/fixed_fem/dev/BUILD.bazel index 4a97dfa6c118..2d0bf4e2599c 100644 --- a/multibody/fixed_fem/dev/BUILD.bazel +++ b/multibody/fixed_fem/dev/BUILD.bazel @@ -11,6 +11,20 @@ package( default_visibility = ["//visibility:private"], ) +drake_cc_library( + name = "acceleration_newmark_scheme", + srcs = [ + "acceleration_newmark_scheme.cc", + ], + hdrs = [ + "acceleration_newmark_scheme.h", + ], + deps = [ + ":newmark_scheme", + "//common:default_scalars", + ], +) + drake_cc_library( name = "collision_objects", srcs = [ @@ -231,9 +245,9 @@ drake_cc_library( "dynamic_elasticity_model.h", ], deps = [ + ":acceleration_newmark_scheme", ":damping_model", ":elasticity_model", - ":newmark_scheme", "//geometry/proximity:volume_mesh", ], ) @@ -551,6 +565,20 @@ drake_cc_library( ], ) +drake_cc_library( + name = "velocity_newmark_scheme", + srcs = [ + "velocity_newmark_scheme.cc", + ], + hdrs = [ + "velocity_newmark_scheme.h", + ], + deps = [ + ":newmark_scheme", + "//common:default_scalars", + ], +) + drake_cc_library( name = "zeroth_order_state_updater", hdrs = [ @@ -571,6 +599,16 @@ drake_cc_library( ], ) +drake_cc_googletest( + name = "acceleration_newmark_scheme_test", + deps = [ + ":acceleration_newmark_scheme", + ":dummy_element", + ":velocity_newmark_scheme", + "//common/test_utilities:eigen_matrix_compare", + ], +) + drake_cc_googletest( name = "collision_objects_test", data = ["//geometry:test_obj_files"], @@ -795,15 +833,6 @@ drake_cc_googletest( ], ) -drake_cc_googletest( - name = "newmark_scheme_test", - deps = [ - ":dummy_element", - ":newmark_scheme", - "//common/test_utilities:eigen_matrix_compare", - ], -) - drake_cc_googletest( name = "permute_block_sparse_matrix_test", deps = [ @@ -862,6 +891,16 @@ drake_cc_googletest( ], ) +drake_cc_googletest( + name = "velocity_newmark_scheme_test", + deps = [ + ":acceleration_newmark_scheme", + ":dummy_element", + ":velocity_newmark_scheme", + "//common/test_utilities:eigen_matrix_compare", + ], +) + drake_cc_googletest( name = "zeroth_order_state_updater_test", deps = [ diff --git a/multibody/fixed_fem/dev/acceleration_newmark_scheme.cc b/multibody/fixed_fem/dev/acceleration_newmark_scheme.cc new file mode 100644 index 000000000000..0b89b0452b10 --- /dev/null +++ b/multibody/fixed_fem/dev/acceleration_newmark_scheme.cc @@ -0,0 +1,39 @@ +#include "drake/multibody/fixed_fem/dev/acceleration_newmark_scheme.h" + +namespace drake { +namespace multibody { +namespace fem { +namespace internal { + +template +void AccelerationNewmarkScheme::DoUpdateStateFromChangeInUnknowns( + const VectorX& dz, FemStateBase* state) const { + const VectorX& a = state->qddot(); + const VectorX& v = state->qdot(); + const VectorX& x = state->q(); + state->SetQddot(a + dz); + state->SetQdot(v + dt() * gamma() * dz); + state->SetQ(x + dt() * dt() * beta() * dz); +} + +template +void AccelerationNewmarkScheme::DoAdvanceOneTimeStep( + const FemStateBase& prev_state, const VectorX& unknown_variable, + FemStateBase* state) const { + const VectorX& an = prev_state.qddot(); + const VectorX& vn = prev_state.qdot(); + const VectorX& xn = prev_state.q(); + const VectorX& a = unknown_variable; + state->SetQddot(a); + state->SetQdot(vn + dt() * (gamma() * a + (1.0 - gamma()) * an)); + state->SetQ(xn + dt() * vn + + dt() * dt() * (beta() * a + (0.5 - beta()) * an)); +} + +} // namespace internal +} // namespace fem +} // namespace multibody +} // namespace drake + +DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS( + class ::drake::multibody::fem::internal::AccelerationNewmarkScheme) diff --git a/multibody/fixed_fem/dev/acceleration_newmark_scheme.h b/multibody/fixed_fem/dev/acceleration_newmark_scheme.h new file mode 100644 index 000000000000..9b0133afea0f --- /dev/null +++ b/multibody/fixed_fem/dev/acceleration_newmark_scheme.h @@ -0,0 +1,64 @@ +#pragma once + +#include "drake/common/default_scalars.h" +#include "drake/multibody/fixed_fem/dev/newmark_scheme.h" + +namespace drake { +namespace multibody { +namespace fem { +namespace internal { + +/* Implements NewmarkScheme with acceleration as the unknown variable. + Given the value for the current time step acceleration `a`, the current state + can be calculated from the state from the previous time step according to the + following equations: + + v = vₙ + dt ⋅ (γ ⋅ a + (1−γ) ⋅ aₙ) + x = xₙ + dt ⋅ vₙ + dt² ⋅ [β ⋅ a + (0.5−β) ⋅ aₙ]. + + See NewmarkScheme for the reference to the Newmark-beta integration scheme. + See VelocityNewmarkScheme for the same integration scheme implemented with + velocity as the unknown variable. + @tparam_nonsymbolic_scalar */ +template +class AccelerationNewmarkScheme final : public NewmarkScheme { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(AccelerationNewmarkScheme); + + /* Constructs a Newmark scheme with acceleration as the unknown variable. + @pre dt_in > 0. + @pre 0.5 <= gamma_in <= 1. + @pre 0 <= beta_in <= 0.5. */ + AccelerationNewmarkScheme(double dt_in, double gamma_in, double beta_in) + : NewmarkScheme(dt_in, gamma_in, beta_in) {} + + ~AccelerationNewmarkScheme() = default; + + private: + using NewmarkScheme::dt; + using NewmarkScheme::gamma; + using NewmarkScheme::beta; + + Vector3 do_get_weights() const final { + return {beta() * dt() * dt(), gamma() * dt(), 1.0}; + } + + const VectorX& DoGetUnknowns(const FemStateBase& state) const final { + return state.qddot(); + } + + void DoUpdateStateFromChangeInUnknowns(const VectorX& dz, + FemStateBase* state) const final; + + void DoAdvanceOneTimeStep(const FemStateBase& prev_state, + const VectorX& unknown_variable, + FemStateBase* state) const final; +}; + +} // namespace internal +} // namespace fem +} // namespace multibody +} // namespace drake + +DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS( + class ::drake::multibody::fem::internal::AccelerationNewmarkScheme) diff --git a/multibody/fixed_fem/dev/dynamic_elasticity_model.h b/multibody/fixed_fem/dev/dynamic_elasticity_model.h index 7506ae6941ae..321d2cab44bf 100644 --- a/multibody/fixed_fem/dev/dynamic_elasticity_model.h +++ b/multibody/fixed_fem/dev/dynamic_elasticity_model.h @@ -6,9 +6,9 @@ #include "drake/common/eigen_types.h" #include "drake/geometry/proximity/volume_mesh.h" +#include "drake/multibody/fixed_fem/dev/acceleration_newmark_scheme.h" #include "drake/multibody/fixed_fem/dev/damping_model.h" #include "drake/multibody/fixed_fem/dev/elasticity_model.h" -#include "drake/multibody/fixed_fem/dev/newmark_scheme.h" namespace drake { namespace multibody { @@ -34,7 +34,8 @@ class DynamicElasticityModel : public ElasticityModel { */ explicit DynamicElasticityModel(double dt) : ElasticityModel( - std::make_unique>(dt, 0.5, 0.25)) {} + std::make_unique>(dt, 0.5, + 0.25)) {} ~DynamicElasticityModel() = default; diff --git a/multibody/fixed_fem/dev/elasticity_element.h b/multibody/fixed_fem/dev/elasticity_element.h index c277807b1ac0..128ac39663f2 100644 --- a/multibody/fixed_fem/dev/elasticity_element.h +++ b/multibody/fixed_fem/dev/elasticity_element.h @@ -197,7 +197,7 @@ class ElasticityElement : public FemElement { @param[in] reference_positions The positions (in world frame) of the nodes of this element in the reference configuration. @param[in] denstiy The mass density of the element with unit kg/m³. - @param[in] gravity The gravitational accleration (in world frame) for the + @param[in] gravity The gravitational acceleration (in world frame) for the new element with unit m/s². @pre element_index must be valid. @pre density > 0. */ diff --git a/multibody/fixed_fem/dev/newmark_scheme.h b/multibody/fixed_fem/dev/newmark_scheme.h index fc59d5499069..d2113c120d64 100644 --- a/multibody/fixed_fem/dev/newmark_scheme.h +++ b/multibody/fixed_fem/dev/newmark_scheme.h @@ -6,6 +6,7 @@ namespace drake { namespace multibody { namespace fem { namespace internal { + /* Implements the interface StateUpdater with Newmark-beta time integration scheme. Given the value for the current time step acceleration `a`, the states are calculated from states from the previous time step according to the @@ -14,64 +15,41 @@ namespace internal { v = vₙ + dt ⋅ (γ ⋅ a + (1−γ) ⋅ aₙ) x = xₙ + dt ⋅ vₙ + dt² ⋅ [β ⋅ a + (0.5−β) ⋅ aₙ]. + Note that the scheme is unconditionally unstable for gamma < 0.5 and therefore + we require gamma >= 0.5. See [Newmark, 1959] for the original reference for the method. [Newmark, 1959] Newmark, Nathan M. "A method of computation for structural dynamics." Journal of the engineering mechanics division 85.3 (1959): 67-94. @tparam_nonsymbolic_scalar */ template -class NewmarkScheme final : public StateUpdater { +class NewmarkScheme : public StateUpdater { public: + ~NewmarkScheme() = default; + + protected: DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(NewmarkScheme); /* Constructs a Newmark scheme with the provided timestep, `gamma` and `beta`. @pre dt > 0. - @pre 0 <= gamma <= 1. + @pre 0.5 <= gamma <= 1. @pre 0 <= beta <= 0.5. */ NewmarkScheme(double dt, double gamma, double beta) : dt_(dt), gamma_(gamma), beta_(beta) { DRAKE_DEMAND(dt > 0); - DRAKE_DEMAND(0 <= gamma && gamma <= 1); + DRAKE_DEMAND(0.5 <= gamma && gamma <= 1); DRAKE_DEMAND(0 <= beta && beta <= 0.5); } - ~NewmarkScheme() = default; - - private: - Vector3 do_get_weights() const final { - return {beta_ * dt_ * dt_, gamma_ * dt_, 1.0}; - } - - const VectorX& DoGetUnknowns(const FemStateBase& state) const final { - return state.qddot(); - } - - void DoUpdateStateFromChangeInUnknowns(const VectorX& dz, - FemStateBase* state) const final { - const VectorX& a = state->qddot(); - const VectorX& v = state->qdot(); - const VectorX& x = state->q(); - state->SetQddot(a + dz); - state->SetQdot(v + dt_ * gamma_ * dz); - state->SetQ(x + dt_ * dt_ * beta_ * dz); - } - - void DoAdvanceOneTimeStep(const FemStateBase& prev_state, - const VectorX& unknown_variable, - FemStateBase* state) const final { - const VectorX& an = prev_state.qddot(); - const VectorX& vn = prev_state.qdot(); - const VectorX& xn = prev_state.q(); - const VectorX& a = unknown_variable; - state->SetQddot(a); - state->SetQdot(vn + dt_ * (gamma_ * a + (1.0 - gamma_) * an)); - state->SetQ(xn + dt_ * vn + dt_ * dt_ * (beta_ * a + (0.5 - beta_) * an)); - } + double dt() const { return dt_; } + double gamma() const { return gamma_; } + double beta() const { return beta_; } double dt_{0}; double gamma_{0.5}; double beta_{0.25}; }; + } // namespace internal } // namespace fem } // namespace multibody diff --git a/multibody/fixed_fem/dev/test/acceleration_newmark_scheme_test.cc b/multibody/fixed_fem/dev/test/acceleration_newmark_scheme_test.cc new file mode 100644 index 000000000000..3e555017296e --- /dev/null +++ b/multibody/fixed_fem/dev/test/acceleration_newmark_scheme_test.cc @@ -0,0 +1,105 @@ +#include "drake/multibody/fixed_fem/dev/acceleration_newmark_scheme.h" + +#include + +#include "drake/common/test_utilities/eigen_matrix_compare.h" +#include "drake/multibody/fixed_fem/dev/test/dummy_element.h" +#include "drake/multibody/fixed_fem/dev/velocity_newmark_scheme.h" + +namespace drake { +namespace multibody { +namespace fem { +namespace internal { +namespace { + +using fem::test::DummyElement; + +const double kDt = 1e-3; +const double kGamma = 0.6; +const double kBeta = 0.3; +const double kTolerance = 4 * std::numeric_limits::epsilon(); + +Vector4 MakeQ() { return Vector4(1.23, 2.34, 3.45, 4.56); } +Vector4 MakeQdot() { return Vector4(5.67, 6.78, 7.89, 9.10); } +Vector4 MakeQddot() { + return Vector4(0.1011, 0.1112, 0.1213, 0.1314); +} + +/* Verify that the weights returned by the accessor match expectation. */ +GTEST_TEST(AccelerationNewmarkSchemeTest, Weights) { + AccelerationNewmarkScheme scheme{kDt, kGamma, kBeta}; + EXPECT_TRUE(CompareMatrices( + scheme.weights(), Vector3(kBeta * kDt * kDt, kGamma * kDt, 1.0), + 0)); +} + +/* Verify that the result of + `AccelerationNewmarkScheme::UpdateStateFromChangeInUnknowns()` is consistent + with the weights. */ +GTEST_TEST(AccelerationNewmarkSchemeTest, UpdateStateFromChangeInUnknowns) { + AccelerationNewmarkScheme scheme{kDt, kGamma, kBeta}; + FemState> state0(MakeQ(), MakeQdot(), MakeQddot()); + FemState> state(state0); + const Vector4 dz(1.234, 4.567, 7.890, 0.123); + const Vector3& weights = scheme.weights(); + scheme.UpdateStateFromChangeInUnknowns(dz, &state); + EXPECT_TRUE( + CompareMatrices(state.q() - state0.q(), weights(0) * dz, kTolerance)); + EXPECT_TRUE(CompareMatrices(state.qdot() - state0.qdot(), weights(1) * dz, + kTolerance)); + EXPECT_TRUE(CompareMatrices(state.qddot() - state0.qddot(), weights(2) * dz, + kTolerance)); +} + +/* Tests that AccelerationNewmarkScheme reproduces analytical solutions with + constant acceleration. */ +GTEST_TEST(AccelerationNewmarkSchemeTest, AdvanceOneTimeStep) { + AccelerationNewmarkScheme scheme{kDt, kGamma, kBeta}; + const Vector4 q = MakeQ(); + const Vector4 qdot = MakeQdot(); + const Vector4 qddot = MakeQddot(); + const FemState> state_0(q, qdot, qddot); + FemState> state_n(state_0); + FemState> state_np1(state_0); + const int kTimeSteps = 10; + for (int i = 0; i < kTimeSteps; ++i) { + scheme.AdvanceOneTimeStep(state_n, qddot, &state_np1); + state_n = state_np1; + } + const double total_time = kDt * kTimeSteps; + EXPECT_TRUE(CompareMatrices(state_n.qddot(), state_0.qddot())); + EXPECT_TRUE(CompareMatrices(state_n.qdot(), + state_0.qdot() + total_time * state_0.qddot(), + kTimeSteps * kTolerance)); + EXPECT_TRUE( + CompareMatrices(state_n.q(), + state_0.q() + total_time * state_0.qdot() + + 0.5 * total_time * total_time * state_0.qddot(), + kTimeSteps * kTolerance)); +} + +/* Tests that `AccelerationNewmarkScheme` is equivalent with + `VelocityNewmarkScheme` by advancing one time step with each integration + scheme using the same initial state and verifying that the resulting new states + are the same. */ +GTEST_TEST(VelocityNewmarkSchemeTest, EquivalenceWithAccelerationNewmark) { + AccelerationNewmarkScheme acceleration_scheme{kDt, kGamma, kBeta}; + FemState> state0(MakeQ(), MakeQdot(), MakeQddot()); + FemState> state_a(state0); + acceleration_scheme.AdvanceOneTimeStep(state0, MakeQddot(), &state_a); + + VelocityNewmarkScheme velocity_scheme{kDt, kGamma, kBeta}; + FemState> state_v(state0); + velocity_scheme.AdvanceOneTimeStep(state0, state_a.qdot(), &state_v); + /* Set a larger error tolerance to accomodate the division by `dt` used in the + VelocityNewmarkScheme. */ + EXPECT_TRUE(CompareMatrices(state_v.qddot(), state_a.qddot(), 2.0e-12)); + EXPECT_TRUE(CompareMatrices(state_v.qdot(), state_a.qdot(), kTolerance)); + EXPECT_TRUE(CompareMatrices(state_v.q(), state_a.q(), kTolerance)); +} + +} // namespace +} // namespace internal +} // namespace fem +} // namespace multibody +} // namespace drake diff --git a/multibody/fixed_fem/dev/test/newmark_scheme_test.cc b/multibody/fixed_fem/dev/test/newmark_scheme_test.cc deleted file mode 100644 index 00fee32db5b1..000000000000 --- a/multibody/fixed_fem/dev/test/newmark_scheme_test.cc +++ /dev/null @@ -1,82 +0,0 @@ -#include "drake/multibody/fixed_fem/dev/newmark_scheme.h" - -#include - -#include "drake/common/test_utilities/eigen_matrix_compare.h" -#include "drake/multibody/fixed_fem/dev/test/dummy_element.h" - -namespace drake { -namespace multibody { -namespace fem { -namespace internal { -namespace { -using fem::test::DummyElement; -const double kDt = 1e-4; -const double kGamma = 0.2; -const double kBeta = 0.3; -const double kTol = std::numeric_limits::epsilon(); -class NewmarkSchemeTest : public ::testing::Test { - protected: - static Vector4 MakeQ() { - return Vector4(1.23, 2.34, 3.45, 4.56); - } - - static Vector4 MakeQdot() { - return Vector4(5.67, 6.78, 7.89, 9.10); - } - - static Vector4 MakeQddot() { - return Vector4(0.1011, 0.1112, 0.1213, 0.1314); - } - - NewmarkScheme newmark_scheme_{kDt, kGamma, kBeta}; -}; - -TEST_F(NewmarkSchemeTest, Weights) { - EXPECT_TRUE(CompareMatrices( - newmark_scheme_.weights(), - Vector3(kBeta * kDt * kDt, kGamma * kDt, 1.0), 0)); -} - -TEST_F(NewmarkSchemeTest, UpdateStateFromChangeInUnknowns) { - const Vector4 q = MakeQ(); - const Vector4 qdot = MakeQdot(); - const Vector4 qddot = MakeQddot(); - FemState> state(q, qdot, qddot); - const Vector4 dqddot(1.234, 4.567, 7.890, 0.123); - newmark_scheme_.UpdateStateFromChangeInUnknowns(dqddot, &state); - EXPECT_TRUE(CompareMatrices(state.qddot(), qddot + dqddot, 0)); - EXPECT_TRUE( - CompareMatrices(state.qdot(), qdot + dqddot * kDt * kGamma, kTol)); - EXPECT_TRUE(CompareMatrices(state.q(), q + dqddot * kDt * kDt * kBeta, kTol)); -} - -/* Tests that Newmark scheme reproduces analytical solutions under qddot. */ -TEST_F(NewmarkSchemeTest, AdvanceOneTimeStep) { - const Vector4 q = MakeQ(); - const Vector4 qdot = MakeQdot(); - const Vector4 qddot = MakeQddot(); - const FemState> state_0(q, qdot, qddot); - FemState> state_n(state_0); - FemState> state_np1(state_0); - const int kTimeSteps = 10; - for (int i = 0; i < kTimeSteps; ++i) { - newmark_scheme_.AdvanceOneTimeStep(state_n, qddot, &state_np1); - state_n = state_np1; - } - const double total_time = kDt * kTimeSteps; - EXPECT_TRUE(CompareMatrices(state_n.qddot(), state_0.qddot())); - EXPECT_TRUE(CompareMatrices(state_n.qdot(), - state_0.qdot() + total_time * state_0.qddot(), - kTimeSteps * kTol)); - EXPECT_TRUE( - CompareMatrices(state_n.q(), - state_0.q() + total_time * state_0.qdot() + - 0.5 * total_time * total_time * state_0.qddot(), - kTimeSteps * kTol)); -} -} // namespace -} // namespace internal -} // namespace fem -} // namespace multibody -} // namespace drake diff --git a/multibody/fixed_fem/dev/test/velocity_newmark_scheme_test.cc b/multibody/fixed_fem/dev/test/velocity_newmark_scheme_test.cc new file mode 100644 index 000000000000..6c1609801c64 --- /dev/null +++ b/multibody/fixed_fem/dev/test/velocity_newmark_scheme_test.cc @@ -0,0 +1,106 @@ +#include "drake/multibody/fixed_fem/dev/velocity_newmark_scheme.h" + +#include + +#include "drake/common/test_utilities/eigen_matrix_compare.h" +#include "drake/multibody/fixed_fem/dev/acceleration_newmark_scheme.h" +#include "drake/multibody/fixed_fem/dev/test/dummy_element.h" + +namespace drake { +namespace multibody { +namespace fem { +namespace internal { +namespace { + +using Eigen::Vector4d; +using fem::test::DummyElement; + +const double kDt = 1e-3; +const double kGamma = 0.6; +const double kBeta = 0.3; +const double kTolerance = 4 * std::numeric_limits::epsilon(); + +Vector4 MakeQ() { return Vector4(1.23, 2.34, 3.45, 4.56); } +Vector4 MakeQdot() { return Vector4(5.67, 6.78, 7.89, 9.10); } +Vector4 MakeQddot() { + return Vector4(0.1011, 0.1112, 0.1213, 0.1314); +} + +/* Verify that the weights returned by the accessor match expectation. */ +GTEST_TEST(VelocityNewmarkSchemeTest, Weights) { + VelocityNewmarkScheme scheme{kDt, kGamma, kBeta}; + EXPECT_TRUE(CompareMatrices( + scheme.weights(), + Vector3(kBeta / kGamma * kDt, 1.0, 1.0 / (kGamma * kDt)), 0)); +} + +/* Verify that the result of + `VelocityNewmarkScheme::UpdateStateFromChangeInUnknowns()` is consistent with + the weights. */ +GTEST_TEST(VelocityNewmarkSchemeTest, UpdateStateFromChangeInUnknowns) { + VelocityNewmarkScheme scheme{kDt, kGamma, kBeta}; + FemState> state0(MakeQ(), MakeQdot(), MakeQddot()); + FemState> state(state0); + const Vector4 dz(1.234, 4.567, 7.890, 0.123); + const Vector3& weights = scheme.weights(); + scheme.UpdateStateFromChangeInUnknowns(dz, &state); + EXPECT_TRUE( + CompareMatrices(state.q() - state0.q(), weights(0) * dz, kTolerance)); + EXPECT_TRUE(CompareMatrices(state.qdot() - state0.qdot(), weights(1) * dz, + kTolerance)); + EXPECT_TRUE(CompareMatrices(state.qddot() - state0.qddot(), weights(2) * dz, + kTolerance)); +} + +/* Tests that VelocityNewmarkScheme reproduces analytical solutions with + constant acceleration and linear velocity. */ +GTEST_TEST(VelocityNewmarkSchemeTest, AdvanceOneTimeStep) { + VelocityNewmarkScheme scheme{kDt, kGamma, kBeta}; + const Vector4d q = MakeQ(); + Vector4d qdot = MakeQdot(); + const Vector4d qddot = MakeQddot(); + const FemState> state_0(q, qdot, qddot); + FemState> state_n(state_0); + FemState> state_np1(state_0); + const int kTimeSteps = 10; + for (int i = 0; i < kTimeSteps; ++i) { + qdot += kDt * qddot; + scheme.AdvanceOneTimeStep(state_n, qdot, &state_np1); + state_n = state_np1; + } + const double total_time = kDt * kTimeSteps; + EXPECT_TRUE(CompareMatrices(state_n.qddot(), state_0.qddot(), + kTolerance * kTimeSteps / kDt)); + EXPECT_TRUE(CompareMatrices(state_n.qdot(), + state_0.qdot() + total_time * state_0.qddot(), + kTimeSteps * kTolerance)); + EXPECT_TRUE( + CompareMatrices(state_n.q(), + state_0.q() + total_time * state_0.qdot() + + 0.5 * total_time * total_time * state_0.qddot(), + kTimeSteps * kTolerance)); +} + +/* Tests that `VelocityNewmarkScheme` is equivalent with + `AccelerationNewmarkScheme` by advancing one time step with each integration + scheme using the same initial state and verifying that the resulting new states + are the same. */ +GTEST_TEST(VelocityNewmarkSchemeTest, EquivalenceWithAccelerationNewmark) { + VelocityNewmarkScheme velocity_scheme{kDt, kGamma, kBeta}; + FemState> state0(MakeQ(), MakeQdot(), MakeQddot()); + FemState> state_v(state0); + velocity_scheme.AdvanceOneTimeStep(state0, MakeQdot(), &state_v); + + AccelerationNewmarkScheme acceleration_scheme{kDt, kGamma, kBeta}; + FemState> state_a(state0); + acceleration_scheme.AdvanceOneTimeStep(state0, state_v.qddot(), &state_a); + EXPECT_TRUE(CompareMatrices(state_v.qddot(), state_a.qddot(), kTolerance)); + EXPECT_TRUE(CompareMatrices(state_v.qdot(), state_a.qdot(), kTolerance)); + EXPECT_TRUE(CompareMatrices(state_v.q(), state_a.q(), kTolerance)); +} + +} // namespace +} // namespace internal +} // namespace fem +} // namespace multibody +} // namespace drake diff --git a/multibody/fixed_fem/dev/velocity_newmark_scheme.cc b/multibody/fixed_fem/dev/velocity_newmark_scheme.cc new file mode 100644 index 000000000000..aab5ba1082ca --- /dev/null +++ b/multibody/fixed_fem/dev/velocity_newmark_scheme.cc @@ -0,0 +1,41 @@ +#include "drake/multibody/fixed_fem/dev/velocity_newmark_scheme.h" + +namespace drake { +namespace multibody { +namespace fem { +namespace internal { + +template +void VelocityNewmarkScheme::DoUpdateStateFromChangeInUnknowns( + const VectorX& dz, FemStateBase* state) const { + const VectorX& a = state->qddot(); + const VectorX& v = state->qdot(); + const VectorX& x = state->q(); + state->SetQddot(a + one_over_dt_gamma_ * dz); + state->SetQdot(v + dz); + state->SetQ(x + dt() * beta_over_gamma_ * dz); +} + +template +void VelocityNewmarkScheme::DoAdvanceOneTimeStep( + const FemStateBase& prev_state, const VectorX& unknown_variable, + FemStateBase* state) const { + const VectorX& an = prev_state.qddot(); + const VectorX& vn = prev_state.qdot(); + const VectorX& xn = prev_state.q(); + const VectorX& v = unknown_variable; + state->SetQddot(one_over_dt_gamma_ * (v - vn) - + (1.0 - gamma()) / gamma() * an); + state->SetQdot(v); + state->SetQ(xn + + dt() * (beta_over_gamma_ * v + (1.0 - beta_over_gamma_) * vn) + + dt() * dt() * (0.5 - beta_over_gamma_) * an); +} + +} // namespace internal +} // namespace fem +} // namespace multibody +} // namespace drake + +DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS( + class ::drake::multibody::fem::internal::VelocityNewmarkScheme) diff --git a/multibody/fixed_fem/dev/velocity_newmark_scheme.h b/multibody/fixed_fem/dev/velocity_newmark_scheme.h new file mode 100644 index 000000000000..3ed025c96204 --- /dev/null +++ b/multibody/fixed_fem/dev/velocity_newmark_scheme.h @@ -0,0 +1,68 @@ +#pragma once + +#include "drake/common/default_scalars.h" +#include "drake/multibody/fixed_fem/dev/newmark_scheme.h" + +namespace drake { +namespace multibody { +namespace fem { +namespace internal { + +/* Implements the interface StateUpdater with Newmark-beta time integration + scheme with velocity being the unknown variable. Given the value for the + current time step velocity `v`, the states are calculated from states from the + previous time step according to the following equations: + + a = (v - vₙ) / (dt ⋅ γ) - (1 − γ) / γ ⋅ aₙ + x = xₙ + dt ⋅ (β/γ ⋅ v + (1 - β/γ) ⋅ vₙ) + dt² ⋅ (0.5 − β/γ) ⋅ aₙ. + + See NewmarkScheme for the reference to the Newmark-beta integration scheme. + See AccelerationNewmarkScheme for the same integration scheme implemented with + acceleration as the unknown variable. + @tparam_nonsymbolic_scalar */ +template +class VelocityNewmarkScheme final : public NewmarkScheme { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(VelocityNewmarkScheme); + + /* Constructs a Newmark scheme with velocity as the unknown variable. + @pre dt_in > 0. + @pre 0.5 <= gamma_in <= 1. + @pre 0 <= beta_in <= 0.5. */ + VelocityNewmarkScheme(double dt_in, double gamma_in, double beta_in) + : NewmarkScheme(dt_in, gamma_in, beta_in), + beta_over_gamma_(beta_in / gamma_in), + one_over_dt_gamma_(1.0 / (dt() * gamma())) {} + + ~VelocityNewmarkScheme() = default; + + private: + using NewmarkScheme::dt; + using NewmarkScheme::gamma; + + Vector3 do_get_weights() const final { + return {beta_over_gamma_ * dt(), 1.0, one_over_dt_gamma_}; + } + + const VectorX& DoGetUnknowns(const FemStateBase& state) const final { + return state.qdot(); + } + + void DoUpdateStateFromChangeInUnknowns(const VectorX& dz, + FemStateBase* state) const final; + + void DoAdvanceOneTimeStep(const FemStateBase& prev_state, + const VectorX& unknown_variable, + FemStateBase* state) const final; + + double beta_over_gamma_; + double one_over_dt_gamma_; +}; + +} // namespace internal +} // namespace fem +} // namespace multibody +} // namespace drake + +DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS( + class ::drake::multibody::fem::internal::VelocityNewmarkScheme)