Debian Codegraph @

unittests/integration/test_Optimizer.cpp

raw
1 /*
2 * Copyright (c) 2011-2022, The DART development contributors
3 * All rights reserved.
4 *
5 * The list of contributors can be found at:
6 * https://github.com/dartsim/dart/blob/master/LICENSE
7 *
8 * This file is provided under the following "BSD-style" License:
9 * Redistribution and use in source and binary forms, with or
10 * without modification, are permitted provided that the following
11 * conditions are met:
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19 * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26 * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30 * POSSIBILITY OF SUCH DAMAGE.
31 */
32
33 // For problem
34 #include <cstdio>
35 #include <fstream>
36 #include <iostream>
37 #include <sstream>
38
39 #include <Eigen/Dense>
40 #include <gtest/gtest.h>
41
42 #include "dart/common/Console.hpp"
43 #include "dart/config.hpp"
44 #include "dart/dynamics/FreeJoint.hpp"
45 #include "dart/dynamics/InverseKinematics.hpp"
46 #include "dart/dynamics/Skeleton.hpp"
47 #include "dart/optimizer/Function.hpp"
48 #include "dart/optimizer/GradientDescentSolver.hpp"
49 #include "dart/optimizer/Problem.hpp"
50
51 #include "TestHelpers.hpp"
52 #if HAVE_NLOPT
53 #include "dart/optimizer/nlopt/NloptSolver.hpp"
54 #endif
55 #if HAVE_IPOPT
56 #include "dart/optimizer/ipopt/IpoptSolver.hpp"
57 #endif
58 #if HAVE_SNOPT
59 #include "dart/optimizer/snopt/SnoptSolver.hpp"
60 #endif
61
62 using namespace std;
63 using namespace Eigen;
64 using namespace dart::optimizer;
65 using namespace dart::dynamics;
66
67 //==============================================================================
68 /// \brief class SampleObjFunc
69 class SampleObjFunc : public Function
70 {
71 public:
72 /// \brief Constructor
73 SampleObjFunc() : Function() {}
74
75 /// \brief Destructor
76 virtual ~SampleObjFunc() {}
77
78 /// \copydoc Function::eval
79 double eval(const Eigen::VectorXd& _x) override
80 {
81 return std::sqrt(_x[1]);
82 }
83
84 /// \copydoc Function::evalGradient
85 void evalGradient(
86 const Eigen::VectorXd& _x, Eigen::Map<Eigen::VectorXd> _grad) override
87 {
88 _grad[0] = 0.0;
89 _grad[1] = 0.5 / std::sqrt(_x[1]);
90 }
91 };
92
93 //==============================================================================
94 class SampleConstFunc : public Function
95 {
96 public:
97 /// \brief Constructor
98 SampleConstFunc(double _a, double _b) : Function(), mA(_a), mB(_b) {}
99
100 /// \brief Destructor
101 virtual ~SampleConstFunc() {}
102
103 /// \copydoc Function::eval
104 double eval(const Eigen::VectorXd& _x) override
105 {
106 return ((mA * _x[0] + mB) * (mA * _x[0] + mB) * (mA * _x[0] + mB) - _x[1]);
107 }
108
109 /// \copydoc Function::evalGradient
110 void evalGradient(
111 const Eigen::VectorXd& _x, Eigen::Map<Eigen::VectorXd> _grad) override
112 {
113 _grad[0] = 3 * mA * (mA * _x[0] + mB) * (mA * _x[0] + mB);
114 _grad[1] = -1.0;
115 }
116
117 private:
118 /// \brief Data
119 double mA;
120
121 /// \brief Data
122 double mB;
123 };
124
125 //==============================================================================
126 TEST(Optimizer, GradientDescent)
127 {
128 std::shared_ptr<Problem> prob = std::make_shared<Problem>(2);
129
130 prob->setLowerBounds(Eigen::Vector2d(-HUGE_VAL, 0));
131 prob->setInitialGuess(Eigen::Vector2d(1.234, 5.678));
132
133 FunctionPtr obj = std::make_shared<SampleObjFunc>();
134 prob->setObjective(obj);
135
136 GradientDescentSolver solver(prob);
137 EXPECT_TRUE(solver.solve());
138
139 double minF = prob->getOptimumValue();
140 Eigen::VectorXd optX = prob->getOptimalSolution();
141
142 EXPECT_NEAR(minF, 0, 1e-6);
143 EXPECT_EQ(optX.size(), static_cast<int>(prob->getDimension()));
144 EXPECT_NEAR(optX[0], 1.234, 0.0);
145 EXPECT_NEAR(optX[1], 0.0, solver.getTolerance());
146 }
147
148 //==============================================================================
149 #if HAVE_NLOPT
150 TEST(Optimizer, BasicNlopt)
151 {
152 // Problem reference: http://ab-initio.mit.edu/wiki/index.php/NLopt_Tutorial
153
154 std::shared_ptr<Problem> prob = std::make_shared<Problem>(2);
155
156 prob->setLowerBounds(Eigen::Vector2d(-HUGE_VAL, 0));
157 prob->setInitialGuess(Eigen::Vector2d(1.234, 5.678));
158
159 FunctionPtr obj = std::make_shared<SampleObjFunc>();
160 prob->setObjective(obj);
161
162 FunctionPtr const1 = std::make_shared<SampleConstFunc>(2, 0);
163 FunctionPtr const2 = std::make_shared<SampleConstFunc>(-1, 1);
164 prob->addIneqConstraint(const1);
165 prob->addIneqConstraint(const2);
166
167 NloptSolver solver(prob, NloptSolver::LD_MMA);
168 EXPECT_TRUE(solver.solve());
169
170 double minF = prob->getOptimumValue();
171 Eigen::VectorXd optX = prob->getOptimalSolution();
172
173 EXPECT_NEAR(minF, 0.544330847, 1e-6);
174 EXPECT_EQ(static_cast<std::size_t>(optX.size()), prob->getDimension());
175 EXPECT_NEAR(optX[0], 0.333334, 1e-6);
176 EXPECT_NEAR(optX[1], 0.296296, 1e-6);
177 }
178 #endif
179
180 //==============================================================================
181 #if HAVE_IPOPT
182 TEST(Optimizer, BasicIpopt)
183 {
184 std::shared_ptr<Problem> prob = std::make_shared<Problem>(2);
185
186 prob->setLowerBounds(Eigen::Vector2d(-HUGE_VAL, 0));
187 prob->setInitialGuess(Eigen::Vector2d(1.234, 5.678));
188
189 FunctionPtr obj = std::make_shared<SampleObjFunc>();
190 prob->setObjective(obj);
191
192 FunctionPtr const1 = std::make_shared<SampleConstFunc>(2, 0);
193 FunctionPtr const2 = std::make_shared<SampleConstFunc>(-1, 1);
194 prob->addIneqConstraint(const1);
195 prob->addIneqConstraint(const2);
196
197 IpoptSolver solver(prob);
198 solver.solve();
199
200 double minF = prob->getOptimumValue();
201 Eigen::VectorXd optX = prob->getOptimalSolution();
202
203 EXPECT_NEAR(minF, 0.544330847, 1e-6);
204 EXPECT_EQ(static_cast<std::size_t>(optX.size()), prob->getDimension());
205 EXPECT_NEAR(optX[0], 0.333334, 1e-6);
206 EXPECT_NEAR(optX[1], 0.296296, 1e-6);
207 }
208 #endif
209
210 //==============================================================================
211 #if HAVE_SNOPT
212 TEST(Optimizer, BasicSnopt)
213 {
214 dterr << "SNOPT is not implemented yet.\n";
215 return;
216 }
217 #endif
218
219 //==============================================================================
220 bool compareStringAndFile(
221 const std::string& content, const std::string& fileName)
222 {
223 std::ifstream ifs(fileName, std::ifstream::in);
224 EXPECT_TRUE(ifs.is_open());
225
226 auto itr = content.begin();
227
228 char c = ifs.get();
229 while (ifs.good())
230 {
231 if (*itr != c)
232 return false;
233
234 c = ifs.get();
235 itr++;
236 }
237
238 ifs.close();
239
240 return true;
241 }
242
243 //==============================================================================
244 TEST(Optimizer, OutStream)
245 {
246 std::shared_ptr<Problem> prob = std::make_shared<Problem>(2);
247
248 prob->setLowerBounds(Eigen::Vector2d(-HUGE_VAL, 0));
249 prob->setInitialGuess(Eigen::Vector2d(1.234, 5.678));
250
251 FunctionPtr obj = std::make_shared<SampleObjFunc>();
252 prob->setObjective(obj);
253
254 GradientDescentSolver solver(prob);
255 solver.setIterationsPerPrint(50);
256
257 // Print the progess to a std::string
258 std::stringstream ss;
259 solver.setOutStream(&ss);
260 EXPECT_TRUE(solver.solve());
261 std::string outputString = ss.str();
262
263 // Print the progress to a file
264 std::string outputFile = "test_optimizer_outstream.txt";
265 std::ofstream ofs(outputFile);
266 EXPECT_TRUE(ofs.is_open());
267 solver.setOutStream(&ofs);
268 EXPECT_TRUE(solver.solve());
269 ofs.close();
270
271 // Compare them
272 EXPECT_TRUE(compareStringAndFile(outputString, outputFile));
273
274 std::remove(outputFile.c_str());
275 }