[fem] Allow velocity to be the unknown variable in NewmarkScheme (#15593

) * Update NewmarkScheme so that it only stores the parameters and delegates the DoFoo() methods to concrete derived classes. * Add AccelerationNewmarkScheme which implements the original NewmarkScheme with `a` as the unknown variable. * Add VelocityNewmarkScheme that implements the NewmarkScheme with `v` as the unknown variable. * Restrict `gamma` to be greater than or equal to 0.5. The scheme is unstable otherwise.
RobotLocomotion · Aug 17, 2021 · 1df1c47 · 1df1c47
1 parent 5c7f033
commit 1df1c47
Show file tree

Hide file tree

Showing 11 changed files with 489 additions and 130 deletions.
diff --git 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
@@ -0,0 +1,39 @@
+#include "drake/multibody/fixed_fem/dev/acceleration_newmark_scheme.h"
+
+namespace drake {
+namespace multibody {
+namespace fem {
+namespace internal {
+
+template <typename T>
+void AccelerationNewmarkScheme<T>::DoUpdateStateFromChangeInUnknowns(
+    const VectorX<T>& dz, FemStateBase<T>* state) const {
+  const VectorX<T>& a = state->qddot();
+  const VectorX<T>& v = state->qdot();
+  const VectorX<T>& x = state->q();
+  state->SetQddot(a + dz);
+  state->SetQdot(v + dt() * gamma() * dz);
+  state->SetQ(x + dt() * dt() * beta() * dz);
+}
+
+template <typename T>
+void AccelerationNewmarkScheme<T>::DoAdvanceOneTimeStep(
+    const FemStateBase<T>& prev_state, const VectorX<T>& unknown_variable,
+    FemStateBase<T>* state) const {
+  const VectorX<T>& an = prev_state.qddot();
+  const VectorX<T>& vn = prev_state.qdot();
+  const VectorX<T>& xn = prev_state.q();
+  const VectorX<T>& 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
@@ -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 <typename T>
+class AccelerationNewmarkScheme final : public NewmarkScheme<T> {
+ 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<T>(dt_in, gamma_in, beta_in) {}
+
+  ~AccelerationNewmarkScheme() = default;
+
+ private:
+  using NewmarkScheme<T>::dt;
+  using NewmarkScheme<T>::gamma;
+  using NewmarkScheme<T>::beta;
+
+  Vector3<T> do_get_weights() const final {
+    return {beta() * dt() * dt(), gamma() * dt(), 1.0};
+  }
+
+  const VectorX<T>& DoGetUnknowns(const FemStateBase<T>& state) const final {
+    return state.qddot();
+  }
+
+  void DoUpdateStateFromChangeInUnknowns(const VectorX<T>& dz,
+                                         FemStateBase<T>* state) const final;
+
+  void DoAdvanceOneTimeStep(const FemStateBase<T>& prev_state,
+                            const VectorX<T>& unknown_variable,
+                            FemStateBase<T>* 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
@@ -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<Element> {
    */
   explicit DynamicElasticityModel(double dt)
       : ElasticityModel<Element>(
-            std::make_unique<internal::NewmarkScheme<T>>(dt, 0.5, 0.25)) {}
+            std::make_unique<internal::AccelerationNewmarkScheme<T>>(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
@@ -197,7 +197,7 @@ class ElasticityElement : public FemElement<DerivedElement, DerivedTraits> {
    @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
@@ -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 <typename T>
-class NewmarkScheme final : public StateUpdater<T> {
+class NewmarkScheme : public StateUpdater<T> {
  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<T> do_get_weights() const final {
-    return {beta_ * dt_ * dt_, gamma_ * dt_, 1.0};
-  }
-
-  const VectorX<T>& DoGetUnknowns(const FemStateBase<T>& state) const final {
-    return state.qddot();
-  }
-
-  void DoUpdateStateFromChangeInUnknowns(const VectorX<T>& dz,
-                                         FemStateBase<T>* state) const final {
-    const VectorX<T>& a = state->qddot();
-    const VectorX<T>& v = state->qdot();
-    const VectorX<T>& x = state->q();
-    state->SetQddot(a + dz);
-    state->SetQdot(v + dt_ * gamma_ * dz);
-    state->SetQ(x + dt_ * dt_ * beta_ * dz);
-  }
-
-  void DoAdvanceOneTimeStep(const FemStateBase<T>& prev_state,
-                            const VectorX<T>& unknown_variable,
-                            FemStateBase<T>* state) const final {
-    const VectorX<T>& an = prev_state.qddot();
-    const VectorX<T>& vn = prev_state.qdot();
-    const VectorX<T>& xn = prev_state.q();
-    const VectorX<T>& 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