1 /*
2 * This file is part of CasADi.
3 *
4 * CasADi -- A symbolic framework for dynamic optimization.
5 * Copyright (C) 2010-2014 Joel Andersson, Joris Gillis, Moritz Diehl,
6 * K.U. Leuven. All rights reserved.
7 * Copyright (C) 2011-2014 Greg Horn
8 *
9 * CasADi is free software; you can redistribute it and/or
10 * modify it under the terms of the GNU Lesser General Public
11 * License as published by the Free Software Foundation; either
12 * version 3 of the License, or (at your option) any later version.
13 *
14 * CasADi is distributed in the hope that it will be useful,
15 * but WITHOUT ANY WARRANTY; without even the implied warranty of
16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
17 * Lesser General Public License for more details.
18 *
19 * You should have received a copy of the GNU Lesser General Public
20 * License along with CasADi; if not, write to the Free Software
21 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
22 *
23 */
24
25
26 #include <iostream>
27 #include <fstream>
28 #include <ctime>
29 #include <casadi/casadi.hpp>
30
31 using namespace casadi;
32 using namespace std;
33
main()34 int main(){
35
36 cout << "program started" << endl;
37
38 // Dimensions
39 int nu = 20; // Number of control segments
40 int nj = 100; // Number of integration steps per control segment
41
42 // optimization variable
43 SX u = SX::sym("u",nu); // control
44
45 SX s_0 = 0; // initial position
46 SX v_0 = 0; // initial speed
47 SX m_0 = 1; // initial mass
48
49 SX dt = 10.0/(nj*nu); // time step
50 SX alpha = 0.05; // friction
51 SX beta = 0.1; // fuel consumption rate
52
53 // Trajectory
54 SX s_traj = SX::zeros(nu);
55 SX v_traj = SX::zeros(nu);
56 SX m_traj = SX::zeros(nu);
57
58 // Integrate over the interval with Euler forward
59 SX s = s_0, v = v_0, m = m_0;
60 for(int k=0; k<nu; ++k){
61 for(int j=0; j<nj; ++j){
62 s += dt*v;
63 v += dt / m * (u(k)- alpha * v*v);
64 m += -dt * beta*u(k)*u(k);
65 }
66 s_traj(k) = s;
67 v_traj(k) = v;
68 m_traj(k) = m;
69 }
70
71 // Objective function
72 SX f = dot(u,u);
73
74 // Terminal constraints
75 SX g = vertcat(s, v, v_traj);
76
77 // Create an NLP
78 SXDict nlp = {{"x", u}, {"f", f}, {"g", g}};
79
80 // Create an NLP solver and buffers
81 Function solver = nlpsol("solver", "snopt", nlp);
82 std::map<std::string, DM> arg, res;
83
84 // Bounds on u and initial condition
85 arg["lbx"] = -10;
86 arg["ubx"] = 10;
87 arg["x0"] = 0.4;
88
89 // Bounds on g
90 vector<double> gmin(2), gmax(2);
91 gmin[0] = gmax[0] = 10;
92 gmin[1] = gmax[1] = 0;
93 gmin.resize(2+nu, -numeric_limits<double>::infinity());
94 gmax.resize(2+nu, 1.1);
95 arg["lbg"] = gmin;
96 arg["ubg"] = gmax;
97
98 // Solve the problem
99 res = solver(arg);
100
101 // Print the optimal cost
102 double cost(res.at("f"));
103 cout << "optimal cost: " << cost << endl;
104
105 // Print the optimal solution
106 vector<double> uopt(res.at("x"));
107 cout << "optimal control: " << uopt << endl;
108
109 // Get the state trajectory
110 Function xfcn("xfcn", {u}, {s_traj, v_traj, m_traj});
111 vector<double> sopt, vopt, mopt;
112 xfcn({uopt}, {&sopt, &vopt, &mopt});
113 cout << "position: " << sopt << endl;
114 cout << "velocity: " << vopt << endl;
115 cout << "mass: " << mopt << endl;
116
117 // Create Matlab script to plot the solution
118 ofstream file;
119 string filename = "rocket_snopt_results.m";
120 file.open(filename.c_str());
121 file << "% Results file from " __FILE__ << endl;
122 file << "% Generated " __DATE__ " at " __TIME__ << endl;
123 file << endl;
124 file << "cost = " << cost << ";" << endl;
125 file << "u = " << uopt << ";" << endl;
126
127 // Save results to file
128 file << "t = linspace(0,10.0," << nu << ");"<< endl;
129 file << "s = " << sopt << ";" << endl;
130 file << "v = " << vopt << ";" << endl;
131 file << "m = " << mopt << ";" << endl;
132
133 // Finalize the results file
134 file << endl;
135 file << "% Plot the results" << endl;
136 file << "figure(1);" << endl;
137 file << "clf;" << endl << endl;
138
139 file << "subplot(2,2,1);" << endl;
140 file << "plot(t,s);" << endl;
141 file << "grid on;" << endl;
142 file << "xlabel('time [s]');" << endl;
143 file << "ylabel('position [m]');" << endl << endl;
144
145 file << "subplot(2,2,2);" << endl;
146 file << "plot(t,v);" << endl;
147 file << "grid on;" << endl;
148 file << "xlabel('time [s]');" << endl;
149 file << "ylabel('velocity [m/s]');" << endl << endl;
150
151 file << "subplot(2,2,3);" << endl;
152 file << "plot(t,m);" << endl;
153 file << "grid on;" << endl;
154 file << "xlabel('time [s]');" << endl;
155 file << "ylabel('mass [kg]');" << endl << endl;
156
157 file << "subplot(2,2,4);" << endl;
158 file << "plot(t,u);" << endl;
159 file << "grid on;" << endl;
160 file << "xlabel('time [s]');" << endl;
161 file << "ylabel('Thrust [kg m/s^2]');" << endl << endl;
162
163 file.close();
164 cout << "Results saved to \"" << filename << "\"" << endl;
165
166 return 0;
167 }
168