Branch data     Line data    Source code

       1                 :            : // Copyright 2019 Christopher Ho
       2                 :            : //
       3                 :            : // Licensed under the Apache License, Version 2.0 (the "License");
       4                 :            : // you may not use this file except in compliance with the License.
       5                 :            : // You may obtain a copy of the License at
       6                 :            : //
       7                 :            : //     http://www.apache.org/licenses/LICENSE-2.0
       8                 :            : //
       9                 :            : // Unless required by applicable law or agreed to in writing, software
      10                 :            : // distributed under the License is distributed on an "AS IS" BASIS,
      11                 :            : // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
      12                 :            : // See the License for the specific language governing permissions and
      13                 :            : // limitations under the License.
      14                 :            : #include <acado/acado_code_generation.hpp>
      15                 :            : 
      16                 :            : USING_NAMESPACE_ACADO
      17                 :            : 
      18                 :          0 : int main(int argc, char * const argv[])
      19                 :            : {
      20                 :            :   #include <kinematic_bicycle.snippet.hpp>
      21                 :            :   // Weighting matrices and reference functions: not prespecified
      22                 :            :   //
      23                 :            : 
      24                 :          0 :   Function rf;
      25                 :          0 :   Function rfN;
      26                 :          0 :   rf << x << y << yaw << u << ax << delta;
      27                 :          0 :   rfN << x << y << yaw << u;
      28                 :            : 
      29                 :          0 :   BMatrix W = eye<bool>(rf.getDim());
      30                 :          0 :   BMatrix WN = eye<bool>(rfN.getDim());
      31                 :            : 
      32                 :            :   //
      33                 :            :   // Optimal Control Problem
      34                 :            :   //
      35                 :            : 
      36                 :            :   // 5 second time horizon
      37                 :          0 :   const int N = 30;  // Number of steps
      38                 :          0 :   const int Ni = 4;  // number of integrators
      39                 :          0 :   const double Ts = 0.1;
      40                 :            : 
      41                 :          0 :   OCP ocp(0, N * Ts, N);
      42                 :            : 
      43                 :          0 :   ocp.subjectTo(f);
      44                 :            : 
      45                 :          0 :   ocp.minimizeLSQ(W, rf);
      46                 :          0 :   ocp.minimizeLSQEndTerm(WN, rfN);
      47                 :            : 
      48                 :          0 :   ocp.subjectTo(0.01 <= u <= 35.0);
      49                 :          0 :   ocp.subjectTo(-0.331 <= delta <= 0.331);
      50                 :          0 :   ocp.subjectTo(-3.0 <= ax <= 3.0);
      51                 :            : 
      52                 :            :   //
      53                 :            :   // Export the code:
      54                 :            :   //
      55                 :          0 :   OCPexport mpc(ocp);
      56                 :            : 
      57                 :            :   // See https://github.com/cho3/acado/blob/master/acado/utils/acado_types.hpp#L331..L427
      58                 :            :   // for all options
      59                 :            : 
      60                 :          0 :   mpc.set(HESSIAN_APPROXIMATION, GAUSS_NEWTON);
      61                 :          0 :   mpc.set(DISCRETIZATION_TYPE, MULTIPLE_SHOOTING);
      62                 :            : 
      63                 :          0 :   mpc.set(INTEGRATOR_TYPE, INT_RK45);
      64                 :          0 :   mpc.set(NUM_INTEGRATOR_STEPS, N * Ni);
      65                 :            : 
      66                 :          0 :   mpc.set(SPARSE_QP_SOLUTION, FULL_CONDENSING);
      67                 :            :   // mpc.set(SPARSE_QP_SOLUTION, CONDENSING);
      68                 :          0 :   mpc.set(QP_SOLVER, QP_QPOASES);
      69                 :          0 :   mpc.set(MAX_NUM_QP_ITERATIONS, 999);
      70                 :          0 :   mpc.set(HOTSTART_QP, YES);
      71                 :          0 :   mpc.set(CG_HARDCODE_CONSTRAINT_VALUES, NO);
      72                 :          0 :   mpc.set(CG_USE_VARIABLE_WEIGHTING_MATRIX, YES);
      73                 :            :   // mpc.set(SPARSE_QP_SOLUTION, SPARSE_SOLVER);
      74                 :            :   // mpc.set(QP_SOLVER, QP_FORCES);
      75                 :            : 
      76                 :          0 :   mpc.set(LEVENBERG_MARQUARDT, 1.0E2);
      77                 :            : 
      78                 :          0 :   mpc.set(GENERATE_TEST_FILE, NO);
      79                 :          0 :   mpc.set(GENERATE_MAKE_FILE, NO);
      80                 :          0 :   mpc.set(GENERATE_MATLAB_INTERFACE, NO);
      81                 :            : 
      82                 :            :   // mpc.set(USE_SINGLE_PRECISION, YES);
      83                 :            :   // mpc.set(CG_USE_OPENMP, YES);
      84                 :            : 
      85                 :          0 :   if (mpc.exportCode("single_track_dynamics") != SUCCESSFUL_RETURN) {
      86                 :          0 :     exit(EXIT_FAILURE);
      87                 :            :   }
      88                 :            : 
      89                 :          0 :   mpc.printDimensionsQP();
      90                 :            : 
      91                 :          0 :   return EXIT_SUCCESS;
      92                 :            : }