-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
initial_value_problem_test.cc
239 lines (202 loc) · 9.82 KB
/
initial_value_problem_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
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
#include "drake/systems/analysis/initial_value_problem.h"
#include <algorithm>
#include <gtest/gtest.h>
#include "drake/common/test_utilities/eigen_matrix_compare.h"
#include "drake/common/test_utilities/expect_throws_message.h"
#include "drake/systems/analysis/integrator_base.h"
#include "drake/systems/analysis/runge_kutta2_integrator.h"
#include "drake/systems/framework/basic_vector.h"
#include "drake/systems/framework/parameters.h"
namespace drake {
namespace systems {
namespace analysis {
namespace {
// Checks IVP solver usage with multiple integrators.
GTEST_TEST(InitialValueProblemTest, SolutionUsingMultipleIntegrators) {
// Accuracy upper bound, as not all the integrators used below support
// error control.
const double kAccuracy = 1e-2;
// The initial time t₀, for IVP definition.
const double kInitialTime = 0.0;
// The initial state 𝐱₀, for IVP definition.
const VectorX<double> kInitialState = VectorX<double>::Zero(2);
// The default parameters 𝐤₀, for IVP definition.
const VectorX<double> kParameters = VectorX<double>::Constant(2, 1.0);
// Instantiates a generic IVP for test purposes only,
// using a generic ODE d𝐱/dt = -𝐱 + 𝐤, that does not
// model (nor attempts to model) any physical process.
InitialValueProblem<double> ivp(
[](const double& t, const VectorX<double>& x,
const VectorX<double>& k) -> VectorX<double> {
unused(t);
return -x + k;
}, kInitialState, kParameters);
// Testing against closed form solution of above's IVP, which can be written
// as 𝐱(t; 𝐤) = 𝐤 + (𝐱₀ - 𝐤) * e^(-(t - t₀)).
const double t0 = kInitialTime;
const VectorX<double>& x0 = kInitialState;
const VectorX<double>& k1 = kParameters;
const double t1 = kInitialTime + 1.0;
EXPECT_TRUE(CompareMatrices(
ivp.Solve(t0, t1), k1 + (x0 - k1) * std::exp(-(t1 - t0)), kAccuracy));
// Replaces default integrator.
const double kMaximumStep = 0.1;
const IntegratorBase<double>& default_integrator = ivp.get_integrator();
IntegratorBase<double>* configured_integrator =
ivp.reset_integrator<RungeKutta2Integrator<double>>(kMaximumStep);
EXPECT_NE(configured_integrator, &default_integrator);
EXPECT_EQ(configured_integrator, &ivp.get_integrator());
const double t2 = kInitialTime + 0.3;
// Testing against closed form solution of above's IVP, which can be written
// as 𝐱(t; 𝐤) = 𝐤 + (𝐱₀ - 𝐤) * e^(-(t - t₀)).
EXPECT_TRUE(CompareMatrices(
ivp.Solve(t0, t2), k1 + (x0 - k1) * std::exp(-(t2 - t0)), kAccuracy));
}
// Parameterized fixture for testing accuracy of IVP solutions.
class InitialValueProblemAccuracyTest
: public ::testing::TestWithParam<double> {
protected:
void SetUp() {
integration_accuracy_ = GetParam();
}
// Expected accuracy for numerical integral
// evaluation in the relative tolerance sense.
double integration_accuracy_{0.};
};
// Accuracy test of the solution for the momentum 𝐩 of a particle
// with mass m travelling through a gas with dynamic viscosity μ,
// where d𝐩/dt = -μ * 𝐩/m and 𝐩(t₀; [m, μ]) = 𝐩₀.
TEST_P(InitialValueProblemAccuracyTest, ParticleInAGasMomentum) {
// The initial time t₀.
const double kInitialTime = 0.0;
// The initial momentum 𝐩₀ of the particle at time t₀.
const VectorX<double> kInitialParticleMomentum = (
VectorX<double>(3) << -3.0, 1.0, 2.0).finished();
const double kLowestGasViscosity = 1.0;
const double kHighestGasViscosity = 10.0;
const double kGasViscosityStep = 1.0;
const double kLowestParticleMass = 1.0;
const double kHighestParticleMass = 10.0;
const double kParticleMassStep = 1.0;
const double kTotalTime = 1.0;
const double kTimeStep = 0.1;
const double t0 = kInitialTime;
const double tf = kTotalTime;
const VectorX<double>& p0 = kInitialParticleMomentum;
for (double mu = kLowestGasViscosity; mu <= kHighestGasViscosity;
mu += kGasViscosityStep) {
for (double m = kLowestParticleMass; m <= kHighestParticleMass;
m += kParticleMassStep) {
const Eigen::Vector2d parameters{mu, m};
// Instantiates the particle momentum IVP.
InitialValueProblem<double> particle_momentum_ivp(
[](const double& t, const VectorX<double>& p,
const VectorX<double>& k) -> VectorX<double> {
const double mu_ = k[0];
const double m_ = k[1];
return -mu_ * p / m_;
},
kInitialParticleMomentum, parameters);
IntegratorBase<double>& inner_integrator =
particle_momentum_ivp.get_mutable_integrator();
inner_integrator.set_target_accuracy(integration_accuracy_);
const std::unique_ptr<DenseOutput<double>> particle_momentum_approx =
particle_momentum_ivp.DenseSolve(t0, tf);
for (double t = kInitialTime; t <= kTotalTime; t += kTimeStep) {
// Tests are performed against the closed form
// solution for the IVP described above, which is
// 𝐩(t; [μ, m]) = 𝐩₀ * e^(-μ * (t - t₀) / m).
const VectorX<double> solution = p0 * std::exp(-mu * (t - t0) / m);
EXPECT_TRUE(CompareMatrices(particle_momentum_ivp.Solve(t0, t),
solution, integration_accuracy_))
<< "Failure solving d𝐩/dt = -μ * 𝐩/m"
<< " using 𝐩(" << t0 << "; [μ, m]) = " << p0 << " for t = " << t
<< ", μ = " << mu << " and m = " << m << " to an accuracy of "
<< integration_accuracy_;
EXPECT_TRUE(CompareMatrices(particle_momentum_approx->Evaluate(t),
solution, integration_accuracy_))
<< "Failure approximating the solution for d𝐩/dt = -μ * 𝐩/m"
<< " using 𝐩(" << t0 << "; [μ, m]) = " << p0 << " for t = " << t
<< ", μ = " << mu << " and m = " << m << " to an accuracy of "
<< integration_accuracy_ << " with solver's continuous extension.";
}
}
}
}
// Accuracy test of the solution for the velocity 𝐯 of a particle
// with mass m travelling through a gas with dynamic viscosity μ
// and being pushed by a constant force 𝐅, where
// d𝐯/dt = (𝐅 - μ * 𝐯) / m and 𝐯(t₀; [m, μ]) = 𝐯₀.
TEST_P(InitialValueProblemAccuracyTest, ParticleInAGasForcedVelocity) {
// The initial time t₀.
const double kInitialTime = 0.0;
// The initial velocity 𝐯₀ of the particle at time t₀.
const VectorX<double> kInitialParticleVelocity = VectorX<double>::Unit(3, 0);
// The force 𝐅 pushing the particle.
const VectorX<double> kPushingForce = VectorX<double>::Unit(3, 1);
// The mass m of the particle and the dynamic viscosity μ of the gas.
const double kLowestGasViscosity = 1.0;
const double kHighestGasViscosity = 10.0;
const double kGasViscosityStep = 1.0;
const double kLowestParticleMass = 1.0;
const double kHighestParticleMass = 10.0;
const double kParticleMassStep = 1.0;
const double kTotalTime = 1.0;
const double kTimeStep = 0.1;
const double t0 = kInitialTime;
const double tf = kTotalTime;
const VectorX<double>& F = kPushingForce;
const VectorX<double>& v0 = kInitialParticleVelocity;
for (double mu = kLowestGasViscosity; mu <= kHighestGasViscosity;
mu += kGasViscosityStep) {
for (double m = kLowestParticleMass; m <= kHighestParticleMass;
m += kParticleMassStep) {
const Eigen::Vector2d parameters{mu, m};
// Instantiates the particle velocity IVP.
InitialValueProblem<double> particle_velocity_ivp(
[&kPushingForce](const double& t, const VectorX<double>& v,
const VectorX<double>& k) -> VectorX<double> {
const double mu_ = k[0];
const double m_ = k[1];
const VectorX<double>& F_ = kPushingForce;
return (F_ - mu_ * v) / m_;
},
kInitialParticleVelocity, parameters);
IntegratorBase<double>& inner_integrator =
particle_velocity_ivp.get_mutable_integrator();
inner_integrator.set_target_accuracy(integration_accuracy_);
const std::unique_ptr<DenseOutput<double>> particle_velocity_approx =
particle_velocity_ivp.DenseSolve(t0, tf);
for (double t = kInitialTime; t <= kTotalTime; t += kTimeStep) {
// Tests are performed against the closed form
// solution for the IVP described above, which is
// 𝐯(t; [μ, m]) = 𝐯₀ * e^(-μ * (t - t₀) / m) +
// 𝐅 / μ * (1 - e^(-μ * (t - t₀) / m))
// with 𝐅 = (0., 1., 0.).
const VectorX<double> solution =
v0 * std::exp(-mu * (t - t0) / m) +
F / mu * (1. - std::exp(-mu * (t - t0) / m));
EXPECT_TRUE(CompareMatrices(particle_velocity_ivp.Solve(t0, t),
solution, integration_accuracy_))
<< "Failure solving d𝐯/dt = (-μ * 𝐯 + 𝐅) / m"
<< " using 𝐯(" << t0 << "; [μ, m]) = " << v0 << " for t = " << t
<< ", μ = " << mu << ", m = " << m << "and 𝐅 = " << F
<< " to an accuracy of " << integration_accuracy_;
EXPECT_TRUE(CompareMatrices(particle_velocity_approx->Evaluate(t),
solution, integration_accuracy_))
<< "Failure approximating the solution for "
<< "d𝐯/dt = (-μ * 𝐯 + 𝐅) / m using 𝐯(" << t0 << "; [μ, m]) = " << v0
<< " for t = " << t << ", μ = " << mu << ", m = " << m
<< "and 𝐅 = " << F << " to an accuracy of " << integration_accuracy_
<< " with solver's continuous extension.";
}
}
}
}
INSTANTIATE_TEST_SUITE_P(IncreasingAccuracyInitialValueProblemTests,
InitialValueProblemAccuracyTest,
::testing::Values(1e-1, 1e-2, 1e-3, 1e-4, 1e-5));
} // namespace
} // namespace analysis
} // namespace systems
} // namespace drake