Velocity-Implicit Euler Fixed Steps. Simplified code and added descri…

…ptions.
RobotLocomotion · Jan 7, 2020 · 8ef182c · 8ef182c
1 parent da61fd3
commit 8ef182c
Show file tree

Hide file tree

Showing 5 changed files with 865 additions and 0 deletions.
diff --git a/systems/analysis/BUILD.bazel b/systems/analysis/BUILD.bazel
@@ -35,6 +35,7 @@ drake_cc_package_library(
         ":simulator",
         ":simulator_status",
         ":stepwise_dense_output",
+        ":velocity_implicit_euler_integrator",
     ],
 )
 
@@ -187,6 +188,19 @@ drake_cc_library(
     ],
 )
 
+drake_cc_library(
+    name = "velocity_implicit_euler_integrator",
+    srcs = ["velocity_implicit_euler_integrator.cc"],
+    hdrs = [
+        "velocity_implicit_euler_integrator.h",
+    ],
+    deps = [
+        ":implicit_integrator",
+        ":runge_kutta2_integrator",
+        "//math:gradient",
+    ],
+)
+
 drake_cc_library(
     name = "implicit_integrator",
     srcs = ["implicit_integrator.cc"],
@@ -381,6 +395,21 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "velocity_implicit_euler_integrator_test",
+    timeout = "moderate",
+    shard_count = 2,
+    tags = [
+        "no_memcheck",
+    ],
+    deps = [
+        ":velocity_implicit_euler_integrator",
+        "//common/test_utilities:expect_throws_message",
+        "//systems/analysis/test_utilities",
+        "//systems/plants/spring_mass_system",
+    ],
+)
+
 drake_cc_googletest(
     name = "implicit_integrator_test",
     deps = [

diff --git a/systems/analysis/implicit_integrator.h b/systems/analysis/implicit_integrator.h
@@ -350,6 +350,20 @@ class ImplicitIntegrator : public IntegratorBase<T> {
   /// @copydoc IntegratorBase::DoStep()
   virtual bool DoImplicitIntegratorStep(const T& h) = 0;
 
+  // Methods for derived classes to increment the factorization and Jacobian
+  // evaluation counts.
+  inline void increment_num_iter_factorizations() {
+    ++num_iter_factorizations_;
+  }
+
+  inline void increment_jacobian_computation_derivative_evaluations(int count) {
+    num_jacobian_function_evaluations_ += count;
+  }
+
+  inline void increment_jacobian_evaluations() {
+    ++num_jacobian_evaluations_;
+  }
+
  private:
   bool DoStep(const T& h) final {
     bool result = DoImplicitIntegratorStep(h);

diff --git a/systems/analysis/test/velocity_implicit_euler_integrator_test.cc b/systems/analysis/test/velocity_implicit_euler_integrator_test.cc
@@ -0,0 +1,29 @@
+#include "drake/systems/analysis/velocity_implicit_euler_integrator.h"
+
+#include <gtest/gtest.h>
+
+#include "drake/common/test_utilities/expect_throws_message.h"
+#include "drake/common/unused.h"
+#include "drake/systems/analysis/test_utilities/cubic_scalar_system.h"
+#include "drake/systems/analysis/test_utilities/discontinuous_spring_mass_damper_system.h"
+#include "drake/systems/analysis/test_utilities/implicit_integrator_test.h"
+#include "drake/systems/analysis/test_utilities/linear_scalar_system.h"
+#include "drake/systems/analysis/test_utilities/quadratic_scalar_system.h"
+#include "drake/systems/analysis/test_utilities/robertson_system.h"
+#include "drake/systems/analysis/test_utilities/spring_mass_damper_system.h"
+#include "drake/systems/analysis/test_utilities/stationary_system.h"
+#include "drake/systems/analysis/test_utilities/stiff_double_mass_spring_system.h"
+#include "drake/systems/plants/spring_mass_system/spring_mass_system.h"
+
+namespace drake {
+namespace systems {
+namespace analysis_test {
+
+// Test Velocity-Implicit Euler integrator on common implicit tests.
+typedef ::testing::Types<VelocityImplicitEulerIntegrator<double>> MyTypes;
+INSTANTIATE_TYPED_TEST_SUITE_P(My, ImplicitIntegratorTest, MyTypes);
+
+}  // namespace analysis_test
+
+}  // namespace systems
+}  // namespace drake
diff --git a/systems/analysis/velocity_implicit_euler_integrator.cc b/systems/analysis/velocity_implicit_euler_integrator.cc
@@ -0,0 +1,7 @@
+#include "drake/systems/analysis/velocity_implicit_euler_integrator.h"
+
+#include "drake/common/autodiff.h"
+
+DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
+    class ::drake::systems::VelocityImplicitEulerIntegrator)
+