├── CMakeLists.txt
├── Optimal_Control.pdf
├── README.md
├── include
    ├── DirectCollocationSolver.hpp
    ├── DirectTranscriptSolver.hpp
    ├── Timer.hpp
    ├── common.hpp
    └── solver.hpp
└── src
    ├── DirectCollocationSolver.cpp
    ├── DirectTranscriptSolver.cpp
    ├── simple_test.cpp
    ├── solver.cpp
    └── test.cpp


/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 3.5)
 2 | project(cartpole_examples LANGUAGES CXX)
 3 | add_definitions(-std=c++14)
 4 | 
 5 | find_package(casadi REQUIRED)
 6 | 
 7 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include)
 8 | 
 9 | add_library(cartpole_solver src/DirectTranscriptSolver.cpp src/DirectCollocationSolver.cpp) 
10 | target_link_libraries(cartpole_solver casadi)
11 | 
12 | add_executable(mytest src/test.cpp)
13 | target_link_libraries(mytest cartpole_solver)
14 | 
15 | add_executable(simpletest src/simple_test.cpp)
16 | target_link_libraries(simpletest cartpole_solver)


--------------------------------------------------------------------------------
/Optimal_Control.pdf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/ytwboxing/cartpole_casadi_cplusplus/719f40f637bcf10f010e19323d67f0e16a8d0064/Optimal_Control.pdf


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # cartpole_casadi_cplusplus  
 2 | 小车倒立摆模型轨迹优化，模型及问题定义见https://epubs.siam.org/doi/pdf/10.1137/16M1062569  
 3 | 求解工具：casadi c++接口(ipopt)  
 4 | optimal contorl problem -> nonlinear optimization problem: direct transcription: single/multiple shooting、 collocatin  
 5 | 文档链接：https://zhuanlan.zhihu.com/p/391903468  
 6 |   
 7 | 运行代码  
 8 | 安装casadi,安装教程：https://github.com/casadi/casadi/wiki/InstallationLinux  
 9 | 下载代码后进入根目录  
10 | mkdir build && cd build && cmake ..  
11 | make  
12 | ./mytest  
13 | 


--------------------------------------------------------------------------------
/include/DirectCollocationSolver.hpp:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include "DirectTranscriptSolver.hpp"
 4 | 
 5 | using casadi::DM;
 6 | using casadi::MX;
 7 | using casadi::Function;
 8 | using casadi::Opti;
 9 | 
10 | class DirectCollocationSolver: public DirectTransSolver{
11 | public:
12 |     DirectCollocationSolver(const cart_pole_model& _model, const constraint_value& _constraint);
13 |     ~DirectCollocationSolver(){ };
14 |     bool setupProblemColloc(const Settings& _settings);
15 |     bool solveColloc(const State& initialState, const State& finalState);
16 |     void getSolutionColloc(casadi::DM& state, casadi::DM& control);
17 | private:
18 |     double CubicHermitePoly(const double& x0, const double& xf, const double& v0, const double& vf, 
19 |                     const double& totalTime, const double& nowTime);
20 |     /*
21 |     Simpson quadrature, also known as Simpson’s rule for integration, works by approximating the 
22 |     integrand of the integral as a piecewise quadratic function
23 |     see https://epubs.siam.org/doi/pdf/10.1137/16M1062569
24 |     */
25 |     double SimpsonCollocation(const double& x0, const double& x_mid, const double& xf, const double& totalTime);
26 |     Function systemDynamics;
27 |     Function getSystemDynamics();
28 |     void setOptColloc();
29 | };


--------------------------------------------------------------------------------
/include/DirectTranscriptSolver.hpp:
--------------------------------------------------------------------------------
 1 | #ifndef SOLVER_H
 2 | #define SOLVER_H
 3 | 
 4 | #include <common.hpp>
 5 | #include <vector>
 6 | #include <string>
 7 | #include <cassert>
 8 | #include <memory>
 9 | 
10 | enum TranscriptMethod{SINGLE_SHOOTING=0, MULTIPLE_SHOOTING};
11 | 
12 | class DirectTransSolver{
13 | public:
14 |     DirectTransSolver(const cart_pole_model& _model, const constraint_value& _constraint);
15 |     ~DirectTransSolver() { };
16 |     bool setupProblem(const Settings& _settings);
17 |     bool solve(const State& initialState, const State& finalState);
18 |     void getSolution(casadi::DM& state, casadi::DM& control);
19 | 
20 | public:
21 |     enum SolverState{NOT_INITIALIZED=0, PROBLEM_SET, PROBLEM_SOLVED};
22 |     SolverState solverState;
23 |     TranscriptMethod transMethod;
24 |     Settings settings;
25 |     casadi::Function integratorDynamics;
26 |     casadi::Function accelerationConsisitencyConstraint;
27 |     casadi::MX initialStateParameters;
28 |     casadi::MX finalStateParameters;
29 |     casadi::MX X, A, U, T;
30 |     casadi::MX minCartHorizonPos, maxCartHorizonPos, minU, maxU;
31 |     
32 |     cart_pole_model model;
33 | 
34 |     casadi::Opti opti;
35 |     std::unique_ptr<casadi::OptiSol> solution;
36 |     casadi::Function getIntegratorDynamics();
37 |     casadi::Function getAccelerationConsistencyConstraintFunction();
38 |     void setupOpti();
39 |     void setParametersValue(const State& initialState, const State& finalState);
40 |     void setTranscriptMethod(const TranscriptMethod& method){transMethod = method;}
41 | };
42 | 
43 | #endif


--------------------------------------------------------------------------------
/include/Timer.hpp:
--------------------------------------------------------------------------------
 1 | #ifndef TIMER_H
 2 | #define TIMER_H
 3 | 
 4 | #include <assert.h>
 5 | #include <stdint.h>
 6 | #include <sys/time.h>
 7 | 
 8 | class Timer {
 9 |  public:
10 |   explicit Timer() { start(); }
11 | 
12 |   void start() { clock_gettime(CLOCK_MONOTONIC, &_startTime); }
13 |   double getMs() { return (double)getNs() / 1.e6; }
14 |   int64_t getNs() {
15 |     struct timespec now;
16 |     clock_gettime(CLOCK_MONOTONIC, &now);
17 |     return (int64_t)(now.tv_nsec - _startTime.tv_nsec) +
18 |            1000000000 * (now.tv_sec - _startTime.tv_sec);
19 |   }
20 | 
21 |   double getSeconds() { return (double)getNs() / 1.e9; }
22 |   struct timespec _startTime;
23 | };
24 | 
25 | #endif 
26 | 


--------------------------------------------------------------------------------
/include/common.hpp:
--------------------------------------------------------------------------------
 1 | #ifndef COMMON_H
 2 | #define COMMON_H
 3 | 
 4 | #include <casadi/casadi.hpp>
 5 | 
 6 | typedef struct{
 7 |     double control;
 8 | }costWeights;
 9 | 
10 | typedef struct{
11 |     int phaseLength;
12 |     double time;
13 |     costWeights _costWeights;
14 |     double solverVerbosity;
15 |     /*
16 |     ipopt's available linear solvers = {"ma27", "ma57", "ma77", "ma86", "ma97", "pardiso", "wsmp", "mumps"};
17 |     used for it's interior point method
18 |     */
19 |     std::string ipoptLinearSolver;
20 | }Settings;
21 | 
22 | typedef struct {
23 |     casadi::DM state = casadi::DM::zeros(4,1);
24 | }State;
25 | 
26 | typedef struct{
27 |     double l;
28 |     double m1;
29 |     double m2;
30 |     double g;
31 | }cart_pole_model;
32 | 
33 | typedef struct{
34 |     double d_min;
35 |     double d_max;
36 |     double u_min;
37 |     double u_max;
38 | }constraint_value;
39 | #endif


--------------------------------------------------------------------------------
/include/solver.hpp:
--------------------------------------------------------------------------------
 1 | #ifndef SOLVER_H
 2 | #define SOLVER_H
 3 | 
 4 | #include <common.hpp>
 5 | #include <vector>
 6 | #include <string>
 7 | #include <cassert>
 8 | #include <memory>
 9 | 
10 | class Solver{
11 | public:
12 |     Solver(const cart_pole_model& _model, const constraint_value& _constraint);
13 |     ~Solver() { };
14 |     bool setupProblem(const Settings& _settings);
15 |     bool solve(const State& initialState, const State& finalState);
16 |     void getSolution(casadi::DM& state, casadi::DM& control);
17 | 
18 | private:
19 |     enum SolverState{NOT_INITIALIZED=0, PROBLEM_SET, PROBLEM_SOLVED};
20 |     SolverState solverState;
21 |     Settings settings;
22 |     casadi::Function integratorDynamics;
23 |     casadi::Function accelerationConsisitencyConstraint;
24 |     casadi::MX initialStateParameters;
25 |     casadi::MX finalStateParameters;
26 |     casadi::MX X, A, U, T;
27 |     casadi::MX minCartHorizonPos, maxCartHorizonPos, minU, maxU;
28 |     
29 |     cart_pole_model model;
30 | 
31 |     casadi::Opti opti;
32 |     std::unique_ptr<casadi::OptiSol> solution;
33 |     casadi::Function getIntegratorDynamics();
34 |     casadi::Function getAccelerationConsistencyConstraintFunction();
35 |     void setupOpti();
36 |     void setParametersValue(const State& initialState, const State& finalState);
37 | };
38 | 
39 | #endif


--------------------------------------------------------------------------------
/src/DirectCollocationSolver.cpp:
--------------------------------------------------------------------------------
  1 | #include "DirectCollocationSolver.hpp"
  2 | using S1 = casadi::Slice;
  3 | 
  4 | DirectCollocationSolver::DirectCollocationSolver(const cart_pole_model& _model, const constraint_value& _constraint)
  5 |     :DirectTransSolver(_model, _constraint){
  6 |         solution = nullptr;
  7 |         minCartHorizonPos = opti.parameter();
  8 |         maxCartHorizonPos = opti.parameter();
  9 |         minU = opti.parameter();
 10 |         maxU = opti.parameter();
 11 |         T = opti.parameter();
 12 |         initialStateParameters = opti.parameter(4);
 13 |         finalStateParameters = opti.parameter(4);
 14 |         opti.set_value(minCartHorizonPos, _constraint.d_min);
 15 |         opti.set_value(maxCartHorizonPos, _constraint.d_max);
 16 |         opti.set_value(minU, _constraint.u_min);
 17 |         opti.set_value(maxU, _constraint.u_max);
 18 |     }
 19 | double DirectCollocationSolver::CubicHermitePoly(const double& x0, const double& xf, const double& v0, const double& vf, 
 20 |                     const double& totalTime, const double& nowTime)
 21 |     {
 22 |         double a, b, c, d, xt;
 23 |         double t = nowTime;
 24 |         a = x0;
 25 |         b = v0;
 26 |         c = -( (3 * (x0 - xf) + totalTime * (2 * v0 + vf)) ) / std::pow(totalTime, 2);
 27 |         d = ( 2 * (x0 - xf) + totalTime * (v0 + vf) ) / std::pow(totalTime, 3);
 28 | 
 29 |         xt = a + b * t + c * std::pow(t, 2) + d * std::pow(t, 3);
 30 | 
 31 |         return xt;
 32 |     }
 33 | double DirectCollocationSolver::SimpsonCollocation(const double& x0, const double& x_mid, const double& xf, const double& totalTime)
 34 |     {
 35 |         double xt = totalTime * (x0 + 4 * x_mid + xf) / 6;
 36 | 
 37 |         return xt;
 38 |     }
 39 | Function DirectCollocationSolver::getSystemDynamics(){
 40 |     casadi::MX X = casadi::MX::sym("x", 4);
 41 |     casadi::MX U = casadi::MX::sym("u", 1);
 42 |     casadi::MX dt = casadi::MX::sym("dt");
 43 |     MX A(2, 1);
 44 |     casadi::MX currentPosition = X(casadi::Slice(0,2));
 45 |     casadi::MX currentVelocity = X(casadi::Slice(2,4));
 46 | 
 47 |     casadi::MX temp1 = model.l * model.m2 * sin(currentPosition(1)) * pow(currentVelocity(1), 2);
 48 |     casadi::MX temp2 = model.m2 * model.g * cos(currentPosition(1)) * sin(currentPosition(1));
 49 |     casadi::MX temp3 = model.m1 + model.m2 * (1 - pow(cos(currentPosition(1)), 2));
 50 |     A(0) = (temp1 + U + temp2) / temp3;
 51 | 
 52 |     temp1 = model.l * model.m2 * cos(currentPosition(1)) * sin(currentPosition(1)) * pow(currentVelocity(1), 2);
 53 |     temp2 = (model.m1 + model.m2) * model.g * sin(currentPosition(1));
 54 |     temp3 = model.l * model.m1 + model.l * model.m2 * (1 - pow(cos(currentPosition(1)), 2));
 55 |     A(1) = (temp1 + U * cos(currentPosition(1)) + temp2) / temp3;
 56 |     casadi::MX dynamics = casadi::MX::vertcat({X(S1(2,4)), A});
 57 | 
 58 |     return casadi::Function("dynamics", {X, U, dt}, {dynamics});
 59 | }
 60 | void DirectCollocationSolver::setOptColloc(){
 61 |     casadi_int phaseLength = static_cast<casadi_int> (settings.phaseLength);
 62 |     opti.set_value(T, settings.time);
 63 |     casadi_int N = 2 * phaseLength;
 64 |     X = opti.variable(4, N + 1);
 65 |     A = opti.variable(2, N + 1);
 66 |     U = opti.variable(1, N + 1);
 67 | 
 68 |     opti.subject_to(X(S1(), 0) == initialStateParameters);
 69 |     opti.subject_to(X(S1(), N) == finalStateParameters);
 70 | 
 71 |     casadi::MX dT = T / phaseLength;
 72 |     MX f_curr, f_mid, f_next;
 73 |     MX costFunction = 0;
 74 |     costWeights w = settings._costWeights;
 75 |     for(casadi_int k = 0; k < N; ++k){
 76 |         //samples: x(k), x(k+1), x(k+2),,,,,,
 77 |         if(k % 2 == 0 && k + 2 <= N){
 78 |             f_curr = casadi::MX::vertcat(systemDynamics({X(S1(), k), U(S1(), k), 0}));
 79 |             f_next = casadi::MX::vertcat(systemDynamics({X(S1(), k + 2), U(S1(), k + 2), 0}));
 80 |             f_mid = casadi::MX::vertcat(systemDynamics({X(S1(), k + 1), U(S1(), k + 1), 0}));
 81 |             opti.subject_to(X(S1(), k + 2) == X(S1(), k) + dT * (f_curr + 4 * f_mid + f_next) / 6);
 82 |             
 83 |             costFunction += w.control * dT / 6 * ( pow(U(0,k),2) + 4 * pow(U(0,k+1),2) + pow(U(0,k+2),2));
 84 |         }
 85 |         /*
 86 |         collocation points, because states uses cubic hermite polynomial, collocation points are midium points at 
 87 |         each interval
 88 |         x(k+0.5), x(k+1+0.5),,,,,
 89 |         */
 90 |         else if(k % 2 != 0){
 91 |             opti.subject_to(X(S1(), k) == 0.5 * (X(S1(), k - 1) + X(S1(), k + 1)) + dT * (f_curr - f_next) / 8);
 92 |             opti.subject_to(U(S1(), k) == 0.5 * (U(S1(), k - 1) + U(S1(), k + 1)));
 93 | 
 94 |         }
 95 |         opti.subject_to(minCartHorizonPos <= X(0, k) <= maxCartHorizonPos);
 96 |         opti.subject_to(minU <= U(0, k) <= maxU);
 97 |     }   
 98 |     opti.subject_to(minCartHorizonPos <= X(0, N) <= maxCartHorizonPos);
 99 |     opti.subject_to(minU <= U(0, N) <= maxU);
100 |     opti.minimize(costFunction);
101 | }
102 | bool DirectCollocationSolver::setupProblemColloc(const Settings& _settings){
103 |     settings = _settings;
104 |     systemDynamics = getSystemDynamics();
105 |     //accelerationConsisitencyConstraint = getAccelerationConsistencyConstraintFunction();
106 | 
107 |     setOptColloc();
108 | 
109 |     casadi::Dict casadiOptions;
110 |     casadi::Dict ipoptOptions;
111 | 
112 |     casadiOptions["expand"] = true;//Replace MX with SX expressions in problem formulation, speed up
113 |     unsigned long solverVerbosity = settings.solverVerbosity;
114 |     if (solverVerbosity) {
115 |         casadi_int ipoptVerbosity = static_cast<long long>(solverVerbosity - 1);
116 |         ipoptOptions["print_level"] = ipoptVerbosity;
117 |         casadiOptions["print_time"] = true;
118 |         casadiOptions["bound_consistency"] = false;
119 |     } else {
120 |         ipoptOptions["print_level"] = 0;
121 |         casadiOptions["print_time"] = false;
122 |         //casadiOptions["bound_consistency"] = false;
123 |         //ipoptOptions["fixed_variable_treatment"] = "make_constraint";
124 |     }
125 |     ipoptOptions["linear_solver"] = settings.ipoptLinearSolver;
126 | 
127 |     opti.solver("ipopt", casadiOptions, ipoptOptions);
128 | 
129 |     solverState = SolverState::PROBLEM_SET;
130 | 
131 |     return true;
132 | }
133 | bool DirectCollocationSolver::solveColloc(const State& initialState, const State& finalState){
134 |     if(solverState == SolverState::NOT_INITIALIZED){
135 |         throw std::runtime_error("problem not initialized");
136 |         return false;
137 |     }
138 |     setParametersValue(initialState, finalState);
139 |     casadi_int npoints = 2 * static_cast<casadi_int> (settings.phaseLength);
140 |     casadi::DM initPos = casadi::DM::zeros(2,1);
141 |     casadi::DM finalPos = casadi::DM::zeros(2,1);
142 |     initPos = initialState.state(casadi::Slice(0,2));
143 |     finalPos = finalState.state(casadi::Slice(0,2));
144 |     casadi::DM interpolatedPosition(2, 1);
145 |     casadi::DM linSpacePoints = casadi::DM::linspace(0, 1, npoints + 1);
146 |     for(casadi_int k = 0; k < npoints + 1; ++k){
147 |         /*
148 |         initial guess, pos using linear interpolatation from init to final, 
149 |         vel and control all set zero
150 |         */
151 |         interpolatedPosition = initPos + linSpacePoints(k) * (finalPos - initPos);
152 |         opti.set_initial(X(casadi::Slice(0,2), k), interpolatedPosition);
153 |         opti.set_initial(X(casadi::Slice(2,4), k), 0);
154 |     }
155 |     for(casadi_int k = 0; k < npoints; ++k){
156 |         opti.set_initial(U(casadi::Slice(), k), 0);
157 |     }
158 |     
159 |     solverState = SolverState::PROBLEM_SET;
160 |     
161 |     try{
162 |         solution = std::make_unique<casadi::OptiSol>(opti.solve());
163 |     }catch(std::exception &e){
164 |         opti.debug().show_infeasibilities(1e-5);
165 |         std::cerr << "error while solving the optimization" << std::endl;
166 |         std::cerr << "Details:\n " << e.what() << std::endl;
167 |         return false;
168 |     }
169 |     solverState = SolverState::PROBLEM_SOLVED;
170 |     std::cout << "\nsolve success\n\n";
171 |     return true;
172 | }
173 | void DirectCollocationSolver::getSolutionColloc(casadi::DM& state, casadi::DM& control){
174 |     DM state_all = solution -> value(X);
175 |     DM control_all = solution -> value(U);
176 |     int a = 0;
177 |     state = DM::zeros(4, settings.phaseLength + 1);
178 |     control = DM::zeros(1, settings.phaseLength + 1);
179 |     for(int i = 0; i < 2 * settings.phaseLength + 1; ++i){
180 |         if(i % 2 == 0){
181 |             //std::cout <<"i\n" << i << "\n"<<control_all(S1(), i) << "\n\n";
182 |             state(S1(),a) = DM::vertcat({state_all(S1(), i)});
183 |             control(S1(),a) = control_all(S1(), i);
184 |             a++;
185 |         }
186 |     }
187 | }
188 | 


--------------------------------------------------------------------------------
/src/DirectTranscriptSolver.cpp:
--------------------------------------------------------------------------------
  1 | #include "DirectTranscriptSolver.hpp"
  2 | 
  3 | DirectTransSolver::DirectTransSolver(const cart_pole_model& _model, const constraint_value& _constraint)
  4 |  {
  5 |     model = _model;
  6 |     solverState = SolverState::NOT_INITIALIZED;
  7 |     transMethod = TranscriptMethod::SINGLE_SHOOTING;
  8 |     solution = nullptr;
  9 |     minCartHorizonPos = opti.parameter();
 10 |     maxCartHorizonPos = opti.parameter();
 11 |     minU = opti.parameter();
 12 |     maxU = opti.parameter();
 13 |     T = opti.parameter();
 14 |     initialStateParameters = opti.parameter(4);
 15 |     finalStateParameters = opti.parameter(4);
 16 |     opti.set_value(minCartHorizonPos, _constraint.d_min);
 17 |     opti.set_value(maxCartHorizonPos, _constraint.d_max);
 18 |     opti.set_value(minU, _constraint.u_min);
 19 |     opti.set_value(maxU, _constraint.u_max);
 20 |  }
 21 | 
 22 |  casadi::Function DirectTransSolver::getIntegratorDynamics()
 23 |  {
 24 |      casadi::MX x = casadi::MX::sym("x", 4);
 25 |      casadi::MX a = casadi::MX::sym("a", 2);
 26 |      casadi::MX dT = casadi::MX::sym("dt");
 27 | 
 28 |      casadi::MX p = x(casadi::Slice(0,2));
 29 |      casadi::MX v = x(casadi::Slice(2,4));
 30 |      casadi::MX rhs = casadi::MX::vertcat({p + dT * (v + 0.5 * dT * a),
 31 |                                            v + dT * a});
 32 |     
 33 |     return casadi::Function("Integrator", {x,a,dT}, {rhs});
 34 |  }
 35 | 
 36 |  casadi::Function DirectTransSolver::getAccelerationConsistencyConstraintFunction()
 37 |  {
 38 |     casadi::MX X = casadi::MX::sym("x", 4);
 39 |     casadi::MX U = casadi::MX::sym("u", 1);
 40 |     casadi::MX A = casadi::MX::sym("a", 2);
 41 | 
 42 |     casadi::MX currentPosition = X(casadi::Slice(0,2));
 43 |     casadi::MX currentVelocity = X(casadi::Slice(2,4));
 44 | 
 45 |     casadi::MX temp1 = model.l * model.m2 * sin(currentPosition(1)) * pow(currentVelocity(1), 2);
 46 |     casadi::MX temp2 = model.m2 * model.g * cos(currentPosition(1)) * sin(currentPosition(1));
 47 |     casadi::MX temp3 = model.m1 + model.m2 * (1 - pow(cos(currentPosition(1)), 2));
 48 |     casadi::MX constraint_cart = A(0) - (temp1 + U + temp2) / temp3;
 49 | 
 50 |     temp1 = model.l * model.m2 * cos(currentPosition(1)) * sin(currentPosition(1)) * pow(currentVelocity(1), 2);
 51 |     temp2 = (model.m1 + model.m2) * model.g * sin(currentPosition(1));
 52 |     temp3 = model.l * model.m1 + model.l * model.m2 * (1 - pow(cos(currentPosition(1)), 2));
 53 |     casadi::MX constraint_pole = A(1) + (temp1 + U * cos(currentPosition(1)) + temp2) / temp3;
 54 | 
 55 |     casadi::MX constarint = casadi::MX::vertcat({constraint_cart, constraint_pole});
 56 |     return casadi::Function("accelerationConsistency", {X, U, A}, {constarint});
 57 | 
 58 |  }
 59 | 
 60 |  void DirectTransSolver::setupOpti()
 61 |  {
 62 |     using S1 = casadi::Slice;
 63 |     casadi_int phaseLength, N;
 64 |     opti.set_value(T, settings.time);
 65 |     if(transMethod == TranscriptMethod::SINGLE_SHOOTING){
 66 |         phaseLength = static_cast<casadi_int> (settings.phaseLength);
 67 |         N = phaseLength;
 68 |     }
 69 |     else if(transMethod == TranscriptMethod::MULTIPLE_SHOOTING){
 70 |         phaseLength = static_cast<casadi_int> (settings.phaseLength / 2);
 71 |         N = phaseLength * 2;
 72 |     }
 73 |     X = opti.variable(4, N + 1);
 74 |     A = opti.variable(2, N);
 75 |     // casadi::MX X(4, N + 1);// = opti.variable(4, N + 1);
 76 |     // casadi::MX A(2, N);// = opti.variable(2, N);
 77 |     U = opti.variable(1, N);
 78 | 
 79 |     opti.subject_to(X(S1(), 0) == initialStateParameters);
 80 |     opti.subject_to(X(S1(), N) == finalStateParameters);
 81 |     if(transMethod == TranscriptMethod::SINGLE_SHOOTING){
 82 |         casadi::MX dT = T / phaseLength;
 83 |         for(casadi_int k = 0; k < N; ++k){
 84 |             opti.subject_to(X(S1(), k + 1) == casadi::MX::vertcat(integratorDynamics({X(S1(), k), A(S1(), k), dT})));
 85 |             opti.subject_to(minCartHorizonPos <= X(0, k) <= maxCartHorizonPos);
 86 |             opti.subject_to(minU <= U(0, k) <= maxU);
 87 |             opti.subject_to(casadi::MX::vertcat(accelerationConsisitencyConstraint({X(S1(), k), U(S1(), k), A(S1(), k)})) == casadi::MX::zeros(2, 1));
 88 |         } 
 89 |     }
 90 |     else if(transMethod == TranscriptMethod::MULTIPLE_SHOOTING){
 91 |         casadi::MX dT = T / (phaseLength * 2);
 92 |         for(casadi_int phase = 0;phase<2;++phase){
 93 |             for(casadi_int k = phaseLength*phase;k < phaseLength*(phase+1);++k){
 94 |                 if(k != (phaseLength-1)){
 95 |                     opti.subject_to(X(S1(), k + 1) == casadi::MX::vertcat(integratorDynamics({X(S1(), k), A(S1(), k), dT})));
 96 |                 }
 97 |                 opti.subject_to(minCartHorizonPos <= X(0, k) <= maxCartHorizonPos);
 98 |                 opti.subject_to(minU <= U(0, k) <= maxU);
 99 | 
100 |                 opti.subject_to(casadi::MX::vertcat(accelerationConsisitencyConstraint({X(S1(), k), U(S1(), k), A(S1(), k)})) == casadi::MX::zeros(2, 1));
101 |             }
102 |     }
103 |     opti.subject_to( X(S1(), phaseLength) 
104 |         == casadi::MX::vertcat(integratorDynamics({X(S1(), phaseLength-1), A(S1(), phaseLength-1), dT}))
105 |         - casadi::DM::vertcat({0.2, 0.4, 0.01, 0.01}) 
106 |         );
107 |     }
108 |     costWeights w = settings._costWeights;
109 |     casadi::MX costFunction = w.control * casadi::MX::sumsqr(U(0, S1()));
110 |     opti.minimize(costFunction);
111 |  }
112 | 
113 | bool DirectTransSolver::setupProblem(const Settings& _settings)
114 | {
115 |     settings = _settings;
116 |     integratorDynamics = getIntegratorDynamics();
117 |     accelerationConsisitencyConstraint = getAccelerationConsistencyConstraintFunction();
118 | 
119 |     setupOpti();
120 | 
121 |     casadi::Dict casadiOptions;
122 |     casadi::Dict ipoptOptions;
123 | 
124 |     casadiOptions["expand"] = true;//Replace MX with SX expressions in problem formulation, speed up
125 |     unsigned long solverVerbosity = settings.solverVerbosity;
126 |     if (solverVerbosity) {
127 |         casadi_int ipoptVerbosity = static_cast<long long>(solverVerbosity - 1);
128 |         ipoptOptions["print_level"] = ipoptVerbosity;
129 |         casadiOptions["print_time"] = true;
130 |         casadiOptions["bound_consistency"] = false;
131 |     } else {
132 |         ipoptOptions["print_level"] = 0;
133 |         casadiOptions["print_time"] = false;
134 |         //casadiOptions["bound_consistency"] = false;
135 |         //ipoptOptions["fixed_variable_treatment"] = "make_constraint";
136 |     }
137 |     ipoptOptions["linear_solver"] = settings.ipoptLinearSolver;
138 | 
139 |     opti.solver("ipopt", casadiOptions, ipoptOptions);
140 | 
141 |     solverState = SolverState::PROBLEM_SET;
142 | 
143 |     return true;
144 | }
145 | 
146 | void DirectTransSolver::setParametersValue(const State& initialState, const State& finalState)
147 | {
148 |     opti.set_value(initialStateParameters, initialState.state);
149 |     opti.set_value(finalStateParameters, finalState.state);
150 | }
151 | 
152 | bool DirectTransSolver::solve(const State& initialState, const State& finalState)
153 | {
154 |     if(solverState == SolverState::NOT_INITIALIZED){
155 |         throw std::runtime_error("problem not initialized");
156 |         return false;
157 |     }
158 |     setParametersValue(initialState, finalState);
159 |     casadi_int npoints = static_cast<casadi_int> (settings.phaseLength);
160 | 
161 |     casadi::DM initPos = casadi::DM::zeros(2,1);
162 |     casadi::DM finalPos = casadi::DM::zeros(2,1);
163 |     initPos = initialState.state(casadi::Slice(0,2));
164 |     finalPos = finalState.state(casadi::Slice(0,2));
165 |     casadi::DM interpolatedPosition(2, 1);
166 |     casadi::DM linSpacePoints = casadi::DM::linspace(0, 1, npoints + 1);
167 |     for(casadi_int k = 0; k < npoints + 1; ++k){
168 |         /*
169 |         initial guess, pos using linear interpolatation from init to final, 
170 |         vel and control all set zero
171 |         */
172 |         interpolatedPosition = initPos + linSpacePoints(k) * (finalPos - initPos);
173 |         opti.set_initial(X(casadi::Slice(0,2), k), interpolatedPosition);
174 |         opti.set_initial(X(casadi::Slice(2,4), k), 0);
175 |     }
176 |     for(casadi_int k = 0; k < npoints; ++k){
177 |         opti.set_initial(U(casadi::Slice(), k), 0);
178 |     }
179 |     
180 |     solverState = SolverState::PROBLEM_SET;
181 |     
182 |     try{
183 |         solution = std::make_unique<casadi::OptiSol>(opti.solve());
184 |     }catch(std::exception &e){
185 |         opti.debug().show_infeasibilities(1e-5);
186 |         std::cerr << "error while solving the optimization" << std::endl;
187 |         std::cerr << "Details:\n " << e.what() << std::endl;
188 |         return false;
189 |     }
190 |     solverState = SolverState::PROBLEM_SOLVED;
191 |     std::cout << "\nsolve success\n\n";
192 |     return true;
193 | }
194 | 
195 | void DirectTransSolver::getSolution(casadi::DM& state, casadi::DM& control){
196 |     casadi::DM state_all = solution -> value(X);
197 |     casadi::DM control_all = solution -> value(U);
198 |     state = casadi::DM::zeros(4, settings.phaseLength + 1);
199 |     control = casadi::DM::zeros(1, settings.phaseLength + 1);
200 |     state = state_all;
201 |     control = control_all;
202 | }


--------------------------------------------------------------------------------
/src/simple_test.cpp:
--------------------------------------------------------------------------------
 1 | #include <casadi/casadi.hpp>
 2 | #include <iostream>
 3 | 
 4 | int main(){
 5 |     casadi::Opti opti;
 6 |     casadi::MX x,y;
 7 |     std::unique_ptr<casadi::OptiSol> sol;
 8 |     casadi::DM D_sol;
 9 | 
10 |     x = opti.variable();
11 |     y = opti.variable();
12 | 
13 |     opti.minimize(pow(x, 3)+pow(y, 2));
14 |     opti.subject_to(0 <= x <= 0.4);
15 |     opti.subject_to(0.7 <= y <= 0.9);
16 | 
17 |     casadi::Dict casadiOptions;
18 |     casadi::Dict ipoptOptions;
19 |     casadiOptions["expand"] = true;//Replace MX with SX expressions in problem formulation, speed up
20 |     ipoptOptions["print_level"] = 0;
21 |     ipoptOptions["linear_solver"] = "mumps";
22 |     opti.solver("ipopt", casadiOptions, ipoptOptions);
23 |     opti.set_initial(x, 0);
24 |     opti.set_initial(y, 0);
25 |     sol = std::make_unique<casadi::OptiSol>(opti.solve());
26 | 
27 |     D_sol = casadi::DM::vertcat({sol -> value(x), sol -> value(y)});
28 |     std::cout << "x: " << D_sol(0) << "\n" << "y: " << D_sol(1) << std::endl;
29 | 
30 |     return 0;
31 | }
32 | 
33 | 
34 | 


--------------------------------------------------------------------------------
/src/solver.cpp:
--------------------------------------------------------------------------------
  1 | #include "solver.hpp"
  2 | 
  3 | Solver::Solver(const cart_pole_model& _model, const constraint_value& _constraint)
  4 |  :model(_model), solverState(SolverState::NOT_INITIALIZED), solution(nullptr)
  5 | {
  6 |     minCartHorizonPos = opti.parameter();
  7 |     maxCartHorizonPos = opti.parameter();
  8 |     minU = opti.parameter();
  9 |     maxU = opti.parameter();
 10 |     T = opti.parameter();
 11 |     initialStateParameters = opti.parameter(4);
 12 |     finalStateParameters = opti.parameter(4);
 13 |     opti.set_value(minCartHorizonPos, _constraint.d_min);
 14 |     opti.set_value(maxCartHorizonPos, _constraint.d_max);
 15 |     opti.set_value(minU, _constraint.u_min);
 16 |     opti.set_value(maxU, _constraint.u_max);
 17 |     opti.set_value(T, 2);
 18 |  }
 19 | 
 20 |  casadi::Function Solver::getIntegratorDynamics()
 21 |  {
 22 |      casadi::MX x = casadi::MX::sym("x", 4);
 23 |      casadi::MX a = casadi::MX::sym("a", 2);
 24 |      casadi::MX dT = casadi::MX::sym("dt");
 25 | 
 26 |      casadi::MX p = x(casadi::Slice(0,2));
 27 |      casadi::MX v = x(casadi::Slice(2,4));
 28 |      casadi::MX rhs = casadi::MX::vertcat({p + dT * (v + 0.5 * dT * a),
 29 |                                            v + dT * a});
 30 |     
 31 |     return casadi::Function("Integrator", {x,a,dT}, {rhs});
 32 |  }
 33 | 
 34 |  casadi::Function Solver::getAccelerationConsistencyConstraintFunction()
 35 |  {
 36 |     casadi::MX X = casadi::MX::sym("x", 4);
 37 |     casadi::MX U = casadi::MX::sym("u", 1);
 38 |     casadi::MX A = casadi::MX::sym("a", 2);
 39 | 
 40 |     casadi::MX currentPosition = X(casadi::Slice(0,2));
 41 |     casadi::MX currentVelocity = X(casadi::Slice(2,4));
 42 | 
 43 |     casadi::MX temp1 = model.l * model.m2 * sin(currentPosition(1)) * pow(currentVelocity(1), 2);
 44 |     casadi::MX temp2 = model.m2 * model.g * cos(currentPosition(1)) * sin(currentPosition(1));
 45 |     casadi::MX temp3 = model.m1 + model.m2 * (1 - pow(cos(currentPosition(1)), 2));
 46 |     casadi::MX constraint_cart = A(0) - (temp1 + U + temp2) / temp3;
 47 | 
 48 |     temp1 = model.l * model.m2 * cos(currentPosition(1)) * sin(currentPosition(1)) * pow(currentVelocity(1), 2);
 49 |     temp2 = (model.m1 + model.m2) * model.g * sin(currentPosition(1));
 50 |     temp3 = model.l * model.m1 + model.l * model.m2 * (1 - pow(cos(currentPosition(1)), 2));
 51 |     casadi::MX constraint_pole = A(1) + (temp1 + U * cos(currentPosition(1)) + temp2) / temp3;
 52 | 
 53 |     casadi::MX constarint = casadi::MX::vertcat({constraint_cart, constraint_pole});
 54 |     return casadi::Function("accelerationConsistency", {X, U, A}, {constarint});
 55 | 
 56 |  }
 57 | 
 58 |  void Solver::setupOpti()
 59 |  {
 60 |     using S1 = casadi::Slice;
 61 |     casadi_int phaseLength = static_cast<casadi_int> (settings.phaseLength);
 62 |     casadi_int N = phaseLength;//single phase
 63 | 
 64 |     X = opti.variable(4, N + 1);
 65 |     A = opti.variable(2, N);
 66 |     U = opti.variable(1, N);
 67 | 
 68 |     opti.subject_to(X(S1(), 0) == initialStateParameters);
 69 |     opti.subject_to(X(S1(), N) == finalStateParameters);
 70 | 
 71 |     casadi::MX dT = T / phaseLength;
 72 |     for(casadi_int k = 0; k < N; ++k){
 73 |         opti.subject_to(X(S1(), k + 1) == casadi::MX::vertcat(integratorDynamics({X(S1(), k), A(S1(), k), dT})));
 74 |         opti.subject_to(minCartHorizonPos <= X(0, k) <= maxCartHorizonPos);
 75 |     } 
 76 | 
 77 |     for(casadi_int k = 0; k < N; ++k){
 78 |         opti.subject_to(minU <= U(0, k) <= maxU);
 79 |         opti.subject_to(casadi::MX::vertcat(accelerationConsisitencyConstraint({X(S1(), k), U(S1(), k), A(S1(), k)})) == casadi::MX::zeros(2, 1));
 80 |     }
 81 |    
 82 |     costWeights w = settings._costWeights;
 83 |     casadi::MX costFunction = w.control * casadi::MX::sumsqr(U(0, S1()));
 84 |     opti.minimize(costFunction);
 85 |  }
 86 | 
 87 | bool Solver::setupProblem(const Settings& _settings)
 88 | {
 89 |     settings = _settings;
 90 |     integratorDynamics = getIntegratorDynamics();
 91 |     accelerationConsisitencyConstraint = getAccelerationConsistencyConstraintFunction();
 92 | 
 93 |     setupOpti();
 94 | 
 95 |     casadi::Dict casadiOptions;
 96 |     casadi::Dict ipoptOptions;
 97 | 
 98 |     casadiOptions["expand"] = true;//Replace MX with SX expressions in problem formulation, speed up
 99 |     unsigned long solverVerbosity = settings.solverVerbosity;
100 |     if (solverVerbosity) {
101 |         casadi_int ipoptVerbosity = static_cast<long long>(solverVerbosity - 1);
102 |         ipoptOptions["print_level"] = ipoptVerbosity;
103 |         casadiOptions["print_time"] = true;
104 |         casadiOptions["bound_consistency"] = false;
105 |     } else {
106 |         ipoptOptions["print_level"] = 0;
107 |         casadiOptions["print_time"] = false;
108 |         //casadiOptions["bound_consistency"] = false;
109 |         //ipoptOptions["fixed_variable_treatment"] = "make_constraint";
110 |     }
111 |     ipoptOptions["linear_solver"] = settings.ipoptLinearSolver;
112 | 
113 |     opti.solver("ipopt", casadiOptions, ipoptOptions);
114 | 
115 |     solverState = SolverState::PROBLEM_SET;
116 | 
117 |     return true;
118 | }
119 | 
120 | void Solver::setParametersValue(const State& initialState, const State& finalState)
121 | {
122 |     opti.set_value(initialStateParameters, initialState.state);
123 |     opti.set_value(finalStateParameters, finalState.state);
124 | }
125 | 
126 | bool Solver::solve(const State& initialState, const State& finalState)
127 | {
128 |     if(solverState == SolverState::NOT_INITIALIZED){
129 |         throw std::runtime_error("problem not initialized");
130 |         return false;
131 |     }
132 |     setParametersValue(initialState, finalState);
133 |     casadi_int npoints = static_cast<casadi_int> (settings.phaseLength);
134 |     casadi::DM initPos = casadi::DM::zeros(2,1);
135 |     casadi::DM finalPos = casadi::DM::zeros(2,1);
136 |     initPos = initialState.state(casadi::Slice(0,2));
137 |     finalPos = finalState.state(casadi::Slice(0,2));
138 |     casadi::DM interpolatedPosition(2, 1);
139 |     casadi::DM linSpacePoints = casadi::DM::linspace(0, 1, npoints + 1);
140 |     for(casadi_int k = 0; k < npoints + 1; ++k){
141 |         /*
142 |         initial guess, pos using linear interpolatation from init to final, 
143 |         vel and control all set zero
144 |         */
145 |         interpolatedPosition = initPos + linSpacePoints(k) * (finalPos - initPos);
146 |         opti.set_initial(X(casadi::Slice(0,2), k), interpolatedPosition);
147 |         opti.set_initial(X(casadi::Slice(2,4), k), 0);
148 |     }
149 |     for(casadi_int k = 0; k < npoints; ++k){
150 |         opti.set_initial(U(casadi::Slice(), k), 0);
151 |     }
152 |     
153 |     solverState = SolverState::PROBLEM_SET;
154 |     
155 |     try{
156 |         solution = std::make_unique<casadi::OptiSol>(opti.solve());
157 |     }catch(std::exception &e){
158 |         opti.debug().show_infeasibilities(1e-5);
159 |         std::cerr << "error while solving the optimization" << std::endl;
160 |         std::cerr << "Details:\n " << e.what() << std::endl;
161 |         return false;
162 |     }
163 |     solverState = SolverState::PROBLEM_SOLVED;
164 |     std::cout << "\nsolve success\n\n";
165 |     return true;
166 | }
167 | 
168 | void Solver::getSolution(casadi::DM& state, casadi::DM& control){
169 |     state = solution -> value(X);
170 |     control = solution -> value(U);
171 | }


--------------------------------------------------------------------------------
/src/test.cpp:
--------------------------------------------------------------------------------
 1 | #include "DirectTranscriptSolver.hpp"
 2 | #include "DirectCollocationSolver.hpp"
 3 | 
 4 | int main(){
 5 |     Settings settings;
 6 |     settings.phaseLength = 10;
 7 |     settings.time = 2;
 8 |     settings._costWeights.control = 1;
 9 |     settings.solverVerbosity = 1;
10 |     settings.ipoptLinearSolver = "mumps";
11 |     
12 |     State initialState, finalState;
13 |     initialState.state.zeros(4);
14 |     finalState.state = {1, 3.14, 0, 0};
15 |     
16 |     cart_pole_model model = {0.5, 1, 0.3, 9.81};
17 |     constraint_value constraint = {0, 2, -100, 100};
18 | 
19 |     DirectTransSolver solver(model, constraint);
20 |     solver.setTranscriptMethod(TranscriptMethod::SINGLE_SHOOTING);
21 |     solver.setupProblem(settings);
22 |     bool ok = solver.solve(initialState, finalState);
23 |     
24 |     casadi::DM sol_state, sol_control;
25 |     if(ok) solver.getSolution(sol_state, sol_control);
26 |     std::cout<<"single shooting transcript\n";
27 |     std::cout << "state:\n" << sol_state << "\ncontrol:\n" << sol_control << "\n\n";
28 | 
29 |     solver.setTranscriptMethod(TranscriptMethod::MULTIPLE_SHOOTING);
30 |     solver.setupProblem(settings);
31 |     ok = solver.solve(initialState, finalState);
32 |     
33 |     if(ok) solver.getSolution(sol_state, sol_control);
34 |     std::cout<<"multiple shooting transcript\n";
35 |     std::cout << "state:\n" << sol_state << "\ncontrol:\n" << sol_control << "\n\n";
36 | 
37 |     Settings co_settings;
38 |     co_settings.phaseLength = 10;
39 |     co_settings.time = 2;
40 |     co_settings._costWeights.control = 1;
41 |     co_settings.solverVerbosity = 1;
42 |     co_settings.ipoptLinearSolver = "mumps";
43 |     DirectCollocationSolver co_solver(model, constraint);
44 |     co_solver.setupProblemColloc(co_settings);
45 |     bool co_ok = co_solver.solveColloc(initialState, finalState);
46 |     casadi::DM co_sol_state, co_sol_control;
47 |     if(co_ok) co_solver.getSolutionColloc(co_sol_state, co_sol_control);
48 |     std::cout<<"collocation\n";
49 |     std::cout << "state:\n" << co_sol_state << "\ncontrol:\n" << co_sol_control << std::endl;
50 | 
51 |     return 0;
52 | }


--------------------------------------------------------------------------------