-
Notifications
You must be signed in to change notification settings - Fork 1.3k
/
implicit_integrator_test.cc
141 lines (117 loc) · 5.16 KB
/
implicit_integrator_test.cc
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
#include "drake/systems/analysis/implicit_integrator.h"
#include <gtest/gtest.h>
#include "drake/systems/analysis/test_utilities/spring_mass_system.h"
using Eigen::VectorXd;
namespace drake {
namespace systems {
namespace {
// Dummy implicit integrator for testing protected methods of
// ImplicitIntegrator class.
class DummyImplicitIntegrator final : public ImplicitIntegrator<double> {
public:
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(DummyImplicitIntegrator)
DummyImplicitIntegrator(const System<double>& system,
Context<double>* context)
: ImplicitIntegrator(system, context) {}
bool supports_error_estimation() const override { return false; }
int get_error_estimate_order() const override { return 0; }
using ImplicitIntegrator<double>::IsUpdateZero;
// Returns whether DoResetCachedMatrices() has been called.
bool get_has_reset_cached_matrices() {
return has_reset_cached_matrices_;
}
private:
// There is no stepping so no stats should accumulate.
int64_t do_get_num_newton_raphson_iterations() const final { return 0; }
int64_t do_get_num_error_estimator_derivative_evaluations() const final {
return 0;
}
int64_t do_get_num_error_estimator_jacobian_evaluations() const final {
return 0;
}
int64_t do_get_num_error_estimator_derivative_evaluations_for_jacobian()
const final {
return 0;
}
int64_t do_get_num_error_estimator_newton_raphson_iterations() const final {
return 0;
}
int64_t do_get_num_error_estimator_iteration_matrix_factorizations()
const final {
return 0;
}
void DoResetCachedJacobianRelatedMatrices() final {
has_reset_cached_matrices_ = true;
}
bool DoImplicitIntegratorStep(const double& h) override {
throw std::logic_error("Dummy integrator not meant to be stepped.");
}
bool has_reset_cached_matrices_{false};
};
GTEST_TEST(ImplicitIntegratorTest, IsUpdateZero) {
const double mass = 1.0;
const double spring_k = 1.0;
SpringMassSystem<double> dummy_system(spring_k, mass, false /* unforced */);
std::unique_ptr<Context<double>> context =
dummy_system.CreateDefaultContext();
DummyImplicitIntegrator dummy_integrator(dummy_system, context.get());
// Machine epsilon is used in the tests below. The tolerance used to check
// whether a dimension of the update is zero in IsUpdateZero() will be
// 10 * eps. eps will also be used to construct the test below.
const double eps = std::numeric_limits<double>::epsilon();
// Test the case where the value of interest is unity and the update is near
// machine epsilon. The other state and update are set to zero so that these
// would *not* cause the update to be treated as *non*-zero.
VectorXd xc(2), dxc(2);
xc << 1.0, 0.0;
dxc << eps * 5, 0.0;
EXPECT_TRUE(dummy_integrator.IsUpdateZero(xc, dxc, eps * 10));
// Test the case where the value of interest is large and the update is still
// large, but not large enough relative to the value of interest.
xc << 1e10, 0.0;
dxc << eps * 1e6, 0.0;
EXPECT_TRUE(dummy_integrator.IsUpdateZero(xc, dxc, eps * 10));
// Test the case where we have two values of interest, one large and the
// other small with both updates "significant".
xc << 1e10, 1.0;
dxc << 1e0, eps * 1e6;
EXPECT_FALSE(dummy_integrator.IsUpdateZero(xc, dxc, eps * 10));
// Verify that a reasonable tolerance is used when a negative or zero
// tolerance is used.
xc << 1.0, 0.0;
dxc << eps, 0.0;
EXPECT_TRUE(dummy_integrator.IsUpdateZero(xc, dxc, -1.0));
EXPECT_TRUE(dummy_integrator.IsUpdateZero(xc, dxc, 0.0));
dxc << 0.1, 0.0;
EXPECT_FALSE(dummy_integrator.IsUpdateZero(xc, dxc, -1.0));
EXPECT_FALSE(dummy_integrator.IsUpdateZero(xc, dxc, 0.0));
}
// This test verifies that if we change the Jacobian computation scheme from
// the default scheme (kForwardDifference), then ImplicitIntegrator calls
// DoResetCachedMatrices() to reset any cached matrices.
GTEST_TEST(ImplicitIntegratorTest, SetComputationSchemeResetsCachedMatrices) {
const double mass = 1.0;
const double spring_k = 1.0;
SpringMassSystem<double> dummy_system(spring_k, mass, false /* unforced */);
std::unique_ptr<Context<double>> context =
dummy_system.CreateDefaultContext();
DummyImplicitIntegrator dummy_integrator(dummy_system, context.get());
// The default scheme should be kForwardDifference.
EXPECT_EQ(dummy_integrator.get_jacobian_computation_scheme(),
ImplicitIntegrator<double>
::JacobianComputationScheme::kForwardDifference);
// Verify that DoResetCachedMatrices() has not been called yet.
EXPECT_FALSE(dummy_integrator.get_has_reset_cached_matrices());
// Set the scheme to something other than kForwardDifference.
dummy_integrator.set_jacobian_computation_scheme(
ImplicitIntegrator<double>::JacobianComputationScheme::kAutomatic);
// Verify that DoResetCachedMatrices() has been called.
EXPECT_TRUE(dummy_integrator.get_has_reset_cached_matrices());
// Verify that the scheme has been properly changed.
EXPECT_EQ(dummy_integrator.get_jacobian_computation_scheme(),
ImplicitIntegrator<double>
::JacobianComputationScheme::kAutomatic);
}
} // namespace
} // namespace systems
} // namespace drake