1 /*
2 * Copyright (C) 2018 Open Source Robotics Foundation
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *
16 */
17
18 #include <iostream>
19 #include <iomanip>
20 #include <chrono>
21 #include <cassert>
22 #include <cmath>
23 #include <set>
24
25 #include <ignition/plugin/Loader.hh>
26 #include <ignition/common/SystemPaths.hh>
27
28 #include "plugins/integrators.hh"
29
30 #ifdef HAVE_BOOST_PROGRAM_OPTIONS
31 #include <boost/program_options.hpp>
32 namespace bpo = boost::program_options;
33 #endif
34
35 using NumericalIntegrator = ignition::plugin::examples::NumericalIntegrator;
36 using ODESystem = ignition::plugin::examples::ODESystem;
37 using ODESystemFactory = ignition::plugin::examples::ODESystemFactory;
38
39 // The macro that this uses is provided as a compile definition in the
40 // examples/CMakeLists.txt file.
41 const std::string PluginLibDir = IGN_PLUGIN_EXAMPLES_LIBDIR;
42
43 /// \brief Return structure for numerical integration test results. If the name
44 /// is blank, that means the test was not run.
45 struct TestResult
46 {
47 /// \brief Name of the test run
48 public: std::string name;
49
50 /// \brief Name of the actual time spent computing
51 public: long timeSpent_us;
52
53 /// \brief The percent error in each component of the state when compared to
54 /// an exact solution.
55 public: std::vector<double> percentError;
56 };
57
58 /// \brief A simple container to hold a plugin and the name that it was
59 /// instantiated from.
60 struct PluginHolder
61 {
62 std::string name;
63 ignition::plugin::PluginPtr plugin;
64 };
65
66 /// \brief Compute the component-wise percent error of the estimate produced by
67 /// a numerical integrator, compared to the theoretical exact solution of the
68 /// system.
ComputeError(const NumericalIntegrator::State & _estimate,const NumericalIntegrator::State & _exact)69 std::vector<double> ComputeError(
70 const NumericalIntegrator::State &_estimate,
71 const NumericalIntegrator::State &_exact)
72 {
73 assert(_estimate.size() == _exact.size());
74 std::vector<double> result(_estimate.size());
75
76 for(std::size_t i = 0 ; i < _estimate.size(); ++i)
77 result[i] = (_estimate[i] - _exact[i])/_exact[i] * 100.0;
78
79 return result;
80 }
81
82 /// \brief Pass in a plugin that provides the numerical integration interface.
83 /// The numerical integration results of the plugin will be tested against the
84 /// results of the _exactFunction.
TestIntegrator(const PluginHolder & _pluginHolder,const ODESystem & _system,const double _timeStep,const unsigned int _numSteps)85 TestResult TestIntegrator(
86 const PluginHolder &_pluginHolder,
87 const ODESystem &_system,
88 const double _timeStep,
89 const unsigned int _numSteps)
90 {
91 const ignition::plugin::PluginPtr &plugin = _pluginHolder.plugin;
92 NumericalIntegrator* integrator =
93 plugin->QueryInterface<NumericalIntegrator>();
94
95 if(!integrator)
96 {
97 std::cout << "The plugin named [" << _pluginHolder.name << "] does not "
98 << "provide a NumericalIntegrator interface. It will not be "
99 << "tested." << std::endl;
100 return TestResult();
101 }
102
103 TestResult result;
104 result.name = _pluginHolder.name;
105
106 integrator->SetFunction(_system.ode);
107 integrator->SetTimeStep(_timeStep);
108
109 double time = _system.initialTime;
110 NumericalIntegrator::State state = _system.initialState;
111
112 auto performanceStart = std::chrono::high_resolution_clock::now();
113 for(std::size_t i=0; i < _numSteps; ++i)
114 {
115 state = integrator->Integrate(time, state);
116 time += integrator->GetTimeStep();
117 }
118 auto performanceStop = std::chrono::high_resolution_clock::now();
119
120 result.timeSpent_us = std::chrono::duration_cast<std::chrono::microseconds>(
121 performanceStop - performanceStart).count();
122
123 result.percentError = ComputeError(state, _system.exact(time));
124
125 return result;
126 }
127
128 /// \brief Print out the result of the test.
PrintResult(const TestResult & _result)129 void PrintResult(const TestResult &_result)
130 {
131 if (_result.name.empty())
132 {
133 // An empty name means that something went wrong with the test.
134 return;
135 }
136
137 std::cout << "\nMethod: " << _result.name << "\n";
138
139 std::cout << "Runtime(us): " << std::setfill(' ') << std::setw(8)
140 << std::right << _result.timeSpent_us << "\n";
141
142 std::cout << std::setw(0);
143
144 std::cout << "Component-wise error: ";
145 for (const double result : _result.percentError)
146 {
147 std::cout << std::setfill(' ') << std::setw(9) << std::right
148 << std::setprecision(3) << std::fixed << result;
149
150 std::cout << std::setw(0) << "%";
151 }
152
153 std::cout << "\n";
154 }
155
156 /// \brief Test a set of plugins against each system, using the specified
157 /// parameters.
TestPlugins(const std::vector<PluginHolder> & _factories,const std::vector<PluginHolder> & _integrators,const double _timeStep,const unsigned int _numSteps)158 void TestPlugins(
159 const std::vector<PluginHolder> &_factories,
160 const std::vector<PluginHolder> &_integrators,
161 const double _timeStep,
162 const unsigned int _numSteps)
163 {
164 bool quit = false;
165 if (_factories.empty())
166 {
167 std::cout << "You did not specify any ODE System plugins to test against!"
168 #ifdef HAVE_BOOST_PROGRAM_OPTIONS
169 << "\n -- Pass in the -a flag to automatically use all plugins"
170 #endif
171 << std::endl;
172 quit = true;
173 }
174
175 if (_integrators.empty())
176 {
177 std::cout << "You did not specify any numerical integrator plugins to test "
178 << "against!"
179 #ifdef HAVE_BOOST_PROGRAM_OPTIONS
180 << "\n -- Pass in the -a flag to automatically use all plugins"
181 #endif
182 << std::endl;
183 quit = true;
184 }
185
186 if(quit)
187 return;
188
189 for (const PluginHolder &factory : _factories)
190 {
191 const std::vector<ODESystem> systems =
192 factory.plugin->QueryInterface<ODESystemFactory>()->CreateSystems();
193
194 for (const ODESystem &system : systems)
195 {
196 std::cout << "\n\n ================================================== \n";
197 std::cout << "System [" << system.name << "] from factory ["
198 << factory.name << "]\n";
199
200 for (const PluginHolder &integrator : _integrators)
201 {
202 const TestResult result = TestIntegrator(
203 integrator, system, _timeStep, _numSteps);
204
205 PrintResult(result);
206 }
207 }
208 }
209 }
210
211 /// \brief Load all the plugins that implement _interface.
LoadPlugins(const ignition::plugin::Loader & _loader,const std::string & _interface)212 std::vector<PluginHolder> LoadPlugins(
213 const ignition::plugin::Loader &_loader,
214 const std::string &_interface)
215 {
216 // Fill in the holders object with each plugin.
217 std::vector<PluginHolder> holders;
218
219 const auto pluginNames = _loader.PluginsImplementing(_interface);
220
221 for (const std::string &name : pluginNames)
222 {
223 ignition::plugin::PluginPtr plugin = _loader.Instantiate(name);
224 if (!plugin)
225 {
226 std::cout << "Failed to load [" << name << "] as a class"
227 << std::endl;
228 continue;
229 }
230
231 holders.push_back({name, plugin});
232 }
233
234 return holders;
235 }
236
237 /// \brief Load all plugins that implement the NumericalIntegrator interface.
LoadIntegratorPlugins(const ignition::plugin::Loader & _loader)238 std::vector<PluginHolder> LoadIntegratorPlugins(
239 const ignition::plugin::Loader &_loader)
240 {
241 return LoadPlugins(
242 _loader, "ignition::plugin::examples::NumericalIntegrator");
243 }
244
245 /// \brief Load all plugins that implement the ODESystemFactory interface
LoadSystemFactoryPlugins(const ignition::plugin::Loader & _loader)246 std::vector<PluginHolder> LoadSystemFactoryPlugins(
247 const ignition::plugin::Loader &_loader)
248 {
249 return LoadPlugins(
250 _loader, "ignition::plugin::examples::ODESystemFactory");
251 }
252
253 /// \brief Prime the plugin loader with the paths and library names that it
254 /// should try to get plugins from.
PrimeTheLoader(ignition::common::SystemPaths & _paths,ignition::plugin::Loader & _loader,const std::set<std::string> & _pluginNames)255 void PrimeTheLoader(
256 ignition::common::SystemPaths &_paths, /* TODO: This should be const */
257 ignition::plugin::Loader &_loader,
258 const std::set<std::string> &_pluginNames)
259 {
260 for (const std::string &name : _pluginNames)
261 {
262 const std::string pluginPath = _paths.FindSharedLibrary(name);
263 if (pluginPath.empty())
264 {
265 std::cout << "Failed to find path for plugin library [" << name << "]"
266 #ifdef HAVE_BOOST_PROGRAM_OPTIONS
267 << "\n -- Use the -I flag to specify the plugin library directory"
268 #endif
269 << std::endl;
270 continue;
271 }
272
273 std::cout << "Path for [" << name << "] is [" << pluginPath << "]"
274 << std::endl;
275
276 if (_loader.LoadLibrary(pluginPath).empty())
277 {
278 std::cout << "Failed to load [" << name << "] as a plugin library"
279 << std::endl;
280 }
281 }
282 }
283
main(int argc,char * argv[])284 int main(int argc, char *argv[])
285 {
286 // Create an object that can search the system paths for the plugin libraries.
287 ignition::common::SystemPaths paths;
288
289 // Create a plugin loader
290 ignition::plugin::Loader loader;
291
292 // Add the build directory path for the plugin libraries so the SystemPaths
293 // object will know to search through it.
294 paths.AddPluginPaths(PluginLibDir);
295
296 // Add the default plugins
297 std::set<std::string> pluginNames = {
298 "ForwardEuler", "RungeKutta4",
299 "PolynomialODE", "ExponentialODE"
300 };
301
302 #ifdef HAVE_BOOST_PROGRAM_OPTIONS
303
304 double timeStep;
305 unsigned int numSteps;
306
307 std::string usage;
308 usage +=
309 "The 'integrators' example performs benchmark tests on numerical\n"
310 "integrator plugins, testing them against differential equation plugins."
311 "\nNumerical integrator plugins must inherit the NumericalIntegrator \n"
312 "interface, and differential equation plugins must inherit the \n"
313 "ODESystemFactory interface. Both interfaces can be found in the header\n"
314 "ign-plugin/examples/plugins/Interfaces.hh.\n\n"
315
316 "Custom plugins can be used by passing in the custom plugin library\n"
317 "directory to the -I flag, and the library name(s) to the -p flag,\n"
318 "as described below";
319
320 bpo::options_description desc(usage);
321 desc.add_options()
322
323 ("help,h", "Print this usage message")
324
325 ("plugins,p", bpo::value<std::vector<std::string>>(),
326 "Plugins libraries to use")
327
328 ("all,a", "Use all plugin libraries that come with this example")
329
330 ("include-dirs,I", bpo::value<std::vector<std::string>>()->multitoken(),
331 "Additional directories that may contain plugin libraries")
332
333 ("timestep,s", bpo::value<double>(&timeStep)->default_value(0.01),
334 "Size of the time step (s) to take")
335
336 ("numsteps,n", bpo::value<unsigned int>(&numSteps)->default_value(10000),
337 "Number of time steps to take")
338 ;
339
340 bpo::positional_options_description p;
341 p.add("plugins", -1);
342
343 bpo::variables_map vm;
344 bpo::store(bpo::command_line_parser(argc, argv)
345 .options(desc).positional(p).run(), vm);
346 bpo::notify(vm);
347
348 if (vm.count("help") > 0)
349 {
350 std::cout << desc << std::endl;
351 return 1;
352 }
353
354 if (vm.count("all") == 0)
355 {
356 pluginNames.clear();
357 }
358
359 if (vm.count("plugins"))
360 {
361 const std::vector<std::string> inputPlugins =
362 vm["plugins"].as<std::vector<std::string>>();
363
364 for (const std::string &input : inputPlugins)
365 pluginNames.insert(input);
366 }
367
368 if (vm.count("include-dirs"))
369 {
370 const std::vector<std::string> inputDirs =
371 vm["include-dirs"].as<std::vector<std::string>>();
372
373 for (const std::string &input : inputDirs)
374 {
375 std::cout << "Including additional plugin directory: ["
376 << input << "]" << std::endl;
377
378 paths.AddPluginPaths(input);
379 }
380 }
381
382 #else
383
384 const double timeStep = 0.01;
385 const unsigned int numSteps = 10000;
386
387 std::cout
388 << "boost::program_options was not found when this example was\n"
389 << "compiled, so we will default to using all the plugin libraries\n"
390 << "that came with this example program. We will also default to:\n"
391 << " -- time step = " << timeStep << "\n"
392 << " -- num steps = " << numSteps << "\n"
393 << std::endl;
394
395 #endif
396
397 PrimeTheLoader(paths, loader, pluginNames);
398
399 // Load the plugins
400 const std::vector<PluginHolder> integrators = LoadIntegratorPlugins(loader);
401 const std::vector<PluginHolder> systems = LoadSystemFactoryPlugins(loader);
402
403 TestPlugins(systems, integrators, timeStep, numSteps);
404 }
405