Cheetah Software  1.0
Functions
test_casadi.cpp File Reference
#include "gmock/gmock.h"
#include "gtest/gtest.h"
#include <casadi/casadi.hpp>
#include <ctime>
#include <fstream>
#include <iostream>
#include "Dynamics/DynamicsSimulator.h"
#include "Dynamics/FloatingBaseModel.h"
#include "Dynamics/MiniCheetah.h"
#include "Dynamics/Quadruped.h"
#include <Utilities/Utilities_print.h>
+ Include dependency graph for test_casadi.cpp:

Go to the source code of this file.

Functions

 TEST (casadi, rocket_ipopt)
 
 TEST (casadi, part1)
 
MX f (const MX &x, const MX &u)
 
 TEST (casadi, race_car)
 
MX simple_dyn (const MX &x, const MX &u, const MX &P, int i)
 
DVec< double > simple_dyn_vec (const DVec< double > &x, const DVec< double > &u, const DVec< double > &P, int i)
 
 TEST (casadi, jump_opt)
 

Function Documentation

MX f ( const MX &  x,
const MX &  u 
)

Definition at line 202 of file test_casadi.cpp.

202 { return vertcat(x(1), u - x(1)); }

+ Here is the caller graph for this function:

MX simple_dyn ( const MX &  x,
const MX &  u,
const MX &  P,
int  i 
)

Definition at line 297 of file test_casadi.cpp.

297  {
298  double I(0.21);
299  double M(8.91);
300  double g(9.81);
301 
302  if( i<15 ){
303  return vertcat (vertcat(x(3), x(4), x(5)),
304  vertcat(u(0)/M, u(1)/M -g), ((P(1) - x(1))*u(0) - (P(0) - x(0)) * u(1))/I ); // Velocity
305  }else{
306  return vertcat (vertcat(x(3), x(4), x(5)),
307  vertcat(u(0)/M, u(1)/M -g), ((P(3) - x(1))*u(0) - (P(2) - x(0)) * u(1))/I ); // Velocity
308  }
309 }

+ Here is the caller graph for this function:

DVec<double> simple_dyn_vec ( const DVec< double > &  x,
const DVec< double > &  u,
const DVec< double > &  P,
int  i 
)

Definition at line 311 of file test_casadi.cpp.

312  {
313  double I(0.21);
314  double M(8.91);
315  double g(9.81);
316 
317  DVec<double> ret(6);
318  if( i<15 ){
319  ret << x(3), x(4), x(5),
320  u(0)/M, u(1)/M -g, ((P(1) - x(1))*u(0) - (P(0) - x(0)) * u(1))/I ; // Velocity
321  }else{
322  ret << x(3), x(4), x(5),
323  u(0)/M, u(1)/M -g, ((P(3) - x(1))*u(0) - (P(2) - x(0)) * u(1))/I ; // Velocity
324  }
325 
326  return ret;
327 }
DVec
typename Eigen::Matrix< T, Eigen::Dynamic, 1 > DVec
Definition: cppTypes.h:102

+ Here is the caller graph for this function:

TEST ( casadi  ,
rocket_ipopt   
)

Definition at line 19 of file test_casadi.cpp.

References f().

19  {
20  cout << "program started" << endl;
21 
22  // Dimensions
23  int nu = 20; // Number of control segments
24  int nj = 100; // Number of integration steps per control segment
25 
26  // optimization variable
27  SX u = SX::sym("u", nu); // control
28 
29  SX s_0 = 0; // initial position
30  SX v_0 = 0; // initial speed
31  SX m_0 = 1; // initial mass
32 
33  SX dt = 10.0 / (nj * nu); // time step
34  SX alpha = 0.05; // friction
35  SX beta = 0.1; // fuel consumption rate
36 
37  // Trajectory
38  vector<SX> s_k, v_k, m_k;
39 
40  // Integrate over the interval with Euler forward
41  SX s = s_0, v = v_0, m = m_0;
42  for (int k = 0; k < nu; ++k) {
43  for (int j = 0; j < nj; ++j) {
44  s += dt * v;
45  v += dt / m * (u(k) - alpha * v * v);
46  m += -dt * beta * u(k) * u(k);
47  }
48  s_k.push_back(s);
49  v_k.push_back(v);
50  m_k.push_back(m);
51  }
52  SX s_all = vertcat(s_k), v_all = vertcat(v_k), m_all = vertcat(m_k);
53 
54  // Objective function
55  SX f = dot(u, u);
56 
57  // Terminal constraints
58  SX g = vertcat(s, v, v_all);
59 
60  // Create the NLP
61  SXDict nlp = {{"x", u}, {"f", f}, {"g", g}};
62 
63  // Allocate an NLP solver and buffers
64  Function solver = nlpsol("solver", "ipopt", nlp);
65 
66  // Bounds on g
67  vector<double> gmin = {10, 0};
68  vector<double> gmax = {10, 0};
69  gmin.resize(2 + nu, -numeric_limits<double>::infinity());
70  gmax.resize(2 + nu, 1.1);
71 
72  // Solve the problem
73  DMDict arg = {
74  {"lbx", -10}, {"ubx", 10}, {"x0", 0.4}, {"lbg", gmin}, {"ubg", gmax}};
75  DMDict res = solver(arg);
76 
77  // Print the optimal cost
78  double cost(res.at("f"));
79  cout << "optimal cost: " << cost << endl;
80 
81  // Print the optimal solution
82  vector<double> uopt(res.at("x"));
83  cout << "optimal control: " << uopt << endl;
84 
85  // Get the state trajectory
86  Function xfcn("xfcn", {u}, {s_all, v_all, m_all});
87  vector<double> sopt, vopt, mopt;
88  xfcn({uopt}, {&sopt, &vopt, &mopt});
89  cout << "position: " << sopt << endl;
90  cout << "velocity: " << vopt << endl;
91  cout << "mass: " << mopt << endl;
92 
93  bool b_save_matlab_file(false);
94  if (b_save_matlab_file) {
95  // Create Matlab script to plot the solution
96  ofstream file;
97  string filename = "rocket_ipopt_results.m";
98  file.open(filename.c_str());
99  file << "% Results file from " __FILE__ << endl;
100  file << "% Generated " __DATE__ " at " __TIME__ << endl;
101  file << endl;
102  file << "cost = " << cost << ";" << endl;
103  file << "u = " << uopt << ";" << endl;
104 
105  // Save results to file
106  file << "t = linspace(0,10.0," << nu << ");" << endl;
107  file << "s = " << sopt << ";" << endl;
108  file << "v = " << vopt << ";" << endl;
109  file << "m = " << mopt << ";" << endl;
110 
111  // Finalize the results file
112  file << endl;
113  file << "% Plot the results" << endl;
114  file << "figure(1);" << endl;
115  file << "clf;" << endl << endl;
116 
117  file << "subplot(2,2,1);" << endl;
118  file << "plot(t,s);" << endl;
119  file << "grid on;" << endl;
120  file << "xlabel('time [s]');" << endl;
121  file << "ylabel('position [m]');" << endl << endl;
122 
123  file << "subplot(2,2,2);" << endl;
124  file << "plot(t,v);" << endl;
125  file << "grid on;" << endl;
126  file << "xlabel('time [s]');" << endl;
127  file << "ylabel('velocity [m/s]');" << endl << endl;
128 
129  file << "subplot(2,2,3);" << endl;
130  file << "plot(t,m);" << endl;
131  file << "grid on;" << endl;
132  file << "xlabel('time [s]');" << endl;
133  file << "ylabel('mass [kg]');" << endl << endl;
134 
135  file << "subplot(2,2,4);" << endl;
136  file << "plot(t,u);" << endl;
137  file << "grid on;" << endl;
138  file << "xlabel('time [s]');" << endl;
139  file << "ylabel('Thrust [kg m/s^2]');" << endl << endl;
140 
141  file.close();
142  cout << "Results saved to \"" << filename << "\"" << endl;
143  }
144 }
f
MX f(const MX &x, const MX &u)
Definition: test_casadi.cpp:202

+ Here is the call graph for this function:

TEST ( casadi  ,
part1   
)

Test problem

min x0^2 + x1^2 s.t. x0 + x1 - 10 = 0

Definition at line 146 of file test_casadi.cpp.

References f().

146  {
153  // Optimization variables
154  MX x = MX::sym("x", 2);
155 
156  // Objective
157  MX f = x(0) * x(0) + x(1) * x(1);
158 
159  // Constraints
160  MX g = x(0) + x(1) - 10;
161 
162  // Create an NLP solver instance
163  Function solver = nlpsol("solver", "ipopt", {{"x", x}, {"f", f}, {"g", g}});
164 
165  // Generate C code for the NLP functions
166  solver.generate_dependencies("nlp.c");
167 
168  // Just-in-time compilation?
169  bool jit = false;
170  if (jit) {
171  // Create a new NLP solver instance using just-in-time compilation
172  solver = nlpsol("solver", "ipopt", "nlp.c");
173  } else {
174  // Compile the c-code
175  int flag = system("gcc -fPIC -shared -O3 nlp.c -o nlp.so");
176  casadi_assert(flag == 0, "Compilation failed");
177 
178  // Create a new NLP solver instance from the compiled code
179  solver = nlpsol("solver", "ipopt", "nlp.so");
180  }
181 
182  // Bounds and initial guess
183  std::map<std::string, DM> arg, res;
184  arg["lbx"] = -DM::inf();
185  arg["ubx"] = DM::inf();
186  arg["lbg"] = 0;
187  arg["ubg"] = DM::inf();
188  arg["x0"] = 0;
189 
190  // Solve the NLP
191  res = solver(arg);
192 
193  // Print solution
194  cout << "-----" << endl;
195  cout << "objective at solution = " << res.at("f") << endl;
196  cout << "primal solution = " << res.at("x") << endl;
197  cout << "dual solution (x) = " << res.at("lam_x") << endl;
198  cout << "dual solution (g) = " << res.at("lam_g") << endl;
199 }
f
MX f(const MX &x, const MX &u)
Definition: test_casadi.cpp:202

+ Here is the call graph for this function:

TEST ( casadi  ,
race_car   
)

Definition at line 204 of file test_casadi.cpp.

References f(), and ori::X.

204  {
205  // Car race along a track
206  // ----------------------
207  // An optimal control problem (OCP),
208  // solved with direct multiple-shooting.
209  //
210  // For more information see: http://labs.casadi.org/OCP
211 
212  int N = 100; // number of control intervals
213 
214  auto opti = casadi::Opti(); // Optimization problem
215 
216  Slice all;
217  // ---- decision variables ---------
218  auto X = opti.variable(2, N + 1); // state trajectory
219  auto pos = X(0, all);
220  auto speed = X(1, all);
221  auto U = opti.variable(1, N); // control trajectory (throttle)
222  auto T = opti.variable(); // final time
223 
224  // ---- objective ---------
225  opti.minimize(T); // race in minimal time
226 
227  // ---- dynamic constraints --------
228  auto dt = T / N;
229  for (int k = 0; k < N; ++k) {
230  auto k1 = f(X(all, k), U(all, k));
231  auto k2 = f(X(all, k) + dt / 2 * k1, U(all, k));
232  auto k3 = f(X(all, k) + dt / 2 * k2, U(all, k));
233  auto k4 = f(X(all, k) + dt * k3, U(all, k));
234  auto x_next = X(all, k) + dt / 6 * (k1 + 2 * k2 + 2 * k3 + k4);
235  opti.subject_to(X(all, k + 1) == x_next); // close the gaps
236  }
237 
238  // ---- path constraints -----------
239  opti.subject_to(speed <= 1 - sin(2 * pi * pos) / 2); // track speed limit
240  opti.subject_to(0 <= U <= 1); // control is limited
241 
242  // ---- boundary conditions --------
243  opti.subject_to(pos(1) == 0); // start at position 0 ...
244  opti.subject_to(speed(1) == 0); // ... from stand-still
245  opti.subject_to(pos(N) == 1); // finish line at position 1
246 
247  // ---- misc. constraints ----------
248  opti.subject_to(T >= 0); // Time must be positive
249 
250  // ---- initial values for solver ---
251  opti.set_initial(speed, 1);
252  opti.set_initial(T, 1);
253 
254  // ---- solve NLP ------
255  opti.solver("ipopt"); // set numerical backend
256  auto sol = opti.solve(); // actual solve
257 
258  bool b_matlab_file(false);
259  if (b_matlab_file) {
260  // Create Matlab script to plot the solution
261  ofstream file;
262  string filename = "race_car_results.m";
263  file.open(filename.c_str());
264  file << "% Results file from " __FILE__ << endl;
265  file << "% Generated " __DATE__ " at " __TIME__ << endl;
266  file << endl;
267 
268  // Save results to file
269  file << "t = linspace(0," << sol.value(T) << "," << N << "+1);" << endl;
270  file << "speed = " << std::vector<double>(sol.value(speed)) << ";" << endl;
271  file << "pos = " << std::vector<double>(sol.value(pos)) << ";" << endl;
272  file << "U = " << std::vector<double>(sol.value(U)) << ";" << endl;
273 
274  file << "figure;" << endl;
275  file << "hold on;" << endl;
276  file << "plot(t,speed);" << endl;
277  file << "plot(t,pos);" << endl;
278  file << "plot(t,1-sin(2*pi*pos)/2,'r--');" << endl;
279  file << "stairs(t(1:end-1),U,'k');" << endl;
280  file << "xlabel('Time [s]');" << endl;
281  file << "legend('speed','pos','speed "
282  "limit','throttle','Location','northwest');"
283  << endl;
284 
285  // Have a look at the constraint Jacobian
286  jacobian(opti.g(), opti.x()).sparsity().spy_matlab("race_car_jac_g.m");
287 
288  file << "figure" << std::endl;
289  file << "race_car_jac_g;" << std::endl;
290  file << "xlabel('decision variables');" << std::endl;
291  file << "ylabel('constraints');" << std::endl;
292  file << "print('jac_sp','-dpng');" << std::endl;
293  }
294 }
f
MX f(const MX &x, const MX &u)
Definition: test_casadi.cpp:202

+ Here is the call graph for this function:

TEST ( casadi  ,
jump_opt   
)

Definition at line 330 of file test_casadi.cpp.

References almostEqual(), f(), pretty_print(), simple_dyn(), simple_dyn_vec(), and ori::X.

330  {
331  // Mass Matrix Test
332  //FloatingBaseModel<double> cheetah = buildMiniCheetah<double>().buildModel();
333  //FBModelState<double> state;
334  //state.q = DVec<double>::Zero(cheetah::num_act_joint);
335  //state.qd = DVec<double>::Zero(cheetah::num_act_joint);
336 
337  //for(size_t i(0); i<4; ++i){
338  //state.q[1 + 3*i] = -M_PI/2.;
339  //state.q[2 + 3*i] = M_PI;
340  //}
341  //state.q[7] = M_PI/2.;
342  //state.q[10] = M_PI/2.;
343  //state.bodyPosition.setZero();
344  //state.bodyVelocity.setZero();
345  //state.bodyOrientation[0] = 1.;
346  //state.bodyOrientation[1] = 0.;
347  //state.bodyOrientation[2] = 0.;
348  //state.bodyOrientation[3] = 0.;
349 
350  //cheetah.setState(state);
351  //cheetah.forwardKinematics();
352 
353  //DMat<double> A = cheetah.massMatrix();
354  //std::cout<<A<<std::endl;
355 
356  // Jump optimization
357  // ----------------------
358  // An optimal control problem (OCP),
359  // solved with direct multiple-shooting.
360  //
361  // For more information see: http://labs.casadi.org/OCP
362 
363  int N = 31; // number of control intervals
364  int N_fc = 15;
365  //int N_air = 1;
366  //int N_hc = 15;
367  auto opti = casadi::Opti(); // Optimization problem
368 
369  Slice all;
370  // ---- decision variables ---------
371  auto X = opti.variable(6, N+1); // state trajectory
372  auto P = opti.variable(4); // Landing Location
373  auto F = opti.variable(2, N); // Reaction force (15: front, 1: air, 15: hind)
374  auto cost = opti.variable();
375 
376  DVec<double> X0(6); X0.setZero();
377  X0<< 0.0, 0.2, 0.2, 0.5, -0.1, 0.1;
378 
379  DVec<double> Xf(6); Xf.setZero();
380  //Xf<< 0.1, 0.2, -M_PI/4., 0.1, 0.3, 0.0;
381  Xf<< 0.5, 0.23, -M_PI/4., 0.8, 0.8, 0.1;
382 
383  cost = (X(0,N) - Xf(0)) * (X(0,N) - Xf(0))
384  + (X(1,N) - Xf(1)) * (X(1,N) - Xf(1))
385  + (X(2,N) - Xf(2)) * (X(2,N) - Xf(2))
386  + (X(3,N) - Xf(3)) * (X(3,N) - Xf(3))
387  + (X(4,N) - Xf(4)) * (X(4,N) - Xf(4))
388  + (X(5,N) - Xf(5)) * (X(5,N) - Xf(5));
389  //
390  // ---- objective ---------
391  opti.minimize(cost); // race in minimal time
392 
393  // ---- dynamic constraints --------
394  auto dt = 0.01;
395  double mu(0.5);
396 
397  for (int k = 0; k < N; ++k) {
398  auto dX = simple_dyn(X(all, k), F(all, k), P, k);
399  auto X_next = X(all, k) + dt * dX;
400  opti.subject_to(X(all, k + 1) == X_next); // close the gaps
401 
402  // ---- contact constraints -----------
403  opti.subject_to(-mu * F(1,k) <= F(0, k) <= mu* F(1,k));
404  opti.subject_to(0<=F(1,k) <= 300.);
405  // Kinematics constraints
406  opti.subject_to (-0.15 <= P(0) <= 0.15);
407  opti.subject_to (-0.15 <= P(2) <= 0.15);
408  opti.subject_to (P(1) == 0.);
409  opti.subject_to (P(3) == 0.);
410 
411  }
412  opti.subject_to(F(1, N_fc) == 0.);
413 
414  for(int i(0); i<6; ++i){
415  opti.subject_to(X(i, 1) == X0(i));
416  }
417  // ---- solve NLP ------
418  opti.solver("ipopt"); // set numerical backend
419  auto sol = opti.solve(); // actual solve
420  //std::vector<double> result = std::vector<double>(sol.debug.value(X(0, all) ) );
421 
422  //for(int i(0); i<N; ++i){
423  //printf("xb: %f\n", result[i]);
424  //}
425 
426 
427 
428  for(int k(0); k<N; ++k){
429  std::vector<double> x_std_vec = std::vector<double>(sol.value(X(all, k)));
430  std::vector<double> x_next_std_vec = std::vector<double>(sol.value(X(all, k+1)));
431  std::vector<double> f = std::vector<double>(sol.value(F(all, k)));
432  std::vector<double> p = std::vector<double>(sol.value(P(all)));
433 
434  DVec<double> x_current(6);
435  DVec<double> x_next(6);
436  DVec<double> f_eigen(2);
437  f_eigen[0] = f[0];
438  f_eigen[1] = f[1];
439 
440  DVec<double> p_eigen(4);
441  p_eigen[0] = p[0];
442  p_eigen[1] = p[1];
443  p_eigen[2] = p[2];
444  p_eigen[3] = p[3];
445 
446  for(int i(0); i<6; ++i){
447  x_current[i] = x_std_vec[i];
448  x_next[i] = x_next_std_vec[i];
449  }
450  DVec<double> dx_eigen = simple_dyn_vec(x_current, f_eigen, p_eigen, k);
451  DVec<double> x_next_eigen = x_current + dt * dx_eigen;
452  EXPECT_TRUE(almostEqual(x_next, x_next_eigen, .0001));
453  pretty_print(x_next, std::cout, "x_next_casadi");
454  pretty_print(x_next_eigen, std::cout, "x_next_eigen");
455  printf("\n");
456  }
457 
458  bool b_matlab_file(true);
459  if (b_matlab_file) {
460  ofstream file;
461  string filename = "jump_result.m";
462  file.open(filename.c_str());
463  file << "% Results file from " __FILE__ << endl;
464  file << "% Generated " __DATE__ " at " __TIME__ << endl;
465  file << endl;
466 
467  //file << "t = linspace(0," << 0.31 << "," << N << ");" << endl;
468  file << "xb= " << std::vector<double>(sol.value(X(0, all) ) ) << ";" << endl;
469  file << "zb= " << std::vector<double>(sol.value(X(1, all))) << ";" << endl;
470  file << "theta= " << std::vector<double>(sol.value(X(2, all))) << ";" << endl;
471  file << "dot_xb= " << std::vector<double>(sol.value(X(3, all) ) ) << ";" << endl;
472  file << "dot_zb= " << std::vector<double>(sol.value(X(4, all))) << ";" << endl;
473  file << "dot_theta= " << std::vector<double>(sol.value(X(5, all))) << ";" << endl;
474 
475  file << "Fx = " << std::vector<double>(sol.value(F(0, all) ) ) << ";" << endl;
476  file << "Fz = " << std::vector<double>(sol.value(F(1, all) ) )<< ";" << endl;
477 
478  file << "P = " << std::vector<double>(sol.value(P(all) ) ) << ";" << endl;
479 
480  //file << "figure;" << endl;
481  //file << "hold on;" << endl;
482  //file << "plot(xb, zb);" << endl;
483 
484  //jacobian(opti.g(), opti.x()).sparsity().spy_matlab("jump_jac_g.m");
485 
486  //file << "figure" << std::endl;
487  //file << "jump_jac_g;" << std::endl;
488  //file << "xlabel('decision variables');" << std::endl;
489  //file << "ylabel('constraints');" << std::endl;
490  //file << "print('jac_sp','-dpng');" << std::endl;
491  file.close();
492  }
493 }
simple_dyn
MX simple_dyn(const MX &x, const MX &u, const MX &P, int i)
Definition: test_casadi.cpp:297
pretty_print
void pretty_print(DMat< T > const &mm, std::ostream &os, std::string const &title, std::string const &prefix="", bool vecmode=false, bool nonl=false)
Definition: Utilities_print.h:24
almostEqual
bool almostEqual(const Eigen::MatrixBase< T > &a, const Eigen::MatrixBase< T > &b, T2 tol)
Definition: MathUtilities.h:23
DVec
typename Eigen::Matrix< T, Eigen::Dynamic, 1 > DVec
Definition: cppTypes.h:102
simple_dyn_vec
DVec< double > simple_dyn_vec(const DVec< double > &x, const DVec< double > &u, const DVec< double > &P, int i)
Definition: test_casadi.cpp:311
f
MX f(const MX &x, const MX &u)
Definition: test_casadi.cpp:202

+ Here is the call graph for this function: