Skip to content

Commit

Permalink
Add tests for MexLtiMpc
Browse files Browse the repository at this point in the history
  • Loading branch information
@olejodd10
olejodd10 committed May 21, 2024
1 parent 76cb1e8 commit ea85671
Show file tree
Hide file tree
Showing 2 changed files with 87 additions and 0 deletions.
36 changes: 36 additions & 0 deletions test/mex/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,3 +38,39 @@ add_test(NAME TestMexMmcMpc_Compare_xoutN10_With_TestMmcMpc COMMAND python ${CMA
${CMAKE_SOURCE_DIR}/test/data/TestMexMmcMpc/xoutN10.csv ${CMAKE_SOURCE_DIR}/test/data/TestMmcMpc/xoutN10.csv ${TEST_MEX_SIMULATE_MMC_COMPARISON_LIMIT})
add_test(NAME TestMexMmcMpc_Compare_uoutN10_With_TestMmcMpc COMMAND python ${CMAKE_SOURCE_DIR}/test/scripts/compare_csvs.py
${CMAKE_SOURCE_DIR}/test/data/TestMexMmcMpc/uoutN10.csv ${CMAKE_SOURCE_DIR}/test/data/TestMmcMpc/uoutN10.csv ${TEST_MEX_SIMULATE_MMC_COMPARISON_LIMIT})


add_test(NAME TestMexLtiMpc_AllModels COMMAND matlab ${TEST_MEX_MATLAB_FLAGS} -r
"addpath('${CMAKE_BINARY_DIR}/mex'), \
addpath('${CMAKE_CURRENT_SOURCE_DIR}'), \
TestMexLtiMpc('${CMAKE_SOURCE_DIR}/test/data/QPramp/example1', '${CMAKE_SOURCE_DIR}/test/data/QPramp/example1', 10, 100), \
clear mex, \
TestMexLtiMpc('${CMAKE_SOURCE_DIR}/test/data/QPramp/example2', '${CMAKE_SOURCE_DIR}/test/data/QPramp/example2', 10, 100), \
clear mex, \
TestMexLtiMpc('${CMAKE_SOURCE_DIR}/test/data/QPramp/example3', '${CMAKE_SOURCE_DIR}/test/data/QPramp/example3', 10, 100), \
clear mex, \
TestMexLtiMpc('${CMAKE_SOURCE_DIR}/test/data/OSQP/model4', '${CMAKE_SOURCE_DIR}/test/data/OSQP/model4', 10, 100), \
clear mex, \
TestMexLtiMpc('${CMAKE_SOURCE_DIR}/test/data/OSQP/model5', '${CMAKE_SOURCE_DIR}/test/data/OSQP/model5', 10, 100), \
clear mex, \
TestMexLtiMpc('${CMAKE_SOURCE_DIR}/test/data/OSQP/model6', '${CMAKE_SOURCE_DIR}/test/data/OSQP/model6', 10, 100), \
clear mex, \
exit")

add_test(NAME TestMexLtiMpc_QPrampExample1_Verify_Uout COMMAND python ${CMAKE_SOURCE_DIR}/test/scripts/compare_csvs.py ${CMAKE_SOURCE_DIR}/test/data/QPramp/example1/uref.csv ${CMAKE_SOURCE_DIR}/test/data/QPramp/example1/uout.csv 4e-5)
add_test(NAME TestMexLtiMpc_QPrampExample1_Verify_Xout COMMAND python ${CMAKE_SOURCE_DIR}/test/scripts/compare_csvs.py ${CMAKE_SOURCE_DIR}/test/data/QPramp/example1/xref.csv ${CMAKE_SOURCE_DIR}/test/data/QPramp/example1/xout.csv 5e-5)

add_test(NAME TestMexLtiMpc_QPrampExample2_Verify_Uout COMMAND python ${CMAKE_SOURCE_DIR}/test/scripts/compare_csvs.py ${CMAKE_SOURCE_DIR}/test/data/QPramp/example2/uref.csv ${CMAKE_SOURCE_DIR}/test/data/QPramp/example2/uout.csv 3e-2)
add_test(NAME TestMexLtiMpc_QPrampExample2_Verify_Xout COMMAND python ${CMAKE_SOURCE_DIR}/test/scripts/compare_csvs.py ${CMAKE_SOURCE_DIR}/test/data/QPramp/example2/xref.csv ${CMAKE_SOURCE_DIR}/test/data/QPramp/example2/xout.csv 5e-3)

add_test(NAME TestMexLtiMpc_QPrampExample3_Verify_Uout COMMAND python ${CMAKE_SOURCE_DIR}/test/scripts/compare_csvs.py ${CMAKE_SOURCE_DIR}/test/data/QPramp/example3/uref.csv ${CMAKE_SOURCE_DIR}/test/data/QPramp/example3/uout.csv 6e-6)
add_test(NAME TestMexLtiMpc_QPrampExample3_Verify_Xout COMMAND python ${CMAKE_SOURCE_DIR}/test/scripts/compare_csvs.py ${CMAKE_SOURCE_DIR}/test/data/QPramp/example3/xref.csv ${CMAKE_SOURCE_DIR}/test/data/QPramp/example3/xout.csv 5e-5)

add_test(NAME TestMexLtiMpc_OsqpModel4_Verify_Uout COMMAND python ${CMAKE_SOURCE_DIR}/test/scripts/compare_csvs.py ${CMAKE_SOURCE_DIR}/test/data/OSQP/model4/uref.csv ${CMAKE_SOURCE_DIR}/test/data/OSQP/model4/uout.csv 6e-4)
add_test(NAME TestMexLtiMpc_OsqpModel4_Verify_Xout COMMAND python ${CMAKE_SOURCE_DIR}/test/scripts/compare_csvs.py ${CMAKE_SOURCE_DIR}/test/data/OSQP/model4/xref.csv ${CMAKE_SOURCE_DIR}/test/data/OSQP/model4/xout.csv 2e-3)

add_test(NAME TestMexLtiMpc_OsqpModel5_Verify_Uout COMMAND python ${CMAKE_SOURCE_DIR}/test/scripts/compare_csvs.py ${CMAKE_SOURCE_DIR}/test/data/OSQP/model5/uref.csv ${CMAKE_SOURCE_DIR}/test/data/OSQP/model5/uout.csv 8e-4)
add_test(NAME TestMexLtiMpc_OsqpModel5_Verify_Xout COMMAND python ${CMAKE_SOURCE_DIR}/test/scripts/compare_csvs.py ${CMAKE_SOURCE_DIR}/test/data/OSQP/model5/xref.csv ${CMAKE_SOURCE_DIR}/test/data/OSQP/model5/xout.csv 3e-3)

add_test(NAME TestMexLtiMpc_OsqpModel6_Verify_Uout COMMAND python ${CMAKE_SOURCE_DIR}/test/scripts/compare_csvs.py ${CMAKE_SOURCE_DIR}/test/data/OSQP/model6/uref.csv ${CMAKE_SOURCE_DIR}/test/data/OSQP/model6/uout.csv 2e-2)
add_test(NAME TestMexLtiMpc_OsqpModel6_Verify_Xout COMMAND python ${CMAKE_SOURCE_DIR}/test/scripts/compare_csvs.py ${CMAKE_SOURCE_DIR}/test/data/OSQP/model6/xref.csv ${CMAKE_SOURCE_DIR}/test/data/OSQP/model6/xout.csv 1e-1)
51 changes: 51 additions & 0 deletions test/mex/TestMexLtiMpc.m
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,51 @@
function TestMexLtiMpc(input_dir, output_dir, N, simulation_timesteps)
% Input data
% Matrices are transposed to be compatible with C
Q = csvread(input_dir + "/Q.csv")';
S = csvread(input_dir + "/S.csv")';
R = csvread(input_dir + "/R.csv")';
fx = csvread(input_dir + "/fx.csv")';
fu = csvread(input_dir + "/fu.csv")';
A = csvread(input_dir + "/A.csv")';
B = csvread(input_dir + "/B.csv")';
C = csvread(input_dir + "/C.csv")';
y_min = csvread(input_dir + "/y_min.csv")';
y_max = csvread(input_dir + "/y_max.csv")';
Lt = csvread(input_dir + "/_Lt.csv")';
lt = csvread(input_dir + "/lt.csv")';
u_min = csvread(input_dir + "/u_min.csv")';
u_max = csvread(input_dir + "/u_max.csv")';
x0 = csvread(input_dir + "/x0.csv")';

n_x = size(Q, 1);
n_u = size(R, 1);
n_y = size(C, 2);
n_t = size(Lt, 2);

% State and input allocations, and initial state
xout = zeros(simulation_timesteps+1, n_x, 'double');
xout(1,:) = x0;
uout = zeros(simulation_timesteps, n_u, 'double');

% Simulate
simulation_time_s = 0.0;
for i = 1:simulation_timesteps
tic;
% Solve QP
xout(i,:); % Some weird precision bug makes this necessary
[x,u] = MexLtiMpc(n_x, n_u, n_y, n_t, N, Q, S, R, fx, fu, A, B, C, y_min, y_max, Lt, lt, u_min, u_max, xout(i,:));
simulation_time_s = simulation_time_s + toc;

% Simulate using linearized discrete model
u0 = u(:,1);
xout(i+1,:) = (A'*xout(i,:)' + B'*u0)';
uout(i,:) = u(:,1)';
end

% Timer output
fprintf("%d timesteps with horizon %d finished in %f ms\n", simulation_timesteps, N, simulation_time_s*1000);

% Save output
writematrix(xout, output_dir + "/xout.csv");
writematrix(uout, output_dir + "/uout.csv");
end

0 comments on commit ea85671

Please sign in to comment.