├── .gitignore ├── .travis.yml ├── CMakeLists.txt ├── LICENSE ├── Matlab ├── README.md ├── lbfgs │ ├── lbfgs.m │ ├── lbfgs_rosenbrock_run.m │ ├── lbfgs_run.m │ └── lib │ │ └── fminlbfgs_version2c │ │ ├── example.m │ │ ├── fminlbfgs.m │ │ ├── license.txt │ │ ├── myfun.m │ │ └── myfun_poly.m ├── panoc │ ├── FBE.m │ └── panoc_run.m ├── problems │ ├── g_1.m │ ├── g_2.m │ ├── prox_g_1.m │ └── prox_g_2.m └── proximal_grad │ ├── estimate_lipschitz.m │ ├── prox_grad_descent_run.m │ └── prox_grad_descent_step.m ├── PANOC ├── CMakeLists.txt ├── buffer.c ├── buffer.h ├── casadi_definitions.h ├── casadi_interface.c ├── casadi_interface.h ├── lbfgs.c ├── lbfgs.h ├── lipschitz.c ├── lipschitz.h ├── matrix_operations.c ├── matrix_operations.h ├── nmpc.c ├── panoc.c ├── panoc.h ├── proximal_gradient_descent.c └── proximal_gradient_descent.h ├── README.md ├── build └── README.md ├── casadi ├── CMakeLists.txt ├── README.md ├── cost_function.c ├── cost_function.h ├── cost_function_derivative_combined.c ├── cost_function_derivative_combined.h ├── g.c ├── integrator.c ├── integrator.h └── proxg.c ├── codestyle.md ├── docs ├── _config.yml ├── _layouts │ └── default.html ├── about.md ├── example_Matlab.md ├── example_Python.md ├── img │ ├── nmpc_codegen_packages.png │ ├── nmpc_codegen_scheme.png │ ├── timings.png │ ├── trailer.png │ └── visio_software_arch.png ├── index.md ├── install │ ├── Matlab_install.md │ ├── Python_dev_install.md │ ├── Python_install.md │ └── Toolchain_install.md └── structure_library.md ├── globals ├── globals.h └── globals_dyn.h ├── include └── nmpc.h ├── minimum_build_system ├── CMakeLists_casadi.txt ├── CMakeLists_panoc.txt ├── CMakeLists_root.txt ├── main_trailer.c ├── nmpc.c └── special_nmpc.c ├── old_code ├── Matlab_test_utils.m ├── Matlab_test_utils_test_if_command_available.m ├── PANOC.pdf ├── demos │ ├── Matlab │ │ ├── .gitignore │ │ ├── TwoCircTrailer.m │ │ ├── TwoCircTrailer_constraint.m │ │ ├── TwoCircTrailer_constraint_LA.m │ │ ├── TwoCircTrailer_display.m │ │ ├── compare_libs.m │ │ ├── compare_libs_limited_graphs.m │ │ ├── compare_libs_table.m │ │ ├── demo_lbfgs_speedup.m │ │ ├── demo_set_obstacles.m │ │ ├── demos.m │ │ ├── draw_obstacle_border.m │ │ ├── figures_simulations │ │ │ ├── .gitignore │ │ │ └── README.md │ │ ├── matrix2latex.m │ │ ├── prepare_demo_trailer.m │ │ ├── quadcopter │ │ │ ├── circle.m │ │ │ ├── demo_error_panoc.m │ │ │ ├── demo_error_panoc_LA.m │ │ │ ├── performance_measurement.m │ │ │ ├── plotcube.m │ │ │ ├── prepare_demo_quadcopter.m │ │ │ ├── quadcopter_constraint.m │ │ │ ├── simple_controller.m │ │ │ ├── simple_simulation.m │ │ │ ├── simulate_OPTI_ipopt.m │ │ │ ├── simulate_demo_controller.m │ │ │ ├── simulate_panoc_draft.m │ │ │ └── simulate_panoc_matlab.m │ │ ├── simulate_demo_trailer.m │ │ ├── simulate_demo_trailer_OPTI_ipopt.m │ │ ├── simulate_demo_trailer_casadi_ipopt.m │ │ ├── simulate_demo_trailer_fmincon.m │ │ ├── simulate_demo_trailer_interior_point_matlab.m │ │ ├── simulate_demo_trailer_panoc_draft.m │ │ ├── simulate_demo_trailer_panoc_matlab.m │ │ └── tables │ │ │ ├── .gitignore │ │ │ └── README.md │ ├── python │ │ ├── .gitignore │ │ ├── OneCircTwoRectTrailer.py │ │ ├── README.md │ │ ├── TwoCircTrailer.py │ │ ├── TwoCircTrailer_constraint.py │ │ ├── demo.py │ │ ├── demo1.py │ │ ├── demo2.py │ │ ├── demo3.py │ │ ├── demo4.py │ │ ├── demo_obs.py │ │ └── trailer_simulations_python.py │ ├── python_internal │ │ ├── chain_dyn_casadi_demo.py │ │ ├── function_generator_demo.py │ │ ├── globals_generator_demo.py │ │ └── trailer_simulation_demo.py │ └── thesis │ │ ├── .gitignore │ │ └── trailer_simulations.m ├── generate_test_files.py ├── src_matlab │ ├── +nmpccodegen │ │ ├── +Cfunctions │ │ │ ├── @Cfunction │ │ │ │ └── Cfunction.m │ │ │ ├── @IndicatorBoxFunction │ │ │ │ └── IndicatorBoxFunction.m │ │ │ ├── @IndicatorBoxFunctionProx │ │ │ │ └── IndicatorBoxFunctionProx.m │ │ │ ├── @ProximalFunction │ │ │ │ └── ProximalFunction.m │ │ │ └── @Source_file_generator │ │ │ │ └── Source_file_generator.m │ │ ├── +controller │ │ │ ├── +constraints │ │ │ │ ├── @Constraint │ │ │ │ │ └── Constraint.m │ │ │ │ ├── @Input_norm │ │ │ │ │ └── Input_norm.m │ │ │ │ └── @Max_state │ │ │ │ │ └── Max_state.m │ │ │ ├── +obstacles │ │ │ │ ├── Circular.m │ │ │ │ ├── Nonconvex_constraints.m │ │ │ │ ├── Obstacle.m │ │ │ │ ├── Polyhedral.m │ │ │ │ └── Rectangular.m │ │ │ ├── @Casadi_code_generator │ │ │ │ ├── Casadi_code_generator.m │ │ │ │ └── code_generator_options.md │ │ │ ├── @Globals_generator │ │ │ │ └── Globals_generator.m │ │ │ ├── @Nmpc_panoc │ │ │ │ └── Nmpc_panoc.m │ │ │ ├── @Single_shot_LA_definition │ │ │ │ └── Single_shot_LA_definition.m │ │ │ ├── @Single_shot_definition │ │ │ │ └── Single_shot_definition.m │ │ │ └── @Stage_cost_QR │ │ │ │ └── Stage_cost_QR.m │ │ ├── +example_models │ │ │ ├── chain_dyn.m │ │ │ ├── get_quadcopter_model.m │ │ │ ├── get_trailer_model.m │ │ │ ├── quadcopter_dyn.m │ │ │ ├── trailer_dyn.m │ │ │ └── trailer_printer.m │ │ ├── +models │ │ │ ├── @Model │ │ │ │ └── Model.m │ │ │ ├── @Model_continuous │ │ │ │ └── Model_continuous.m │ │ │ ├── integrate.m │ │ │ └── integrator_tableaus │ │ │ │ ├── BS5.mat │ │ │ │ ├── BuRK65.mat │ │ │ │ ├── CMR6.mat │ │ │ │ ├── DP5.mat │ │ │ │ ├── FE.mat │ │ │ │ ├── Fehlberg45.mat │ │ │ │ ├── Heun33.mat │ │ │ │ ├── Lambert65.mat │ │ │ │ ├── MTE22.mat │ │ │ │ ├── Merson43.mat │ │ │ │ ├── Mid22.mat │ │ │ │ ├── NSSP32.mat │ │ │ │ ├── NSSP33.mat │ │ │ │ ├── PD8.mat │ │ │ │ ├── RK44.mat │ │ │ │ ├── SSP104.mat │ │ │ │ ├── SSP22.mat │ │ │ │ ├── SSP22star.mat │ │ │ │ ├── SSP33.mat │ │ │ │ ├── SSP53.mat │ │ │ │ ├── SSP54.mat │ │ │ │ ├── SSP63.mat │ │ │ │ ├── SSP75.mat │ │ │ │ ├── SSP85.mat │ │ │ │ └── SSP95.mat │ │ └── +tools │ │ │ ├── @Bootstrapper │ │ │ └── Bootstrapper.m │ │ │ ├── @Simulation_data │ │ │ └── Simulation_data.m │ │ │ └── @Simulator │ │ │ └── Simulator.m │ └── .gitignore ├── src_python │ ├── README.md │ ├── __init__.py │ └── nmpccodegen │ │ ├── Cfunctions │ │ ├── Function.py │ │ ├── IndicatorBoxFunction.py │ │ ├── ProximalFunction.py │ │ ├── __init__.py │ │ ├── readme.md │ │ └── source_file_operations.py │ │ ├── __init__.py │ │ ├── controller │ │ ├── __init__.py │ │ ├── casadi_code_generator.py │ │ ├── constraints │ │ │ ├── Constraint.py │ │ │ ├── Input_norm.py │ │ │ └── __init__.py │ │ ├── globals_generator.py │ │ ├── nmpc_panoc.py │ │ ├── nmpc_problem_multiple_shot.py │ │ ├── nmpc_problem_single_shot.py │ │ ├── nmpc_problem_single_shot_LA.py │ │ ├── obstacles │ │ │ ├── __init__.py │ │ │ ├── circular.py │ │ │ ├── nonconvex_constraints.py │ │ │ ├── obstacle.py │ │ │ ├── polyhedral.py │ │ │ ├── rectangular.py │ │ │ └── test_obstacle.py │ │ └── stage_costs.py │ │ ├── example_models │ │ ├── __init__.py │ │ ├── chain_model.py │ │ ├── example_model.py │ │ ├── trailer_model.py │ │ └── trailer_printer.py │ │ ├── models │ │ ├── __init__.py │ │ ├── generate_integrator_tableaus.py │ │ ├── integrator_tableaus │ │ │ ├── BS5.npz │ │ │ ├── BuRK65.npz │ │ │ ├── CMR6.npz │ │ │ ├── DP5.npz │ │ │ ├── FE.npz │ │ │ ├── Fehlberg45.npz │ │ │ ├── Heun33.npz │ │ │ ├── Lambert65.npz │ │ │ ├── MTE22.npz │ │ │ ├── Merson43.npz │ │ │ ├── Mid22.npz │ │ │ ├── NSSP32.npz │ │ │ ├── NSSP33.npz │ │ │ ├── PD8.npz │ │ │ ├── RK44.npz │ │ │ ├── SSP104.npz │ │ │ ├── SSP22.npz │ │ │ ├── SSP22star.npz │ │ │ ├── SSP33.npz │ │ │ ├── SSP53.npz │ │ │ ├── SSP54.npz │ │ │ ├── SSP63.npz │ │ │ ├── SSP75.npz │ │ │ ├── SSP85.npz │ │ │ └── SSP95.npz │ │ ├── integrators.py │ │ ├── model.py │ │ ├── model_continious.py │ │ └── table_integrators.tex │ │ └── tools │ │ ├── __init__.py │ │ ├── bootstrapper.py │ │ └── simulator.py ├── toturial_nmpc_codegen.py ├── tutorial.pdf └── tutorial_nmpc_codegen_multiple_shot.py ├── python_interface ├── libpython_interface.h ├── nmpc_python.c ├── nmpc_python.h ├── test_timer.c ├── timer.h ├── timer_empty.c ├── timer_linux.c ├── timer_mac.c └── timer_windows.c ├── trailer_example.png ├── trailer_example_time_log.png └── tst ├── .gitignore ├── CMakeLists.txt ├── bin └── README.md ├── python ├── .gitignore ├── README.md ├── test_single_shot_chain_dyn.py └── trailer │ ├── .gitignore │ ├── test_trailer.py │ ├── trailer_generate_controller.py │ ├── trailer_generate_controller_with_obs.py │ └── trailer_simple_controller.py └── src ├── README.md ├── casadi_interface_test.c ├── example_problems.c ├── example_problems.h ├── lbfgs_poly_test.c ├── lipschitz_estimator_test.c ├── matrix_operations_norms.c ├── matrix_operations_vectors.c ├── mocks ├── README.md ├── casadi_interface_test.c ├── casadi_interface_test.h ├── lbfgs_rosenbrock_interface.h ├── lbfgs_test_buffer.c └── lbfgs_test_proximal_gradient_descent.c ├── panoc_poly_test.c └── prox_grad_descent_poly_test.c /.gitignore: -------------------------------------------------------------------------------- 1 | CMakeCache.txt 2 | CMakeFiles 3 | CMakeScripts 4 | Testing 5 | Makefile 6 | cmake_install.cmake 7 | install_manifest.txt 8 | compile_commands.json 9 | CTestTestfile.cmake 10 | 11 | *.exe 12 | *.a 13 | *.json 14 | *.gch 15 | *.pyc 16 | 17 | build 18 | .vscode 19 | test_controller_builds 20 | 21 | # visual studio stuff 22 | *.vcxproj* 23 | *.dir 24 | Win32 25 | Debug 26 | PANOC/Debug 27 | PANOC/Release 28 | tst/Release 29 | tst/Debug 30 | *.sln -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | language: c 2 | 3 | compiler: 4 | - gcc 5 | - clang 6 | 7 | before_script: 8 | - cd build 9 | - cmake .. 10 | 11 | script: make && make test -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 KUL ForBES 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /Matlab/README.md: -------------------------------------------------------------------------------- 1 | # Matlab files 2 | 3 | The Matlab scripts in this folder are for debugging purposes only. They demonstrate some functionality of the c code in Matlab code. 4 | 5 | ## lbgfs 6 | The lbfgs.m and lbfgs_run.m files are simple implementations of the lbgfs algortihm implemented in C. 7 | 8 | ## proximal gradient descent 9 | The proximal_grad folder contains a simple example of the proximal gradient descent. -------------------------------------------------------------------------------- /Matlab/lbfgs/lbfgs.m: -------------------------------------------------------------------------------- 1 | % f=function 2 | % df=gradient of function 3 | % g_i=df(x(i)) 4 | 5 | % The buffer of length m contains 2 variables 6 | % s_i = x_{i+1} - x_{i} 7 | % y_i = g_{i+1} - g_{i} 8 | 9 | function [ s,y,new_x] = lbfgs(iteration_index,buffer_size,x,df,s,y) 10 | % if this is the first time, use the gradient descent 11 | if(iteration_index==1) 12 | direction=df(x); 13 | new_x = x-direction; 14 | % start to fill in the 15 | s(:,1) = new_x-x; 16 | y(:,1) = df(new_x) - df(x); 17 | else 18 | % if we dont have enough past values lower the max buffer size 19 | if(iteration_index<=buffer_size+1) 20 | buffer_size_max=iteration_index-1; 21 | else 22 | buffer_size_max=buffer_size; % maximum buffer size 23 | end 24 | 25 | q=df(x); 26 | rho=zeros(1,buffer_size_max); 27 | alpha=zeros(1,buffer_size_max); 28 | beta=zeros(1,buffer_size_max); 29 | 30 | % loop over most recent to oldest 31 | for i=1:buffer_size_max 32 | rho(i)=1/(y(:,i)'*s(:,i)); 33 | 34 | alpha(i) = rho(i)*s(:,i)'*q; 35 | q = q - alpha(i)*y(:,i); 36 | end 37 | 38 | z=(y(:,buffer_size_max)*s(:,buffer_size_max)'*q)... 39 | /(y(:,buffer_size_max)'*y(:,buffer_size_max)); 40 | 41 | for i=buffer_size_max:-1:1 % oldest first 42 | beta(i)=rho(i)*y(:,i)'*z; 43 | z=z+s(:,i)*(alpha(i) - beta(i)); 44 | end 45 | 46 | new_x = x - z; % z is upward direction 47 | 48 | % After the new values have been found, 49 | % fix the buffers for the next iteration. 50 | s(:,2:end)=s(:,1:end-1); 51 | y(:,2:end)=y(:,1:end-1); 52 | s(:,1) = new_x-x; 53 | y(:,1) = df(new_x) - df(x); 54 | end 55 | 56 | 57 | end -------------------------------------------------------------------------------- /Matlab/lbfgs/lbfgs_rosenbrock_run.m: -------------------------------------------------------------------------------- 1 | clc; 2 | %% 3 | a=1; 4 | b=100; 5 | f =@(x) (a-x(1))^2 + b*(x(2)-x(1))^2; 6 | df = @(x) [-2*(a-(b+1)*x(1)+b*x(2)); 2*b*(x(2)-x(1)) ]; 7 | theoretical_solution=[a;a^2]; 8 | 9 | %% 10 | dimension=2; 11 | number_of_iterations=40; % number of iterations 12 | x0=[-1.2;1.]; 13 | 14 | buffer_size=20; % buffer_size 15 | 16 | % internal buffers used 17 | alpha=zeros(1,buffer_size); 18 | beta=zeros(1,buffer_size); 19 | 20 | s=zeros(dimension,buffer_size); % x_{k+1} - x_{k} 21 | y=zeros(dimension,buffer_size); % df(x_{k+1}) - df(x_{k}) 22 | 23 | % array to save x values that need to be printed on the surface plot 24 | x_steps=zeros(dimension,number_of_iterations); 25 | 26 | x=x0; 27 | for interation_index=1:number_of_iterations 28 | [s_new,y_new,x_new] = lbfgs(interation_index,buffer_size,x,df,s,y); 29 | x_steps(:,interation_index)=x_new; 30 | disp(['i=' num2str(interation_index) ' step x1=' num2str(x(1)) ' x2=' num2str(x(2)) ' with cost=' num2str(f(x))]); 31 | x=x_new;s=s_new;y=y_new; 32 | end 33 | 34 | % x_steps = [x0 x_steps]; 35 | %% plot the function 36 | 37 | % create a grid 38 | x = linspace(-2,2,100); 39 | y = linspace(-2,2,100); 40 | [X,Y] = meshgrid(x,y); 41 | Z=X; 42 | 43 | % evaluate the function on the grid 44 | for i=1:size(X,1) 45 | for j=1:size(X,2) 46 | Z(i,j)=f([X(i,j);Y(i,j)]); 47 | end 48 | end 49 | 50 | % plot the surface 51 | % figure(1);clf; 52 | % contour(X,Y,Z); 53 | 54 | %% plot the convergence 55 | 56 | figure(2);clf; 57 | subplot(2,1,1); 58 | plot(abs(x_steps(1,2:end)-theoretical_solution(1))./abs(x_steps(1,1:end-1)-theoretical_solution(1))); 59 | subplot(2,1,2); 60 | plot(abs(x_steps(1,2:end)-theoretical_solution(1))./abs(x_steps(1,1:end-1)-theoretical_solution(1))); 61 | -------------------------------------------------------------------------------- /Matlab/lbfgs/lbfgs_run.m: -------------------------------------------------------------------------------- 1 | clc; 2 | %% 3 | polynomial_degree=10; 4 | % Example function 5 | f =@(x) x(1)^polynomial_degree +x(2)^polynomial_degree ; 6 | % Gradient 7 | df=@(x) [polynomial_degree*x(1)^(polynomial_degree-1);polynomial_degree*x(2)^(polynomial_degree-1)]; 8 | 9 | %% 10 | dimension=2; 11 | number_of_iterations=40; % number of iterations 12 | x0=[0.5;0.5]; 13 | 14 | buffer_size=50; % buffer_size 15 | 16 | % internal buffers used 17 | alpha=zeros(1,buffer_size); 18 | beta=zeros(1,buffer_size); 19 | 20 | s=zeros(dimension,buffer_size); % x_{k+1} - x_{k} 21 | y=zeros(dimension,buffer_size); % df(x_{k+1}) - df(x_{k}) 22 | 23 | % array to save x values that need to be printed on the surface plot 24 | x_steps=zeros(dimension,number_of_iterations); 25 | 26 | x=x0; 27 | for interation_index=1:number_of_iterations 28 | [s_new,y_new,x_new] = lbfgs(interation_index,buffer_size,x,df,s,y); 29 | x_steps(:,interation_index)=x_new; 30 | disp(['step x1=' num2str(x(1)) ' x2=' num2str(x(2)) 'with cost=' num2str(f(x))]); 31 | x=x_new;s=s_new;y=y_new; 32 | end 33 | 34 | % x_steps = [x0 x_steps]; 35 | %% plot the function 36 | 37 | % create a grid 38 | x = linspace(-2,2,100); 39 | y = linspace(-2,2,100); 40 | [X,Y] = meshgrid(x,y); 41 | Z=X; 42 | 43 | % evaluate the function on the grid 44 | for i=1:size(X,1) 45 | for j=1:size(X,2) 46 | Z(i,j)=f([X(i,j);Y(i,j)]); 47 | end 48 | end 49 | 50 | % plot the surface 51 | figure(1);clf; 52 | contour(X,Y,Z); 53 | 54 | %% plot the convergence 55 | 56 | figure(2);clf; 57 | plot(abs(x_steps(1,2:end)-0.017081)./abs(x_steps(1,1:end-1)-0.017081)); 58 | -------------------------------------------------------------------------------- /Matlab/lbfgs/lib/fminlbfgs_version2c/example.m: -------------------------------------------------------------------------------- 1 | % options = struct('GradObj','on','Display','iter','LargeScale','off','HessUpdate','bfgs','InitialHessType','identity','GoalsExactAchieve',0); 2 | a=1; 3 | b=100; 4 | theoretical_solution=[a;a^2]; 5 | options = struct('GradObj','on','Display','iter','LargeScale','off','HessUpdate','lbfgs','InitialHessType','identity','GoalsExactAchieve',1,'GradConstr',false,'StoreN',20); 6 | 7 | x0 =[-1.2;1.]; 8 | tic 9 | [x2,fval2] = fminlbfgs(@myfun,x0,options); 10 | toc 11 | 12 | x0 =[0.5;0.5]; 13 | tic 14 | [x2,fval2] = fminlbfgs(@myfun_poly,x0,options); 15 | toc 16 | 17 | 18 | % tic 19 | % [x,fval] = fminunc(@myfun,x0,options); 20 | % toc 21 | -------------------------------------------------------------------------------- /Matlab/lbfgs/lib/fminlbfgs_version2c/fminlbfgs.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/Matlab/lbfgs/lib/fminlbfgs_version2c/fminlbfgs.m -------------------------------------------------------------------------------- /Matlab/lbfgs/lib/fminlbfgs_version2c/license.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2009, Dirk-Jan Kroon 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are 6 | met: 7 | 8 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in 12 | the documentation and/or other materials provided with the distribution 13 | 14 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 15 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 16 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 17 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 18 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 19 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 20 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 21 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 22 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 23 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 24 | POSSIBILITY OF SUCH DAMAGE. 25 | -------------------------------------------------------------------------------- /Matlab/lbfgs/lib/fminlbfgs_version2c/myfun.m: -------------------------------------------------------------------------------- 1 | % where myfun is a MATLAB function such as: 2 | % function [f,g] = myfun(x) 3 | % f = sum(sin(x) + 3); 4 | % if ( nargout > 1 ), g = cos(x); end 5 | 6 | function [f,g] = myfun(x) 7 | a=1; 8 | b=100; 9 | f =(a-x(1))^2 + b*(x(2)-x(1))^2; 10 | g = [-2*(a-(b+1)*x(1)+b*x(2)); 2*b*(x(2)-x(1)) ]; 11 | end -------------------------------------------------------------------------------- /Matlab/lbfgs/lib/fminlbfgs_version2c/myfun_poly.m: -------------------------------------------------------------------------------- 1 | % where myfun is a MATLAB function such as: 2 | % function [f,g] = myfun(x) 3 | % f = sum(sin(x) + 3); 4 | % if ( nargout > 1 ), g = cos(x); end 5 | 6 | function [f,g] = myfun_poly(x) 7 | f =x(1)^10 + x(2)^10; 8 | g = [10*x(1)^9; 10*x(2)^9 ]; 9 | end -------------------------------------------------------------------------------- /Matlab/panoc/FBE.m: -------------------------------------------------------------------------------- 1 | function [ FBE_x] = FBE( x,gamma,beta,f,df,g,proxg ) 2 | x_new = proxg( x-df(x)*gamma ); 3 | FBE_x = f(x) + g(x_new) - dot(df(x),x-x_new)+ ... 4 | ((1-beta)/(2*gamma))*norm(x-x_new,2)^2; 5 | end -------------------------------------------------------------------------------- /Matlab/panoc/panoc_run.m: -------------------------------------------------------------------------------- 1 | % PANOC algorithm using the lbgfs and proximal gradient methods 2 | % include the Matlab folder with all its subfolders to your path 3 | number_of_steps=10; 4 | dimension=2; 5 | x_steps=zeros(dimension,number_of_steps); 6 | taus=zeros(1,number_of_steps); 7 | x0=[0.5;0.5]; 8 | % x0=100; 9 | %% variables proximal gradient descent 10 | beta_safety_value=0.05; % safety constant 11 | 12 | w=2; 13 | g = @(x) g_1(x,w); 14 | proxg = @(x) prox_g_1( x,w ); 15 | % g = @(x) g_2(x); 16 | % proxg = @(x) prox_g_2( x ); 17 | 18 | degree_polynomial=20; 19 | f = @(x) sum(x.^degree_polynomial); 20 | df = @(x) (degree_polynomial)*x.^(degree_polynomial-1); 21 | 22 | % get starting value gamma 23 | lipschitz_constant= estimate_lipschitz( df,x0 ); 24 | gamma0 = (1-beta_safety_value)/lipschitz_constant; 25 | gamma=gamma0; 26 | %% variables lbgfs 27 | buffer_size=50; % buffer_size 28 | 29 | % internal buffers used 30 | alpha=zeros(1,buffer_size); 31 | beta=zeros(1,buffer_size); 32 | 33 | s=zeros(dimension,buffer_size); % x_{k+1} - x_{k} 34 | y=zeros(dimension,buffer_size); % df(x_{k+1}) - df(x_{k}) 35 | %% iterate 36 | x=x0;% set the starting point, and iterate 37 | for interation_index=1:number_of_steps 38 | [x_prox_grad_descent,new_gamma] = prox_grad_descent_step( x,gamma,beta,proxg,f,df ); 39 | gamma=new_gamma; 40 | step_prox = (x-x_prox_grad_descent); 41 | 42 | R= @(x) (1/gamma)*(x - proxg(x-df(x)*gamma)); 43 | [s_new,y_new,x_lbfgs] = lbfgs(interation_index,buffer_size,x,R,s,y); 44 | s=s_new;y=y_new; 45 | 46 | % find the right convex combination trough backtracking 47 | tau=1; 48 | max_number_of_steps_backtracking=100; 49 | for i=1:max_number_of_steps_backtracking 50 | d=x_lbfgs-x; 51 | potential_x=x-(1-tau)*step_prox + tau*d; 52 | sigma=beta_safety_value/(4*gamma); 53 | if(FBE( potential_x,gamma,beta_safety_value,f,df,g,proxg )<= FBE( x,gamma,beta_safety_value,f,df,g,proxg )-sigma*norm(step_prox/gamma,2)^2) 54 | break; % if this is statified stop right away 55 | else 56 | tau=tau/2; 57 | end 58 | end 59 | x_steps(:,interation_index)=potential_x; 60 | taus(interation_index)=tau; 61 | end 62 | %% plot the convergence rate 63 | figure(1);clf; 64 | plot(taus); 65 | 66 | figure(2);clf; 67 | plot(x_steps(1,2:end)./x_steps(1,1:end-1)); -------------------------------------------------------------------------------- /Matlab/problems/g_1.m: -------------------------------------------------------------------------------- 1 | function [ g_x ] = g_1( x,w ) 2 | g_x = max(norm(x,1)-w,0); 3 | end 4 | 5 | -------------------------------------------------------------------------------- /Matlab/problems/g_2.m: -------------------------------------------------------------------------------- 1 | function [ g_x ] = g_2( x ) 2 | % indicator function of {-1;0;1} 3 | if(x==1 || x==0 || x==-1 ) 4 | g_x=0; 5 | else 6 | g_x=10^10; 7 | end 8 | end 9 | 10 | -------------------------------------------------------------------------------- /Matlab/problems/prox_g_1.m: -------------------------------------------------------------------------------- 1 | function [ prox_g_x ] = prox_g_1( x,w ) 2 | if(norm(x,1)2*w) 5 | prox_g_x = sign(x)*(norm(x,1)-w); 6 | else 7 | prox_g_x = sign(x)*w; 8 | end 9 | end 10 | 11 | -------------------------------------------------------------------------------- /Matlab/problems/prox_g_2.m: -------------------------------------------------------------------------------- 1 | function [ prox_g_x ] = prox_g_2( x ) 2 | if(x<-0.5) 3 | prox_g_x=-1; 4 | elseif(x>0.5) 5 | prox_g_x=1; 6 | else 7 | prox_g_x=0; 8 | end 9 | 10 | -------------------------------------------------------------------------------- /Matlab/proximal_grad/estimate_lipschitz.m: -------------------------------------------------------------------------------- 1 | function [ estimation_lipschitz_constant ] = estimate_lipschitz( df,x ) 2 | delta=10^-5; 3 | estimation_lipschitz_constant = norm((df(x+delta)-df(x))/delta); 4 | end 5 | 6 | -------------------------------------------------------------------------------- /Matlab/proximal_grad/prox_grad_descent_run.m: -------------------------------------------------------------------------------- 1 | % demonstration of the proximal gradient descent 2 | clc;clear all; 3 | number_of_steps=5; 4 | dimension=2; 5 | 6 | x0=[0.5;0.5]; % starting position 7 | beta=0.05; % safety constant 8 | 9 | w=5; 10 | g = @(x) g_1(x,w); 11 | proxg = @(x) prox_g_1( x,w ); 12 | 13 | degree_polynomial=5; 14 | f = @(x) sum(x.^degree_polynomial); 15 | df = @(x) (degree_polynomial)*x.^(degree_polynomial-1); 16 | 17 | % get starting value gamma 18 | lipschitz_constant= estimate_lipschitz( df,x0 ); 19 | gamma0 = (1-beta)/lipschitz_constant; 20 | 21 | %% 22 | x_steps=zeros(dimension,number_of_steps); 23 | x=x0; 24 | gamma=gamma0; 25 | for i=1:number_of_steps 26 | [new_x,new_gamma] = prox_grad_descent_step( x,gamma,beta,proxg,f,df ); 27 | x=new_x; 28 | gamma=new_gamma; 29 | 30 | x_steps(:,i)=x; % log steps 31 | end 32 | % calculate the convergence 33 | rate_of_convergence= x_steps(:,2:end)./(x_steps(:,1:end-1)); 34 | %% Rate of convergence 35 | figure(1);clf; 36 | title('rate of convergence'); 37 | semilogy(rate_of_convergence(1,:)); 38 | xlabel('step index');ylabel('rate of convergence') 39 | disp(['the average are of convergence is ' num2str(mean(rate_of_convergence(1,:)))]) -------------------------------------------------------------------------------- /Matlab/proximal_grad/prox_grad_descent_step.m: -------------------------------------------------------------------------------- 1 | function [ x_new,gamma ] = prox_grad_descent_step( x,gamma,beta,proxg,f,df ) 2 | x_new = proxg(x-gamma*df(x)); 3 | r=x_new-x; 4 | while(f(x_new)>f(x)- dot(df(x),x_new-x)+(1-beta)/(2*gamma) * norm(x_new-x,2)) 5 | gamma=gamma/2; 6 | x_new = proxg(x-gamma*df(x)); 7 | end 8 | end -------------------------------------------------------------------------------- /PANOC/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | file(GLOB panoc_SRC 2 | "*.c" 3 | ) 4 | add_library(PANOC_lib STATIC ${panoc_SRC}) 5 | set_property(TARGET PANOC_lib PROPERTY C_STANDARD 90) 6 | 7 | # this is nessesary for the python interface, and doesnt really matter for the tests 8 | set_property(TARGET PANOC_lib PROPERTY POSITION_INDEPENDENT_CODE ON) 9 | 10 | add_library(PANOC_fake STATIC lbfgs.c lipschitz.c matrix_operations.c panoc.c proximal_gradient_descent.c buffer.c "${PROJECT_SOURCE_DIR}/tst/src/mocks/casadi_interface_test.c") 11 | set_property(TARGET PANOC_fake PROPERTY C_STANDARD 90) -------------------------------------------------------------------------------- /PANOC/buffer.h: -------------------------------------------------------------------------------- 1 | #include "../globals/globals.h" 2 | #include 3 | #include 4 | 5 | #ifndef BUFFER_H 6 | #define BUFFER_H 7 | 8 | #ifdef __cplusplus 9 | extern "C" { 10 | #endif 11 | 12 | int buffer_init(void); 13 | int buffer_cleanup(void); 14 | 15 | int buffer_renew(const real_t* current_location); 16 | 17 | real_t buffer_get_current_f(void); 18 | const real_t* buffer_get_current_df(void); 19 | const real_t* buffer_get_current_location(void); 20 | int buffer_reset_cycle(void); 21 | 22 | /* 23 | * 4 extra functions, these reuse df(x) and f(x) if it was a pure lbfgs step 24 | */ 25 | int buffer_evaluate_new_location(const real_t* lbfgs_new_location); 26 | real_t buffer_get_new_location_f(void); 27 | const real_t* buffer_get_new_location_df(void); 28 | int buffer_set_new_location_as_current_location(void); 29 | 30 | 31 | #ifdef SINGLE_COST_MODE 32 | int buffer_evaluate_pure_prox_location(const real_t* pure_prox_location); 33 | int buffer_set_pure_prox_location_as_current_location(void); 34 | const real_t* buffer_get_pure_prox_df(void); 35 | real_t buffer_get_pure_prox_f(void); 36 | #endif 37 | 38 | #ifdef __cplusplus 39 | } 40 | #endif 41 | 42 | #endif /* BUFFER_H */ -------------------------------------------------------------------------------- /PANOC/casadi_definitions.h: -------------------------------------------------------------------------------- 1 | #include"../globals/globals.h" 2 | 3 | #ifndef CASADI_DEFINITIONS_H 4 | #define CASADI_DEFINITIONS_H 5 | 6 | #ifdef __cplusplus 7 | extern "C" { 8 | #endif 9 | 10 | #define to_double(x) (double) x 11 | #define to_int(x) (int) x 12 | #define CASADI_CAST(x,y) (x) y 13 | 14 | /* Pre-c99 compatibility */ 15 | #if __STDC_VERSION__ < 199901L 16 | real_t fmin(real_t x, real_t y) { return xy ? x : y;} 18 | #endif 19 | 20 | real_t sq(real_t x) { return x*x;} 21 | real_t sign(real_t x) { return x<0 ? -1 : x>0 ? 1 : x;} 22 | 23 | #ifdef __cplusplus 24 | } 25 | #endif 26 | 27 | #endif /* CASADI_DEFINITIONS_H */ -------------------------------------------------------------------------------- /PANOC/casadi_interface.h: -------------------------------------------------------------------------------- 1 | #include "../globals/globals.h" 2 | #include 3 | #include 4 | 5 | #ifndef CASADI_INTERFACE_H 6 | #define CASADI_INTERFACE_H 7 | 8 | #ifdef __cplusplus 9 | extern "C" { 10 | #endif 11 | 12 | typedef struct { 13 | int inputSize; 14 | int outputSize; 15 | int buffer_intSize; 16 | int buffer_realSize; 17 | int* buffer_int; 18 | real_t* buffer_real; 19 | int (*cost_function)(const real_t** arg, real_t** res, int* iw, real_t* w, int mem); 20 | } CasadiFunction; 21 | 22 | int casadi_interface_init(); 23 | int casadi_interface_cleanup(); 24 | int casadi_prepare_cost_function(const real_t* current_state); 25 | #ifdef INTEGRATOR_CASADI 26 | int casadi_integrate(const real_t* state,const real_t* input,real_t* new_state); 27 | #endif 28 | /* size_t casadi_interface_get_dimension(); */ 29 | 30 | /* constraint related functions */ 31 | real_t casadi_get_weight_constraints(int index_constraint);/* returns zero if index was out of range */ 32 | int casadi_set_weight_constraints(int index_constraint,real_t weight);/* returns failure if the index is out of range */ 33 | 34 | /* cost functions */ 35 | #ifndef SINGLE_COST_MODE 36 | real_t casadi_interface_f(const real_t* input); 37 | #endif 38 | 39 | real_t casadi_interface_f_df(const real_t* input,real_t* output); 40 | real_t casadi_interface_g_proxg(real_t* state); 41 | 42 | /* 43 | * Function only used when using lagrangian 44 | */ 45 | #ifdef USE_LA 46 | int casadi_evaluate_constraints(const real_t* inputs,real_t* constraint_values); 47 | #endif 48 | 49 | 50 | #ifdef __cplusplus 51 | } 52 | #endif 53 | 54 | #endif -------------------------------------------------------------------------------- /PANOC/lbfgs.h: -------------------------------------------------------------------------------- 1 | #include "../globals/globals.h" 2 | #include 3 | #include 4 | 5 | #ifndef LBFGS_H 6 | #define LBFGS_H 7 | 8 | #ifdef __cplusplus 9 | extern "C" { 10 | #endif 11 | 12 | int lbfgs_init(const unsigned int buffer_size_); 13 | int lbfgs_cleanup(void); 14 | int lbfgs_update_hessian(real_t tau, const real_t* current_location, const real_t* new_location); 15 | 16 | /* 17 | * returns the direction calculated with lbfgs 18 | */ 19 | const real_t* lbfgs_get_direction(void); 20 | int lbfgs_reset_iteration_counters(void); 21 | unsigned int lbfgs_get_active_buffer_size(void); 22 | 23 | #ifdef __cplusplus 24 | } 25 | #endif 26 | 27 | #endif /* LBFGS_H */ -------------------------------------------------------------------------------- /PANOC/lipschitz.c: -------------------------------------------------------------------------------- 1 | #include"lipschitz.h" 2 | #include"matrix_operations.h" 3 | #include"math.h" 4 | #include"casadi_interface.h" 5 | #include 6 | #include 7 | #include "buffer.h" 8 | 9 | static real_t get_lipschitz_get_delta(const real_t* current_position,real_t * current_position_delta); 10 | 11 | /* 12 | * Get the step used to estimate the lipschitz constant 13 | * delta= max{small number,10^{-6}*u_0} 14 | */ 15 | static real_t get_lipschitz_get_delta(const real_t* current_position,real_t * current_position_delta){ 16 | real_t norm_delta=0; 17 | 18 | size_t i; 19 | for (i = 0; i < DIMENSION_PANOC; i++) 20 | { 21 | if(DELTA_LIPSCHITZ_SAFETY_VALUE>current_position[i] ){ 22 | current_position_delta[i] = current_position[i] + DELTA_LIPSCHITZ; 23 | norm_delta += sq(DELTA_LIPSCHITZ); 24 | }else{ 25 | current_position_delta[i] = current_position[i] + current_position[i]*DELTA_LIPSCHITZ_SAFETY_VALUE; 26 | norm_delta += sq(current_position[i]*DELTA_LIPSCHITZ_SAFETY_VALUE); 27 | } 28 | } 29 | 30 | return sqrt(norm_delta); 31 | } 32 | /* 33 | * Estimate the lipschitz constant by using the numerical hessian as an estimation 34 | * Theorem: 35 | * ||gradient(x)|| < B 36 | * f is B-lipschitz 37 | */ 38 | real_t get_lipschitz(void){ 39 | /* get the curernt position an its gradient */ 40 | const real_t* current_position = buffer_get_current_location(); 41 | const real_t* df_current_position = buffer_get_current_df(); 42 | 43 | /* get the shifted position */ 44 | real_t current_position_delta[DIMENSION_PANOC]; 45 | const real_t denominator = get_lipschitz_get_delta(current_position,current_position_delta); 46 | 47 | /* get shifted gradient */ 48 | real_t df_current_position_delta[DIMENSION_PANOC]; 49 | casadi_interface_f_df(current_position_delta,df_current_position_delta); 50 | 51 | /* 52 | * L = norm((df(x+delta)-df(x))/delta) 53 | * reuse the current_position_delta values as buffer 54 | */ 55 | vector_sub(df_current_position,df_current_position_delta, DIMENSION_PANOC,current_position_delta); /* step1: df(x+delta)-df(x) */ 56 | const real_t numerator = vector_norm2(current_position_delta, DIMENSION_PANOC); /* step2: norm((df(x+delta)-df(x))) */ 57 | 58 | return numerator/denominator; 59 | } -------------------------------------------------------------------------------- /PANOC/lipschitz.h: -------------------------------------------------------------------------------- 1 | #include "../globals/globals.h" 2 | #include 3 | #include 4 | 5 | #ifndef LIPSCHITZ_H 6 | #define LIPSCHITZ_H 7 | 8 | #ifdef __cplusplus 9 | extern "C" { 10 | #endif 11 | 12 | real_t get_lipschitz(void); 13 | 14 | #ifdef __cplusplus 15 | } 16 | #endif 17 | 18 | #endif -------------------------------------------------------------------------------- /PANOC/matrix_operations.h: -------------------------------------------------------------------------------- 1 | #include "../globals/globals.h" 2 | #include "stddef.h" 3 | #include "math.h" 4 | 5 | #ifndef MATRIX_OPERATIONS_H 6 | #define MATRIX_OPERATIONS_H 7 | 8 | #ifdef __cplusplus 9 | extern "C" { 10 | #endif 11 | 12 | /* squared with preprocessor */ 13 | #define sq(x) ((x)*(x)) 14 | 15 | 16 | /* copy vector1 into vector2 */ 17 | void vector_copy(const real_t* vector1,real_t* vector2,const int size_vector); 18 | 19 | /* add vector1 and vector2 save the result in result */ 20 | void vector_add(const real_t* vector1,const real_t* vector2,const int size_vector,real_t* result); 21 | 22 | /* add vector with real */ 23 | void vector_real_add(const real_t* vector,const int size_vector,const real_t real,real_t* result); 24 | 25 | /* subtract vector2 from vector1 save the result in result */ 26 | void vector_sub(const real_t* vector1,const real_t* vector2,const int size_vector,real_t* result); 27 | 28 | /* multiply vector with real */ 29 | void vector_real_mul(const real_t* vector,const int size_vector,const real_t real,real_t* result); 30 | 31 | /* 32 | * calculate the 2 norm of a vector defined as 33 | * sqrt(x[0]^2 + x[1]^2 + ... x[n]^2) 34 | */ 35 | real_t vector_norm2(const real_t* vector,const int vector_size); 36 | 37 | real_t vector_norm1(const real_t* vector,const int vector_size); 38 | 39 | real_t vector_norm_inf(const real_t* vector,const int vector_size); 40 | 41 | real_t vector_norm_max(const real_t* vector,const int vector_size); 42 | 43 | real_t vector_norm_min(const real_t* vector,const int vector_size); 44 | 45 | real_t vector_norm_inf_signed(const real_t* vector,const int vector_size); 46 | 47 | int vector_norm_inf_element_index(const real_t* vector,const int vector_size); 48 | 49 | /* multiply each element of vector1 times -1 and save it in vector2 */ 50 | void vector_minus(const real_t* vector1,real_t* vector2,const int size_vector); 51 | 52 | /* inner product between vector 1 and 2 */ 53 | real_t inner_product(const real_t* vector1,const real_t* vector2,const int size_vector); 54 | 55 | /* add vector2 n times to vector1 */ 56 | void vector_add_ntimes(real_t* vector1,const real_t* vector2,const int size_vector,const real_t n); 57 | 58 | /* add vector2 a_vector2 times to vector1 and add vector3 a_vector3 times to vector1*/ 59 | void vector_add_2_vectors_a_times(const real_t* vector1,const real_t* vector2,const real_t* vector3,const int size_vector, 60 | const real_t a_vector2,const real_t a_vector3,real_t* result); 61 | 62 | #ifdef __cplusplus 63 | } 64 | #endif 65 | 66 | #endif -------------------------------------------------------------------------------- /PANOC/panoc.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include"../globals/globals.h" 3 | 4 | #ifndef PANOC_H 5 | #define PANOC_H 6 | 7 | #ifdef __cplusplus 8 | extern "C" { 9 | #endif 10 | 11 | int panoc_init(); 12 | int panoc_cleanup(); 13 | 14 | real_t panoc_get_new_location(const real_t* current_location, real_t* optimal_inputs); 15 | 16 | /* 17 | * call this function from nmpc at the end to reset the buffers/counters 18 | */ 19 | int panoc_reset_cycli(void); 20 | 21 | real_t panoc_get_tau(void); 22 | 23 | #ifdef __cplusplus 24 | } 25 | #endif 26 | 27 | #endif -------------------------------------------------------------------------------- /PANOC/proximal_gradient_descent.h: -------------------------------------------------------------------------------- 1 | #include"../globals/globals.h" 2 | #include 3 | #include 4 | 5 | #ifndef PROXIMAL_GRADIENT_DESCENT_H 6 | #define PROXIMAL_GRADIENT_DESCENT_H 7 | 8 | #ifdef __cplusplus 9 | extern "C" { 10 | #endif 11 | 12 | int proximal_gradient_descent_init(void); 13 | int proximal_gradient_descent_cleanup(void); 14 | const real_t* proximal_gradient_descent_get_direction(void); 15 | const real_t* proximal_gradient_descent_get_buffered_direction(void); 16 | 17 | /* 18 | * Reset iteration index and gamma, call me if your starting with a new problem 19 | */ 20 | int proximal_gradient_descent_reset_iteration_counters(void); 21 | 22 | /* 23 | * calculate the forward backward envelope using the internal gamma 24 | * Matlab cache.FBE = cache.fx + cache.gz - cache.gradfx(:)'*cache.FPR(:) + (0.5/gam)*(cache.normFPR^2); 25 | */ 26 | real_t proximal_gradient_descent_forward_backward_envelope(const real_t* location); 27 | /* 28 | * return the precomputed forward backward envelope of the current location 29 | */ 30 | real_t proximal_gradient_descent_get_current_forward_backward_envelope(void); 31 | 32 | /* 33 | * Calculate the residual at current_location from the buffer 34 | */ 35 | int proximal_gradient_descent_get_current_residual(real_t* residual); 36 | /* 37 | * Calculate the residual at a certain location 38 | */ 39 | int proximal_gradient_descent_get_new_residual_buffered(real_t* residual); 40 | int proximal_gradient_descent_get_new_residual(const real_t* input,real_t* output); 41 | 42 | 43 | /* 44 | * returns the linesearch parameter or 1/Lipschitz of the gradient 45 | */ 46 | real_t proximal_gradient_descent_get_gamma(void); 47 | /* 48 | * returns the residual from the current proximal gradient step 49 | */ 50 | real_t proximal_gradient_descent_get_current_residual_inf_norm(void); 51 | 52 | #ifdef __cplusplus 53 | } 54 | #endif 55 | 56 | #endif // !PROXIMAL_GRADIENT_DESCENT_H -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # PANOC NMPC solver kernel ![travis build](https://travis-ci.org/kul-forbes/nmpc-codegen.svg?branch=master) 2 | ## What is this? 3 | Nmpc-codegen generates MPC controllers. The user provides the dynamics of the system, a number of conditions and some MPC parameters in python. Nmpc-codegen will then generate the NMPC controller in c89 code that can be used on embedded devices. Check out the [website](https://kul-forbes.github.io/nmpc-codegen/) for more information. 4 | 5 | This repo contains the C code used with the code generation and the documentation. Nmpc-codegen framework has 2 implementations one in Matlab and one in Python: 6 | 7 | - [nmpc-codegen-matlab](https://github.com/kul-forbes/nmpc-codegen-matlab) github repo 8 | - [nmpc-codegen-python](https://github.com/kul-forbes/nmpc-codegen-python) github repo 9 | 10 | If you have an issue with nmpc-codegen feel free to contact willem.melis at outlook dot com. 11 | 12 | # How to compile and test me? (for internal developers) 13 | This is only for those who want to check if the library works on there device. 14 | 15 | ## Windows with Mingw/Mingw-W64 (!!! make sure python and the toolchain are either BOTH 32 or BOTH 64 bit) 16 | - Generate the test functions by running the generate_test_files.py script with python3 17 | - Run Cmake to generate the make files: cmake -H. -Bbuild -G "MinGW Makefiles" 18 | - Run Make to compile everything: make 19 | - Run make test to test everything: make test 20 | 21 | ![alt text](trailer_example_time_log.png "Time till convergence simple simulation") 22 | 23 | ## Notes 24 | - The tested compilers on the raw controller code are gcc GNU compiler, Clang LLVM compiler, Intel C compiler and the Microsoft C Compiler. 25 | - cmake -H. -Bbuild -DCMAKE_C_COMPILER=clang creates a build system with clang compiler and Cmake 26 | -------------------------------------------------------------------------------- /build/README.md: -------------------------------------------------------------------------------- 1 | ## output folder cmake 2 | -------------------------------------------------------------------------------- /casadi/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | if (EXISTS integrator.c) 2 | set(files_casadi cost_function_derivative_combined.c cost_function.c integrator.c) 3 | else() 4 | set(files_casadi cost_function_derivative_combined.c cost_function.c) 5 | endif() 6 | 7 | add_library(CASADI_lib ${files_casadi}) 8 | set_property(TARGET CASADI_lib PROPERTY C_STANDARD 90) 9 | set_property(TARGET CASADI_lib PROPERTY POSITION_INDEPENDENT_CODE ON) -------------------------------------------------------------------------------- /casadi/README.md: -------------------------------------------------------------------------------- 1 | This folder contains the functions generated with casadi. -------------------------------------------------------------------------------- /casadi/cost_function.h: -------------------------------------------------------------------------------- 1 | /* This file was automatically generated by CasADi. 2 | The CasADi copyright holders make no ownership claim of its contents. */ 3 | #ifdef __cplusplus 4 | extern "C" { 5 | #endif 6 | 7 | #ifndef casadi_real 8 | #define casadi_real double 9 | #endif 10 | 11 | #ifndef casadi_int 12 | #define casadi_int long int 13 | #endif 14 | 15 | int cost_function(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, void* mem); 16 | void cost_function_incref(void); 17 | void cost_function_decref(void); 18 | casadi_int cost_function_n_out(void); 19 | casadi_int cost_function_n_in(void); 20 | const char* cost_function_name_in(casadi_int i); 21 | const char* cost_function_name_out(casadi_int i); 22 | const casadi_int* cost_function_sparsity_in(casadi_int i); 23 | const casadi_int* cost_function_sparsity_out(casadi_int i); 24 | int cost_function_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w); 25 | #ifdef __cplusplus 26 | } /* extern "C" */ 27 | #endif 28 | -------------------------------------------------------------------------------- /casadi/cost_function_derivative_combined.h: -------------------------------------------------------------------------------- 1 | /* This file was automatically generated by CasADi. 2 | The CasADi copyright holders make no ownership claim of its contents. */ 3 | #ifdef __cplusplus 4 | extern "C" { 5 | #endif 6 | 7 | #ifndef casadi_real 8 | #define casadi_real double 9 | #endif 10 | 11 | #ifndef casadi_int 12 | #define casadi_int long int 13 | #endif 14 | 15 | int cost_function_derivative_combined(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, void* mem); 16 | void cost_function_derivative_combined_incref(void); 17 | void cost_function_derivative_combined_decref(void); 18 | casadi_int cost_function_derivative_combined_n_out(void); 19 | casadi_int cost_function_derivative_combined_n_in(void); 20 | const char* cost_function_derivative_combined_name_in(casadi_int i); 21 | const char* cost_function_derivative_combined_name_out(casadi_int i); 22 | const casadi_int* cost_function_derivative_combined_sparsity_in(casadi_int i); 23 | const casadi_int* cost_function_derivative_combined_sparsity_out(casadi_int i); 24 | int cost_function_derivative_combined_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w); 25 | #ifdef __cplusplus 26 | } /* extern "C" */ 27 | #endif 28 | -------------------------------------------------------------------------------- /casadi/g.c: -------------------------------------------------------------------------------- 1 | /* file generated on 09/12/18 at 23:06:02 */ 2 | 3 | // real_t casadi_interface_g(const real_t* state){ 4 | // size_t i; 5 | // for(i=0;i2){ 8 | // return LARGE; 9 | // } 10 | // /* check if the value of the border is outside the box, if so return zero */ 11 | // if(state[1]<-2 || state[1]>2){ 12 | // return LARGE; 13 | // } 14 | // state+=2; 15 | // } 16 | // /* if the value's where never outside the box, return zero */ 17 | // return 0; 18 | 19 | // } 20 | -------------------------------------------------------------------------------- /casadi/integrator.h: -------------------------------------------------------------------------------- 1 | /* This file was automatically generated by CasADi. 2 | The CasADi copyright holders make no ownership claim of its contents. */ 3 | #ifdef __cplusplus 4 | extern "C" { 5 | #endif 6 | 7 | #ifndef casadi_real 8 | #define casadi_real double 9 | #endif 10 | 11 | #ifndef casadi_int 12 | #define casadi_int long int 13 | #endif 14 | 15 | int integrator(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, void* mem); 16 | void integrator_incref(void); 17 | void integrator_decref(void); 18 | casadi_int integrator_n_out(void); 19 | casadi_int integrator_n_in(void); 20 | const char* integrator_name_in(casadi_int i); 21 | const char* integrator_name_out(casadi_int i); 22 | const casadi_int* integrator_sparsity_in(casadi_int i); 23 | const casadi_int* integrator_sparsity_out(casadi_int i); 24 | int integrator_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w); 25 | #ifdef __cplusplus 26 | } /* extern "C" */ 27 | #endif 28 | -------------------------------------------------------------------------------- /casadi/proxg.c: -------------------------------------------------------------------------------- 1 | /* file generated on 09/12/18 at 23:06:02 */ 2 | 3 | real_t casadi_interface_g_proxg(real_t* state){ 4 | size_t i; 5 | for(i=0;i2){ 10 | state[0]=2; 11 | }else{ 12 | state[0]=state[0]; 13 | } 14 | /* check if the value of the border is outside the box, if so go to the nearest point inside the box */ 15 | if(state[1]<-2){ 16 | state[1]=-2; 17 | }else if(state[1]>2){ 18 | state[1]=2; 19 | }else{ 20 | state[1]=state[1]; 21 | } 22 | state+=2; 23 | } 24 | return 0; /* always ends up inside the box so g(x_bar)=0 */ 25 | } 26 | -------------------------------------------------------------------------------- /codestyle.md: -------------------------------------------------------------------------------- 1 | # Code style 2 | 3 | ## C code 4 | 5 | - Only strict C89 standard rules with the exception of the place of definition of variables. Variable should only be defined when used, not earlier then absolutely necessary. 6 | - Function/variable names should be lowercase, with words separated by underscores as necessary to improve readability. 7 | - Tabs are not allowed 4 spaces are used instead 8 | - Functions are outlined like: 9 | 10 | ``` 11 | void function_name(int a){ 12 | ìnt b=3*a; 13 | } 14 | ``` 15 | 16 | ## Python code 17 | - private variables start with an underscore and are lower caps (example: "_start_time") 18 | - Function/variable names should be lowercase, with words separated by underscores as necessary to improve readability. 19 | 20 | ## Matlab code 21 | - Function/variable names should be lowercase, with words separated by underscores as necessary to improve readability. -------------------------------------------------------------------------------- /docs/_config.yml: -------------------------------------------------------------------------------- 1 | theme: jekyll-theme-cayman -------------------------------------------------------------------------------- /docs/about.md: -------------------------------------------------------------------------------- 1 | # About this library 2 | This library was made by Willem Melis as part of the thesis for the Master mathematical engineering at KuLeuven. If you encounter problems installing or using the is library, feel free to make issue. Or contact Willem Melis directly by email: willem.melis at outlook dot com . 3 | 4 | ## Install instructions 5 | [Install with Python3](./install/Python_install.md) 6 | 7 | [Install with Matlab](./install/Matlab_install.md) 8 | 9 | ## Workflow 10 | The Python or Matlab library provides a framework to construct a Python or Matlab script that generates a controller. The only thing the user has to provide are the control parameters and the mathematical model of the system. 11 | 12 | Additionally a few algorithm depending parameters must be provided. At the moment the only parameters avaialable to the user are the amount of iterations the algorithm is allowed to execute as long as algorithm does not converge. And an optional parameter that specifies if the cost function and the gradient should always be evaluated together or not. 13 | ![alt text](img/nmpc_codegen_scheme.png "General scheme of the library") 14 | 15 | ## Internal structure Python/Matlab 16 | The structure of the Python or Matlab library is displayed below. There are five sub packages tools, models, example models, controller and Cfunctions. These are displayed in the drawing below. The two classes in the sub package models, represent the mathematical model of the system. The user must construct an object of one of these classes, that contains the function equation of the system. 17 | 18 | The Cfunctions sub package contains proximal functions that represent constraints on the inputs. The user manual contains a table at all available constraints to the user. The tools sub package contains two classes. The bootstrapper can generate the static code, and the simulator that allows the user to call the generated C code from Python or Matlab. 19 | 20 | The controller sub package contains the Nmpc_panoc class, an object of this class represents the actual controller. In order to construct an object of the Nmpc_panoc class user must provide a model object a proximal function object and one or two stage costs object. 21 | 22 | Finally the user can also add obstacles, these will be added a soft constraints into the cost function. The obstacles can be found in the sub package obstacles from the sub package controller. 23 | ![alt text](img/nmpc_codegen_packages.png "General scheme of the library") 24 | -------------------------------------------------------------------------------- /docs/example_Matlab.md: -------------------------------------------------------------------------------- 1 | # Example of library using Matlab 2 | 3 | TODO -------------------------------------------------------------------------------- /docs/example_Python.md: -------------------------------------------------------------------------------- 1 | # Example of library using Python 2 | 3 | TODO -------------------------------------------------------------------------------- /docs/img/nmpc_codegen_packages.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/docs/img/nmpc_codegen_packages.png -------------------------------------------------------------------------------- /docs/img/nmpc_codegen_scheme.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/docs/img/nmpc_codegen_scheme.png -------------------------------------------------------------------------------- /docs/img/timings.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/docs/img/timings.png -------------------------------------------------------------------------------- /docs/img/trailer.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/docs/img/trailer.png -------------------------------------------------------------------------------- /docs/img/visio_software_arch.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/docs/img/visio_software_arch.png -------------------------------------------------------------------------------- /docs/index.md: -------------------------------------------------------------------------------- 1 | # Nmpccodegen library 2 | 3 | ## What is this? 4 | This project is still under construction but the goal is to generate an MPC controller. The user will provide the dynamics of the system, a number of conditions and some MPC parameters in python. The program will then generate an NMPC controller in C code that that is close to C89 so it can be executed on embedded devices. 5 | 6 | ![alt text](img/nmpc_codegen_scheme.png "General scheme of the library") 7 | 8 | Below you can find a comparison between the Matlab implementation (ForBes zerofpr2) fmincon(interior point method of matlab) and nmpc-codegen. The time represents the time till convergence in milliseconds of every step of the controller simulation.(source code: ./demos/Matlab/compare_libs.m). 9 | 10 | 11 | ![alt text](img/timings.png "Time till convergence simple simulation") 12 | 13 | The first version of Python is ready to be used, a Matlab version is on its way, right now its experimental.(run ./demos/thesis/TwoCircTrailer.py in the github repo to get an idea what this is about) 14 | 15 | At the moment there is no package, but very soon the python package will be available in pip. 16 | 17 | ## Installation Instructions (Windows/Mac OS/Linux) 18 | 19 | - [Install with Python3](./install/Python_install.md) 20 | - [Install with Matlab](./install/Matlab_install.md) 21 | -------------------------------------------------------------------------------- /docs/install/Matlab_install.md: -------------------------------------------------------------------------------- 1 | # Installation of nmpccodegen on Matlab 2 | 3 | ## What do I need? 4 | - GNU toolchain with make (more info [here](Toolchain_install.md)) 5 | - Matlab with casadi 3.2.x or higher 6 | - Cmake 7 | 8 | ## !!! Make sure Matlab and the toolchain are either BOTH 32 or BOTH 64 bit !! 9 | 10 | ## How to install 11 | - make sure Cmake,make and gcc are installed (run Matlab_test_utils.m to test if they are available) 12 | - git clone (with the --recursive flag on) the repo [nmpc-codegen-matlab](https://github.com/kul-forbes/nmpc-codegen-matlab) 13 | - add the src folder to your matlab path 14 | - !! no compilation is required when installing , Matlab takes care of any compilation !! 15 | - install the following library's [casadi](http://casadi.org), No toolboxes are required 16 | - Check out demos/Matlab in the nmpccodegen repo 17 | -------------------------------------------------------------------------------- /docs/install/Python_dev_install.md: -------------------------------------------------------------------------------- 1 | # How to compile and test me? (only for internal developers !) 2 | This is only for those who want to check if the library works on there device. 3 | ## Windows with Mingw/Mingw-W64 (!!! make sure python and the toolchain are either BOTH 32 or BOTH 64 bit) 4 | - Generate the test functions by running the generate_test_files.py script with python3 5 | - Run Cmake to generate the make files: cmake -H. -Bbuild -G "MinGW Makefiles" 6 | - Run Make to compile everything: make 7 | - Run make test to test everything: make test 8 | 9 | ## Unix-like operating systems 10 | - Generate the test functions by running the generate_test_files.py script with python3 11 | - Run Cmake to generate the make files: cmake -H. -Bbuild 12 | - Run Make inside the ./build folder to compile everything: make 13 | - Run make test to test everything: make test -------------------------------------------------------------------------------- /docs/install/Python_install.md: -------------------------------------------------------------------------------- 1 | # Installation of nmpccodegen on Python 3 2 | At the moment there is no package, but very soon the python package will be available in pip. 3 | 4 | ## What do I need? 5 | - GNU toolchain with make (more info [here](Toolchain_install.md)) 6 | - python 3 with casadi 3.2.x or higher and numpy/matplotlib installed on it 7 | - Cmake 8 | - Some of the demos might require aditions library's such as matplotlib to visualize the results 9 | 10 | ## How to install 11 | - make sure Cmake,make and gcc are installed 12 | - git clone (with the --recursive flag on) the [nmpc-codegen-python](https://github.com/kul-forbes/nmpc-codegen-python) repo 13 | - add src to your python path 14 | - !! no compilation is required when installing , Python takes care of any compilation !! 15 | - Install the following library's numpy,casadi,matplotlib 16 | - Check out the demos in the nmpccodegen-python repo 17 | -------------------------------------------------------------------------------- /docs/install/Toolchain_install.md: -------------------------------------------------------------------------------- 1 | # The nmpccodegen requires the GNU toolchain to compile the generated C code if you want to simulate. 2 | 3 | ## Tips for windows users 4 | - Install the correct mingw/mingw-64 5 | - [MSYS](http://www.mingw.org/wiki/MSYS) offers an easy way to install mingw/mingw64 6 | 7 | ## Tips for mac users 8 | 9 | ## Tips for Linux users 10 | 1. When using some Matlab versions the following error appears: 11 | cmake: /usr/local/MATLAB/R2016a/bin/glnxa64/libcurl.so.4: no version information available (required by cmake) 12 | cmake: /usr/local/MATLAB/R2016a/sys/os/glnxa64/libstdc++.so.6: version `GLIBCXX_3.4.18' not found (required by cmake) 13 | cmake: /usr/local/MATLAB/R2016a/sys/os/glnxa64/libstdc++.so.6: version `CXXABI_1.3.9' not found (required by cmake) 14 | cmake: /usr/local/MATLAB/R2016a/sys/os/glnxa64/libstdc++.so.6: version `GLIBCXX_3.4.20' not found (required by cmake) 15 | cmake: /usr/local/MATLAB/R2016a/sys/os/glnxa64/libstdc++.so.6: version `GLIBCXX_3.4.21' not found (required by cmake) 16 | cmake: /usr/local/MATLAB/R2016a/sys/os/glnxa64/libstdc++.so.6: version `GLIBCXX_3.4.21' not found (required by /usr/lib/x86_64-linux-gnu/libjsoncpp.so.1) 17 | cmake: /usr/local/MATLAB/R2016a/sys/os/glnxa64/libstdc++.so.6: version `CXXABI_1.3.8' not found (required by /usr/lib/x86_64-linux-gnu/libicuuc.so.57) 18 | 19 | This is due to Matlab using some old version from gcc. 20 | 21 | solution: start matlab as following: "LD_PRELOAD=/usr/lib/x86_64-linux-gnu/libstdc++.so.6 matlab" -------------------------------------------------------------------------------- /docs/structure_library.md: -------------------------------------------------------------------------------- 1 | # Package structure library 2 | 3 | TODO -------------------------------------------------------------------------------- /globals/globals_dyn.h: -------------------------------------------------------------------------------- 1 | /* file generated on 09/12/18 at 23:06:02 */ 2 | 3 | /* 4 | * --------------------------------- 5 | * Problem specific definitions 6 | * --------------------------------- 7 | */ 8 | #define DIMENSION_INPUT 2 9 | #define DIMENSION_STATE 18 10 | #define DIMENSION_PANOC 2 11 | #define MPC_HORIZON 50 12 | #define SHIFT_INPUT 1.0 13 | /* 14 | * --------------------------------- 15 | * Lagrangian related values, only visible if there are general constraints 16 | * --------------------------------- 17 | */ 18 | /* 19 | * --------------------------------- 20 | * Constraint related values 21 | * --------------------------------- 22 | */ 23 | #define NUMBER_OF_CONSTRAINTS 0 24 | #define DEFAULT_CONSTRAINT_WEIGHT 1 25 | /* 26 | * --------------------------------- 27 | * constants used with double data type 28 | * --------------------------------- 29 | */ 30 | #define real_t double 31 | /* data types have different absolute value functions */ 32 | #define ABS(x) fabs(x) 33 | /* Machine accuracy of IEEE double */ 34 | #define MACHINE_ACCURACY DBL_EPSILON 35 | /* large number use with things like indicator functions */ 36 | #define LARGE 10000000000 37 | /* 38 | * --------------------------------- 39 | * lbgfs solver definitions 40 | * --------------------------------- 41 | */ 42 | #define LBGFS_BUFFER_SIZE 20 43 | /* 44 | * --------------------------------- 45 | * NMPC-PANOC solver definitions 46 | * --------------------------------- 47 | */ 48 | #define PANOC_MAX_STEPS 20 49 | #define PANOC_MIN_STEPS 0 50 | #define MIN_RESIDUAL (1e-5) 51 | /* 52 | * --------------------------------- 53 | * options used to test: 54 | * --------------------------------- 55 | */ 56 | /* 57 | * --------------------------------- 58 | * Optional features 59 | * --------------------------------- 60 | */ 61 | #define INTEGRATOR_CASADI 1 62 | -------------------------------------------------------------------------------- /include/nmpc.h: -------------------------------------------------------------------------------- 1 | #ifndef NMPC_H 2 | #define NMPC_H 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #include 9 | #include "../globals/globals.h" 10 | 11 | int nmpc_init(void); 12 | int nmpc_cleanup(void); 13 | int npmc_solve( const real_t* current_state, 14 | const real_t* state_reference, 15 | const real_t* input_reference, 16 | real_t* optimal_inputs); 17 | int nmpc_get_last_full_solution(real_t* output); 18 | 19 | real_t nmpc_get_weight_constraints(int index_constraint);/* returns zero if index was out of range */ 20 | int nmpc_set_weight_constraints(int index_constraint,real_t weight);/* returns failure if the index is out of range */ 21 | int nmpc_set_buffer_solution(real_t value, int index); /* set the internal value of the input of the cost function */ 22 | real_t nmpc_get_residual_inf_norm(void); 23 | 24 | #ifdef __cplusplus 25 | } 26 | #endif 27 | 28 | #endif /* NMPC_H */ -------------------------------------------------------------------------------- /minimum_build_system/CMakeLists_casadi.txt: -------------------------------------------------------------------------------- 1 | set(files_casadi cost_function_derivative_combined.c cost_function.c) 2 | 3 | if (EXISTS "${PROJECT_SOURCE_DIR}/casadi/integrator.c") 4 | set(files_casadi "${files_casadi}" integrator.c) 5 | endif() 6 | 7 | if(EXISTS "${PROJECT_SOURCE_DIR}/casadi/evaluate_constraints.c") 8 | set(files_casadi "${files_casadi}" evaluate_constraints.c) 9 | endif() 10 | 11 | MESSAGE(STATUS "Detected the following casadi files: ${files_casadi}") 12 | 13 | add_library(CASADI_lib ${files_casadi}) 14 | set_property(TARGET CASADI_lib PROPERTY C_STANDARD 90) 15 | set_property(TARGET CASADI_lib PROPERTY POSITION_INDEPENDENT_CODE ON) -------------------------------------------------------------------------------- /minimum_build_system/CMakeLists_panoc.txt: -------------------------------------------------------------------------------- 1 | file(GLOB panoc_SRC 2 | "*.c" 3 | ) 4 | add_library(PANOC_lib STATIC ${panoc_SRC}) 5 | set_property(TARGET PANOC_lib PROPERTY C_STANDARD 90) 6 | 7 | # this is nessesary for the python interface, and doesnt really matter for the tests 8 | set_property(TARGET PANOC_lib PROPERTY POSITION_INDEPENDENT_CODE ON) -------------------------------------------------------------------------------- /minimum_build_system/CMakeLists_root.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required (VERSION 2.6) 2 | project (npmc_minimum C) 3 | 4 | # set the compiler with its flags 5 | SET(CMAKE_C_COMPILER gcc) 6 | 7 | # if not specified assume release. 8 | # specify that you want debug with: cmake . -DCMAKE_BUILD_TYPE=Debug 9 | if(NOT CMAKE_BUILD_TYPE) 10 | set(CMAKE_BUILD_TYPE Release) 11 | endif() 12 | 13 | if(NOT MSVC) 14 | set(CMAKE_C_FLAGS_DEBUG "-g -Wall -Wextra") 15 | set(CMAKE_C_FLAGS_RELEASE "-O3") 16 | endif() 17 | 18 | # subdirectory with lib 19 | include_directories ("${PROJECT_SOURCE_DIR}/PANOC") 20 | add_subdirectory (PANOC) # read out the cmakelist file 21 | 22 | include_directories ("${PROJECT_SOURCE_DIR}/casadi") 23 | add_subdirectory (casadi) 24 | 25 | if(WIN32) 26 | SET(TIMER_LIB "${PROJECT_SOURCE_DIR}/python_interface/timer_windows.c") 27 | elseif(APPLE) 28 | SET(TIMER_LIB "${PROJECT_SOURCE_DIR}/python_interface/timer_mac.c" ) 29 | elseif(UNIX) 30 | SET(TIMER_LIB "${PROJECT_SOURCE_DIR}/python_interface/timer_linux.c") 31 | endif() 32 | 33 | add_library(python_interface SHARED "${PROJECT_SOURCE_DIR}/python_interface/nmpc_python" "${TIMER_LIB}") 34 | target_link_libraries (python_interface PANOC_lib CASADI_lib) 35 | set_property(TARGET python_interface PROPERTY C_STANDARD 90) 36 | set_property(TARGET python_interface PROPERTY POSITION_INDEPENDENT_CODE ON) 37 | 38 | SET(MAIN_FILE_PATH "${PROJECT_SOURCE_DIR}/main.c" ) 39 | 40 | if (EXISTS "${MAIN_FILE_PATH}") 41 | MESSAGE( STATUS "main.c detected use 'make main_exe' to compile it" ) 42 | add_executable(main_exe "${MAIN_FILE_PATH}") 43 | target_link_libraries (main_exe PANOC_lib CASADI_lib) 44 | target_link_libraries(main_exe m) # link with the math lib, some toolchains need this 45 | else() 46 | MESSAGE( STATUS "No main.c detected in project root folder" ) 47 | endif() -------------------------------------------------------------------------------- /minimum_build_system/main_trailer.c: -------------------------------------------------------------------------------- 1 | #include "stdio.h" 2 | #include "./include/nmpc.h" 3 | #include "./PANOC/casadi_interface.h" 4 | 5 | int main(){ 6 | printf("Debugging controller: \n"); 7 | size_t number_of_steps=100; 8 | size_t print_limit=10; 9 | 10 | double ref_state[3]={2,0.5,0}; 11 | double ref_input[3]={0,0}; 12 | 13 | double state[3]={0,0,0}; 14 | double new_state[3]; 15 | double input[2]; 16 | 17 | nmpc_init(); 18 | printf("# initial state x=%f y=%f theta=%f \n \n",state[0],state[1],state[2]); 19 | 20 | size_t i; 21 | for (i = 0; i < number_of_steps; i++) 22 | { 23 | npmc_solve(state,ref_state,ref_input,input); 24 | if(i> plotcube([5 5 5],[ 2 2 2],.8,[1 0 0]); 14 | % >> plotcube([5 5 5],[10 10 10],.8,[0 1 0]); 15 | % >> plotcube([5 5 5],[20 20 20],.8,[0 0 1]); 16 | 17 | % Default input arguments 18 | inArgs = { ... 19 | [10 56 100] , ... % Default edge sizes (x,y and z) 20 | [10 10 10] , ... % Default coordinates of the origin point of the cube 21 | .7 , ... % Default alpha value for the cube's faces 22 | [1 0 0] ... % Default Color for the cube 23 | }; 24 | 25 | % Replace default input arguments by input values 26 | inArgs(1:nargin) = varargin; 27 | 28 | % Create all variables 29 | [edges,origin,alpha,clr] = deal(inArgs{:}); 30 | 31 | XYZ = { ... 32 | [0 0 0 0] [0 0 1 1] [0 1 1 0] ; ... 33 | [1 1 1 1] [0 0 1 1] [0 1 1 0] ; ... 34 | [0 1 1 0] [0 0 0 0] [0 0 1 1] ; ... 35 | [0 1 1 0] [1 1 1 1] [0 0 1 1] ; ... 36 | [0 1 1 0] [0 0 1 1] [0 0 0 0] ; ... 37 | [0 1 1 0] [0 0 1 1] [1 1 1 1] ... 38 | }; 39 | 40 | XYZ = mat2cell(... 41 | cellfun( @(x,y,z) x*y+z , ... 42 | XYZ , ... 43 | repmat(mat2cell(edges,1,[1 1 1]),6,1) , ... 44 | repmat(mat2cell(origin,1,[1 1 1]),6,1) , ... 45 | 'UniformOutput',false), ... 46 | 6,[1 1 1]); 47 | 48 | 49 | cellfun(@patch,XYZ{1},XYZ{2},XYZ{3},... 50 | repmat({clr},6,1),... 51 | repmat({'FaceAlpha'},6,1),... 52 | repmat({alpha},6,1)... 53 | ); -------------------------------------------------------------------------------- /old_code/demos/Matlab/quadcopter/prepare_demo_quadcopter.m: -------------------------------------------------------------------------------- 1 | function controller = prepare_demo_quadcopter( controller_folder_name,step_size,Q,R,Q_terminal,R_terminal ) 2 | %PREPARE_DEMO_CONTROLLER Prepare a controller 3 | % Construct the controller from the quadcopter model and bootstrap the 4 | % enviroment. (output location is repo/test_controller_builds/controller_folder_name) 5 | 6 | % generate static files 7 | trailer_controller_output_location = ['../../../test_controller_builds/' controller_folder_name]; 8 | simulation_tools=true; 9 | nmpccodegen.tools.Bootstrapper.bootstrap(trailer_controller_output_location, simulation_tools); 10 | 11 | % get example model from lib 12 | [system_equations, number_of_states, number_of_inputs, coordinates_indices] ... 13 | = nmpccodegen.example_models.get_quadcopter_model(); 14 | 15 | integrator = 'RK44'; % select a Runga-Kutta integrator (FE is forward euler) 16 | constraint_input = nmpccodegen.Cfunctions.IndicatorBoxFunction([0, 0, 0, 0], [100, 100, 100, 100]); % input needs stay within these borders 17 | model = nmpccodegen.models.Model_continuous(system_equations, constraint_input, step_size, number_of_states, ... 18 | number_of_inputs, coordinates_indices, integrator); 19 | 20 | % define the controller 21 | stage_cost = nmpccodegen.controller.Stage_cost_QR(model, Q, R); 22 | terminal_cost = nmpccodegen.controller.Stage_cost_QR(model, Q_terminal, R_terminal); 23 | controller = nmpccodegen.controller.Nmpc_panoc(trailer_controller_output_location, model, stage_cost,terminal_cost); 24 | end 25 | 26 | -------------------------------------------------------------------------------- /old_code/demos/Matlab/quadcopter/quadcopter_constraint.m: -------------------------------------------------------------------------------- 1 | clear all; 2 | % close all; 3 | noise_amplitude=zeros(12,1); 4 | % 5 | step_size=0.01; 6 | 7 | % Q and R matrixes determined by the control engineer. 8 | Q = diag([1e2 1e2 1e2 0. 0. 0. 0. 0. 0. 0. 0. 0. ]); 9 | R = diag([1. 1. 1. 1.])*0.05 ; 10 | 11 | Q_terminal = diag([1e2 1e2 1e2 1. 1. 1. 1. 1. 1. 1. 1. 1. ])*2; 12 | R_terminal = R*10; 13 | 14 | controller_folder_name = 'quadcopter_matlab_constraint'; 15 | controller = prepare_demo_quadcopter(controller_folder_name,step_size,Q,R,Q_terminal,R_terminal); 16 | 17 | controller.horizon = 100; % NMPC parameter 18 | controller.panoc_max_steps = 1000; % the maximum amount of iterations the PANOC algorithm is allowed to do. 19 | controller.min_residual=-3; 20 | controller.lbgfs_buffer_size=150; 21 | 22 | % positions_state = [4;5;6]; 23 | % maximums_state = [10;3;3]; 24 | % max_constraint = nmpccodegen.controller.constraints.Max_state(positions_state,maximums_state); 25 | % controller = controller.add_constraint(max_constraint); 26 | % 27 | % obstacle_weights=[1e2]; 28 | obstacle_weights=[]; 29 | 30 | % Generate the C code 31 | controller = controller.generate_code(); 32 | % 33 | initial_state = zeros(12,1);initial_state(3)=2; 34 | reference_state = zeros(12,1); 35 | reference_state(1)=15.; 36 | reference_state(2)=0.; 37 | reference_state(3)=3.; 38 | 39 | reference_input=[41;41;41;41]; % hover 40 | 41 | %% 42 | [state_history,time_history,iteration_history,simulator] = simulate_demo_controller(controller,initial_state,reference_state,reference_input,obstacle_weights,noise_amplitude); 43 | nmpccodegen.tools.Simulator.force_unload(); 44 | %% Visualize the result 45 | figure(1);clf; 46 | plot3(state_history(1,:),state_history(2,:),state_history(3,:));hold all; 47 | 48 | plot3(initial_state(1),initial_state(2),initial_state(3),'k*'); 49 | plot3(reference_state(1),reference_state(2),reference_state(3),'ko'); 50 | 51 | disp('--------------------------'); 52 | disp(['Quadcopter arrived at point [' num2str(state_history(1,end)) ',' num2str(state_history(2,end)) ',' num2str(state_history(3,end)) ']' ]) 53 | disp(['Distance from reference point [' num2str(reference_state(1)) ',' ... 54 | num2str(reference_state(2)) ',' num2str(reference_state(3)) ']' ... 55 | ' ' num2str(norm(state_history(1:3,end)-reference_state(1:3)))]); -------------------------------------------------------------------------------- /old_code/demos/Matlab/quadcopter/simple_simulation.m: -------------------------------------------------------------------------------- 1 | % get example model from lib 2 | [system_equations, number_of_states, number_of_inputs, coordinates_indices] ... 3 | = nmpccodegen.example_models.get_quadcopter_model(); 4 | 5 | step_size=0.05; 6 | integrator = 'RK44'; % select a Runga-Kutta integrator (FE is forward euler) 7 | 8 | constraint_input = nmpccodegen.Cfunctions.IndicatorBoxFunction([0, 0, 0, 0], [100, 100, 100, 100]); % input needs stay within these borders 9 | 10 | model = nmpccodegen.models.Model_continuous(system_equations, constraint_input, step_size, number_of_states, ... 11 | number_of_inputs, coordinates_indices, integrator); 12 | 13 | % set starting point 14 | init_state = zeros(12,1);init_state(1)=3;init_state(2)=3;init_state(3)=3; 15 | 16 | % simulate 17 | number_of_steps=100; 18 | state = init_state; 19 | input=zeros(4,1)*10; % fly straight up 20 | state_history=zeros(12,number_of_steps); 21 | for i=1:100 22 | state = model.get_next_state_double(state,input); 23 | state_history(:,i)=state; 24 | 25 | end 26 | %% 27 | figure; 28 | range=1:10; 29 | plot3(state_history(1,range),state_history(2,range),state_history(3,range)) -------------------------------------------------------------------------------- /old_code/demos/Matlab/quadcopter/simulate_demo_controller.m: -------------------------------------------------------------------------------- 1 | function [ state_history,time_history,iteration_history,sim ] = simulate_demo_controller( controller, ... 2 | initial_state,reference_state,reference_input,obstacle_weights,noise_amplitude ) 3 | %SIMULATE_DEMO_CONTROLLER Simulate 3 seconds of the quadcopter controller 4 | 5 | % -- simulate controller -- 6 | simulation_time = 3; 7 | number_of_steps = ceil(simulation_time / controller.model.step_size); 8 | % setup a simulator to test 9 | sim = nmpccodegen.tools.Simulator(controller.location); 10 | 11 | for i=1:length(obstacle_weights) 12 | sim.set_weight_constraint(i-1, obstacle_weights(i)) 13 | end 14 | 15 | state = initial_state; 16 | state_history = zeros(controller.model.number_of_states, number_of_steps); 17 | time_history = zeros(number_of_steps,1); 18 | iteration_history = zeros(number_of_steps,1); 19 | 20 | for i=1:number_of_steps 21 | result_simulation = sim.simulate_nmpc(state, reference_state, reference_input); 22 | disp(['Step [' num2str(i) '/' num2str(number_of_steps) ']: The optimal input is: [' ... 23 | num2str(result_simulation.optimal_input(1)) ',' num2str(result_simulation.optimal_input(2)) ... 24 | ',' num2str(result_simulation.optimal_input(3)) ',' num2str(result_simulation.optimal_input(4)) ']' ... 25 | ' time=' result_simulation.time_string ' number of panoc iterations=' ... 26 | num2str(result_simulation.panoc_interations)]); 27 | 28 | time_history(i)=result_simulation.seconds*1000+result_simulation.milli_seconds; 29 | iteration_history(i)=result_simulation.panoc_interations; 30 | 31 | state = controller.model.get_next_state_double(state, result_simulation.optimal_input)+((rand - 0.5)*2)*noise_amplitude; 32 | state_history(:, i) = state; 33 | end 34 | 35 | disp('Final state:'); 36 | disp(state); 37 | 38 | end 39 | 40 | -------------------------------------------------------------------------------- /old_code/demos/Matlab/quadcopter/simulate_panoc_draft.m: -------------------------------------------------------------------------------- 1 | function [ state_history,time_history,iteration_history ] = simulate_panoc_draft( trailer_controller, simulator, ... 2 | initial_state,reference_state,reference_input,shift_horizon,noise_amplitude ) 3 | %SIMULATE_DEMO_TRAILER_PANOC_MATLAB Summary of this function goes here 4 | % Detailed explanation goes here 5 | % -- simulate controller -- 6 | simulation_time = 3; 7 | number_of_steps = ceil(simulation_time / trailer_controller.model.step_size); 8 | % setup a simulator to test 9 | 10 | %% panocdraft specific stuff 11 | problem.dimension = trailer_controller.model.number_of_inputs*trailer_controller.horizon; 12 | problem.constraint_type = 'box'; 13 | problem.upper_bound=100; 14 | problem.lower_bound=0; 15 | problem.cost_function='cost_func'; 16 | 17 | solver_params.tolerance = 1e-3; 18 | solver_params.buffer_size = 150; 19 | solver_params.max_iterations = 1000; 20 | 21 | panoc('init',problem,solver_params); 22 | %% 23 | inputs = repmat(zeros(trailer_controller.model.number_of_inputs, 1), ... 24 | trailer_controller.horizon, 1); 25 | 26 | %% 27 | state = initial_state; 28 | state_history = zeros(trailer_controller.model.number_of_states, number_of_steps); 29 | time_history = zeros(number_of_steps,1); 30 | iteration_history = zeros(number_of_steps,1); 31 | 32 | for i=1:number_of_steps 33 | cost = @(inputs_horizon) simulator.evaluate_cost_gradient(... 34 | state,reference_state,reference_input,inputs_horizon); 35 | to=tic; 36 | 37 | iteration_history(i) = panoc('solve',inputs,cost); 38 | 39 | time_history(i)=toc(to)*1000;% get time in ms 40 | 41 | optimal_input=inputs(1:trailer_controller.model.number_of_inputs); 42 | if(shift_horizon) 43 | inputs(1:end-trailer_controller.model.number_of_inputs) = ... 44 | inputs(trailer_controller.model.number_of_inputs+1:end); 45 | end 46 | disp(['panoc draft [' num2str(i) '/' num2str(number_of_steps) ']' ' The optimal input is[' num2str(optimal_input(1)) ' ; ' num2str(optimal_input(2)) '] in ' num2str(iteration_history(i)) ' iterations']); 47 | 48 | state = trailer_controller.model.get_next_state_double(state, optimal_input)+((rand - 0.5)*2)*noise_amplitude; 49 | state_history(:, i) = state; 50 | end 51 | 52 | disp("Final state:") 53 | disp(state) 54 | panoc('cleanup'); 55 | 56 | clear('sim'); % remove the simulator so it unloads the shared lib 57 | end 58 | 59 | 60 | -------------------------------------------------------------------------------- /old_code/demos/Matlab/quadcopter/simulate_panoc_matlab.m: -------------------------------------------------------------------------------- 1 | function [ state_history,time_history,iteration_history ] = simulate_panoc_matlab( controller, simulator, ... 2 | initial_state,reference_state,reference_input,shift_horizon,noise_amplitude) 3 | %SIMULATE_DEMO_TRAILER_PANOC_MATLAB Summary of this function goes here 4 | % Detailed explanation goes here 5 | % -- simulate controller -- 6 | simulation_time = 3; 7 | number_of_steps = ceil(simulation_time / controller.model.step_size); 8 | % setup a simulator to test 9 | 10 | %% forbes panoc specific stuff 11 | opt_zerofpr2.tol = 0.001; 12 | opt_zerofpr2.display = 0; 13 | opt_zerofpr2.maxit = 1000; 14 | opt_zerofpr2.adaptive = 1; 15 | opt_zerofpr2.solver = 'zerofpr2'; 16 | opt_zerofpr2.method = 'lbfgs'; 17 | opt_zerofpr2.linesearch = 'backtracking'; 18 | opt_zerofpr2.report = 1; 19 | opt_zerofpr2.memory = 150; 20 | g = indBox(0, 100); 21 | inputs = repmat(zeros(controller.model.number_of_inputs, 1), ... 22 | controller.horizon, 1); 23 | 24 | %% 25 | state = initial_state; 26 | state_history = zeros(controller.model.number_of_states, number_of_steps); 27 | time_history = zeros(number_of_steps,1); 28 | iteration_history = zeros(number_of_steps,1); 29 | 30 | for i=1:number_of_steps 31 | cost = @(inputs_horizon) simulator.evaluate_cost_gradient(... 32 | state,reference_state,reference_input,inputs_horizon); 33 | f = smoothFunction(cost); 34 | to=tic; 35 | out_zerofpr2 = forbes(f, g, inputs, [], [], opt_zerofpr2); 36 | time_history(i)=toc(to)*1000;% get time in ms 37 | iteration_history(i)=out_zerofpr2.solver.iterations; 38 | inputs = out_zerofpr2.x; 39 | 40 | optimal_input=out_zerofpr2.x(1:controller.model.number_of_inputs); 41 | if(shift_horizon) 42 | % shift the iputs in prepartion of the next iteration 43 | inputs(1:end-controller.model.number_of_inputs) = ... 44 | inputs(controller.model.number_of_inputs+1:end); 45 | end 46 | disp(['ForBeS zerofpr2 ' num2str(i) '/' num2str(number_of_steps) ' The optimal input is[' num2str(optimal_input(1)) ' ; ' num2str(optimal_input(2)) ']']); 47 | 48 | state = controller.model.get_next_state_double(state, optimal_input)+((rand - 0.5)*2)*noise_amplitude; 49 | state_history(:, i) = state; 50 | end 51 | 52 | disp("Final state:") 53 | disp(state) 54 | 55 | clear('sim'); % remove the simulator so it unloads the shared lib 56 | end 57 | 58 | -------------------------------------------------------------------------------- /old_code/demos/Matlab/simulate_demo_trailer.m: -------------------------------------------------------------------------------- 1 | function [ state_history,time_history,iteration_history,input_history,sim ] = simulate_demo_trailer( trailer_controller, ... 2 | initial_state,reference_state,reference_input,obstacle_weights,noise_amplitude ) 3 | %SIMULATE_DEMO_TRAILER Simulate 3 seconds of the trailer controller 4 | 5 | % -- simulate controller -- 6 | simulation_time = 3; 7 | number_of_steps = ceil(simulation_time / trailer_controller.model.step_size); 8 | % setup a simulator to test 9 | sim = nmpccodegen.tools.Simulator(trailer_controller.location); 10 | 11 | for i=1:length(obstacle_weights) 12 | sim.set_weight_constraint(i-1, obstacle_weights(i)) 13 | end 14 | 15 | state = initial_state; 16 | state_history = zeros(trailer_controller.model.number_of_states, number_of_steps); 17 | time_history = zeros(number_of_steps,1); 18 | iteration_history = zeros(number_of_steps,1); 19 | input_history = zeros(trailer_controller.model.number_of_inputs,number_of_steps); 20 | 21 | for i=1:number_of_steps 22 | result_simulation = sim.simulate_nmpc(state, reference_state, reference_input); 23 | input_history(:,i) = result_simulation.optimal_input; % save the input 24 | disp(['Step [' num2str(i+1) '/' num2str(number_of_steps) ']: The optimal input is: [' ... 25 | num2str(result_simulation.optimal_input(1)) ',' num2str(result_simulation.optimal_input(2)) ']' ... 26 | ' time=' result_simulation.time_string ' number of panoc iterations=' ... 27 | num2str(result_simulation.panoc_interations)]); 28 | 29 | time_history(i)=result_simulation.seconds*1000+result_simulation.milli_seconds; 30 | iteration_history(i)=result_simulation.panoc_interations; 31 | 32 | state = trailer_controller.model.get_next_state_double(state, result_simulation.optimal_input)+((rand - 0.5)*2)*noise_amplitude; 33 | state_history(:, i) = state; 34 | end 35 | 36 | disp('Final state:'); 37 | disp(state); 38 | 39 | end 40 | 41 | -------------------------------------------------------------------------------- /old_code/demos/Matlab/simulate_demo_trailer_fmincon.m: -------------------------------------------------------------------------------- 1 | function [ state_history,time_history ] = simulate_demo_trailer_fmincon( algorithm,trailer_controller, simulator, ... 2 | initial_state,reference_state,reference_input,shift_horizon,noise_amplitude ) 3 | %SIMULATE_DEMO_TRAILER_PANOC_MATLAB Summary of this function goes here 4 | % Detailed explanation goes here 5 | % -- simulate controller -- 6 | simulation_time = 3; 7 | number_of_steps = ceil(simulation_time / trailer_controller.model.step_size); 8 | % setup a simulator to test 9 | 10 | %% fmincon panoc specific stuff 11 | A=eye(trailer_controller.model.number_of_inputs*trailer_controller.horizon); 12 | A = vertcat(A,A); 13 | b = ones(trailer_controller.model.number_of_inputs*trailer_controller.horizon,1); 14 | b = vertcat(b*4,b*4); 15 | 16 | inputs = repmat(zeros(trailer_controller.model.number_of_inputs, 1), ... 17 | trailer_controller.horizon, 1); 18 | 19 | %% 20 | state = initial_state; 21 | state_history = zeros(trailer_controller.model.number_of_states, number_of_steps); 22 | time_history = zeros(number_of_steps,1); 23 | iteration_history = zeros(number_of_steps,1); 24 | 25 | for i=1:number_of_steps 26 | cost = @(inputs_horizon) simulator.evaluate_cost_gradient(... 27 | state,reference_state,reference_input,inputs_horizon); 28 | to=tic; 29 | options = optimoptions(@fmincon,'Algorithm',algorithm,'SpecifyObjectiveGradient',true,'TolFun', 1e-3, 'MaxIter',10000,'MaxFunEvals',10000); 30 | inputs = fmincon(cost,inputs,A,b,[],[],[],[],[],options); 31 | 32 | time_history(i)=toc(to)*1000;% get time in ms 33 | iteration_history(i)=0; 34 | 35 | optimal_input=inputs(1:trailer_controller.model.number_of_inputs); 36 | if(shift_horizon) 37 | inputs(1:end-trailer_controller.model.number_of_inputs) = ... 38 | inputs(trailer_controller.model.number_of_inputs+1:end); 39 | end 40 | disp(['The optimal input is[' num2str(optimal_input(1)) ' ; ' num2str(optimal_input(2)) ']']); 41 | 42 | state = trailer_controller.model.get_next_state_double(state, optimal_input)+((rand - 0.5)*2)*noise_amplitude; 43 | state_history(:, i) = state; 44 | end 45 | 46 | disp("Final state:") 47 | disp(state) 48 | 49 | clear('sim'); % remove the simulator so it unloads the shared lib 50 | end 51 | 52 | -------------------------------------------------------------------------------- /old_code/demos/Matlab/simulate_demo_trailer_interior_point_matlab.m: -------------------------------------------------------------------------------- 1 | function [ state_history,time_history,iteration_history ] = simulate_demo_trailer_interior_point_matlab( trailer_controller, simulator, ... 2 | initial_state,reference_state,reference_input,shift_horizon ) 3 | %SIMULATE_DEMO_TRAILER_PANOC_MATLAB Summary of this function goes here 4 | % Detailed explanation goes here 5 | % -- simulate controller -- 6 | simulation_time = 3; 7 | number_of_steps = ceil(simulation_time / trailer_controller.model.step_size); 8 | % setup a simulator to test 9 | 10 | %% fmincon panoc specific stuff 11 | A=eye(trailer_controller.model.number_of_inputs*trailer_controller.horizon); 12 | A = vertcat(A,A); 13 | b = ones(trailer_controller.model.number_of_inputs*trailer_controller.horizon,1); 14 | b = vertcat(b*4,b*4); 15 | 16 | inputs = repmat(zeros(trailer_controller.model.number_of_inputs, 1), ... 17 | trailer_controller.horizon, 1); 18 | 19 | %% 20 | state = initial_state; 21 | state_history = zeros(trailer_controller.model.number_of_states, number_of_steps); 22 | time_history = zeros(number_of_steps,1); 23 | iteration_history = zeros(number_of_steps,1); 24 | 25 | for i=1:number_of_steps 26 | cost = @(inputs_horizon) simulator.evaluate_cost(... 27 | state,reference_state,reference_input,inputs_horizon); 28 | to=tic; 29 | options = optimset('TolFun', 1e-3, 'MaxIter',10000,'MaxFunEvals',10000); 30 | inputs = fmincon(cost,inputs,A,b,[],[],[],[],[],options); 31 | time_history(i)=toc(to)*1000;% get time in ms 32 | iteration_history(i)=0; 33 | 34 | optimal_input=inputs(1:trailer_controller.model.number_of_inputs); 35 | if(shift_horizon) 36 | % shift the iputs in prepartion of the next iteration 37 | inputs(1:end-trailer_controller.model.number_of_inputs) = ... 38 | inputs(trailer_controller.model.number_of_inputs+1:end); 39 | end 40 | disp(['The optimal input is[' num2str(optimal_input(1)) ' ; ' num2str(optimal_input(2)) ']']); 41 | 42 | state = trailer_controller.model.get_next_state_double(state, optimal_input)+((rand - 0.5)*2)*noise_amplitude; 43 | state_history(:, i) = state; 44 | end 45 | 46 | disp("Final state:") 47 | disp(state) 48 | 49 | clear('sim'); % remove the simulator so it unloads the shared lib 50 | end 51 | 52 | -------------------------------------------------------------------------------- /old_code/demos/Matlab/simulate_demo_trailer_panoc_draft.m: -------------------------------------------------------------------------------- 1 | function [ state_history,time_history,iteration_history ] = simulate_demo_trailer_panoc_draft( trailer_controller, simulator, ... 2 | initial_state,reference_state,reference_input,shift_horizon,noise_amplitude ) 3 | %SIMULATE_DEMO_TRAILER_PANOC_MATLAB Summary of this function goes here 4 | % Detailed explanation goes here 5 | % -- simulate controller -- 6 | simulation_time = 3; 7 | number_of_steps = ceil(simulation_time / trailer_controller.model.step_size); 8 | % setup a simulator to test 9 | 10 | %% panocdraft specific stuff 11 | problem.dimension = trailer_controller.model.number_of_inputs*trailer_controller.horizon; 12 | problem.constraint_type = 'box'; 13 | problem.upper_bound=4; 14 | problem.lower_bound=-4; 15 | problem.cost_function='cost_func'; 16 | 17 | solver_params.tolerance = 1e-3; 18 | solver_params.buffer_size = 50; 19 | solver_params.max_iterations = 1000; 20 | 21 | panoc('init',problem,solver_params); 22 | %% 23 | inputs = repmat(zeros(trailer_controller.model.number_of_inputs, 1), ... 24 | trailer_controller.horizon, 1); 25 | 26 | %% 27 | state = initial_state; 28 | state_history = zeros(trailer_controller.model.number_of_states, number_of_steps); 29 | time_history = zeros(number_of_steps,1); 30 | iteration_history = zeros(number_of_steps,1); 31 | 32 | for i=1:number_of_steps 33 | cost = @(inputs_horizon) simulator.evaluate_cost_gradient(... 34 | state,reference_state,reference_input,inputs_horizon); 35 | to=tic; 36 | 37 | iteration_history(i) = panoc('solve',inputs,cost); 38 | 39 | time_history(i)=toc(to)*1000;% get time in ms 40 | 41 | optimal_input=inputs(1:trailer_controller.model.number_of_inputs); 42 | if(shift_horizon) 43 | inputs(1:end-trailer_controller.model.number_of_inputs) = ... 44 | inputs(trailer_controller.model.number_of_inputs+1:end); 45 | end 46 | disp(['The optimal input is[' num2str(optimal_input(1)) ' ; ' num2str(optimal_input(2)) '] in ' num2str(iteration_history(i)) ' iterations']); 47 | 48 | state = trailer_controller.model.get_next_state_double(state, optimal_input)+((rand - 0.5)*2)*noise_amplitude; 49 | state_history(:, i) = state; 50 | end 51 | 52 | disp("Final state:") 53 | disp(state) 54 | panoc('cleanup'); 55 | 56 | clear('sim'); % remove the simulator so it unloads the shared lib 57 | end 58 | 59 | 60 | -------------------------------------------------------------------------------- /old_code/demos/Matlab/simulate_demo_trailer_panoc_matlab.m: -------------------------------------------------------------------------------- 1 | function [ state_history,time_history,iteration_history ] = simulate_demo_trailer_panoc_matlab( trailer_controller, simulator, ... 2 | initial_state,reference_state,reference_input,shift_horizon,noise_amplitude) 3 | %SIMULATE_DEMO_TRAILER_PANOC_MATLAB Summary of this function goes here 4 | % Detailed explanation goes here 5 | % -- simulate controller -- 6 | simulation_time = 3; 7 | number_of_steps = ceil(simulation_time / trailer_controller.model.step_size); 8 | % setup a simulator to test 9 | 10 | %% forbes panoc specific stuff 11 | opt_zerofpr2.tol = 0.001; 12 | opt_zerofpr2.display = 0; 13 | opt_zerofpr2.maxit = 1000; 14 | opt_zerofpr2.adaptive = 1; 15 | opt_zerofpr2.solver = 'zerofpr2'; 16 | opt_zerofpr2.method = 'lbfgs'; 17 | opt_zerofpr2.linesearch = 'backtracking'; 18 | opt_zerofpr2.report = 1; 19 | g = indBox(-4, 4); 20 | inputs = repmat(zeros(trailer_controller.model.number_of_inputs, 1), ... 21 | trailer_controller.horizon, 1); 22 | 23 | %% 24 | state = initial_state; 25 | state_history = zeros(trailer_controller.model.number_of_states, number_of_steps); 26 | time_history = zeros(number_of_steps,1); 27 | iteration_history = zeros(number_of_steps,1); 28 | 29 | for i=1:number_of_steps 30 | cost = @(inputs_horizon) simulator.evaluate_cost_gradient(... 31 | state,reference_state,reference_input,inputs_horizon); 32 | f = smoothFunction(cost); 33 | to=tic; 34 | out_zerofpr2 = forbes(f, g, inputs, [], [], opt_zerofpr2); 35 | time_history(i)=toc(to)*1000;% get time in ms 36 | iteration_history(i)=out_zerofpr2.solver.iterations; 37 | inputs = out_zerofpr2.x; 38 | 39 | optimal_input=out_zerofpr2.x(1:trailer_controller.model.number_of_inputs); 40 | if(shift_horizon) 41 | % shift the iputs in prepartion of the next iteration 42 | inputs(1:end-trailer_controller.model.number_of_inputs) = ... 43 | inputs(trailer_controller.model.number_of_inputs+1:end); 44 | end 45 | disp(['The optimal input is[' num2str(optimal_input(1)) ' ; ' num2str(optimal_input(2)) ']']); 46 | 47 | state = trailer_controller.model.get_next_state_double(state, optimal_input)+((rand - 0.5)*2)*noise_amplitude; 48 | state_history(:, i) = state; 49 | end 50 | 51 | disp("Final state:") 52 | disp(state) 53 | 54 | clear('sim'); % remove the simulator so it unloads the shared lib 55 | end 56 | 57 | -------------------------------------------------------------------------------- /old_code/demos/Matlab/tables/.gitignore: -------------------------------------------------------------------------------- 1 | *.tex 2 | -------------------------------------------------------------------------------- /old_code/demos/Matlab/tables/README.md: -------------------------------------------------------------------------------- 1 | Ouput file tables Matlab scripts 2 | -------------------------------------------------------------------------------- /old_code/demos/python/.gitignore: -------------------------------------------------------------------------------- 1 | *.png -------------------------------------------------------------------------------- /old_code/demos/python/OneCircTwoRectTrailer.py: -------------------------------------------------------------------------------- 1 | import sys 2 | sys.path.insert(0, '../../src_python') 3 | import nmpccodegen as nmpc 4 | import nmpccodegen.tools as tools 5 | import nmpccodegen.models as models 6 | import nmpccodegen.controller as controller 7 | import nmpccodegen.controller.obstacles as obstacles 8 | import nmpccodegen.Cfunctions as cfunctions 9 | import nmpccodegen.example_models as example_models 10 | 11 | import numpy as np 12 | import matplotlib.pyplot as plt 13 | import math 14 | 15 | from demo import prepare_demo_trailer,simulate_demo 16 | 17 | if __name__ == '__main__': 18 | step_size=0.05 19 | 20 | # Q and R matrixes determined by the control engineer. 21 | Q = np.diag([1., 1., 0.0])*0.2 22 | R = np.diag([1., 1.]) * 0.01 23 | 24 | trailer_controller = prepare_demo_trailer(step_size,Q,R) 25 | 26 | trailer_controller.horizon = 50 # NMPC parameter 27 | trailer_controller.integrator_casadi = True # optional feature that can generate the integrating used in the cost function 28 | trailer_controller.panoc_max_steps = 2000 # the maximum amount of iterations the PANOC algorithm is allowed to do. 29 | trailer_controller.min_residual=-3 30 | trailer_controller.lbgfs_buffer_size = 50 31 | # trailer_controller.pure_prox_gradient=True 32 | 33 | # construct upper rectangular 34 | rectangular_up = obstacles.Rectangular(trailer_controller.model,np.array([1,0.5]),0.4,0.5) 35 | 36 | # construct lower rectangular 37 | rectangular_down = obstacles.Rectangular(trailer_controller.model,np.array([1, -0.2]), 0.4, 0.5) 38 | 39 | # construct circle 40 | circle = obstacles.Circular(trailer_controller.model,np.array([0.2,0.2]),0.2) 41 | 42 | # add obstacles to controller 43 | trailer_controller.add_constraint(rectangular_up) 44 | trailer_controller.add_constraint(rectangular_down) 45 | trailer_controller.add_constraint(circle) 46 | 47 | # generate the dynamic code 48 | trailer_controller.generate_code() 49 | 50 | # simulate everything 51 | initial_state = np.array([-0.1, -0.1,math.pi/4]) 52 | reference_state = np.array([1.5, 0.4, 0]) 53 | reference_input = np.array([0, 0]) 54 | 55 | obstacle_weights = [1e3,1e3,1e3] 56 | 57 | state_history = simulate_demo(trailer_controller,initial_state,reference_state,reference_input,obstacle_weights) 58 | 59 | plt.figure(0) 60 | example_models.trailer_print(state_history) 61 | circle.plot() 62 | rectangular_up.plot() 63 | rectangular_down.plot() 64 | plt.xlim([-0.2, 1.6]) 65 | plt.ylim([-0.5, 1.]) 66 | plt.xlabel('x') 67 | plt.xlabel('y') 68 | plt.title('Trailer') 69 | plt.savefig('OneCricleTwoRectTrailer.png') 70 | plt.show() -------------------------------------------------------------------------------- /old_code/demos/python/README.md: -------------------------------------------------------------------------------- 1 | # demo's demonstrating different internal parts of the lib 2 | If you are looking for a simple example, look in the user manual! -------------------------------------------------------------------------------- /old_code/demos/python/TwoCircTrailer.py: -------------------------------------------------------------------------------- 1 | import sys 2 | sys.path.insert(0, '../../src_python') 3 | import nmpccodegen as nmpc 4 | import nmpccodegen.tools as tools 5 | import nmpccodegen.models as models 6 | import nmpccodegen.controller as controller 7 | import nmpccodegen.controller.obstacles as obstacles 8 | import nmpccodegen.Cfunctions as cfunctions 9 | import nmpccodegen.example_models as example_models 10 | 11 | import numpy as np 12 | import matplotlib.pyplot as plt 13 | import math 14 | 15 | from demo import prepare_demo_trailer,simulate_demo 16 | 17 | if __name__ == '__main__': 18 | step_size=0.03 19 | 20 | # Q and R matrixes determined by the control engineer. 21 | Q = np.diag([1., 1., 0.0])*0.2 22 | R = np.diag([1., 1.]) * 0.01 23 | 24 | Q_terminal = np.diag([1., 1., 0.0])*2 25 | R_terminal = np.diag([1., 1.]) * 0.01 26 | 27 | trailer_controller = prepare_demo_trailer(step_size,Q,R,Q_terminal,R_terminal) 28 | 29 | trailer_controller.horizon = 50 # NMPC parameter 30 | trailer_controller.integrator_casadi = True # optional feature that can generate the integrating used in the cost function 31 | trailer_controller.panoc_max_steps = 500 # the maximum amount of iterations the PANOC algorithm is allowed to do. 32 | trailer_controller.min_residual=-3 33 | 34 | # construct left circle 35 | left_circle = obstacles.Circular(trailer_controller.model,np.array([0.2,0.2]),0.2) 36 | 37 | # construct right circle 38 | right_circle = obstacles.Circular(trailer_controller.model,np.array([0.7, 0.2]), 0.2) 39 | 40 | # add obstacles to controller 41 | trailer_controller.add_constraint(left_circle) 42 | trailer_controller.add_constraint(right_circle) 43 | 44 | # generate the dynamic code 45 | trailer_controller.generate_code() 46 | 47 | # simulate everything 48 | initial_state = np.array([0.2, 0.6, 0]) 49 | reference_state = np.array([0.7, -0.02, math.pi/2]) 50 | reference_input = np.array([0, 0]) 51 | 52 | obstacle_weights = [1000.,1000.] 53 | 54 | state_history = simulate_demo(trailer_controller,initial_state,reference_state,reference_input,obstacle_weights) 55 | 56 | plt.figure(0) 57 | example_models.trailer_print(state_history) 58 | left_circle.plot() 59 | right_circle.plot() 60 | plt.xlim([0, 1]) 61 | plt.ylim([-0.3, 1]) 62 | plt.xlabel('x') 63 | plt.xlabel('y') 64 | plt.title('Trailer') 65 | plt.show() -------------------------------------------------------------------------------- /old_code/demos/python_internal/chain_dyn_casadi_demo.py: -------------------------------------------------------------------------------- 1 | # a simple test of the chain dynamics: 2 | # if the input is held at zero the chain should come to a resting position 3 | 4 | import sys 5 | sys.path.insert(0, '../../src_python') 6 | import nmpccodegen as nmpc 7 | import nmpccodegen.models as models 8 | import nmpccodegen.example_models as example_models 9 | 10 | import math 11 | import numpy as np 12 | import matplotlib.pyplot as plt 13 | 14 | # initial state: 15 | initial_state = np.array([ 0.2, 0. , 0.4,0. , 0.6 , 0. , 0.8 , 0. \ 16 | ,1. , 0., \ 17 | 0.,0., 0.,0.,0.,0.,0.,0. ]) 18 | initial_state = initial_state.reshape([18,1]) 19 | input = np.zeros([2,1]) # do not change the handle 20 | 21 | step_size=0.01 22 | simulation_time=5 23 | number_of_steps= math.ceil(simulation_time/step_size) 24 | 25 | # get the continuous system equations from the existing library 26 | (system_equations, number_of_states, number_of_inputs, coordinates_indices) = nmpc.get_chain_model() 27 | dimension=2 28 | number_of_balls = 4 29 | 30 | integrator="RK44" 31 | g=0 # is not used here so just put it on zero 32 | model =models.Model_continious(system_equations,g,step_size,number_of_states,number_of_inputs,coordinates_indices,integrator) 33 | 34 | current_state = model.get_next_state(initial_state,input) 35 | for i in range(1,number_of_steps): 36 | new_current_state =model.get_next_state_numpy(current_state,input) 37 | current_state=new_current_state 38 | 39 | final_positions = np.concatenate(\ 40 | (np.zeros((dimension,1)),np.reshape(current_state[0:dimension*(number_of_balls+1)],(number_of_balls+1,dimension)).T)\ 41 | ,axis=1) 42 | 43 | print(final_positions) 44 | 45 | plt.figure(1) 46 | plt.plot(final_positions[0,:],final_positions[1,:]) 47 | plt.show() -------------------------------------------------------------------------------- /old_code/demos/python_internal/function_generator_demo.py: -------------------------------------------------------------------------------- 1 | import sys 2 | sys.path.insert(0, '../../src_python') 3 | import nmpccodegen.Cfunctions as cfunctions 4 | 5 | test = cfunctions.IndicatorBoxFunction([1,2],[3,4]) 6 | test.generate_c_code("test.c") 7 | test.prox.generate_c_code("test_prox.c") -------------------------------------------------------------------------------- /old_code/demos/python_internal/globals_generator_demo.py: -------------------------------------------------------------------------------- 1 | import sys 2 | sys.path.insert(0, '../../src_python') 3 | import nmpccodegen as nmpc 4 | import nmpccodegen.tools as tools 5 | import nmpccodegen.models as models 6 | import nmpccodegen.controller as controller 7 | import nmpccodegen.Cfunctions as cfunctions 8 | 9 | import numpy as np 10 | 11 | def main(): 12 | (system_equations, number_of_states, number_of_inputs, coordinates_indices) = \ 13 | nmpc.example_models.get_trailer_model(L=0.5) 14 | 15 | test_generator = controller.Globals_generator("./test_globals.c") 16 | 17 | step_size=0.1 18 | integrator = "RK" 19 | constraint_input = cfunctions.IndicatorBoxFunction([-1, -1], [1, 1]) # input needs stay within these borders 20 | model = models.Model_continious(system_equations, constraint_input, step_size, number_of_states, \ 21 | number_of_inputs, coordinates_indices, integrator) 22 | 23 | Q = np.diag([1., 100., 1.]) 24 | R = np.eye(model.number_of_inputs, model.number_of_inputs) * 1. 25 | 26 | stage_cost = controller.stage_costs.Stage_cost_QR(model, Q, R) 27 | 28 | # define the controller, set the controller on whatever location, as we won't generate code it doesnt matter 29 | trailer_controller = controller.Nmpc_panoc("./", model, stage_cost) 30 | 31 | test_generator.generate_globals(trailer_controller) 32 | 33 | if __name__ == "__main__": 34 | main() -------------------------------------------------------------------------------- /old_code/demos/python_internal/trailer_simulation_demo.py: -------------------------------------------------------------------------------- 1 | import sys 2 | sys.path.insert(0, '../../src_python') 3 | import nmpccodegen as nmpc 4 | import nmpccodegen.tools as tools 5 | import nmpccodegen.models as models 6 | import nmpccodegen.controller as controller 7 | import nmpccodegen.Cfunctions as cfunctions 8 | 9 | import matplotlib.pyplot as plt 10 | import math 11 | import ctypes 12 | import numpy as np 13 | 14 | # get the continious system equations 15 | (system_equations,number_of_states,number_of_inputs,coordinates_indices) = nmpc.example_models.get_trailer_model(L=0.5) 16 | 17 | step_size = 0.01 18 | simulation_time = 5 19 | number_of_steps = math.ceil(simulation_time / step_size) 20 | 21 | integrator = "RK44" 22 | constraint_input = cfunctions.IndicatorBoxFunction([-2,-2],[2,2]) # input needs stay within these borders 23 | model = models.Model_continious(system_equations, constraint_input, step_size, number_of_states,\ 24 | number_of_inputs,coordinates_indices, integrator) 25 | 26 | # simulate if for a bit 27 | initial_state=np.array([0.,0.,math.pi/2]) 28 | input = np.array([0.,1.]) 29 | 30 | test = model.get_next_state(initial_state,input) 31 | 32 | state=initial_state 33 | state_history = np.zeros((number_of_states,number_of_steps)) 34 | for i in range(1,number_of_steps): 35 | state = model.get_next_state_numpy(state,input) 36 | 37 | state_history[:,i] = np.reshape(state[:],number_of_states) 38 | 39 | print(state_history[:,0:5]) 40 | 41 | plt.figure(1) 42 | plt.subplot(211) 43 | plt.plot(state_history[0,:],state_history[1,:]) 44 | plt.subplot(212) 45 | plt.plot(state_history[2,:]) 46 | plt.show() -------------------------------------------------------------------------------- /old_code/demos/thesis/.gitignore: -------------------------------------------------------------------------------- 1 | *.c 2 | *.mexw64 3 | *.png 4 | -------------------------------------------------------------------------------- /old_code/generate_test_files.py: -------------------------------------------------------------------------------- 1 | # import numpy as np 2 | 3 | import sys 4 | sys.path.insert(0, './src_python') 5 | import nmpccodegen as nmpc 6 | import nmpccodegen.tools as tools 7 | import nmpccodegen.models as models 8 | import nmpccodegen.controller as controller 9 | import nmpccodegen.Cfunctions as cfunctions 10 | 11 | import math 12 | import casadi as cd 13 | import numpy as np 14 | 15 | import fileinput 16 | import sys 17 | 18 | # thanks stackoverflow: https://stackoverflow.com/questions/39086/search-and-replace-a-line-in-a-file-in-python 19 | def replaceAll(file,searchExp,replaceExp): 20 | for line in fileinput.input(file, inplace=1): 21 | if searchExp in line: 22 | line = line.replace(searchExp,replaceExp) 23 | sys.stdout.write(line) 24 | 25 | def main(): 26 | (system_equations, number_of_states, number_of_inputs,indices_coordinates) = nmpc.example_models.get_chain_model() 27 | dimension = 2 28 | number_of_balls = 4 29 | 30 | step_size = 0.01 31 | simulation_time = 5 32 | number_of_steps = math.ceil(simulation_time / step_size) 33 | 34 | integrator = "RK44" 35 | constraint_input = cfunctions.IndicatorBoxFunction([-2, -2], [2, 2]) # input needs stay within these borders 36 | model = models.Model_continious(system_equations, constraint_input, step_size, number_of_states, \ 37 | number_of_inputs,indices_coordinates, integrator) 38 | 39 | # Q = np.eye(model.number_of_states, model.number_of_states) 40 | Q = np.diag([0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1])*10 41 | R = np.eye(model.number_of_inputs, model.number_of_inputs) 42 | 43 | # reference_state=np.array([2,2,0]) 44 | stage_cost = controller.Stage_cost_QR(model, Q, R) 45 | 46 | # define the controller 47 | controller_location = "./" 48 | nmpc_controller = controller.Nmpc_panoc(controller_location,model,stage_cost ) 49 | 50 | nmpc_controller.horizon = 50 51 | nmpc_controller.step_size = 0.1 52 | 53 | # add the optional integrator, very useful to debug the code 54 | nmpc_controller.integrator_casadi=True 55 | 56 | nmpc_controller.generate_code() 57 | 58 | # replace the panoc dimension by 2 59 | dyn_globals_location = './globals/globals_dyn.h' 60 | replaceAll(dyn_globals_location,'DIMENSION_PANOC 100','DIMENSION_PANOC 2') 61 | 62 | if __name__ == "__main__": 63 | main() -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+Cfunctions/@Cfunction/Cfunction.m: -------------------------------------------------------------------------------- 1 | classdef Cfunction 2 | %CFUNCTION Interface of a function that can be generated as C code. 3 | % This interface is usually used on a function that is the proximal 4 | % of a ProximalFunction. More information on this in the class 5 | % ProximalFunction. 6 | methods(Abstract) 7 | generate_c_code(obj,location); 8 | end 9 | end 10 | 11 | -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+Cfunctions/@IndicatorBoxFunction/IndicatorBoxFunction.m: -------------------------------------------------------------------------------- 1 | classdef IndicatorBoxFunction < nmpccodegen.Cfunctions.ProximalFunction 2 | %INDICATORBOXFUNCTION An indicator Box function that can be used as 3 | %input constraint. 4 | % - 5 | 6 | properties 7 | lower_limits % the lower limits of the box. 8 | upper_limits % The upper limits of the box. 9 | dimension % Dimension of the box, this is equal to the length of the array's lower_limits and upper_limits. 10 | end 11 | 12 | methods 13 | function obj=IndicatorBoxFunction(lower_limits,upper_limits) 14 | % lower_limits : the lower limits of the box. 15 | % upper_limits : The upper limits of the box. 16 | 17 | obj.lower_limits=lower_limits; 18 | obj.upper_limits=upper_limits; 19 | obj.dimension = min(length(lower_limits),length(upper_limits)); 20 | 21 | obj.prox = nmpccodegen.Cfunctions.IndicatorBoxFunctionProx(lower_limits,upper_limits); 22 | end 23 | function obj=generate_c_code(obj, location) 24 | % Generate the C code at the folder sepcified in location. 25 | 26 | source_file = nmpccodegen.Cfunctions.Source_file_generator(location,'g'); 27 | source_file = source_file.open(); 28 | source_file = source_file.start_for('i','MPC_HORIZON',1); 29 | 30 | for i_dimension=1:obj.dimension 31 | source_file.write_comment_line('check if the value of the border is outside the box, if so return zero', ... 32 | 2); 33 | source_file = source_file.write_line( ['if(state[' num2str(i_dimension-1) ... 34 | ']<' num2str(obj.lower_limits(i_dimension)) ... 35 | ' || state[' num2str(i_dimension-1) ']>' ... 36 | num2str(obj.upper_limits(i_dimension)) ... 37 | '){'],2); 38 | source_file = source_file.write_line('return LARGE;',3); 39 | source_file = source_file.write_line('}',2); 40 | end 41 | 42 | source_file = source_file.write_line(['state+=' num2str(obj.dimension) ';'],2); 43 | source_file = source_file.close_for(1); 44 | 45 | source_file = source_file.write_comment_line('if the values where never outside the box, return zero',1); 46 | source_file = source_file.write_line( 'return 0;', 1); 47 | source_file.close(); 48 | end 49 | end 50 | 51 | end -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+Cfunctions/@IndicatorBoxFunctionProx/IndicatorBoxFunctionProx.m: -------------------------------------------------------------------------------- 1 | classdef IndicatorBoxFunctionProx < nmpccodegen.Cfunctions.Cfunction 2 | %INDICATORBOXFUNCTIONPROX The proximal of an indicator box function. 3 | % This is the function that the results from taking the proximal 4 | % operator with the indicator function. 5 | 6 | properties 7 | lower_limits % the lower limits of the box. 8 | upper_limits % The upper limits of the box. 9 | dimension % Dimension of the box, this is equal to the length of the array's lower_limits and upper_limits 10 | end 11 | 12 | methods 13 | function obj=IndicatorBoxFunctionProx(lower_limits,upper_limits) 14 | % lower_limits : the lower limits of the box. 15 | % upper_limits : The upper limits of the box. 16 | 17 | obj.lower_limits=lower_limits; 18 | obj.upper_limits=upper_limits; 19 | obj.dimension = min(length(lower_limits),length(upper_limits)); 20 | end 21 | function obj=generate_c_code(obj,location) 22 | % Generate the C code at the folder sepcified in location. 23 | 24 | source_file = nmpccodegen.Cfunctions.Source_file_generator(location,'proxg'); 25 | source_file = source_file.open(); 26 | source_file = source_file.start_for('i','MPC_HORIZON',1); 27 | 28 | for i_dimension=1:obj.dimension 29 | source_file = source_file.write_comment_line(... 30 | 'check if the value of the border is outside the box, if so go to the nearest point inside the box',... 31 | 2); 32 | source_file = source_file.write_line(['if(state[' num2str(i_dimension-1) ']<' num2str(obj.lower_limits(i_dimension)) '){'],2); 33 | source_file = source_file.set_output(i_dimension-1,num2str(obj.lower_limits(i_dimension)),3); 34 | source_file = source_file.write_line(['}else if(state[' num2str(i_dimension-1) ']>' num2str(obj.upper_limits(i_dimension)) '){'] ,2); 35 | source_file = source_file.set_output(i_dimension-1, num2str(obj.upper_limits(i_dimension)), 3); 36 | source_file = source_file.write_line('}else{', 2); 37 | source_file = source_file.set_output(i_dimension-1,[ 'state[' num2str(i_dimension-1) ']'], 3); 38 | source_file = source_file.write_line('}', 2); 39 | end 40 | 41 | source_file = source_file.write_line(['state+=' num2str(obj.dimension) ';'],2); 42 | source_file = source_file.close_for(1); 43 | source_file.close(); 44 | end 45 | end 46 | 47 | end 48 | 49 | -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+Cfunctions/@ProximalFunction/ProximalFunction.m: -------------------------------------------------------------------------------- 1 | classdef ProximalFunction < nmpccodegen.Cfunctions.Cfunction 2 | %PROXIMALFUNCTION A function of which the proximal operator is 3 | %defined. 4 | % When creating controller object, and input constraint must be 5 | % provided this this input constraint must be off the Proximal 6 | % function type. Nmpccodegen Provides a few example functions, 7 | % however it is possible to create your own constraint functions. 8 | % This interface must be used on your own defined functions. 9 | 10 | properties 11 | prox 12 | end 13 | end 14 | 15 | -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+controller/+constraints/@Constraint/Constraint.m: -------------------------------------------------------------------------------- 1 | classdef Constraint < matlab.mixin.Heterogeneous 2 | %OBSTACLE Abstract class that represents the minimum interface of an 3 | %general constraint 4 | % An constraint needs at least the following interface: 5 | % - evaluate_cost , evaluates the cost of the constraint 6 | properties 7 | end 8 | 9 | methods(Abstract) 10 | evaluate_cost(obj,state,input); 11 | end 12 | end 13 | 14 | -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+controller/+constraints/@Input_norm/Input_norm.m: -------------------------------------------------------------------------------- 1 | classdef Input_norm < nmpccodegen.controller.constraints.Constraint 2 | %INPUT_NORM Constraint that punishes if the 2-norm of the input is too 3 | %high 4 | properties 5 | max_norm 6 | end 7 | 8 | methods 9 | function obj = Input_norm(max_norm) 10 | obj.max_norm=max_norm; 11 | end 12 | function cost = evaluate_cost(obj,state,input) 13 | % evaluate the cost for a specific state and input 14 | % -state : array of size (n,1) that represents state 15 | % -input : array of size (n,1) that represents input 16 | norm_input = sum1(input.^2); 17 | cost = max(norm_input - obj.max_norm^2,0)^2; % if norm is higer then max_norm return penalty 18 | end 19 | end 20 | 21 | end -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+controller/+constraints/@Max_state/Max_state.m: -------------------------------------------------------------------------------- 1 | classdef Max_state < nmpccodegen.controller.constraints.Constraint 2 | %MAX_STATE 3 | % A constraint needs at least the following function: 4 | % - evaluate_cost , evaluates the cost of the constraint 5 | properties 6 | positions_state 7 | maximums_state 8 | end 9 | methods 10 | function obj = Max_state(positions_state_,maximums_state_) 11 | % Constructs a Max_state constraint object: 12 | % - positions_state: indices of states that are relevant to this 13 | % constraint 14 | % - maximums_state: the maximums of the absulute value of the relevant states 15 | obj.positions_state=positions_state_; 16 | obj.maximums_state=maximums_state_; 17 | end 18 | function cost = evaluate_cost(obj,state,input) 19 | % Evaluate the cost of the constraint, lowest possible cost is 20 | % zero. 21 | selected_states = state(obj.positions_state); 22 | 23 | % if the absolute value of the state is larger then the max, 24 | % punish ! 25 | norm_difference_states = sum1(max(0,abs(selected_states) - obj.maximums_state).^2); 26 | 27 | cost = norm_difference_states; 28 | end 29 | end 30 | end 31 | -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+controller/+obstacles/Circular.m: -------------------------------------------------------------------------------- 1 | classdef Circular < nmpccodegen.controller.obstacles.Obstacle 2 | %RECTANGLE A circular object. 3 | % A simple two-dimensional circular object. 4 | 5 | properties 6 | center_coordinates % The center coordinates of the circle 7 | radius % The radius of the circle. 8 | end 9 | 10 | methods 11 | function obj = Circular(center_coordinates,radius,model) 12 | % - center_coordinates : The center coordinates of the circle 13 | % - radius : The radius of the circle. 14 | % - model : Model of the controlled system. 15 | obj@nmpccodegen.controller.obstacles.Obstacle(model) 16 | 17 | obj.center_coordinates=center_coordinates; 18 | obj.radius=radius; 19 | end 20 | function cost = evaluate_coordinate_state_cost(obj,coordinates_state) 21 | % Evaluate the costs of the obstacle at a particular state. 22 | cost = nmpccodegen.controller.obstacles.Obstacle.trim(obj.radius ... 23 | - sqrt(sum1((obj.center_coordinates-coordinates_state).^2))... 24 | ); 25 | end 26 | function plot(obj) 27 | % Plot the obstacle on the active figure 28 | angles = 0:0.01:2*pi; 29 | plot(obj.radius * cos(angles) + obj.center_coordinates(1), ... 30 | obj.radius * sin(angles) + obj.center_coordinates(2), 'linewidth', 2, ... 31 | 'Color', 'black'); 32 | end 33 | function plot3(obj) 34 | % create sphere around orgin with radius one 35 | [x,y,z] = sphere; 36 | 37 | % amplify the radius 38 | x=x.*obj.radius; 39 | y=y.*obj.radius; 40 | z=z.*obj.radius; 41 | 42 | % shift the center 43 | x=x+obj.center_coordinates(1); 44 | y=y+obj.center_coordinates(2); 45 | z=z+obj.center_coordinates(3); 46 | 47 | surf(x,y,z); 48 | end 49 | end 50 | 51 | end -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+controller/+obstacles/Nonconvex_constraints.m: -------------------------------------------------------------------------------- 1 | classdef Nonconvex_constraints < nmpccodegen.controller.obstacles.Obstacle 2 | %NON_CONVEX_CONSTRAINTS obstacle excisting of non convex 3 | %constrains 4 | % This obstacle allows the users to easily find a custom object. 5 | 6 | properties(Access = private) 7 | constraints 8 | end 9 | methods 10 | function obj = Nonconvex_constraints(model) 11 | % - model = Model of the controlled system. 12 | obj@nmpccodegen.controller.obstacles.Obstacle(model) 13 | obj.constraints={}; 14 | end 15 | function obj = add_constraint(obj,constraint) 16 | % Add an nonconvex constraint. 17 | % constraint = Matlab function. 18 | 19 | number_of_constraints = obj.get_number_of_constraints(); 20 | if(number_of_constraints==0) 21 | obj.constraints ={constraint}; 22 | else 23 | obj.constraints{end+1} = constraint; 24 | end 25 | end 26 | function number_of_constraints = get_number_of_constraints(obj) 27 | % Get the number of nonconvex constraints. 28 | number_of_constraints = length(obj.constraints); 29 | end 30 | function value = evaluate_coordinate_state_cost(obj,coordinates_state) 31 | value = 0; 32 | % if there actually are constraints 33 | if (obj.get_number_of_constraints() ~= 0) 34 | value=1; 35 | for i=1:obj.get_number_of_constraints() 36 | h = obj.constraints{i}; 37 | value = value*nmpccodegen.controller.obstacles.Obstacle.trim( ... 38 | h(coordinates_state)... 39 | ); 40 | end 41 | end 42 | end 43 | end 44 | end 45 | 46 | -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+controller/+obstacles/Obstacle.m: -------------------------------------------------------------------------------- 1 | classdef Obstacle < nmpccodegen.controller.constraints.Constraint 2 | %OBSTACLE Abstract class that represents the minimum interface of an 3 | %obstacle. It is a special case of a constraint, as only certain states 4 | %will be used. 5 | % An obstacle needs at least the following interface: 6 | % - evaluate_cost , evaluates the cost of the soft constraint 7 | 8 | properties 9 | model 10 | end 11 | 12 | methods(Abstract) 13 | evaluate_coordinate_state_cost(obj,coordinates_state); 14 | end 15 | methods 16 | function obj = Obstacle(model) 17 | obj.model=model; 18 | end 19 | function cost = evaluate_cost(obj,state,input) 20 | cost = obj.evaluate_coordinate_state_cost( ... 21 | state(obj.model.indices_coordinates)); 22 | end 23 | end 24 | methods(Static) 25 | function tqs_x = trim(x) 26 | % Used internally by all the obstacles to trim the cost, as in 27 | % make the cost only positive. 28 | tqs_x = max(x,0); 29 | end 30 | end 31 | end 32 | 33 | -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+controller/+obstacles/Polyhedral.m: -------------------------------------------------------------------------------- 1 | classdef Polyhedral < nmpccodegen.controller.obstacles.Obstacle 2 | %POLYHEDRAL A simple polyhedral obstacles 3 | % construct obstacle of form a[i,:]^Tb +b , for all i 4 | 5 | properties 6 | a % The matrix a of a[i,:]^Tb +b , for all i 7 | b % The matrix b of a[i,:]^Tb +b , for all i 8 | number_of_constraints 9 | dimension % The dimension of the polyhedral derived from the matrix a. 10 | end 11 | 12 | methods 13 | function obj = Polyhedral(a,b,model) 14 | % a = The matrix a of a[i,:]^Tb +b , for all i 15 | % b = The matrix b of a[i,:]^Tb +b , for all i 16 | % - model = Model of the controlled system. 17 | obj@nmpccodegen.controller.obstacles.Obstacle(model) 18 | 19 | obj.a=a; 20 | obj.b=b; 21 | 22 | [dimension,number_of_constraints] = size(a); 23 | 24 | obj.dimension = dimension; 25 | obj.number_of_constraints = number_of_constraints; 26 | end 27 | function value = evaluate_coordinate_state_cost(obj,coordinates_state) 28 | % Evaluate the cost of the obstacle at a particular state. 29 | value=1.; 30 | for i=1:obj.number_of_constraints 31 | value = value * nmpccodegen.controller.obstacles.Obstacle.trim(... 32 | (obj.a(:,i)'*coordinates_state)+ obj.b(i)... 33 | ); 34 | end 35 | end 36 | end 37 | 38 | end 39 | 40 | -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+controller/+obstacles/Rectangular.m: -------------------------------------------------------------------------------- 1 | classdef Rectangular < nmpccodegen.controller.obstacles.Obstacle 2 | %RECTANGULAR A simple rectangular obstacle. 3 | % A simple two-dimensional rectangular obstacle. 4 | 5 | properties 6 | width % The width of the rectangular. 7 | height % The height of the rectangular. 8 | center_coordinates % The center coordinates of the rectangular. 9 | end 10 | 11 | methods 12 | function obj = Rectangular(center_coordinates,width,height,model) 13 | % - width = The width of the rectangular. 14 | % - height = The height of the rectangular. 15 | % - center_coordinates = The center coordinates of the rectangular. 16 | % - model = Model of the controlled system. 17 | obj@nmpccodegen.controller.obstacles.Obstacle(model) 18 | obj.center_coordinates=center_coordinates; 19 | obj.width=width; 20 | obj.height=height; 21 | end 22 | function cost = evaluate_coordinate_state_cost(obj,coordinates_state) 23 | % Evaluate the cost of the obstacle at a particular location. 24 | [x_up,x_down,y_up,y_down] = get_corner_coordinates(obj); 25 | a = ([-1. 0.; 1., 0. ;0., -1.; 0., 1.])'; 26 | b = [x_up; -x_down; y_up; -y_down]; 27 | 28 | cost = nmpccodegen.controller.obstacles.Polyhedral(a, b, obj.model).evaluate_coordinate_state_cost(coordinates_state); 29 | end 30 | function plot(obj) 31 | % Plots the rectangular obstacle to the active figure. 32 | [x_up,x_down,y_up,y_down] = get_corner_coordinates(obj); 33 | plot([x_down x_up],[y_down y_down],'linewidth', 2,'Color', 'black'); 34 | plot([x_down x_up],[y_up y_up],'linewidth', 2,'Color', 'black'); 35 | plot([x_up x_up],[y_up y_down],'linewidth', 2,'Color', 'black'); 36 | plot([x_down x_down],[y_down y_up],'linewidth', 2,'Color', 'black'); 37 | plot(y_up) 38 | end 39 | end 40 | methods (Access = private) 41 | function [x_up,x_down,y_up,y_down] = get_corner_coordinates(obj) 42 | % Internal function used to get the coordinates of the corners 43 | % of the rectangular. 44 | x_up = obj.center_coordinates(1) + obj.width / 2; 45 | x_down = obj.center_coordinates(1) - obj.width / 2; 46 | y_up = obj.center_coordinates(2) + obj.height / 2; 47 | y_down = obj.center_coordinates(2) - obj.height / 2; 48 | end 49 | end 50 | 51 | end 52 | 53 | -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+controller/@Casadi_code_generator/code_generator_options.md: -------------------------------------------------------------------------------- 1 | # this file contains the options of the code generator per casadi version 2 | ## casadi 3.4 3 | * verbose = true; 4 | * mex = false; 5 | * cpp = false; 6 | * main = false; 7 | * casadi_real = "double"; 8 | * casadi_int_type = CASADI_INT_TYPE_STR; 9 | * codegen_scalars = false; 10 | * with_header = false; 11 | * with_mem = false; 12 | * with_export = true; 13 | * with_import = false; 14 | * include_math = true; 15 | 16 | ## casadi 3.3 17 | * verbose = true; 18 | * mex = false; 19 | * cpp = false; 20 | * main = false; 21 | * casadi_real = "double"; 22 | * codegen_scalars = false; 23 | * with_header = false; 24 | * with_mem = false; 25 | * with_export = true; 26 | 27 | ## casadi 3.2 28 | * verbose = true; 29 | * mex = false; 30 | * cpp = false; 31 | * main = false; 32 | * real_t = "double"; 33 | * codegen_scalars = false; 34 | * with_header = false; 35 | * with_mem = false; 36 | -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+controller/@Stage_cost_QR/Stage_cost_QR.m: -------------------------------------------------------------------------------- 1 | classdef Stage_cost_QR 2 | %STAGE_COST_QR A simple QR stage cost 3 | % The stage cost exists out of two matrixes Q and R. 4 | % - Q: Matrix penalty on the states. 5 | % - R: Matrix bounty on the inputs. 6 | % The total stage cost can then be represented by: x^T Q x + u^T R u 7 | 8 | properties 9 | Q % Matrix penalty on the states. 10 | R % Matrix penalty on the inputs. 11 | model % Model object of the system. 12 | end 13 | 14 | methods 15 | function obj = Stage_cost_QR(model,Q,R) 16 | obj.Q=Q; 17 | obj.R=R; 18 | obj.model=model; 19 | end 20 | function stage_cost=evaluate_cost(obj,state,input,iteration_index,... 21 | state_reference,input_reference) 22 | % Evaluates the stage cost at a certain state. 23 | % - state = current state 24 | % - input = Input applied to get the next state. 25 | % - state_reference = The preferred state. 26 | % - input_reference = The preferred inputs. 27 | stage_cost=0; 28 | 29 | for i_col=1:obj.model.number_of_states 30 | for i_row=1:obj.model.number_of_states 31 | stage_cost = stage_cost + (state(i_col)-state_reference(i_col))*... 32 | obj.Q(i_col,i_row)*... 33 | (state(i_row)-state_reference(i_row)); 34 | end 35 | end 36 | 37 | for i_col=1:obj.model.number_of_inputs 38 | for i_row=1:obj.model.number_of_inputs 39 | stage_cost = stage_cost + (input(i_col)-input_reference(i_col))*... 40 | obj.R(i_col,i_row)*... 41 | (input(i_row)-input_reference(i_row)); 42 | end 43 | end 44 | end 45 | end 46 | 47 | end 48 | 49 | -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+example_models/chain_dyn.m: -------------------------------------------------------------------------------- 1 | function [dx, dQ] = chain_dyn(x, u, dim, M, m, D, L, g, bet, gam, del, x_ref) 2 | % CHAIN_DYN: compute the dynamics for the system consisting of M masses, 3 | % attached in a chain, with one end fixed (therefore pos_0 = 0, vel_0 = 0) 4 | % and the other end which is used to control the system 5 | % (therefore u = vel_{M+1}). 6 | % 7 | % x: state vector (pos_1, ..., pos_M, pos_{M+1}, vel_1, ..., vel_M) 8 | % u: input vector 9 | % 10 | % Model Parameters: 11 | % 12 | % dim: dimension of the space 13 | % M: number of balls composing the chain 14 | % m: mass of each of the balls 15 | % D: spring constant 16 | % L: rest length of the springs 17 | % g: gravity acceleration 18 | % 19 | % Optimal control problem parameters: 20 | % 21 | % bet: 22 | % gam: 23 | % del: 24 | % x_ref: 25 | % 26 | 27 | % rearrange positions as columns for convenience 28 | X = reshape(x(1:dim*(M+1)), dim, M+1); 29 | 30 | % take the sub-vector of velocities 31 | v = x(dim*(M+1)+1:end); 32 | 33 | % compute distances between the masses 34 | dist_vect = [X(:, 1), X(:, 2:end)-X(:, 1:end-1)]; 35 | dist_norm = sqrt(sum(dist_vect.*dist_vect, 1)); 36 | 37 | % compute forces 38 | F = dist_vect*diag(D*(1-L./dist_norm)); 39 | fs = (F(:, 2:end)-F(:, 1:end-1))/m + repmat(g, 1, M); fs = fs(:); 40 | 41 | % compute derivative of the state 42 | dx = [v; u; fs]; 43 | 44 | if nargout > 1 45 | % compute derivative of the cost function 46 | x_end = x(M*dim+1:(M+1)*dim); 47 | dQ = bet*norm(x_end - x_ref)^2 + gam*norm(v)^2 + del*norm(u)^2; 48 | end 49 | -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+example_models/get_quadcopter_model.m: -------------------------------------------------------------------------------- 1 | function [ system_equations, number_of_states, number_of_inputs, coordinates_indices ] = get_quadcopter_model( ) 2 | %GET_QUADCOPTER_MODEL Get the parameters required to build a model of a 3 | %quadcopter. 4 | % Return parameters: 5 | % system_equations : system equation in the form x_dot = f(x,u) 6 | % number_of_states : number of states 7 | % number_of_inputs : number of inputs 8 | 9 | number_of_states=12; 10 | number_of_inputs=4; 11 | coordinates_indices=[1;2;3]; 12 | 13 | params.mass_quadcopter=0.5; 14 | params.radius_quadcopter=0.25; 15 | params.propeller_lift_coefficient=3e-6; 16 | params.propeller_drag_coefficient=1e-7; 17 | params.gravity_acceleration=9.81; 18 | params.air_friction_coefficient=0.25; 19 | params.quadcopter_initeria_x_axis=5e-3; 20 | params.quadcopter_initeria_y_axis=5e-3; 21 | params.quadcopter_initeria_z_axis=1e-2; 22 | params.motor_constant=1e4; 23 | 24 | system_equations = @(state,input) nmpccodegen.example_models.quadcopter_dyn(params,state,input); 25 | end 26 | 27 | -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+example_models/get_trailer_model.m: -------------------------------------------------------------------------------- 1 | function [system_equations, number_of_states, number_of_inputs, coordinates_indices] = get_trailer_model( L ) 2 | %GET_TRAILER_MODEL Summary of this function goes here 3 | % Detailed explanation goes here 4 | number_of_states=3; 5 | number_of_inputs=2; 6 | coordinates_indices = [1 2]; % only x and y are coordinates, theta has nothing to do with position of the trailer 7 | system_equations = @(x,u) nmpccodegen.example_models.trailer_dyn(L,x,u); 8 | end 9 | 10 | -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+example_models/quadcopter_dyn.m: -------------------------------------------------------------------------------- 1 | function [ dstate ] = quadcopter_dyn( params, state , input ) 2 | %QUADCOPTER_DYN Returns dx = f(x) for the quadcopter 3 | % State 4 | % x = [x y z vx vy vz phi theta gamma omega_x omega_y omega_z]; 5 | % Input: (voltages on each motor) 6 | % u = [u_1 u_2 u_3 u_4] 7 | % The following parameters should be provided 8 | % params.mass_quadcopter; 9 | % params.radius_quadcopter; 10 | % params.propeller_lift_coefficient; 11 | % params.propeller_drag_coefficient; 12 | % params.gravity_acceleration; 13 | % params.air_friction_coefficient; 14 | % params.quadcopter_initeria_x_axis; 15 | % params.quadcopter_initeria_y_axis; 16 | % params.quadcopter_initeria_z_axis; 17 | % params.motor_constant; 18 | m = params.mass_quadcopter; 19 | L = params.radius_quadcopter; 20 | k = params.propeller_lift_coefficient; 21 | b = params.propeller_drag_coefficient; 22 | g = params.gravity_acceleration; 23 | kd = params.air_friction_coefficient; 24 | Ixx = params.quadcopter_initeria_x_axis; 25 | Iyy = params.quadcopter_initeria_y_axis; 26 | Izz = params.quadcopter_initeria_z_axis; 27 | cm = params.motor_constant; 28 | 29 | % read out the current state 30 | x=state(1);vx = state(4);phi=state(7);omega_x=state(10); 31 | y=state(2);vy = state(5);theta=state(8);omega_y=state(11); 32 | z=state(3);vz = state(6);gamma=state(9);omega_z=state(12); 33 | 34 | dstate=casadi.SX.sym('dstate',12,1); 35 | 36 | dstate(1) = vx; 37 | dstate(2) = vy; 38 | dstate(3) = vz; 39 | 40 | dstate(4) = - (kd/m)*vx + ((k*cm)/m)*(sin(gamma)*sin(phi)+cos(gamma)*cos(phi)*sin(theta))*sum(input); 41 | dstate(5) = - (kd/m)*vy + ((k*cm)/m)*(cos(phi)*sin(gamma)*sin(theta)-cos(gamma)*sin(phi))*sum(input); 42 | dstate(6) = - (kd/m)*vz -g + ((k*cm)/m)*(cos(theta)*cos(phi))*sum(input); 43 | 44 | dstate(7) = omega_x + omega_y*(sin(phi)*tan(theta)) + omega_z*(cos(phi)*tan(theta)); 45 | dstate(8) = omega_y*cos(phi) - omega_z*sin(phi); 46 | dstate(9) = omega_y*(sin(phi)/cos(theta)) + omega_z*(cos(phi)/cos(theta)); 47 | 48 | dstate(10) = ((L*k*cm)/(Ixx))*(input(1)-input(3)) - ((Iyy-Izz)/(Ixx))*omega_y*omega_z; 49 | dstate(11) = ((L*k*cm)/(Iyy))*(input(2)-input(4)) - ((Izz-Ixx)/(Iyy))*omega_x*omega_z; 50 | dstate(12) = ((b*cm)/(Izz))*(input(1) - input(2) + input(3) - input(4)) - ((Ixx-Iyy)/(Izz))*omega_x*omega_y; 51 | end 52 | 53 | -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+example_models/trailer_dyn.m: -------------------------------------------------------------------------------- 1 | function [ fxu ] = trailer_dyn(L,x,u) 2 | theta_dot = ( -u(1) * sin(x(3)) + u(2) * cos(x(3)) ) / L; 3 | fxu = [ ( u(1) + L * sin(x(3) ) * theta_dot); 4 | ( u(2) - L * cos(x(3) ) * theta_dot); 5 | theta_dot ]; 6 | end -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+example_models/trailer_printer.m: -------------------------------------------------------------------------------- 1 | function trailer_printer( state_history,amplitude,color ) 2 | %TRAILER_PRINTER Prints state history of trailer 3 | % Detailed explanation goes here 4 | 5 | u = cos(state_history(3,:))*amplitude; 6 | v = sin(state_history(3,:))*amplitude; 7 | for i=1:size(state_history,2) 8 | quiver(state_history(1,i),state_history(2,i),u(i),v(i),'color',color,'MaxHeadSize',0.5); 9 | end 10 | 11 | end 12 | 13 | -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+models/@Model_continuous/Model_continuous.m: -------------------------------------------------------------------------------- 1 | classdef Model_continuous < nmpccodegen.models.Model 2 | %MODEL_CONTINUOUS A contious discretized model describing the system behavior 3 | % - The same as a model, but the user provides a continue model and 4 | % the name of an integrator 5 | properties 6 | integrator % string that contains the name of the integrator, all the names are found in the user manual 7 | end 8 | 9 | methods 10 | function obj = Model_continuous(system_equations,input_constraint,step_size,... 11 | number_of_states,number_of_inputs,indices_coordinates,integrator) 12 | % Constructor of a continuous Model 13 | % - system_equations: The system equations, expressed as Matlab 14 | % functions in the form f(state,input). 15 | % - input_constraint: The input constraints, must be a nmpccodegen.Cfunctions.ProximalFunction object. 16 | % - step_size: The step size of the indicator. 17 | % - number_of_states: The dimension of the state. 18 | % - number_of_inputs: The dimension of the input. 19 | % - indices_coordinates: 20 | % - integrator: A string that contains the name of the integrator, all the names are found in the user manual 21 | obj = obj@nmpccodegen.models.Model(system_equations,input_constraint,step_size,... 22 | number_of_states,number_of_inputs,indices_coordinates); 23 | obj.integrator = integrator; 24 | 25 | end 26 | function next_state = get_next_state(obj,state,input) 27 | % Get the next state of the system using the input and the 28 | % current state. 29 | 30 | % define the discrete system equations 31 | function_system = @(x) obj.system_equations(x,input); 32 | discrete_system_equations = @(discrete_state) ... 33 | nmpccodegen.models.integrate( ... 34 | discrete_state,obj.step_size, ... 35 | function_system,obj.integrator); 36 | 37 | % get next state of discrete system 38 | next_state = discrete_system_equations(state); 39 | end 40 | function next_state = get_next_state_double(obj,state,input) 41 | % Get the next state of the system using the input and the 42 | % current state in doubles and not in casadi variables. 43 | 44 | next_state_casadi = get_next_state(obj,state,input); 45 | next_state = double(full(next_state_casadi(1).evalf)); 46 | end 47 | end 48 | 49 | end -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/BS5.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/BS5.mat -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/BuRK65.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/BuRK65.mat -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/CMR6.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/CMR6.mat -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/DP5.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/DP5.mat -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/FE.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/FE.mat -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/Fehlberg45.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/Fehlberg45.mat -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/Heun33.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/Heun33.mat -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/Lambert65.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/Lambert65.mat -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/MTE22.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/MTE22.mat -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/Merson43.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/Merson43.mat -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/Mid22.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/Mid22.mat -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/NSSP32.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/NSSP32.mat -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/NSSP33.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/NSSP33.mat -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/PD8.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/PD8.mat -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/RK44.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/RK44.mat -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/SSP104.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/SSP104.mat -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/SSP22.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/SSP22.mat -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/SSP22star.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/SSP22star.mat -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/SSP33.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/SSP33.mat -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/SSP53.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/SSP53.mat -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/SSP54.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/SSP54.mat -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/SSP63.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/SSP63.mat -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/SSP75.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/SSP75.mat -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/SSP85.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/SSP85.mat -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/SSP95.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_matlab/+nmpccodegen/+models/integrator_tableaus/SSP95.mat -------------------------------------------------------------------------------- /old_code/src_matlab/+nmpccodegen/+tools/@Simulation_data/Simulation_data.m: -------------------------------------------------------------------------------- 1 | classdef Simulation_data 2 | %SIMULATION_DATA results of a simulation. 3 | % After the simulator simulates, it will return an 4 | % object of this class. 5 | 6 | properties 7 | optimal_input % The optimal input that should be applied to the system. 8 | hours % The convergence time is the sum of hours/minutes/seconds/milli_seconds/micro_seconds/nano_seconds 9 | minutes % The convergence time is the sum of hours/minutes/seconds/milli_seconds/micro_seconds/nano_seconds 10 | seconds % The convergence time is the sum of hours/minutes/seconds/milli_seconds/micro_seconds/nano_seconds 11 | milli_seconds % The convergence time is the sum of hours/minutes/seconds/milli_seconds/micro_seconds/nano_seconds 12 | micro_seconds % The convergence time is the sum of hours/minutes/seconds/milli_seconds/micro_seconds/nano_seconds 13 | nano_seconds % The convergence time is the sum of hours/minutes/seconds/milli_seconds/micro_seconds/nano_seconds 14 | panoc_interations % The amount of iterations to convergence. 15 | time_string % The time till convergnce formatted as a string. 16 | end 17 | 18 | methods 19 | function obj = Simulation_data(time_struct_pointer,optimal_input) 20 | % constructor used internally by the Simulator class 21 | 22 | obj.optimal_input=optimal_input; 23 | 24 | obj.hours=time_struct_pointer.value.hours; 25 | obj.minutes=time_struct_pointer.value.minutes; 26 | obj.seconds=time_struct_pointer.value.seconds; 27 | 28 | obj.milli_seconds=time_struct_pointer.value.milli_seconds; 29 | obj.micro_seconds=time_struct_pointer.value.micro_seconds; 30 | obj.nano_seconds=time_struct_pointer.value.nano_seconds; 31 | 32 | obj.panoc_interations=time_struct_pointer.value.panoc_interations; 33 | 34 | obj.time_string = ['[' num2str(obj.hours) ':' num2str(obj.minutes) ':' num2str(obj.seconds) ']' ... 35 | ' [' num2str(obj.milli_seconds) ':' num2str(obj.micro_seconds) ':' num2str(obj.nano_seconds) ']']; 36 | end 37 | end 38 | 39 | end 40 | 41 | -------------------------------------------------------------------------------- /old_code/src_matlab/.gitignore: -------------------------------------------------------------------------------- 1 | *.asv -------------------------------------------------------------------------------- /old_code/src_python/README.md: -------------------------------------------------------------------------------- 1 | # Attention 2 | This folder contains python code related to the generation of the library. This is only relevant to developers. If your a user, nothing to see here move along. -------------------------------------------------------------------------------- /old_code/src_python/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_python/__init__.py -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/Cfunctions/Function.py: -------------------------------------------------------------------------------- 1 | class Cfunction: 2 | """ 3 | Abstact class representing any function that has a implementation in c89 4 | """ 5 | def __init__(self): 6 | raise NotImplementedError 7 | 8 | # save the implementation in c to "location" 9 | def generate_c_code(self,location): 10 | raise NotImplementedError 11 | -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/Cfunctions/ProximalFunction.py: -------------------------------------------------------------------------------- 1 | from .Function import Cfunction 2 | 3 | class ProximalFunction(Cfunction): 4 | """ 5 | Class that represents a function with its proximal mapping 6 | """ 7 | def __init__(self,prox): 8 | self._prox=prox 9 | 10 | @property 11 | def prox(self): 12 | return self._prox 13 | -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/Cfunctions/__init__.py: -------------------------------------------------------------------------------- 1 | from .ProximalFunction import ProximalFunction 2 | from .Function import Cfunction 3 | from .IndicatorBoxFunction import IndicatorBoxFunction 4 | from .IndicatorBoxFunction import IndicatorBoxFunctionProx 5 | from .source_file_operations import Source_file_generator -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/Cfunctions/readme.md: -------------------------------------------------------------------------------- 1 | # Read me 2 | This folder contains functions that can be used as g(x) in the optimization problem min f(x) + g(x). Keep in mind that the g function used in the nmpc constructor must be of the type Proximal function. So the prox{g(x)} function must also be available -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/Cfunctions/source_file_operations.py: -------------------------------------------------------------------------------- 1 | import os 2 | from pathlib import Path 3 | import time 4 | 5 | class Source_file_generator: 6 | """ 7 | Library to easily generate proximal functions 8 | """ 9 | number_of_spaces_in_tab=4 10 | def __init__(self,location,function_type): 11 | """ 12 | Parameters 13 | --------- 14 | location : target location 15 | function_type : should be either "g" or "proxg" 16 | """ 17 | self._location=location 18 | self._function_type=function_type 19 | def open(self): 20 | """ 21 | Open fil stream 22 | """ 23 | file = Path(self._location) 24 | if (file.exists()): 25 | print(self._location + " already exists, removing it before adding the new file") 26 | 27 | self._source_file = open(self._location, 'w') 28 | self._source_file.write("/* file generated on " + time.strftime("%x") + 29 | " at " + time.strftime("%H:%M:%S") + " */" + "\n\n") 30 | 31 | if (self._function_type == "g"): 32 | print("generating g-type function") 33 | self._source_file.write("real_t casadi_interface_g(const real_t* state){\n") 34 | elif (self._function_type == "proxg"): 35 | print("generating proxg-type function") 36 | self._source_file.write("void casadi_interface_proxg(real_t* state){\n") 37 | else: 38 | print("ERROR wrong function_type pick either g or proxg") 39 | self._source_file.close() 40 | def start_for(self,iterator_name,length,indent): 41 | self.write_line("size_t "+iterator_name+ ";",indent) 42 | self.write_line("for("+iterator_name+"=0;i<"+str(length)+";i++){",indent) 43 | def close_for(self,indent): 44 | self.write_line("}",indent) 45 | def write_line(self,line,indent): 46 | string_indent = " "*indent*Source_file_generator.number_of_spaces_in_tab 47 | self._source_file.write(string_indent+line+"\n") 48 | def write_define(self,name,value,indent): 49 | self.write_line("#define "+name+" "+str(value),indent) 50 | def write_comment_line(self,line,indent): 51 | self.write_line("/* "+line+" */",indent) 52 | def set_output(self,output_index,value,indent): 53 | self.write_line("state["+str(output_index)+"]="+str(value)+";",indent) 54 | def close(self): 55 | self._source_file.write("\n}\n") 56 | 57 | self._source_file.close() -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/__init__.py: -------------------------------------------------------------------------------- 1 | #__all__ = ['tools', 'model'] 2 | from .Cfunctions import * 3 | from .controller import * 4 | from .example_models import * 5 | from .models import * 6 | from .tools import * -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/controller/__init__.py: -------------------------------------------------------------------------------- 1 | from .obstacles import * 2 | from .constraints import * 3 | from .globals_generator import Globals_generator 4 | from .nmpc_panoc import Nmpc_panoc 5 | from .stage_costs import Stage_cost_QR -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/controller/constraints/Constraint.py: -------------------------------------------------------------------------------- 1 | class Constraint: 2 | """ A interface that represents the minimum interface of an 3 | general constraint 4 | A constraint needs at least the following method: 5 | - evaluate_cost , evaluates the cost of the constraint """ 6 | def evaluate_cost(self,state,input): 7 | raise NotImplementedError -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/controller/constraints/Input_norm.py: -------------------------------------------------------------------------------- 1 | import casadi as cd 2 | 3 | class Input_norm: 4 | """ Constraint that punishes if the 2-norm of the input is too high """ 5 | def __init__(self,max_norm): 6 | self._max_norm=max_norm 7 | 8 | def evaluate_cost(self,state,input): 9 | norm_input = cd.sum1(input**2) 10 | cost = cd.fmax(norm_input - self._max_norm ** 2, 0) ** 2 11 | 12 | return cost -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/controller/constraints/__init__.py: -------------------------------------------------------------------------------- 1 | from .Constraint import Constraint 2 | from .Input_norm import Input_norm -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/controller/nmpc_problem_single_shot.py: -------------------------------------------------------------------------------- 1 | import casadi as cd 2 | from .casadi_code_generator import Casadi_code_generator as ccg 3 | 4 | class Single_shot_definition: 5 | """ 6 | Single shot nmpc defintion 7 | """ 8 | def __init__(self,controller): 9 | self._controller=controller 10 | self._dimension=controller.model.number_of_inputs*controller.horizon 11 | def generate_cost_function(self): 12 | initial_state = cd.SX.sym('initial_state', self._controller.model.number_of_states, 1) 13 | state_reference = cd.SX.sym('state_reference', self._controller.model.number_of_states, 1) 14 | input_reference = cd.SX.sym('input_reference', self._controller.model.number_of_inputs, 1) 15 | static_casadi_parameters = cd.vertcat(initial_state, state_reference,input_reference) 16 | 17 | constraint_weights = cd.SX.sym('constraint_weights', self._controller.number_of_constraints, 1) 18 | 19 | input_all_steps = cd.SX.sym('input_all_steps', self._controller.model.number_of_inputs*self._controller.horizon, 1) 20 | cost=cd.SX.sym('cost',1,1) 21 | cost=0 22 | 23 | current_state=initial_state 24 | for i in range(1,self._controller.horizon+1): 25 | input = input_all_steps[(i-1)*self._controller.model.number_of_inputs:i*self._controller.model.number_of_inputs] 26 | current_state = self._controller.model.get_next_state(current_state,input) 27 | 28 | cost = cost + self._controller.stage_cost(current_state,input,i,state_reference,input_reference) 29 | cost = cost + self._controller.generate_cost_constraints(current_state,input,constraint_weights) 30 | 31 | (cost_function, cost_function_derivative_combined) = \ 32 | ccg.setup_casadi_functions_and_generate_c(static_casadi_parameters,input_all_steps,constraint_weights,cost,\ 33 | self._controller.location) 34 | 35 | return (cost_function,cost_function_derivative_combined) 36 | 37 | @property 38 | def dimension(self): 39 | return self._dimension -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/controller/obstacles/__init__.py: -------------------------------------------------------------------------------- 1 | from .obstacle import Obstacle 2 | from .polyhedral import Polyhedral 3 | from .circular import Circular 4 | from .rectangular import Rectangular 5 | from .nonconvex_constraints import Nonconvex_constraints -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/controller/obstacles/circular.py: -------------------------------------------------------------------------------- 1 | from .obstacle import Obstacle 2 | import casadi as cd 3 | import matplotlib as plt 4 | import numpy as np 5 | 6 | class Circular(Obstacle): 7 | def __init__(self,model,center_coordinates,radius): 8 | """ construct obstable of form a[i,:]^Tb +b , for all i """ 9 | super(Circular, self).__init__(model) 10 | 11 | self._center_coordinates=center_coordinates 12 | self._radius=radius 13 | 14 | def evaluate_coordinate_state_cost(self,coordinates_state): 15 | """ check how far coordinates are from center, if inside radius punish """ 16 | return Obstacle.trim_and_square(self._radius - cd.sqrt(cd.sum1((self._center_coordinates-coordinates_state)**2))) 17 | 18 | def plot(self): 19 | circle = plt.patches.Circle((self._center_coordinates[0],self._center_coordinates[1]),radius=self._radius,fill=False) 20 | ax = plt.pyplot.gca() 21 | ax.add_patch(circle) 22 | 23 | @property 24 | def radius(self): 25 | return self._radius 26 | @property 27 | def center_coordinates(self): 28 | return self._center_coordinates -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/controller/obstacles/nonconvex_constraints.py: -------------------------------------------------------------------------------- 1 | from .obstacle import Obstacle 2 | import casadi as cd 3 | 4 | class Nonconvex_constraints(Obstacle): 5 | def __init__(self,model): 6 | """ construct obstacle """ 7 | super(Nonconvex_constraints, self).__init__(model) 8 | self._constraints=[] 9 | def add_constraint(self,constraint): 10 | self._constraints.append(constraint) 11 | def evaluate_coordinate_state_cost(self,coordinates_state): 12 | """ evaluate the function h(x) """ 13 | if self.number_of_constraints == 0: 14 | return 0 15 | # if there actually are constraints: 16 | value=1. 17 | for i in range(0,self.number_of_constraints): 18 | h = self._constraints[i] 19 | value *= Obstacle.trim_and_square( \ 20 | h(coordinates_state)\ 21 | ) 22 | return value 23 | 24 | @property 25 | def number_of_constraints(self): 26 | return len(self._constraints) -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/controller/obstacles/obstacle.py: -------------------------------------------------------------------------------- 1 | import casadi as cd 2 | import numpy as np 3 | 4 | class Obstacle: 5 | def __init__(self,model): 6 | self._model=model 7 | 8 | def evaluate_cost(self,state,input): 9 | """ evaluate the function h(x) """ 10 | cost = self.evaluate_coordinate_state_cost(state[self._model.indices_coordinates]) 11 | return cost 12 | 13 | def evaluate_coordinate_state_cost(self,coordinates_state): 14 | raise NotImplementedError 15 | 16 | @staticmethod 17 | def trim_and_square(x): 18 | return cd.fmax(x,0)**2 19 | 20 | @property 21 | def model(self): 22 | return self._model -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/controller/obstacles/polyhedral.py: -------------------------------------------------------------------------------- 1 | from .obstacle import Obstacle 2 | import casadi as cd 3 | 4 | class Polyhedral(Obstacle): 5 | def __init__(self,model,a,b): 6 | """ construct obstacle of form a[i,:]^Tb +b , for all i """ 7 | super(Polyhedral, self).__init__(model) 8 | 9 | self._a=a 10 | self._b=b 11 | 12 | (dimension,number_of_constraints) = a.shape 13 | 14 | self._number_of_constraints=number_of_constraints 15 | self._dimension=dimension 16 | 17 | def evaluate_coordinate_state_cost(self,coordinates_state): 18 | """ evaluate the function h(x) """ 19 | value=1. 20 | for i in range(0,self._number_of_constraints): 21 | value *= Obstacle.trim_and_square(\ 22 | cd.dot(self._a[:,i], coordinates_state) + self._b[i]\ 23 | ) 24 | 25 | return value 26 | 27 | @property 28 | def number_of_constraints(self): 29 | return self._number_of_constraints -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/controller/obstacles/rectangular.py: -------------------------------------------------------------------------------- 1 | from .obstacle import Obstacle 2 | from .polyhedral import Polyhedral 3 | import casadi as cd 4 | import matplotlib as plt 5 | import numpy as np 6 | 7 | class Rectangular(Obstacle): 8 | def __init__(self,model,center_coordinates,width,height): 9 | """ construct obstable of form a[i,:]^Tb +b , for all i """ 10 | super(Rectangular, self).__init__(model) 11 | 12 | self._center_coordinates=center_coordinates 13 | self._width = width 14 | self._height = height 15 | 16 | def evaluate_coordinate_state_cost(self,coordinates_state): 17 | x_up = self._center_coordinates[0] + self._width / 2 18 | x_down = self._center_coordinates[0] - self._width / 2 19 | y_up = self._center_coordinates[1] + self._height / 2 20 | y_down = self._center_coordinates[1] - self._height / 2 21 | 22 | a = np.matrix([[-1., 0.], [1., 0.], [0., -1.], [0., 1.]]).T 23 | b = np.array([x_up, -x_down, y_up, -y_down]) 24 | return Polyhedral(self._model,a, b).evaluate_coordinate_state_cost(coordinates_state) 25 | def plot(self): 26 | rectangular = plt.patches.Rectangle((self._center_coordinates[0]- self._width / 2,self._center_coordinates[1]- self._height / 2),self._width,self._height,fill=False) 27 | ax = plt.pyplot.gca() 28 | ax.add_patch(rectangular) 29 | 30 | @property 31 | def width(self): 32 | return self._width 33 | @property 34 | def height(self): 35 | return self._height 36 | @property 37 | def center_coordinates(self): 38 | return self._center_coordinates -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/controller/stage_costs.py: -------------------------------------------------------------------------------- 1 | class Stage_cost_QR: 2 | """ 3 | Simple QR stage cost 4 | """ 5 | def __init__(self,model,Q,R): 6 | """ 7 | Constructs stage cost 8 | 9 | Parameters 10 | --------- 11 | model : object of nmpccodegen.models.model or nmpccodegen.models.model_continious 12 | Q : quadratic cost on the state 13 | R : quadratic cost on the input 14 | """ 15 | self._Q=Q 16 | self._R=R 17 | self._model=model 18 | def evaluate_cost(self,state,input,iteration_index, 19 | state_reference,input_reference): 20 | """ 21 | Calculate stage cost 22 | 23 | Parameters 24 | --------- 25 | state : current state of the system 26 | input : current input of the system 27 | iteration_index : step index of the discrete system 28 | state_reference : wanted state of the system 29 | input_reference : wanted input of the system 30 | 31 | Returns 32 | ------ 33 | Stage Cost (x'Qx + u'Ru) 34 | """ 35 | # As state and input are of the stype csadi.SX we can't just do vector matrix product 36 | # Everything must be written out in basic operations 37 | stage_cost=0 38 | 39 | for i_col in range(0,self._model.number_of_states): 40 | for i_row in range(0, self._model.number_of_states): 41 | stage_cost += (state[i_col]-state_reference[i_col])*\ 42 | self._Q[i_col,i_row]*\ 43 | (state[i_row]-state_reference[i_row]) 44 | 45 | for i_col in range(0,self._model.number_of_inputs): 46 | for i_row in range(0, self._model.number_of_inputs): 47 | stage_cost += (input[i_col]-input_reference[i_col])*\ 48 | self._R[i_col,i_row]*\ 49 | (input[i_row]-input_reference[i_row]) 50 | 51 | return stage_cost -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/example_models/__init__.py: -------------------------------------------------------------------------------- 1 | from .example_model import get_chain_model,get_trailer_model 2 | from .trailer_printer import trailer_print,draw_rectangular_obstacle,draw_rectangular_obstacle_around_center -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/example_models/example_model.py: -------------------------------------------------------------------------------- 1 | from .chain_model import chain_dyn,Chain_dyn_parameters 2 | from .trailer_model import Trailer_model 3 | import numpy as np 4 | import math 5 | # from ..models.model_continious import Model_continious 6 | 7 | def get_chain_model(): 8 | # model parameters(use the same as the Matlab code): 9 | dimension = 2 10 | number_of_balls = 4 # M 11 | ball_mass = 0.03 # m 12 | spring_constant = 0.1 # D 13 | rest_length_of_springs = 0.033 # L 14 | gravity_acceleration = 9.81 15 | 16 | model_params = Chain_dyn_parameters(dimension, number_of_balls, ball_mass, spring_constant, 17 | rest_length_of_springs, gravity_acceleration) 18 | 19 | system_equations = lambda state, input: chain_dyn(state, input, model_params) 20 | 21 | number_of_states = model_params.number_of_states 22 | number_of_inputs = model_params.dimension 23 | 24 | # X = [x^1 ... x^{M+1} u ] 25 | indices_coordinates=np.arange(0,dimension*(number_of_balls+1)) 26 | 27 | return (system_equations,number_of_states,number_of_inputs,indices_coordinates) 28 | 29 | def get_trailer_model(L): 30 | number_of_states=3 31 | number_of_inputs=2 32 | indices_coordinates = [0,1] # only x and y are coordinates, theta has nothing to do with position of the trailer 33 | trailer_model = Trailer_model(L) 34 | 35 | system_equations = lambda state,input: trailer_model.system_equation(state,input) 36 | 37 | return (system_equations,number_of_states,number_of_inputs,indices_coordinates) -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/example_models/trailer_model.py: -------------------------------------------------------------------------------- 1 | # import math 2 | import casadi as cd 3 | import numpy as np 4 | 5 | class Trailer_model: 6 | def __init__(self,L): 7 | self._L=L 8 | def system_equation(self,x,u): 9 | theta_dot = (-u[0] * cd.sin(x[2]) + u[1] * cd.cos(x[2])) / self._L 10 | px_dot = ( u[0] + self._L * cd.sin(x[2] ) * theta_dot) 11 | py_dot = ( u[1] - self._L * cd.cos(x[2] ) * theta_dot) 12 | 13 | x_dot = cd.vertcat(px_dot, py_dot, theta_dot) 14 | 15 | return x_dot 16 | 17 | def main(): 18 | initial_state = np.array([0., 0., 0.]) 19 | input = np.array([1., 1.]) 20 | 21 | tm = Trailer_model(L=0.5) 22 | test = tm.system_equation(initial_state, input) 23 | 24 | print(test) 25 | 26 | if __name__ == "__main__": 27 | main() -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/example_models/trailer_printer.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | import math 4 | 5 | def trailer_print(trailer_states,color="k"): 6 | """ print out the states of a trailer """ 7 | # trailer_states = np array of shape (number_of_states,number_of_steps) 8 | (number_of_states, number_of_steps) = np.shape(trailer_states) 9 | size=0.01 10 | 11 | for i in range(0,number_of_steps): 12 | x = trailer_states[0,i] 13 | y = trailer_states[1, i] 14 | dx = math.cos(trailer_states[2, i])*size 15 | dy = math.sin(trailer_states[2, i])*size 16 | 17 | plt.arrow(x, y, dx, dy, fc=color, ec=color, head_width=0.005, head_length=0.01) 18 | 19 | def draw_rectangular_obstacle(x_up,x_down,y_up,y_down): 20 | """ print an obstacle """ 21 | plt.plot([x_down,x_up,x_up,x_down,x_down,],[y_up,y_up,y_down,y_down,y_up]) 22 | def draw_rectangular_obstacle_around_center(center_coordinates,width,height): 23 | x_up = center_coordinates[0] + width / 2 24 | x_down = center_coordinates[0] - width / 2 25 | y_up = center_coordinates[1] + height / 2 26 | y_down = center_coordinates[1] - height / 2 27 | return draw_rectangular_obstacle(x_up, x_down, y_up, y_down) 28 | 29 | if __name__ == '__main__': 30 | example = np.array([[0,0,math.pi/4],[0.5,0.5,math.pi/2]]) 31 | 32 | plt.figure(0) 33 | trailer_print(example.T,[0, 1],[0, 1]) -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/models/__init__.py: -------------------------------------------------------------------------------- 1 | from .model import Model 2 | from .model_continious import Model_continious -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/models/integrator_tableaus/BS5.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_python/nmpccodegen/models/integrator_tableaus/BS5.npz -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/models/integrator_tableaus/BuRK65.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_python/nmpccodegen/models/integrator_tableaus/BuRK65.npz -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/models/integrator_tableaus/CMR6.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_python/nmpccodegen/models/integrator_tableaus/CMR6.npz -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/models/integrator_tableaus/DP5.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_python/nmpccodegen/models/integrator_tableaus/DP5.npz -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/models/integrator_tableaus/FE.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_python/nmpccodegen/models/integrator_tableaus/FE.npz -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/models/integrator_tableaus/Fehlberg45.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_python/nmpccodegen/models/integrator_tableaus/Fehlberg45.npz -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/models/integrator_tableaus/Heun33.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_python/nmpccodegen/models/integrator_tableaus/Heun33.npz -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/models/integrator_tableaus/Lambert65.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_python/nmpccodegen/models/integrator_tableaus/Lambert65.npz -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/models/integrator_tableaus/MTE22.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_python/nmpccodegen/models/integrator_tableaus/MTE22.npz -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/models/integrator_tableaus/Merson43.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_python/nmpccodegen/models/integrator_tableaus/Merson43.npz -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/models/integrator_tableaus/Mid22.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_python/nmpccodegen/models/integrator_tableaus/Mid22.npz -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/models/integrator_tableaus/NSSP32.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_python/nmpccodegen/models/integrator_tableaus/NSSP32.npz -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/models/integrator_tableaus/NSSP33.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_python/nmpccodegen/models/integrator_tableaus/NSSP33.npz -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/models/integrator_tableaus/PD8.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_python/nmpccodegen/models/integrator_tableaus/PD8.npz -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/models/integrator_tableaus/RK44.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_python/nmpccodegen/models/integrator_tableaus/RK44.npz -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/models/integrator_tableaus/SSP104.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_python/nmpccodegen/models/integrator_tableaus/SSP104.npz -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/models/integrator_tableaus/SSP22.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_python/nmpccodegen/models/integrator_tableaus/SSP22.npz -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/models/integrator_tableaus/SSP22star.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_python/nmpccodegen/models/integrator_tableaus/SSP22star.npz -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/models/integrator_tableaus/SSP33.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_python/nmpccodegen/models/integrator_tableaus/SSP33.npz -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/models/integrator_tableaus/SSP53.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_python/nmpccodegen/models/integrator_tableaus/SSP53.npz -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/models/integrator_tableaus/SSP54.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_python/nmpccodegen/models/integrator_tableaus/SSP54.npz -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/models/integrator_tableaus/SSP63.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_python/nmpccodegen/models/integrator_tableaus/SSP63.npz -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/models/integrator_tableaus/SSP75.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_python/nmpccodegen/models/integrator_tableaus/SSP75.npz -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/models/integrator_tableaus/SSP85.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_python/nmpccodegen/models/integrator_tableaus/SSP85.npz -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/models/integrator_tableaus/SSP95.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/src_python/nmpccodegen/models/integrator_tableaus/SSP95.npz -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/models/table_integrators.tex: -------------------------------------------------------------------------------- 1 | \begin{tabular}{ll} 2 | \hline 3 | key & integrator\_name \\ 4 | \hline 5 | BS5 & Bogacki-Shampine RK5(4)8 \\ 6 | BuRK65 & Butcher's RK65 \\ 7 | CMR6 & Calvo 6(5) \\ 8 | DP5 & Dormand-Prince RK5(4)7 \\ 9 | FE & Forward Euler \\ 10 | Fehlberg45 & Fehlberg RK5(4)6 \\ 11 | Heun33 & Heun RK 33 \\ 12 | Lambert65 & Lambert \\ 13 | MTE22 & Minimal Truncation Error 22 \\ 14 | Merson43 & Merson RK4(3) \\ 15 | Mid22 & Midpoint Runge-Kutta \\ 16 | NSSP32 & non-SSPRK 32 \\ 17 | NSSP33 & non-SSPRK 33 \\ 18 | PD8 & Prince-Dormand 8(7) \\ 19 | RK44 & Classical RK4 \\ 20 | SSP104 & SSPRK(10,4) \\ 21 | SSP22 & SSPRK 22 \\ 22 | SSP22star & SSPRK22star \\ 23 | SSP33 & SSPRK 33 \\ 24 | SSP53 & SSP 53 \\ 25 | SSP54 & SSP 54 \\ 26 | SSP63 & SSP 63 \\ 27 | SSP75 & SSP 75 \\ 28 | SSP85 & SSP 85 \\ 29 | SSP95 & SSP 95 \\ 30 | \hline 31 | \end{tabular} -------------------------------------------------------------------------------- /old_code/src_python/nmpccodegen/tools/__init__.py: -------------------------------------------------------------------------------- 1 | from .bootstrapper import Bootstrapper 2 | from .simulator import Simulation_data 3 | from .simulator import Simulator 4 | -------------------------------------------------------------------------------- /old_code/tutorial.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/old_code/tutorial.pdf -------------------------------------------------------------------------------- /python_interface/libpython_interface.h: -------------------------------------------------------------------------------- 1 | #include "./python_interface/timer.h" 2 | 3 | #ifndef LIBPYTHON_INTERFACE_H 4 | #define LIBPYTHON_INTERFACE_H 5 | 6 | #ifdef __cplusplus 7 | extern "C" { 8 | #endif 9 | 10 | #define real_t double 11 | 12 | /* 13 | * Simulates the controller and fill optimal_input with the optimal input. 14 | * -> returns the time till convergence 15 | */ 16 | void simulation_init(); 17 | struct Panoc_time* simulate_nmpc_panoc( real_t* current_state, 18 | real_t* optimal_inputs, 19 | real_t* state_reference, 20 | real_t* input_reference 21 | ); 22 | 23 | int get_last_full_solution(real_t* output); 24 | void simulation_cleanup(); 25 | real_t simulation_get_weight_constraints(int index_constraint); 26 | int simulation_set_weight_constraints(int index_constraint,real_t weight); 27 | int simulation_set_buffer_solution(real_t value, int index); 28 | real_t simulation_evaluate_f_df(real_t* static_casadi_parameters,real_t* input, real_t* output); 29 | real_t simulation_evaluate_f(real_t* static_casadi_parameters,real_t* input); 30 | real_t get_last_buffered_cost(void); 31 | 32 | #ifdef __cplusplus 33 | } 34 | #endif 35 | 36 | #endif -------------------------------------------------------------------------------- /python_interface/nmpc_python.c: -------------------------------------------------------------------------------- 1 | #include "stdlib.h" 2 | #include "../include/nmpc.h" 3 | #include "../PANOC/casadi_interface.h" 4 | #include "../PANOC/buffer.h" 5 | #include "../globals/globals.h" 6 | #include "timer.h" 7 | #include 8 | 9 | #include"nmpc_python.h" 10 | 11 | static struct Panoc_time* time_difference; 12 | 13 | /* 14 | * Simulates the controller and fill optimal_input with the optimal input. 15 | * -> returns the time till convergence 16 | */ 17 | void simulation_init(){ 18 | nmpc_init(); 19 | } 20 | struct Panoc_time* simulate_nmpc_panoc( real_t* current_state, 21 | real_t* optimal_inputs, 22 | real_t* state_reference, 23 | real_t* input_reference 24 | ){ 25 | panoc_timer_start(); 26 | 27 | int number_of_interations = npmc_solve(current_state,optimal_inputs,state_reference,input_reference); 28 | 29 | time_difference = panoc_timer_stop(); 30 | 31 | time_difference->panoc_interations=number_of_interations; 32 | 33 | return time_difference; 34 | } 35 | int get_last_full_solution(real_t* output){ 36 | return nmpc_get_last_full_solution(output); 37 | } 38 | void simulation_cleanup(){ 39 | nmpc_cleanup(); 40 | } 41 | 42 | real_t simulation_get_weight_constraints(int index_constraint){ 43 | return nmpc_get_weight_constraints(index_constraint); 44 | } 45 | int simulation_set_weight_constraints(int index_constraint,real_t weight){ 46 | return nmpc_set_weight_constraints(index_constraint,weight); 47 | } 48 | int simulation_set_buffer_solution(real_t value, int index){ 49 | return nmpc_set_buffer_solution(value,index); 50 | } 51 | real_t simulation_evaluate_f_df(real_t* static_casadi_parameters,real_t* input, real_t* output){ 52 | casadi_prepare_cost_function(static_casadi_parameters); 53 | return casadi_interface_f_df(input,output); 54 | } 55 | real_t simulation_evaluate_f(real_t* static_casadi_parameters,real_t* input){ 56 | casadi_prepare_cost_function(static_casadi_parameters); 57 | #ifndef SINGLE_COST_MODE 58 | return casadi_interface_f(input); 59 | #else 60 | return casadi_interface_f_df(input,NULL); 61 | #endif 62 | 63 | } 64 | real_t get_last_buffered_cost(void){ 65 | return buffer_get_current_f(); 66 | } -------------------------------------------------------------------------------- /python_interface/nmpc_python.h: -------------------------------------------------------------------------------- 1 | #include "timer.h" 2 | 3 | #ifndef NMPC_PYTHON_H 4 | #define NMPC_PYTHON_H 5 | 6 | #ifdef __cplusplus 7 | extern "C" { 8 | #endif 9 | 10 | #define real_t double 11 | 12 | #ifdef _WIN32 13 | #define EXPORT __declspec(dllexport) 14 | #else 15 | #define EXPORT 16 | #endif 17 | 18 | 19 | /* 20 | * Simulates the controller and fill optimal_input with the optimal input. 21 | * -> returns the time till convergence 22 | */ 23 | EXPORT void simulation_init(); 24 | EXPORT struct Panoc_time* simulate_nmpc_panoc( real_t* current_state, 25 | real_t* optimal_inputs, 26 | real_t* state_reference, 27 | real_t* input_reference 28 | ); 29 | 30 | EXPORT int get_last_full_solution(real_t* output); 31 | EXPORT void simulation_cleanup(); 32 | 33 | EXPORT real_t simulation_get_weight_constraints(int index_constraint); 34 | EXPORT int simulation_set_weight_constraints(int index_constraint,real_t weight); 35 | EXPORT int simulation_set_buffer_solution(real_t value, int index); 36 | EXPORT real_t simulation_evaluate_f_df(real_t* static_casadi_parameters,real_t* input, real_t* output); 37 | EXPORT real_t simulation_evaluate_f(real_t* static_casadi_parameters,real_t* input); 38 | EXPORT real_t get_last_buffered_cost(void); 39 | 40 | #ifdef __cplusplus 41 | } 42 | #endif 43 | 44 | #endif /* NMPC_PYTHON_H */ -------------------------------------------------------------------------------- /python_interface/test_timer.c: -------------------------------------------------------------------------------- 1 | #include "./timer.h" 2 | #include "stdio.h" 3 | #include "stdlib.h" 4 | #include "math.h" 5 | #include "time.h" 6 | 7 | #define NUMBER_OF_CALCS 100000000 8 | /* 9 | * compile Linux: gcc -std=c89 python_interface/timer_linux.c python_interface/test_timer.c -lm -g 10 | * compile windows MINGW: gcc -std=c89 .\python_interface\timer_windows.c python_interface/test_timer.c -lm -g 11 | */ 12 | static void print_time(const struct Panoc_time* time_difference); 13 | int main(void){ 14 | printf("START TESTING TIMER \n"); 15 | panoc_timer_start(); 16 | size_t i; 17 | double buffer=0; 18 | /* do some calcs to waste some time */ 19 | for ( i = 0; i < NUMBER_OF_CALCS; i++) 20 | { 21 | buffer += sqrt(rand()); 22 | } 23 | struct Panoc_time* time_difference = panoc_timer_stop(); 24 | print_time(time_difference); 25 | return 0; 26 | } 27 | 28 | static void print_time(const struct Panoc_time* time_difference){ 29 | printf("time executed= [%d:%d:%d %d:%d:%d] \n",\ 30 | time_difference->hours,time_difference->minutes,time_difference->seconds,\ 31 | time_difference->milli_seconds,time_difference->micro_seconds,time_difference->nano_seconds); 32 | } 33 | -------------------------------------------------------------------------------- /python_interface/timer.h: -------------------------------------------------------------------------------- 1 | #ifndef TIMER_H 2 | #define TIMER_H 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #define SECONDS_IN_HOURS 60*60 9 | #define SECONDS_IN_MINUTES 60 10 | 11 | struct Panoc_time{ 12 | int hours; 13 | int minutes; 14 | int seconds; 15 | int milli_seconds; 16 | int micro_seconds; 17 | int nano_seconds; 18 | int panoc_interations; 19 | }; 20 | 21 | void panoc_timer_start(void); 22 | struct Panoc_time* panoc_timer_stop(void); 23 | 24 | 25 | #ifdef __cplusplus 26 | } 27 | #endif 28 | 29 | #endif -------------------------------------------------------------------------------- /python_interface/timer_empty.c: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | 5 | #include "timer.h" 6 | 7 | static struct Panoc_time time_difference; 8 | 9 | void panoc_timer_start(void){ 10 | printf("Not supported platform timer will give zero ! \n"); 11 | } 12 | struct Panoc_time* panoc_timer_stop(void){ 13 | 14 | time_difference.milli_seconds = 0; 15 | time_difference.micro_seconds = 0; 16 | time_difference.nano_seconds = 0; 17 | 18 | time_difference.hours = 0; 19 | time_difference.minutes = 0; 20 | time_difference.seconds = 0; 21 | 22 | printf("Not supported platform timer will give zero ! \n"); 23 | 24 | return &time_difference; 25 | } -------------------------------------------------------------------------------- /python_interface/timer_linux.c: -------------------------------------------------------------------------------- 1 | #define _POSIX_C_SOURCE 199309L 2 | #include 3 | 4 | #include 5 | #include 6 | 7 | #include "timer.h" 8 | 9 | static struct timespec ts_start,ts_end; 10 | static struct Panoc_time time_difference; 11 | static void find_time_difference(void); 12 | static void convert_time_format(const long difference_nano_seconds,const long seconds); 13 | 14 | void panoc_timer_start(void){ 15 | /* get time at start */ 16 | clock_gettime(CLOCK_MONOTONIC, &ts_start); 17 | } 18 | struct Panoc_time* panoc_timer_stop(void){ 19 | /* get current time */ 20 | clock_gettime(CLOCK_MONOTONIC, &ts_end); 21 | 22 | find_time_difference(); 23 | 24 | return &time_difference; 25 | } 26 | 27 | static void convert_time_format(const long difference_nano_seconds,const long seconds){ 28 | /* convert nanoseconds into mili/micro/nano seconds */ 29 | time_difference.milli_seconds = (int)(difference_nano_seconds / (long) pow(10,6)); 30 | long difference_milli_seconds = (long)time_difference.milli_seconds*(long) pow(10,6); 31 | 32 | time_difference.micro_seconds = (int)((difference_nano_seconds- difference_milli_seconds) / (long) pow(10,3)); 33 | long difference_micro_seconds = (long)time_difference.micro_seconds*(long) pow(10,3); 34 | 35 | time_difference.nano_seconds = (int)(difference_nano_seconds- difference_milli_seconds - difference_micro_seconds); 36 | 37 | /* convert seconds into seconds/minutes/hours */ 38 | time_difference.hours = (int)(seconds / (long) SECONDS_IN_HOURS); 39 | long difference_hours = (long)time_difference.hours*(long) SECONDS_IN_HOURS; 40 | 41 | time_difference.minutes = (int)((seconds- difference_hours)/ (long) SECONDS_IN_MINUTES); 42 | long difference_minutes = (long)time_difference.minutes*(long) SECONDS_IN_MINUTES; 43 | 44 | time_difference.seconds = (int)((seconds- difference_hours) - difference_hours - difference_minutes); 45 | } 46 | 47 | static void find_time_difference(void){ 48 | long difference_nano_seconds = ts_end.tv_nsec - ts_start.tv_nsec; 49 | 50 | /* make sure the nano seconds are positive */ 51 | long seconds = 0; 52 | if(difference_nano_seconds<0){ 53 | difference_nano_seconds += pow(10,9); 54 | seconds+=-1; 55 | } 56 | seconds=seconds+(long)(ts_end.tv_sec - ts_start.tv_sec); 57 | 58 | convert_time_format(difference_nano_seconds, seconds); 59 | } -------------------------------------------------------------------------------- /python_interface/timer_windows.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | #include "timer.h" 7 | 8 | /* 9 | * Implementation of the Timer lib on windows using MINGW 10 | */ 11 | static struct Panoc_time time_difference; 12 | static LARGE_INTEGER t1, t2; 13 | static LARGE_INTEGER frequency; 14 | 15 | static void find_time_difference(void); 16 | 17 | void panoc_timer_start(void){ 18 | /* get ticks at start */ 19 | QueryPerformanceCounter(&t1); 20 | 21 | } 22 | struct Panoc_time* panoc_timer_stop(void){ 23 | /* get current ticks */ 24 | QueryPerformanceCounter(&t2); 25 | 26 | find_time_difference(); 27 | 28 | return &time_difference; 29 | } 30 | 31 | static void convert_time_format(long int seconds,long int remaining_ticks){ 32 | /* convert seconds into seconds/minutes/hours */ 33 | time_difference.hours =(int) (seconds / SECONDS_IN_HOURS); 34 | long difference_hours = (long)time_difference.hours*(long) SECONDS_IN_HOURS; 35 | 36 | time_difference.minutes = (int)((seconds- difference_hours)/ SECONDS_IN_MINUTES); 37 | long difference_minutes = (long)time_difference.minutes*(long) SECONDS_IN_MINUTES; 38 | 39 | time_difference.seconds = (int)((seconds- difference_hours) - difference_hours - difference_minutes); 40 | 41 | /* convert remaining ticks into milli/micro/nano seconds */ 42 | long int ticks_per_ms = frequency.QuadPart/1000; 43 | long int ticks_per_us = frequency.QuadPart/1000000; 44 | long int ticks_per_ns = frequency.QuadPart/1000000000; 45 | 46 | if(remaining_ticks>0 && ticks_per_ms>0){ 47 | time_difference.milli_seconds=remaining_ticks/ticks_per_ms; 48 | remaining_ticks = remaining_ticks % ticks_per_ms; 49 | }else 50 | time_difference.milli_seconds=0; 51 | 52 | if(remaining_ticks>0 && ticks_per_us>0){ 53 | time_difference.micro_seconds=remaining_ticks/ticks_per_us; 54 | remaining_ticks = remaining_ticks % ticks_per_us; 55 | }else 56 | time_difference.micro_seconds=0; 57 | 58 | if(remaining_ticks>0 && ticks_per_ns>0) 59 | time_difference.nano_seconds=remaining_ticks/ticks_per_ns; 60 | else 61 | time_difference.nano_seconds=0; 62 | } 63 | 64 | static void find_time_difference(void){ 65 | /* get ticks per second */ 66 | QueryPerformanceFrequency(&frequency); 67 | 68 | long int seconds = (t2.QuadPart - t1.QuadPart) / frequency.QuadPart; 69 | long int remaining_ticks = (t2.QuadPart - t1.QuadPart) % frequency.QuadPart; 70 | 71 | while(remaining_ticks<0){ 72 | seconds--; 73 | remaining_ticks+= (long double) frequency.QuadPart; 74 | } 75 | 76 | convert_time_format(seconds,remaining_ticks); 77 | } -------------------------------------------------------------------------------- /trailer_example.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/trailer_example.png -------------------------------------------------------------------------------- /trailer_example_time_log.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kul-optec/nmpc-codegen/0b96da0840504817472b2bdc62c29c98bdf29c8f/trailer_example_time_log.png -------------------------------------------------------------------------------- /tst/.gitignore: -------------------------------------------------------------------------------- 1 | bin -------------------------------------------------------------------------------- /tst/bin/README.md: -------------------------------------------------------------------------------- 1 | This file contains the test binaries, the source files are in the folder /tst/src 2 | Compile these by running make after executing Cmake. -------------------------------------------------------------------------------- /tst/python/.gitignore: -------------------------------------------------------------------------------- 1 | *.c -------------------------------------------------------------------------------- /tst/python/README.md: -------------------------------------------------------------------------------- 1 | # This dir contains python scripts used for testing -------------------------------------------------------------------------------- /tst/python/test_single_shot_chain_dyn.py: -------------------------------------------------------------------------------- 1 | import math 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | 5 | # little hack to import the local lib and not work with the one in the path 6 | import sys 7 | sys.path.insert(0, '../../src_python') 8 | import nmpc_panoc as npc 9 | import model_continious as modelc 10 | import example_models # this contains the chain example 11 | import stage_costs 12 | import Cfunctions.IndicatorBoxFunction as indbox 13 | 14 | (system_equations, number_of_states, number_of_inputs) = example_models.get_chain_model() 15 | dimension = 2 16 | number_of_balls = 4 17 | 18 | step_size = 0.01 19 | simulation_time = 5 20 | number_of_steps = math.ceil(simulation_time / step_size) 21 | horizon = number_of_steps 22 | 23 | integrator = "RK" 24 | constraint_input = indbox.IndicatorBoxFunction([-2,-2],[2,2]) # input needs stay within these borders 25 | model = modelc.Model_continious(system_equations, constraint_input, step_size, number_of_states,\ 26 | number_of_inputs, integrator) 27 | 28 | Q = np.diag([0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1])*10 29 | R = np.eye(model.number_of_inputs, model.number_of_inputs) 30 | 31 | rest_state = np.array([0.1932, -5.9190 , 0.3874,-8.8949,0.6126,-8.8949,0.8068,-5.9190 \ 32 | ,1. , 0., \ 33 | 0.,0., 0.,0.,0.,0.,0.,0.]) 34 | input=np.zeros((horizon*dimension,1)) 35 | 36 | stage_cost = stage_costs.Stage_cost_QR(model,Q,R) 37 | # define the controller 38 | nmpc_controller_location = "../../" 39 | nmpc_controller = npc.Nmpc_panoc(nmpc_controller_location,model,stage_cost ) 40 | nmpc_controller.horizon=10 41 | 42 | nmpc_controller.generate_code() 43 | 44 | cost_function = nmpc_controller.cost_function 45 | print(cost_function(rest_state*0.8,input)) 46 | print(cost_function(rest_state,input)) 47 | print(cost_function(rest_state*1.5,input)) 48 | 49 | print("---") 50 | 51 | cost_function = nmpc_controller.cost_function_derivative 52 | 53 | derivative_cost_low=cost_function(rest_state*0.8,input) 54 | derivative_cost_optimal=cost_function(rest_state,input) 55 | derivative_cost_high=cost_function(rest_state*1.5,input) 56 | 57 | print_size=2 58 | print(derivative_cost_low[0:print_size]) 59 | print(derivative_cost_optimal[0:print_size]) 60 | print(derivative_cost_high[0:print_size]) -------------------------------------------------------------------------------- /tst/python/trailer/.gitignore: -------------------------------------------------------------------------------- 1 | *.png -------------------------------------------------------------------------------- /tst/src/README.md: -------------------------------------------------------------------------------- 1 | # Test on C code 2 | This folder contains software tests on the static C code. The globals file and cost function are mocks and are generated trough python scripts. See more about this on the main page. 3 | 4 | ## Tests on the linear algebra 5 | The matrix_operations.c/.h files contain linear algebra operations. As NMPC-codegen is aimed at embedded devices it cannot use the commonly used library's for these operations. 6 | ### Tests on norms (matrix_operations_norms.c) 7 | ### Tests on vector operations (matrix_operations_vectors.c) 8 | ### Tests on matrix operations (no tests of this type yet) -------------------------------------------------------------------------------- /tst/src/example_problems.h: -------------------------------------------------------------------------------- 1 | #ifndef EXAMPLE_PROBLEMS_H 2 | #define EXAMPLE_PROBLEMS_H 3 | 4 | #include "../../globals/globals.h" 5 | #include 6 | #include 7 | 8 | /* a polynomial as f */ 9 | int f_poly_init(size_t dimension,size_t degree ); 10 | real_t f_poly(const real_t* input); 11 | void df_poly(const real_t* input,real_t* output); 12 | 13 | /* extra test function, rosenbrock function, minimum at (1,1)*/ 14 | real_t f_rosenbrock(const real_t* x); 15 | void df_rosenbrock(const real_t* x,real_t* output); 16 | 17 | /* g1 and its prox*/ 18 | int example_problems_set_init_problem1(real_t w,size_t dimension); 19 | real_t proxg_1(real_t* x); 20 | real_t proxg_2(real_t* x); 21 | 22 | int example_problems_set_init_problem3(real_t u_min,real_t u_max); 23 | real_t proxg_3(real_t* x); 24 | 25 | #endif -------------------------------------------------------------------------------- /tst/src/lipschitz_estimator_test.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include"../../PANOC/lipschitz.h" 4 | #include"../../PANOC/matrix_operations.h" 5 | #include"../../globals/globals.h" 6 | #include"../../PANOC/buffer.h" 7 | #include"example_problems.h" 8 | #include"./mocks/casadi_interface_test.h" 9 | 10 | static const real_t theoretical_solution[]={0,0}; 11 | static int degree=5; 12 | int simple_estimate_test(void); 13 | 14 | real_t f_grad_descent_poly_test(const real_t* x); 15 | void df_grad_descent_poly_test(const real_t* x ,real_t* df_x); 16 | 17 | 18 | int main(){ 19 | return simple_estimate_test(); 20 | } 21 | /* 22 | * problem: 23 | * p = x^6 + y^6 24 | * norm(gradient(f)) = sqrt((6*5*x^4)^2+(6*5*y^4)^2) 25 | * 26 | * estimate in point [ 0.5 0.5 ] 27 | * matlab solution = norm(p(0.5+10^-10,0.5+10^-10)-p(0.5,0.5))/(sqrt(2)*10^-10) = 2.6517 28 | * theoretical = sqrt((6*5*0.5^4)^2+(6*5*0.5^4)^2) = 2.6517 29 | */ 30 | int simple_estimate_test(void){ 31 | printf("Test1 --- \n"); 32 | degree=6; 33 | real_t current_location[2]={0.5,0.5}; 34 | static size_t dimension=2; 35 | real_t w=0;/* parameter used in g1, set on 0, doesnt matter here */ 36 | example_problems_set_init_problem1(w,dimension); 37 | f_poly_init(dimension,degree ); 38 | casadi_interface_test_init(dimension, 39 | proxg_1, 40 | f_poly, 41 | df_poly); 42 | buffer_init(); 43 | 44 | printf("Estimating lischitz on location x1=0.5 x2=0.5 \n"); 45 | real_t lipschitz_value; 46 | buffer_renew(current_location); 47 | lipschitz_value = get_lipschitz(); 48 | 49 | buffer_cleanup(); 50 | 51 | if(ABS(lipschitz_value-2.6517)<0.001){ 52 | printf("end of test1:SUCCESS --- \n"); 53 | return SUCCESS; 54 | }else{ 55 | printf("Estimating lipschitz value %f at [0.5 0.5] theoretical is 2.6517 \n",lipschitz_value); 56 | printf("end of test1:FAILURE --- \n"); 57 | return FAILURE; 58 | } 59 | } -------------------------------------------------------------------------------- /tst/src/matrix_operations_norms.c: -------------------------------------------------------------------------------- 1 | #include "../PANOC/matrix_operations.h" 2 | #include "../globals/globals.h" 3 | #include 4 | 5 | int test2norm(void); 6 | int test_inner_product(void); 7 | int test2norm_floating_point(void); 8 | int testnorm_floating_point(void); 9 | 10 | int main(){ 11 | return test2norm()+\ 12 | test_inner_product()+\ 13 | test2norm_floating_point()+\ 14 | testnorm_floating_point(); 15 | } 16 | 17 | /* 18 | * test the 2 norm: the 2 norm of [1 2 2 4] should be 5 19 | */ 20 | int test2norm(void){ 21 | real_t testVector[4] = {1, 2, 2, 4}; 22 | real_t testNorm = vector_norm2(testVector,4); 23 | if(testNorm==5){ 24 | return SUCCESS; 25 | }else{ 26 | printf("The 2 norm of [1; 2; 2; 4] should be 5 but is %f",testNorm); 27 | return FAILURE; /* test failed so return 1 */ 28 | } 29 | } 30 | 31 | int test2norm_floating_point(void){ 32 | real_t testVector[4] = {0.1, 0.2, 0.2, 0.4}; 33 | real_t testNorm = vector_norm2(testVector,4); 34 | if(testNorm==0.5){ 35 | return SUCCESS; 36 | }else{ 37 | printf("The 2 norm of [0.1; 0.2; 0.2; 0.4] should be 0.5 but is %f",testNorm); 38 | return FAILURE; /* test failed so return 1 */ 39 | } 40 | } 41 | 42 | int testnorm_floating_point(void){ 43 | real_t testVector[4] = {-0.1, -0.2, 0.2, 0.4}; 44 | real_t testNorm = vector_norm1(testVector,4); 45 | if(testNorm==0.9){ 46 | return SUCCESS; 47 | }else{ 48 | printf("The 1 norm of [-0.1; -0.2; 0.2; 0.4] should be 0.9 but is %f",testNorm); 49 | return FAILURE; /* test failed so return 1 */ 50 | } 51 | } 52 | 53 | 54 | int test_inner_product(void){ 55 | real_t test_vector1[4] = {1, 2, 3, 4}; 56 | real_t test_vector2[4] = {1, 2, 3, 4}; 57 | 58 | real_t inner_product_test =inner_product(test_vector1,test_vector2,4); 59 | if(inner_product_test==30){ 60 | return SUCCESS; 61 | }else{ 62 | printf("The innerproduct of [1; 2; 3; 4] with [1; 2; 3; 4] should be 30 but is %f",inner_product_test); 63 | return FAILURE; /* test failed so return 1 */ 64 | } 65 | } -------------------------------------------------------------------------------- /tst/src/mocks/README.md: -------------------------------------------------------------------------------- 1 | # This file contains mock's used in the software tests -------------------------------------------------------------------------------- /tst/src/mocks/casadi_interface_test.c: -------------------------------------------------------------------------------- 1 | #include "casadi_interface_test.h" 2 | #include "../../../PANOC/casadi_interface.h" 3 | 4 | #include "../../../globals/globals.h" 5 | #include 6 | #include 7 | 8 | static real_t (*proxg)( real_t* state); 9 | static real_t (*f)(const real_t* input); 10 | static void (*df)(const real_t* input, real_t* output); 11 | 12 | static size_t dimension; 13 | 14 | int casadi_interface_init(const real_t* current_state){return FAILURE;} 15 | size_t casadi_interface_get_dimension(){return dimension;} 16 | 17 | /* test init function */ 18 | int casadi_interface_test_init(size_t dimension_, 19 | real_t (*proxg_)(real_t* state), 20 | real_t (*f_)(const real_t* input), 21 | void (*df_)(const real_t* input, real_t* output)){ 22 | 23 | f=f_;proxg=proxg_;df=df_; 24 | dimension=dimension_; 25 | return SUCCESS; 26 | } 27 | 28 | /* cost functions */ 29 | real_t casadi_interface_f(const real_t* input){ 30 | return f(input); 31 | } 32 | void casadi_interface_df(const real_t* input,real_t* output){ 33 | df(input,output); 34 | } 35 | real_t casadi_interface_f_df(const real_t* input,real_t* output){ 36 | df(input,output); /* get gradient */ 37 | return f(input); /* get function value */ 38 | } 39 | real_t casadi_interface_g_proxg(real_t* state){ 40 | return proxg(state); 41 | } -------------------------------------------------------------------------------- /tst/src/mocks/casadi_interface_test.h: -------------------------------------------------------------------------------- 1 | #ifndef CASADI_INTERFACE_TEST_H 2 | #define CASADI_INTERFACE_TEST_H 3 | 4 | #include "../../../globals/globals.h" 5 | #include 6 | #include 7 | /* aditional init function used with mock of course */ 8 | int casadi_interface_test_init(size_t dimension_, 9 | real_t (*proxg_)(real_t* state), 10 | real_t (*f_)(const real_t* input), 11 | void (*df_)(const real_t* input, real_t* output)); 12 | 13 | #endif -------------------------------------------------------------------------------- /tst/src/mocks/lbfgs_rosenbrock_interface.h: -------------------------------------------------------------------------------- 1 | int enable_rosenbrock(void); 2 | int disable_rosenbrock(void); 3 | int lbfgs_prox_grad_descent_test_init(int dimension); 4 | int lbfgs_prox_grad_descent_test_cleanup(void); -------------------------------------------------------------------------------- /tst/src/mocks/lbfgs_test_buffer.c: -------------------------------------------------------------------------------- 1 | #include"../../../globals/globals.h" 2 | #include 3 | #include 4 | #include"buffer.h" 5 | 6 | static const real_t* current_location; 7 | 8 | int buffer_init(){ 9 | return SUCCESS; 10 | } 11 | int buffer_cleanup(){ 12 | return SUCCESS; 13 | } 14 | 15 | int buffer_renew(const real_t* current_location_){ 16 | current_location=current_location_; 17 | return SUCCESS; 18 | } 19 | 20 | real_t buffer_get_current_f(){return 0;} 21 | const real_t* buffer_get_current_location(){return current_location;} 22 | const real_t* buffer_get_current_df(){return NULL;} 23 | -------------------------------------------------------------------------------- /tst/src/mocks/lbfgs_test_proximal_gradient_descent.c: -------------------------------------------------------------------------------- 1 | #include"../../../globals/globals.h" 2 | #include"../../../PANOC/buffer.h" 3 | #include 4 | #include 5 | #include "../example_problems.h" 6 | 7 | /* functions not used by lbfgs test */ 8 | /* 9 | int proximal_gradient_descent_cleanup(void){return SUCCESS;} 10 | const real_t* proximal_gradient_descent_get_direction(const real_t* current_location){return 0;} 11 | real_t proximal_gradient_descent_forward_backward_envelop(const real_t* current_location){return 0;} 12 | 13 | */ 14 | 15 | static unsigned char rosenbrock = FALSE; 16 | int enable_rosenbrock(void){ 17 | rosenbrock=TRUE; 18 | return SUCCESS; 19 | } 20 | int disable_rosenbrock(void){ 21 | rosenbrock=FALSE; 22 | return SUCCESS; 23 | } 24 | 25 | real_t proximal_gradient_descent_get_gamma(void){return 1;} 26 | 27 | real_t* buffer; 28 | int lbfgs_prox_grad_descent_test_init(int dimension){ 29 | buffer = malloc(dimension*sizeof(real_t)); 30 | return SUCCESS; 31 | } 32 | int lbfgs_prox_grad_descent_test_cleanup(void){ 33 | free(buffer); 34 | return SUCCESS; 35 | } 36 | const real_t* proximal_gradient_descent_get_buffered_direction(void){ 37 | if(rosenbrock==TRUE) 38 | df_rosenbrock(buffer_get_current_location(),buffer); 39 | else 40 | df_poly(buffer_get_current_location(),buffer); 41 | return buffer; 42 | } 43 | 44 | /* 45 | * function used with lbgfs, replace this with a polynomial or rosenbrock function, initialize the polynomial first ! 46 | */ 47 | int proximal_gradient_descent_get_residual(const real_t* input,real_t* output){ 48 | if(rosenbrock==TRUE) 49 | df_rosenbrock(input,output); 50 | else 51 | df_poly(input,output); 52 | return SUCCESS; 53 | } 54 | int proximal_gradient_descent_get_current_residual(real_t* residual){ 55 | proximal_gradient_descent_get_residual(buffer_get_current_location(),residual); 56 | return SUCCESS; 57 | } 58 | 59 | int proximal_gradient_descent_get_new_residual(const real_t* input,real_t* output){ 60 | if(rosenbrock==TRUE) 61 | df_rosenbrock(input,output); 62 | else 63 | df_poly(input,output); 64 | return SUCCESS; 65 | } 66 | /* 67 | * compute residual using direction used by fordward backward envelop 68 | */ 69 | int proximal_gradient_descent_get_new_residual_buffered(real_t* residual){ 70 | return SUCCESS; 71 | } -------------------------------------------------------------------------------- /tst/src/prox_grad_descent_poly_test.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include"../../PANOC/proximal_gradient_descent.h" 4 | #include"../../PANOC/matrix_operations.h" 5 | #include"../../PANOC/buffer.h" 6 | #include"../../globals/globals.h" 7 | #include"example_problems.h" 8 | #include"./mocks/casadi_interface_test.h" 9 | 10 | 11 | static const real_t theoretical_solution[]={0,0}; 12 | static int degree=5; 13 | static int w=2; 14 | 15 | int checkIfSolutionIsReached(void); 16 | int checkIfSolutionIsReached_problem2(void); 17 | void print_location(const real_t* location); 18 | void print_location_2D(const real_t* location); 19 | 20 | /* 21 | * TEST proximal gradient descent 22 | * polynomial f(x1,x2,x3) = x1^4+x2^4+ x3^4+x1^3+x2^3+ x3^3 23 | * f(x) =0 if x=[0 0 0 0] 24 | */ 25 | int main(){ 26 | return checkIfSolutionIsReached(); 27 | } 28 | 29 | int checkIfSolutionIsReached(void){ 30 | printf("test1 --- \n"); 31 | const size_t dimension=2; 32 | degree=5; 33 | size_t numer_of_iterations=100; 34 | real_t current_location[2]={0.5,0.5}; 35 | 36 | printf("starting in location x1=0.5 x2=0.5 \n"); 37 | example_problems_set_init_problem1(w,dimension); 38 | f_poly_init(dimension,degree ); 39 | casadi_interface_test_init(dimension, 40 | proxg_1, 41 | f_poly, 42 | df_poly); 43 | 44 | buffer_init(); 45 | proximal_gradient_descent_init(); 46 | 47 | size_t i; 48 | for ( i = 0; i < numer_of_iterations; i++) 49 | { 50 | buffer_renew(current_location); 51 | const real_t* direction = proximal_gradient_descent_get_direction(); /* direction = old_location - new_location */ 52 | vector_add_ntimes(current_location,direction,dimension,-1); 53 | print_location_2D(current_location); 54 | } 55 | proximal_gradient_descent_cleanup(); 56 | buffer_cleanup(); 57 | 58 | if(current_location[0]<0.14){ /* theoretical value is about 0.133333 */ 59 | printf("end of test1:SUCCESS --- \n"); 60 | return SUCCESS; 61 | }else{ 62 | printf("end of test1:FAILURE --- \n"); 63 | return FAILURE; 64 | } 65 | } 66 | 67 | void print_location_2D(const real_t* location){ 68 | printf("x1=%f x2=%f \n",location[0],location[1]); 69 | } 70 | void print_location(const real_t* location){ 71 | printf("x=%f \n",location[0]); 72 | } --------------------------------------------------------------------------------