├── fig ├── .DS_Store ├── fig1.png ├── fig2.png ├── fig3.png ├── fig4.png └── fig5.png ├── inverted_pendulum.py ├── mass_spring.py ├── README.md ├── mass_spring_collect_data.py ├── mass_spring_experiment_N=var_eps=1.py ├── inverted_pendulum_experiment_N=var_eps=1.py ├── inverted_pendulum_experiment_eps=var_N_sample=1.py ├── mass_spring_experiment_eps=var_N_sample=1.py ├── DRO_model.py ├── DRO_simulation.py └── DRO_optproblem.py /fig/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OptiMaL-PSE-Lab/Data-driven-distributionally-robust-MPC/HEAD/fig/.DS_Store -------------------------------------------------------------------------------- /fig/fig1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OptiMaL-PSE-Lab/Data-driven-distributionally-robust-MPC/HEAD/fig/fig1.png -------------------------------------------------------------------------------- /fig/fig2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OptiMaL-PSE-Lab/Data-driven-distributionally-robust-MPC/HEAD/fig/fig2.png -------------------------------------------------------------------------------- /fig/fig3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OptiMaL-PSE-Lab/Data-driven-distributionally-robust-MPC/HEAD/fig/fig3.png -------------------------------------------------------------------------------- /fig/fig4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OptiMaL-PSE-Lab/Data-driven-distributionally-robust-MPC/HEAD/fig/fig4.png -------------------------------------------------------------------------------- /fig/fig5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OptiMaL-PSE-Lab/Data-driven-distributionally-robust-MPC/HEAD/fig/fig5.png -------------------------------------------------------------------------------- /inverted_pendulum.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from DRO_model import Model 3 | from DRO_optproblem import Opt_problem 4 | from DRO_simulation import Simulation 5 | import mosek 6 | import cvxpy as cp 7 | 8 | 9 | def inverted_pendulum_ode(t, x, u): 10 | M = 1.096 11 | m = 0.109 12 | l = 0.25 13 | b = 0.1 14 | g = 9.81 15 | I = 0.0034 16 | 17 | p = I * (M + m) + M * m * l ** 2 18 | 19 | A = np.array([[0, 1, 0, 0], 20 | [0, -(I+m*l**2)*b/p, (m**2*g*l**2)/p, 0], 21 | [0, 0, 0, 1], 22 | [0, -(m*l*b)/p, m*g*l*(M+m)/p, 0]]) 23 | B = np.array([[0], [(I+m*l**2)/p], [0], [m*l/p]]) 24 | 25 | dot_x = A @ x + B @ u 26 | 27 | return dot_x 28 | 29 | 30 | if __name__ == "__main__": 31 | M = 1.096 32 | m = 0.109 33 | l = 0.25 34 | b = 0.1 35 | g = 9.81 36 | I = 0.0034 37 | 38 | p = I * (M + m) + M * m * l ** 2 39 | 40 | A = np.array([[0, 1, 0, 0], 41 | [0, -(I+m*l**2)*b/p, (m**2*g*l**2)/p, 0], 42 | [0, 0, 0, 1], 43 | [0, -(m*l*b)/p, m*g*l*(M+m)/p, 0]]) 44 | B = np.array([[0], [(I+m*l**2)/p], [0], [m*l/p]]) 45 | 46 | delta_t = 0.1 47 | 48 | # x_init = np.array([[0], [0], [-np.pi/2], [0]]) 49 | x_init = np.array([[0], [0], [-0.5], [0]]) 50 | # C = np.diag([0, 0, 0, 1e-2]) 51 | # C = np.array([[0, 0],[0, 0],[1e-2, 0],[0, 0]]) 52 | C = np.array([[0, 0],[0, 0],[0, 0],[1e-2, 0]]) 53 | D = np.array([[0, 0, 1, 0]]) 54 | # D = np.array([[1, 0, 0, 0], [0,0,1,0]]) 55 | E = np.array([[0, 1e-2]]) 56 | # E = np.array([[0, 0, 1e-2, 0]]) 57 | N = 5 58 | model = Model(A, B, C, D, E, x_init, N, delta_t) 59 | 60 | Q = np.diag([1000, 1, 1500, 1]) 61 | Qf = np.diag([1000, 1, 1500, 1]) 62 | R = np.diag([1]) 63 | 64 | d = model.d 65 | mu = np.zeros([d, 1]) 66 | beta = 0.95 67 | N_sample = 3 68 | i_th_state = 3 69 | i_state_ub = 0.5 70 | epsilon = 1 71 | 72 | sin_const = 3 73 | N_sim = 100 74 | 75 | sim = Simulation(model, Q, Qf, R, mu, x_init, beta = beta, N_sample = N_sample, i_th_state = i_th_state, i_state_ub = i_state_ub, epsilon = epsilon, 76 | sin_const = sin_const, N_sim=N_sim, mode = "gene") 77 | print(sim.x_sim) 78 | sim.plot_state() -------------------------------------------------------------------------------- /mass_spring.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from DRO_model import Model 3 | from DRO_optproblem import Opt_problem 4 | from DRO_simulation import Simulation 5 | import mosek 6 | import cvxpy as cp 7 | 8 | 9 | def mass_string_ode(t, x, u): 10 | m = 2 # [kg] 11 | k1 = 3 # [N/m] 12 | k2 = 2 # [N/m] 13 | 14 | A = np.array([[0, 1], [-k2 / m, -k1 / m]]) 15 | B = np.array([[0], [1 / m]]) 16 | 17 | dot_x = A @ x + B @ u 18 | 19 | return dot_x 20 | 21 | def gene_sample(N, d, N_sample, sin_const): 22 | # Generate data: const * sinx 23 | 24 | w_sample = [] 25 | for i in range(N_sample * N): 26 | w_temp = sin_const * np.sin(np.random.randn(d, 1)) 27 | w_sample += [w_temp] 28 | return w_sample 29 | 30 | if __name__ == "__main__": 31 | 32 | m = 2 #[kg] 33 | k1 = 3 # [N/m] 34 | k2 = 2 # [N/m] 35 | 36 | A = np.array([[0,1],[-k2/m, -k1/m]]) 37 | B = np.array([[0],[1/m]]) 38 | delta_t = 0.1 39 | 40 | x_init = np.array([[-2],[0]]) 41 | 42 | 43 | Ck = np.array([[1e-3, 0],[0, 0]]) 44 | D = np.array([[1, 0]]) 45 | E = np.array([[0,1e-3]]) 46 | N = 5 47 | 48 | model = Model(A, B, Ck, D, E, x_init, N, delta_t) 49 | 50 | 51 | Q = np.diag([10, 1]) 52 | Qf = np.diag([15, 1]) 53 | R = np.diag([1]) 54 | 55 | d = model.d 56 | mu = np.zeros([d, 1]) 57 | mu_w = np.vstack([1] + [mu] * N) 58 | M_w = mu_w @ mu_w.T + np.diag([0] + [1] * N * d) 59 | beta = 0.95 60 | N_sample = 1 61 | i_th_state = 1 62 | i_state_ub = 0.5 63 | epsilon = 1 64 | sin_const = 2 65 | N_sim = 60 66 | 67 | sim = Simulation(model, Q, Qf, R, mu, x_init, beta = beta, N_sample = N_sample, i_th_state = i_th_state, i_state_ub = i_state_ub, epsilon = epsilon, 68 | sin_const = sin_const, N_sim=N_sim, mode = "gene") 69 | print(sim.x_sim) 70 | 71 | 72 | # data_set = gene_sample(N, d, N_sample, sin_const) 73 | # N_sample_max = 10 74 | # sim = Simulation(model, Q, Qf, R, mu, x_init, beta = beta, N_sample = N_sample, i_th_state = i_th_state, i_state_ub = i_state_ub, epsilon = epsilon, 75 | # sin_const = sin_const, N_sim=N_sim, mode = "collect", data_set = data_set, N_sample_max = N_sample_max) 76 | # print(sim.x_sim) 77 | # sim.plot_state() 78 | 79 | # opt_problem = Opt_problem(model, Q, Qf, R, mu, beta = beta, N_sample = N_sample, i_th_state = i_th_state, i_state_ub = i_state_ub, epsilon = epsilon, sin_const = sin_const) 80 | # W_sample, W_sample_ext =gene_disturbance(N, d, N_sample, sin_const) 81 | # opt_problem.W_sample_matrix.value = W_sample 82 | # prob = opt_problem.prob 83 | # prob.solve(solver = cp.MOSEK) 84 | # H_cal_dec = opt_problem.H_cal_dec 85 | # print(H_cal_dec.value) -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Data-driven distributionally robust MPC using the Wasserstein metric 2 | 3 | This project contains the experimental framework for distributionally robust MPC and two case studies, per "Data-driven distributionally robust MPC using the Wasserstein metric" by Zhengang Zhong, Ehecatl Antonio del Rio-Chanona and Panagiotis Petsagkourakis, submitted to IEEE CDC 2021. 4 | 5 | ## Abstract 6 | 7 | Distributionally robust optimization is a technique for decision making under uncertainty where the probability distribution of the uncertain problem is itself subject to uncertainty. A novel data-driven MPC scheme is proposed to control constrained stochastic linear systems using distributionally robust optimization. Distributionally robust constraints based on Wasserstein ball are imposed to bound the expected state constraint violation in the presence of process disturbance. A feedback control law is solved to guarantee that the predicted states comply with constraints with regard to the worst-case distribution within the Wasserstein ball centered at the discrete empirical probability distribution. The resulting distributionally robust MPC framework is tractable and efficient. The effectiveness of the proposed scheme is demonstrated through two numerical case studies. 8 | 9 | ## How to Run Experiments 10 | 11 | ``python inverted_pendulum.py `` or ``python mass_spring.py`` to run the simulation for one realization. 12 | 13 | ``python`` other file_name to execute simulations for multiple realizations. 14 | 15 | 16 | 17 | ## Simulation results 18 | 19 | ### Simulation 1: 50 realizations of the data collection algorithm for a mass spring system initialized with different numbers of samples 20 | 21 |

22 | 23 |

24 | 25 | The shaded area denotes the 25-th to 75-th trajectory distribution. 26 | 27 | ### Simulation 2: Mass spring system averaged from 50 realizations with sample number ranging from 1 to 10 28 | 29 |

30 | 31 |

32 | 33 | ### Simulation 3: Relation between sample number and constraint violations within first four seconds in simulation 2 34 | 35 |

36 | 37 |

38 | 39 | ### Simulation 4: Inverted pendulum system averaged from 10 realizations with Wasserstein ball radius ranging from 0.01 to 100 40 | 41 |

42 | 43 |

44 | 45 | ### Simulation 5: Relation between Wasserstein ball radius and constraint violations within first two seconds in simulation 4 46 | 47 |

48 | 49 |

50 | 51 | -------------------------------------------------------------------------------- /mass_spring_collect_data.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from DRO_model import Model 3 | from DRO_simulation import Simulation 4 | import sys 5 | import os 6 | 7 | 8 | def mass_string_ode(t, x, u): 9 | m = 2 # [kg] 10 | k1 = 3 # [N/m] 11 | k2 = 2 # [N/m] 12 | 13 | A = np.array([[0, 1], [-k2 / m, -k1 / m]]) 14 | B = np.array([[0], [1 / m]]) 15 | 16 | dot_x = A @ x + B @ u 17 | 18 | return dot_x 19 | 20 | def gene_sample(N, d, N_sample, sin_const): 21 | # Generate data: const * sinx 22 | 23 | w_sample = [] 24 | for i in range(N_sample * N): 25 | w_temp = sin_const * np.sin(np.random.randn(d, 1)) 26 | w_sample += [w_temp] 27 | return w_sample 28 | 29 | if __name__ == "__main__": 30 | file_name =(os.path.splitext(sys.argv[0])[0]).split("/")[-1] 31 | 32 | m = 2 #[kg] 33 | k1 = 3 # [N/m] 34 | k2 = 2 # [N/m] 35 | 36 | A = np.array([[0,1],[-k2/m, -k1/m]]) 37 | B = np.array([[0],[1/m]]) 38 | delta_t = 0.1 39 | 40 | x_init = np.array([[-2],[0]]) 41 | 42 | 43 | Ck = np.array([[1e-3, 0],[0, 0]]) 44 | D = np.array([[1, 0]]) 45 | E = np.array([[0,1e-3]]) 46 | N = 5 47 | 48 | model = Model(A, B, Ck, D, E, x_init, N, delta_t) 49 | 50 | 51 | Q = np.diag([10, 1]) 52 | Qf = np.diag([15, 1]) 53 | R = np.diag([1]) 54 | 55 | d = model.d 56 | mu = np.zeros([d, 1]) 57 | mu_w = np.vstack([1] + [mu] * N) 58 | M_w = mu_w @ mu_w.T + np.diag([0] + [1] * N * d) 59 | beta = 0.95 60 | N_sample = 5 61 | i_th_state = 1 62 | i_state_ub = 0.4 63 | epsilon = 1 64 | sin_const = 3 65 | N_sim = 70 66 | N_loop = 50 # 50 67 | N_sample_max = 10 68 | 69 | 70 | data_set_init = gene_sample(N, d, N_sample, sin_const) 71 | 72 | 73 | 74 | # Test3: fix N_sample = 1, sin_const = 3, beta = 0.95, i_ub = 0.4, N_loop = 10, eps = 1 75 | # sample 100 trajectories with collected data 76 | result_x = [] 77 | result_u = [] 78 | for i in range(N_loop): 79 | data_set = data_set_init 80 | sim = Simulation(model, Q, Qf, R, mu, x_init, beta=beta, N_sample=N_sample, i_th_state=i_th_state, 81 | i_state_ub=i_state_ub, epsilon=epsilon, 82 | sin_const=sin_const, N_sim=N_sim, mode="collect", data_set=data_set, N_sample_max=N_sample_max) 83 | result_x += [sim.x_sim] 84 | result_u += [sim.u_sim] 85 | print("#" +str(i) + " sim " + "is done") 86 | 87 | write_path = "/Users/zhengangzhong/Dropbox/PhD/documents/paper_writing/CDC2021/result/" + file_name + "_" + "x_tra" + "_N_sample=" + str(N_sample) + ".txt" 88 | with open(write_path, 'w') as f: 89 | for listitem in result_x: 90 | f.write('%s\n' % listitem) 91 | f.close() 92 | write_path = "/Users/zhengangzhong/Dropbox/PhD/documents/paper_writing/CDC2021/result/" + file_name + "_" + "u_tra" + "_N_sample=" + str(N_sample) + ".txt" 93 | with open(write_path, 'w') as f: 94 | for listitem in result_u: 95 | f.write('%s\n' % listitem) 96 | f.close() 97 | 98 | 99 | 100 | -------------------------------------------------------------------------------- /mass_spring_experiment_N=var_eps=1.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from DRO_model import Model 3 | from DRO_simulation import Simulation 4 | import sys 5 | import os 6 | 7 | def mass_string_ode(t, x, u): 8 | m = 2 # [kg] 9 | k1 = 3 # [N/m] 10 | k2 = 2 # [N/m] 11 | 12 | A = np.array([[0, 1], [-k2 / m, -k1 / m]]) 13 | B = np.array([[0], [1 / m]]) 14 | 15 | dot_x = A @ x + B @ u 16 | 17 | return dot_x 18 | 19 | 20 | if __name__ == "__main__": 21 | 22 | file_name =(os.path.splitext(sys.argv[0])[0]).split("/")[-1] 23 | m = 2 #[kg] 24 | k1 = 3 # [N/m] 25 | k2 = 2 # [N/m] 26 | 27 | A = np.array([[0,1],[-k2/m, -k1/m]]) 28 | B = np.array([[0],[1/m]]) 29 | delta_t = 0.1 30 | 31 | x_init = np.array([[-2],[0]]) 32 | 33 | 34 | Ck = np.array([[1e-3, 0],[0, 0]]) 35 | D = np.array([[1, 0]]) 36 | E = np.array([[0,1e-3]]) 37 | N = 5 38 | 39 | model = Model(A, B, Ck, D, E, x_init, N, delta_t) 40 | 41 | 42 | # Q = np.diag([10, 1]) 43 | # Qf = np.diag([15, 1]) 44 | # R = np.diag([1]) 45 | # 46 | # d = model.d 47 | # mu = np.zeros([d, 1]) 48 | # mu_w = np.vstack([1] + [mu] * N) 49 | # M_w = mu_w @ mu_w.T + np.diag([0] + [1] * N * d) 50 | # beta = 0.95 51 | # N_sample = 3 52 | # i_th_state = 1 53 | # i_state_ub = 0.4 54 | # epsilon = 1 55 | # sin_const = 3 56 | # N_sim = 60 57 | 58 | # sim = Simulation(model, Q, Qf, R, mu, x_init, beta = beta, N_sample = N_sample, i_th_state = i_th_state, i_state_ub = i_state_ub, epsilon = epsilon, 59 | # sin_const = sin_const, N_sim=N_sim, mode = "gene") 60 | # print(sim.x_sim) 61 | 62 | Q = np.diag([10, 1]) 63 | Qf = np.diag([15, 1]) 64 | R = np.diag([1]) 65 | 66 | d = model.d 67 | mu = np.zeros([d, 1]) 68 | mu_w = np.vstack([1] + [mu] * N) 69 | M_w = mu_w @ mu_w.T + np.diag([0] + [1] * N * d) 70 | beta = 0.95 71 | N_sample = 1 72 | i_th_state = 1 73 | i_state_ub = 0.4 74 | epsilon = 1 75 | sin_const = 3 76 | N_sim = 75 77 | N_loop = 100 78 | # Test2: fix esp = 1, sin_const = 3, beta = 0.95, i_ub = 0.4, N_loop = 1. 79 | # change N_sample 80 | N_sample_test = [1, 2, 3, 4, 5, 6, 7, 8, 9, 10] 81 | for N_sample_temp in N_sample_test: 82 | result_x = [] 83 | result_u = [] 84 | N_sample = N_sample_temp 85 | for i in range(N_loop): 86 | sim = Simulation(model, Q, Qf, R, mu, x_init, beta = beta, N_sample = N_sample, i_th_state = i_th_state, 87 | i_state_ub = i_state_ub, epsilon = epsilon,sin_const = sin_const, N_sim=N_sim, mode = "gene") 88 | result_x += [sim.x_sim] 89 | result_u += [sim.u_sim] 90 | print("#" +str(i) + " sim of " + str(N_sample) + " is done") 91 | 92 | N_sample_str = str(N_sample) 93 | 94 | write_path = "/Users/zhengangzhong/Dropbox/PhD/documents/paper_writing/CDC2021/result/" + file_name + "/" + N_sample_str+ "_" + "x_tra" + ".txt" 95 | with open(write_path, 'w') as f: 96 | for listitem in result_x: 97 | f.write('%s\n' % listitem) 98 | f.close() 99 | write_path = "/Users/zhengangzhong/Dropbox/PhD/documents/paper_writing/CDC2021/result/" + file_name + "/" + N_sample_str + "_" + "u_tra" +".txt" 100 | with open(write_path, 'w') as f: 101 | for listitem in result_u: 102 | f.write('%s\n' % listitem) 103 | f.close() -------------------------------------------------------------------------------- /inverted_pendulum_experiment_N=var_eps=1.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from DRO_model import Model 3 | from DRO_simulation import Simulation 4 | import sys 5 | import os 6 | 7 | 8 | 9 | def inverted_pendulum_ode(t, x, u): 10 | M = 1.096 11 | m = 0.109 12 | l = 0.25 13 | b = 0.1 14 | g = 9.81 15 | I = 0.0034 16 | 17 | p = I * (M + m) + M * m * l ** 2 18 | 19 | A = np.array([[0, 1, 0, 0], 20 | [0, -(I+m*l**2)*b/p, (m**2*g*l**2)/p, 0], 21 | [0, 0, 0, 1], 22 | [0, -(m*l*b)/p, m*g*l*(M+m)/p, 0]]) 23 | B = np.array([[0], [(I+m*l**2)/p], [0], [m*l/p]]) 24 | 25 | dot_x = A @ x + B @ u 26 | 27 | return dot_x 28 | 29 | 30 | if __name__ == "__main__": 31 | file_name =(os.path.splitext(sys.argv[0])[0]).split("/")[-1] 32 | 33 | M = 1.096 34 | m = 0.109 35 | l = 0.25 36 | b = 0.1 37 | g = 9.81 38 | I = 0.0034 39 | 40 | p = I * (M + m) + M * m * l ** 2 41 | 42 | A = np.array([[0, 1, 0, 0], 43 | [0, -(I+m*l**2)*b/p, (m**2*g*l**2)/p, 0], 44 | [0, 0, 0, 1], 45 | [0, -(m*l*b)/p, m*g*l*(M+m)/p, 0]]) 46 | B = np.array([[0], [(I+m*l**2)/p], [0], [m*l/p]]) 47 | 48 | delta_t = 0.1 49 | 50 | # x_init = np.array([[0], [0], [-np.pi/2], [0]]) 51 | x_init = np.array([[0], [0], [-0.5], [0]]) 52 | # C = np.diag([0, 0, 0, 1e-2]) 53 | C = np.array([[0, 0],[0, 0],[0, 0],[1e-2, 0]]) 54 | D = np.array([[0, 0, 1, 0]]) 55 | # D = np.array([[1, 0, 0, 0], [0,0,1,0]]) 56 | E = np.array([[0, 1e-2]]) 57 | # E = np.array([[0, 0, 1e-2, 0]]) 58 | N = 5 59 | model = Model(A, B, C, D, E, x_init, N, delta_t) 60 | 61 | Q = np.diag([1000, 1, 1500, 1]) 62 | Qf = np.diag([1000, 1, 1500, 1]) 63 | R = np.diag([1]) 64 | 65 | d = model.d 66 | mu = np.zeros([d, 1]) 67 | beta = 0.95 68 | N_sample = 1 69 | i_th_state = 3 70 | i_state_ub = 0.5 71 | epsilon = 1 72 | sin_const = 3 73 | N_sim = 100 74 | N_loop = 10 75 | 76 | # Test2: fix esp = 1, sin_const = 3, beta = 0.95, i_ub = 0.5, N_loop = 1. 77 | # change N_sample 78 | # N_sample_test = [1, 2, 3, 4, 5, 6, 7, 8, 9, 10] 79 | N_sample_test = [3, 4, 5, 6, 7, 8, 9, 10] 80 | for N_sample_temp in N_sample_test: 81 | result_x = [] 82 | result_u = [] 83 | N_sample = N_sample_temp 84 | for i in range(N_loop): 85 | sim = Simulation(model, Q, Qf, R, mu, x_init, beta = beta, N_sample = N_sample, i_th_state = i_th_state, 86 | i_state_ub = i_state_ub, epsilon = epsilon,sin_const = sin_const, N_sim=N_sim, mode = "gene") 87 | result_x += [sim.x_sim] 88 | result_u += [sim.u_sim] 89 | print("#" +str(i) + " sim of " + str(N_sample) + " is done") 90 | 91 | N_sample_str = str(N_sample) 92 | 93 | write_path = "/Users/zhengangzhong/Dropbox/PhD/documents/paper_writing/CDC2021/result/" + file_name + "/" + N_sample_str+ "_" + "x_tra" + ".txt" 94 | with open(write_path, 'w') as f: 95 | for listitem in result_x: 96 | f.write('%s\n' % listitem) 97 | f.close() 98 | write_path = "/Users/zhengangzhong/Dropbox/PhD/documents/paper_writing/CDC2021/result/" + file_name + "/" + N_sample_str + "_" + "u_tra" +".txt" 99 | with open(write_path, 'w') as f: 100 | for listitem in result_u: 101 | f.write('%s\n' % listitem) 102 | f.close() -------------------------------------------------------------------------------- /inverted_pendulum_experiment_eps=var_N_sample=1.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from DRO_model import Model 3 | from DRO_simulation import Simulation 4 | import sys 5 | import os 6 | 7 | 8 | 9 | def inverted_pendulum_ode(t, x, u): 10 | M = 1.096 11 | m = 0.109 12 | l = 0.25 13 | b = 0.1 14 | g = 9.81 15 | I = 0.0034 16 | 17 | p = I * (M + m) + M * m * l ** 2 18 | 19 | A = np.array([[0, 1, 0, 0], 20 | [0, -(I+m*l**2)*b/p, (m**2*g*l**2)/p, 0], 21 | [0, 0, 0, 1], 22 | [0, -(m*l*b)/p, m*g*l*(M+m)/p, 0]]) 23 | B = np.array([[0], [(I+m*l**2)/p], [0], [m*l/p]]) 24 | 25 | dot_x = A @ x + B @ u 26 | 27 | return dot_x 28 | 29 | 30 | if __name__ == "__main__": 31 | file_name =(os.path.splitext(sys.argv[0])[0]).split("/")[-1] 32 | 33 | M = 1.096 34 | m = 0.109 35 | l = 0.25 36 | b = 0.1 37 | g = 9.81 38 | I = 0.0034 39 | 40 | p = I * (M + m) + M * m * l ** 2 41 | 42 | A = np.array([[0, 1, 0, 0], 43 | [0, -(I+m*l**2)*b/p, (m**2*g*l**2)/p, 0], 44 | [0, 0, 0, 1], 45 | [0, -(m*l*b)/p, m*g*l*(M+m)/p, 0]]) 46 | B = np.array([[0], [(I+m*l**2)/p], [0], [m*l/p]]) 47 | 48 | delta_t = 0.1 49 | 50 | # x_init = np.array([[0], [0], [-np.pi/2], [0]]) 51 | x_init = np.array([[0], [0], [-0.5], [0]]) 52 | # C = np.diag([0, 0, 0, 1e-2]) 53 | C = np.array([[0, 0],[0, 0],[0, 0],[1e-2, 0]]) 54 | D = np.array([[0, 0, 1, 0]]) 55 | # D = np.array([[1, 0, 0, 0], [0,0,1,0]]) 56 | E = np.array([[0, 1e-2]]) 57 | # E = np.array([[0, 0, 1e-2, 0]]) 58 | N = 5 59 | model = Model(A, B, C, D, E, x_init, N, delta_t) 60 | 61 | Q = np.diag([1000, 1, 1500, 1]) 62 | Qf = np.diag([1000, 1, 1500, 1]) 63 | R = np.diag([1]) 64 | 65 | d = model.d 66 | mu = np.zeros([d, 1]) 67 | beta = 0.95 68 | N_sample = 1 69 | i_th_state = 3 70 | i_state_ub = 0.5 71 | epsilon = 1 72 | sin_const = 3 73 | N_sim = 100 74 | N_loop = 10 75 | # Test1: fix N_sample = 1, sin_const = 3, beta = 0.95, i_ub = 0.4, N_loop = 1. 76 | # change epsilon 77 | eps_test = [1e-2, 1e-1, 1, 3, 5, 10, 100] 78 | for eps_temp in eps_test: 79 | result_x = [] 80 | result_u = [] 81 | epsilon = eps_temp 82 | for i in range(N_loop): 83 | sim = Simulation(model, Q, Qf, R, mu, x_init, beta = beta, N_sample = N_sample, i_th_state = i_th_state, 84 | i_state_ub = i_state_ub, epsilon = epsilon,sin_const = sin_const, N_sim=N_sim, mode = "gene") 85 | result_x += [sim.x_sim] 86 | result_u += [sim.u_sim] 87 | print("#" +str(i) + " sim of " + str(epsilon) + " is done") 88 | 89 | eps_str = str(epsilon) 90 | if epsilon < 1: 91 | eps_str = str(epsilon).split(".")[0] + str(epsilon).split(".")[1] # 0.01 -> "001" 92 | write_path = "/Users/zhengangzhong/Dropbox/PhD/documents/paper_writing/CDC2021/result/" + file_name + "/" + eps_str+ "_" + "x_tra" + ".txt" 93 | with open(write_path, 'w') as f: 94 | for listitem in result_x: 95 | f.write('%s\n' % listitem) 96 | f.close() 97 | write_path = "/Users/zhengangzhong/Dropbox/PhD/documents/paper_writing/CDC2021/result/" + file_name + "/" + eps_str+ "_" + "u_tra" + ".txt" 98 | with open(write_path, 'w') as f: 99 | for listitem in result_u: 100 | f.write('%s\n' % listitem) 101 | f.close() -------------------------------------------------------------------------------- /mass_spring_experiment_eps=var_N_sample=1.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from DRO_model import Model 3 | from DRO_simulation import Simulation 4 | import sys 5 | import os 6 | 7 | def mass_string_ode(t, x, u): 8 | m = 2 # [kg] 9 | k1 = 3 # [N/m] 10 | k2 = 2 # [N/m] 11 | 12 | A = np.array([[0, 1], [-k2 / m, -k1 / m]]) 13 | B = np.array([[0], [1 / m]]) 14 | 15 | dot_x = A @ x + B @ u 16 | 17 | return dot_x 18 | 19 | 20 | if __name__ == "__main__": 21 | 22 | file_name =(os.path.splitext(sys.argv[0])[0]).split("/")[-1] 23 | m = 2 #[kg] 24 | k1 = 3 # [N/m] 25 | k2 = 2 # [N/m] 26 | 27 | A = np.array([[0,1],[-k2/m, -k1/m]]) 28 | B = np.array([[0],[1/m]]) 29 | delta_t = 0.1 30 | 31 | x_init = np.array([[-2],[0]]) 32 | 33 | 34 | Ck = np.array([[1e-3, 0],[0, 0]]) 35 | D = np.array([[1, 0]]) 36 | E = np.array([[0,1e-3]]) 37 | N = 5 38 | 39 | model = Model(A, B, Ck, D, E, x_init, N, delta_t) 40 | 41 | 42 | # Q = np.diag([10, 1]) 43 | # Qf = np.diag([15, 1]) 44 | # R = np.diag([1]) 45 | # 46 | # d = model.d 47 | # mu = np.zeros([d, 1]) 48 | # mu_w = np.vstack([1] + [mu] * N) 49 | # M_w = mu_w @ mu_w.T + np.diag([0] + [1] * N * d) 50 | # beta = 0.95 51 | # N_sample = 3 52 | # i_th_state = 1 53 | # i_state_ub = 0.4 54 | # epsilon = 1 55 | # sin_const = 3 56 | # N_sim = 60 57 | 58 | # sim = Simulation(model, Q, Qf, R, mu, x_init, beta = beta, N_sample = N_sample, i_th_state = i_th_state, i_state_ub = i_state_ub, epsilon = epsilon, 59 | # sin_const = sin_const, N_sim=N_sim, mode = "gene") 60 | # print(sim.x_sim) 61 | 62 | Q = np.diag([10, 1]) 63 | Qf = np.diag([15, 1]) 64 | R = np.diag([1]) 65 | 66 | d = model.d 67 | mu = np.zeros([d, 1]) 68 | mu_w = np.vstack([1] + [mu] * N) 69 | M_w = mu_w @ mu_w.T + np.diag([0] + [1] * N * d) 70 | beta = 0.95 71 | N_sample = 1 72 | i_th_state = 1 73 | i_state_ub = 0.4 74 | epsilon = 1 75 | sin_const = 3 76 | N_sim = 70 77 | N_loop = 10 78 | # Test1: fix N_sample = 1, sin_const = 3, beta = 0.95, i_ub = 0.4, N_loop = 10. 79 | # change epsilon 80 | eps_test = [1e-2, 1e-1, 1, 3, 5, 10, 100] 81 | for eps_temp in eps_test: 82 | result_x = [] 83 | result_u = [] 84 | epsilon = eps_temp 85 | for i in range(N_loop): 86 | sim = Simulation(model, Q, Qf, R, mu, x_init, beta = beta, N_sample = N_sample, i_th_state = i_th_state, 87 | i_state_ub = i_state_ub, epsilon = epsilon,sin_const = sin_const, N_sim=N_sim, mode = "gene") 88 | result_x += [sim.x_sim] 89 | result_u += [sim.u_sim] 90 | print("#" +str(i) + " sim of " + str(epsilon) + " is done") 91 | 92 | eps_str = str(epsilon) 93 | if epsilon < 1: 94 | eps_str = str(epsilon).split(".")[0] + str(epsilon).split(".")[1] # 0.01 -> "001" 95 | write_path = "/Users/zhengangzhong/Dropbox/PhD/documents/paper_writing/CDC2021/result/" + file_name + "_" + eps_str+ "_" + "x_tra" + ".txt" 96 | with open(write_path, 'w') as f: 97 | for listitem in result_x: 98 | f.write('%s\n' % listitem) 99 | f.close() 100 | write_path = "/Users/zhengangzhong/Dropbox/PhD/documents/paper_writing/CDC2021/result/" + file_name + "_" + eps_str + "_" + "u_tra" +".txt" 101 | with open(write_path, 'w') as f: 102 | for listitem in result_u: 103 | f.write('%s\n' % listitem) 104 | f.close() -------------------------------------------------------------------------------- /DRO_model.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from numpy.linalg import matrix_power 3 | class Model: 4 | 5 | def __init__(self, A, B, C, D, E, x_init, N, delta_t): 6 | ''' 7 | x_next = Ak x_k + Bk u_k + Ck w_k 8 | y_k = D x_k + E w_k 9 | 10 | Args: 11 | A: Continuous time 12 | B: Continuous time 13 | C: Discrete time 14 | D: Discrete time 15 | E: Discrete time 16 | 17 | ''' 18 | 19 | self.A = A 20 | self.B = B 21 | 22 | self.delta_t = delta_t 23 | self.N = N 24 | 25 | Ak, Bk = self.disc_linear_system(A, B, delta_t) 26 | self.Ak = Ak 27 | self.Bk = Bk 28 | self.C = C 29 | self.D = D 30 | self.E = E 31 | 32 | n, m, d, r, Nx, Nu, Ny, Nw, Ax, Bx, Cx, Ay, By, Cy, Ey, Cx_tilde, Cy_tilde, Ey_tilde, D_tilde = self.stack_system(Ak, Bk, C, D, E, x_init, N) 33 | # Define relevant dimension 34 | self.n = n 35 | self.m = m 36 | self.d = d 37 | self.r = r 38 | self.Nx = Nx 39 | self.Nu = Nu 40 | self.Ny = Ny 41 | self.Nw = Nw 42 | # Stack system matrices 43 | self.Ax = Ax 44 | self.Bx = Bx 45 | self.Cx = Cx 46 | self.Ay = Ay 47 | self.By = By 48 | self.Cy = Cy 49 | self.Ey = Ey 50 | self.Cx_tilde = Cx_tilde 51 | self.Cy_tilde = Cy_tilde 52 | self.Ey_tilde = Ey_tilde 53 | self.D_tilde = D_tilde 54 | 55 | 56 | def disc_linear_system(self, A, B, delta_t): 57 | ''' 58 | Discrete a linear system with implicit Euler 59 | x[k+1] = (I - delta_t * A)^{-1} @ x[k] + (I - delta_t * A)^{-1} @ (delta_t * B) @ u[k] 60 | 61 | Returns: 62 | Ak 63 | Bk 64 | 65 | ''' 66 | Nx = np.shape(A)[0] 67 | Ix = np.identity(Nx) 68 | 69 | Ak = np.linalg.inv(Ix - delta_t * A) 70 | Bk = np.linalg.inv(Ix - delta_t * A) @ (delta_t * B) 71 | 72 | return Ak, Bk 73 | 74 | 75 | def stack_system(self, A, B, C, D, E, x_init, N): 76 | ''' 77 | Stack system matrix for N prediction horizon 78 | 79 | x_next = A x_k + B u_k + C w_k 80 | y_k = D x_k + E w_k 81 | 82 | ''' 83 | 84 | n = np.shape(A)[1] # Dimension of state 85 | m = np.shape(B)[1] # Dimension of input 86 | d = np.shape(C)[1] # Dimension of disturbance 87 | r = np.shape(D)[0] # Dimension of output 88 | 89 | # print(Nx,Nu,Nw,Ny) 90 | Nx = (N + 1) * n 91 | Nu = N * m 92 | Ny = N * r 93 | Nw = N * d 94 | 95 | Ax = np.zeros([Nx, n]) 96 | Bx = np.zeros([Nx, Nu]) 97 | Cx = np.zeros([Nx, Nw]) 98 | Ay = np.zeros([Ny, n]) 99 | By = np.zeros([Ny, Nu]) 100 | Cy = np.zeros([Ny, Nw]) 101 | Ey = np.zeros([Ny, Nw]) 102 | Cx_tilde = np.zeros([Nx, Nw + 1]) 103 | Cy_tilde = np.zeros([Ny + 1, Nw + 1]) 104 | Ey_tilde = np.zeros([Ny + 1, Nw + 1]) 105 | D_tilde = np.zeros([Ny + 1, Nx]) 106 | # H 107 | 108 | # Ax 109 | for i in range(N + 1): 110 | Ax[i * n:(i + 1) * n, :] = matrix_power(A, i) 111 | # Bx 112 | for i in range(N): 113 | mat_temp = B 114 | for j in range(i + 1): 115 | Bx[(i + 1) * n: (i + 2) * n, (i - j) * m: (i - j + 1) * m] = mat_temp 116 | mat_temp = A @ mat_temp 117 | # Cx 118 | for i in range(N): 119 | mat_temp = C 120 | for j in range(i + 1): 121 | Cx[(i + 1) * n: (i + 2) * n, (i - j) * d: (i - j + 1) * d] = mat_temp 122 | mat_temp = A @ mat_temp 123 | # Ay 124 | for i in range(N): 125 | Ay[i * r:(i + 1) * r, :] = D @ matrix_power(A, i) 126 | # By 127 | for i in range(N): 128 | mat_temp = B 129 | for j in range(i + 1): 130 | By[(i + 1) * r: (i + 2) * r, (i - j) * m: (i - j + 1) * m] = D @ mat_temp 131 | mat_temp = A @ mat_temp 132 | # Cy 133 | for i in range(N): 134 | mat_temp = C 135 | for j in range(i + 1): 136 | Cy[(i + 1) * r: (i + 2) * r, (i - j) * d: (i - j + 1) * d] = D @ mat_temp 137 | mat_temp = A @ mat_temp 138 | # Ey 139 | for i in range(N): 140 | Ey[i * r: (i + 1) * r, i * d: (i + 1) * d] = E 141 | # Cx_tilde 142 | Cx_tilde[:, [0]] = Ax @ x_init 143 | Cx_tilde[:, 1:] = Cx 144 | # Cy_tilde 145 | Cy_tilde[r:, [0]] = Ay @ x_init 146 | Cy_tilde[r:, 1:] = Cy 147 | # Ey_tilde 148 | Ey_tilde[0, 0] = 1 149 | Ey_tilde[1:, 1:] = Ey 150 | # D_tilde 151 | for i in range(N): 152 | D_tilde[1 + i * r: 1 + (i + 1) * r, i * n: (i + 1) * n] = D 153 | 154 | return n, m, d, r, Nx, Nu, Ny, Nw, Ax, Bx, Cx, Ay, By, Cy, Ey, Cx_tilde, Cy_tilde, Ey_tilde, D_tilde 155 | 156 | 157 | 158 | def change_xinit(self, x_k): 159 | ''' 160 | x_next = A x_k + B u_k + C w_k 161 | y_k = D x_k + E w_k 162 | ''' 163 | 164 | n = self.n 165 | m = self.m 166 | d = self.d 167 | r = self.r 168 | 169 | # print(Nx,Nu,Nw,Ny) 170 | Nx = self.Nx 171 | Nu = self.Nu 172 | Ny = self.Ny 173 | Nw = self.Nw 174 | 175 | N = self.N 176 | 177 | A = self.Ak 178 | B = self.Bk 179 | C = self.C 180 | D = self.D 181 | E = self.E 182 | 183 | 184 | Ax = self.Ax 185 | Bx = self.Bx 186 | Cx = self.Cx 187 | Ay = self.Ay 188 | By = self.By 189 | Cy = self.Cy 190 | Ey = self.Ey 191 | 192 | Cx_tilde = self.Cx_tilde 193 | Cy_tilde = self.Cy_tilde 194 | Ey_tilde = self.Ey_tilde 195 | D_tilde = self.D_tilde 196 | 197 | # Cx_tilde 198 | Cx_tilde[:, [0]] = Ax @ x_k 199 | # Cx_tilde[:, 1:] = Cx 200 | # Cy_tilde 201 | Cy_tilde[r:, [0]] = Ay @ x_k 202 | # Cy_tilde[r:, 1:] = Cy 203 | 204 | # Stack system matrices 205 | 206 | self.Cx_tilde = Cx_tilde 207 | self.Cy_tilde = Cy_tilde 208 | 209 | # return n, m, d, r, Nx, Nu, Ny, Nw, Ax, Bx, Cx, Ay, By, Cy, Ey, Cx_tilde, Cy_tilde, Ey_tilde, D_tilde -------------------------------------------------------------------------------- /DRO_simulation.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from DRO_optproblem import Opt_problem 3 | from DRO_model import Model 4 | import mosek 5 | import cvxpy as cp 6 | import matplotlib.pyplot as plt 7 | 8 | class Simulation: 9 | ''' 10 | @Arg 11 | 12 | mode: "collect" data and incorporate the collected data into constraint 13 | "gene" data at each time instant and use fixed number of data to solve opt problem 14 | 15 | 16 | ''' 17 | 18 | def __init__(self, model, Q, Qf, R, mu, x_init, beta = 0.95, N_sample = 10, i_th_state = 1, i_state_ub = 0.05, epsilon = 1, 19 | sin_const = 1, N_sim=80, mode = "collect",data_set = None, N_sample_max = None): 20 | 21 | self.Q = Q 22 | self.Qf = Qf 23 | self.R = R 24 | self.mu = mu 25 | self.beta = beta 26 | self.N_sample = N_sample 27 | self.i_th_state = i_th_state 28 | self.i_state_ub = i_state_ub 29 | self.epsilon = epsilon 30 | self.sin_const = sin_const 31 | self.model = model 32 | 33 | if mode is "collect": 34 | self.x_sim, self.u_sim = self.simulation_collect(x_init,N_sim, data_set, N_sample_max) 35 | elif mode is "gene": 36 | self.x_sim, self.u_sim = self.simulation_gene(x_init,N_sim) 37 | 38 | else: 39 | print("Error Mode") 40 | 41 | def simulation_collect(self,x_init,N_sim, data_set, N_sample_max): 42 | Ak = self.model.Ak 43 | Bk = self.model.Bk 44 | C = self.model.C 45 | D = self.model.D 46 | E = self.model.E 47 | N = self.model.N 48 | 49 | Q = self.Q 50 | Qf = self.Qf 51 | R = self.R 52 | mu = self.mu 53 | beta = self.beta 54 | 55 | i_th_state = self.i_th_state 56 | i_state_ub = self.i_state_ub 57 | epsilon = self.epsilon 58 | sin_const = self.sin_const 59 | 60 | delta_t = self.model.delta_t 61 | d = self.model.d 62 | 63 | t0 = 0 64 | xk = x_init 65 | uk = 0 66 | t = t0 67 | h = delta_t 68 | 69 | x_list = [] 70 | x_list += xk.flatten().tolist() 71 | u_list = [] 72 | 73 | N_sample = self.N_sample 74 | # N_sample_max 75 | for i in range(N_sim): 76 | if i % N == 0 and i > 0 and N_sample <= N_sample_max: 77 | N_sample += 1 78 | 79 | self.model.change_xinit(xk) 80 | opt_problem = Opt_problem(self.model, Q, Qf, R, mu, beta=beta, N_sample=N_sample, i_th_state=i_th_state, 81 | i_state_ub=i_state_ub, epsilon=epsilon, sin_const=sin_const, collect = True, data_set = data_set) 82 | # W_sample, W_sample_ext = self.gene_disturbance(N, d, N_sample, sin_const) 83 | # opt_problem.W_sample_matrix.value = W_sample 84 | prob = opt_problem.prob 85 | # print(W_sample_matrix) 86 | # print( prob.solve(verbose=True)) 87 | 88 | prob.solve(solver=cp.MOSEK) 89 | 90 | wk = sin_const * np.sin(np.random.randn(d, 1)) 91 | data_set += [wk] 92 | uk = opt_problem.H_cal_dec.value[0, 0] + opt_problem.H_cal_dec.value[0, 1] * (D @ xk + E @ wk) 93 | u_list += uk.flatten().tolist() 94 | # print("current state and input", xk, uk) 95 | x_kp1 = self.simulation_Euler(Ak, Bk, xk, uk) 96 | x_list += x_kp1.flatten().tolist() 97 | xk = x_kp1 98 | xk += C @ wk 99 | return x_list, u_list 100 | 101 | 102 | def simulation_gene(self, x_init, N_sim): 103 | Ak = self.model.Ak 104 | Bk = self.model.Bk 105 | C = self.model.C 106 | D = self.model.D 107 | E = self.model.E 108 | N = self.model.N 109 | 110 | Q = self.Q 111 | Qf = self.Qf 112 | R = self.R 113 | mu = self.mu 114 | beta = self.beta 115 | N_sample = self.N_sample 116 | i_th_state = self.i_th_state 117 | i_state_ub = self.i_state_ub 118 | epsilon = self.epsilon 119 | sin_const = self.sin_const 120 | 121 | delta_t = self.model.delta_t 122 | d = self.model.d 123 | 124 | t0 = 0 125 | xk = x_init 126 | uk = 0 127 | t = t0 128 | h = delta_t 129 | 130 | x_list = [] 131 | x_list += xk.flatten().tolist() 132 | u_list = [] 133 | 134 | for i in range(N_sim): 135 | # if i % N == 0: 136 | self.model.change_xinit(xk) 137 | 138 | 139 | 140 | opt_problem = Opt_problem(self.model, Q, Qf, R, mu, beta=beta, N_sample=N_sample, i_th_state=i_th_state, 141 | i_state_ub=i_state_ub, epsilon=epsilon, sin_const=sin_const) 142 | # W_sample, W_sample_ext = self.gene_disturbance(N, d, N_sample, sin_const) 143 | # opt_problem.W_sample_matrix.value = W_sample 144 | prob = opt_problem.prob 145 | # print(W_sample_matrix) 146 | # print( prob.solve(verbose=True)) 147 | 148 | prob.solve(solver=cp.MOSEK) 149 | # print("opt value:", prob.value) 150 | # print( prob.solve(verbose=True)) 151 | # prob.solve(solver = cp.MOSEK,verbose = True, mosek_params = {mosek.dparam.basis_tol_s:1e-9, mosek.dparam.ana_sol_infeas_tol:0}) 152 | # print(Ax @ x_init + Bx @ H.value @ W_sample_matrix_ext[:,0:1] + Cx @ W_sample_matrix[:,0:1]) 153 | # print("status:", prob.status) 154 | # print("Controller", opt_problem.H_cal_dec.value[0,0], opt_problem.H_cal_dec.value[0,1]) 155 | # print("dual:", constraint[0].dual_value) 156 | # print("gamma", gamma_matrix[0].value, gamma_matrix[1].value, gamma_matrix[2].value, gamma_matrix[3].value) 157 | # print("lambda",opt_problem.lambda_var.value) 158 | # print("lambda time epsilon",lambda_var.value * epsilon) 159 | # print("si",opt_problem.si_var.value) 160 | # print("si average",np.sum(opt_problem.si_var.value)/N_sample) 161 | # print("state_constraint", np.mean(opt_problem.si_var.value) + opt_problem.lambda_var.value * epsilon) 162 | # print("state",(self.model.Bx @ opt_problem.H_cal_dec.value @ (self.model.Cy_tilde + self.model.Ey_tilde) + self.model.Cx_tilde) @ opt_problem.W_sample_matrix_ext) 163 | # print("disturbance data", W_sample_matrix) 164 | wk = sin_const * np.sin(np.random.randn(d, 1)) 165 | uk = opt_problem.H_cal_dec.value[0, 0] + opt_problem.H_cal_dec.value[0, 1] * (D @ xk + E @ wk) 166 | u_list += uk.flatten().tolist() 167 | # print("current state and input", xk, uk) 168 | x_kp1 = self.simulation_Euler(Ak, Bk, xk, uk) 169 | x_list += x_kp1.flatten().tolist() 170 | xk = x_kp1 171 | xk += C @ wk 172 | return x_list, u_list 173 | 174 | 175 | 176 | def gene_disturbance(self, N, d, N_sample, sin_const): 177 | # Generate data: const * sinx 178 | 179 | w_sample = [] 180 | for i in range(N_sample): 181 | w_temp = sin_const * np.sin(np.random.randn(N*d)) 182 | w_sample += [w_temp] 183 | W_sample_matrix = np.array(w_sample).T 184 | 185 | W_sample_matrix_ext = np.vstack( [np.ones([1, N_sample]),W_sample_matrix]) 186 | return W_sample_matrix, W_sample_matrix_ext 187 | 188 | def simulation_Euler(self, Ak, Bk, x, u): 189 | ''' 190 | Simulate with implicit Euler 191 | x[k+1] = (I - delta_t * A)^{-1} @ x[k] + (I - delta_t * A)^{-1} @ (delta_t * B) @ u[k] 192 | ''' 193 | x_next = Ak @ x + Bk @ u 194 | 195 | return x_next 196 | 197 | def RK4_np(self, f, x, u, t, h): 198 | """ 199 | Runge-Kutta 4th order solver using numpy array data type. 200 | 201 | Args: 202 | f: A function returning first order ODE in 2D numpy array (Nx x 1). 203 | x: Current value (list or numpy array). 204 | t: Current time. 205 | h: Step length. 206 | Returns: 207 | x_next: Vector of next value in 2D numpy array (Nx x 1) 208 | """ 209 | x = np.reshape(x, (np.shape(x)[0], -1)) # Reshape x to col vector in np 2D array 210 | k1 = f(t, x, u) 211 | k2 = f(t + h / 2, x + h / 2 * k1, u) 212 | k3 = f(t + h / 2, x + h / 2 * k2, u) 213 | k4 = f(t + h, x + h * k3, u) 214 | x_next = x + h / 6 * (k1 + 2 * k2 + 2 * k3 + k4) 215 | return x_next 216 | 217 | def plot_state(self): 218 | delta_t = self.model.delta_t 219 | n = self.model.n 220 | 221 | x_traj = self.x_sim 222 | 223 | Nt = np.shape(x_traj[::n])[0] 224 | t_plot = [delta_t * i for i in range(Nt)] 225 | 226 | plt.figure(1, figsize=(10, 20)) 227 | plt.clf() 228 | for i in range (n): 229 | plt.subplot( str(n) + str(1) + str(i + 1) ) 230 | plt.grid() 231 | x_traj_temp = x_traj[i::n] 232 | plt.plot(t_plot, x_traj_temp) 233 | plt.ylabel('x' + str(i + 1)) 234 | 235 | plt.xlabel('t') 236 | plt.show() 237 | -------------------------------------------------------------------------------- /DRO_optproblem.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cvxpy as cp 3 | import mosek 4 | 5 | 6 | class Opt_problem: 7 | def __init__(self, model, Q, Qf, R, mu, beta = 0.95, N_sample = 5, i_th_state = 1, i_state_ub = 0.05, epsilon = 1, sin_const = 1, collect = None, data_set = None): 8 | 9 | N = model.N 10 | 11 | n = model.n 12 | m = model.m 13 | d = model.d 14 | r = model.r 15 | Nx = model.Nx 16 | Nu = model.Nu 17 | Ny = model.Ny 18 | Nw = model.Nw 19 | # Stack system matrices 20 | Ax = model.Ax 21 | Bx = model.Bx 22 | Cx = model.Cx 23 | Ay = model.Ay 24 | By = model.By 25 | Cy = model.Cy 26 | Ey = model.Ey 27 | Cx_tilde = model.Cx_tilde 28 | Cy_tilde = model.Cy_tilde 29 | Ey_tilde = model.Ey_tilde 30 | D_tilde = model.D_tilde 31 | 32 | self.Q = Q 33 | self.Qf = Qf 34 | self.R = R 35 | 36 | if not collect: 37 | Jx, Ju, eigval,eigvec, H_cal_dec, H, H_new_matrix, H_new, loss_func = self.define_loss_func(n, m, d, r, Nu, Nw, Bx, Cx_tilde, N, Q, Qf, R, mu, beta, sin_const) 38 | # W_sample_matrix, W_sample_matrix_ext = self.disturbance_para(N, d, N_sample, sin_const) 39 | W_sample_matrix, W_sample_matrix_ext = self.gene_disturbance(N, d, N_sample, sin_const) 40 | 41 | lambda_var, gamma_matrix, si_var, constraint = self.define_constraint(W_sample_matrix, W_sample_matrix_ext, eigvec, H_cal_dec, H, H_new, n, d, Bx, Cx_tilde, Cy_tilde, 42 | Ey_tilde, N, N_sample, sin_const, i_th_state, i_state_ub, epsilon) 43 | if collect: 44 | Jx, Ju, eigval, eigvec, H_cal_dec, H, H_new_matrix, H_new, loss_func = self.define_loss_func(n, m, d, r, Nu, 45 | Nw, Bx, 46 | Cx_tilde, N, Q, 47 | Qf, R, mu, 48 | beta, 49 | sin_const) 50 | W_sample_matrix, W_sample_matrix_ext = self.select_disturbance(N, d, N_sample, sin_const,data_set) 51 | lambda_var, gamma_matrix, si_var, constraint = self.define_constraint(W_sample_matrix, W_sample_matrix_ext, eigvec, H_cal_dec, H, H_new, n, d, Bx, Cx_tilde, Cy_tilde, 52 | Ey_tilde, N, N_sample, sin_const, i_th_state, i_state_ub, epsilon) 53 | 54 | 55 | 56 | self.lambda_var = lambda_var 57 | self.si_var = si_var 58 | 59 | self.H_cal_dec = H_cal_dec 60 | self.W_sample_matrix = W_sample_matrix 61 | self.W_sample_matrix_ext = W_sample_matrix_ext 62 | 63 | self.obj = cp.Minimize(loss_func) 64 | self.prob = cp.Problem(self.obj, constraint) 65 | 66 | def select_disturbance(self, N, d, N_sample, sin_const,data_set): 67 | W_sample_matrix = np.vstack(data_set[- N * N_sample:]) 68 | W_sample_matrix = W_sample_matrix.T.reshape(d * N, -1, order='F') 69 | W_sample_matrix_ext = np.vstack([np.ones([1, N_sample]), W_sample_matrix]) 70 | 71 | return W_sample_matrix, W_sample_matrix_ext 72 | def gene_disturbance(self, N, d, N_sample, sin_const): 73 | # Generate data: const * sinx 74 | 75 | w_sample = [] 76 | for i in range(N_sample): 77 | w_temp = sin_const * np.sin(np.random.randn(N * d)) 78 | w_sample += [w_temp] 79 | W_sample_matrix = np.array(w_sample).T 80 | 81 | W_sample_matrix_ext = np.vstack([np.ones([1, N_sample]), W_sample_matrix]) 82 | return W_sample_matrix, W_sample_matrix_ext 83 | 84 | # def disturbance_para(self, N, d, N_sample, sin_const): 85 | # W_sample_matrix = cp.Parameter((N * d, N_sample)) 86 | # W_sample_matrix_ext = cp.vstack([np.ones([1, N_sample]),W_sample_matrix]) 87 | # return W_sample_matrix, W_sample_matrix_ext 88 | 89 | def define_loss_func(self, n, m, d, r, Nu, Nw, Bx, Cx_tilde, N, Q, Qf, R, mu, beta, sin_const): 90 | # Define decision variables for POB affine constrol law 91 | 92 | sigma = 1 # var of normal distribution 93 | 94 | H_cal_dec = cp.Variable((Nu, 1)) 95 | # print(H_cal_dec) 96 | # \mathcal{H} matrix 97 | for i in range(N): 98 | H_col = cp.Variable((Nu - (i * m), r)) 99 | if i > 0: 100 | H_col = cp.vstack([np.zeros([i * m, r]), H_col]) 101 | # print(H_col) 102 | H_cal_dec = cp.hstack([H_cal_dec, H_col]) 103 | # print(H_cal_dec) 104 | # print(np.shape(H_cal_dec)) 105 | # Define intermediate decision variables for objective function 106 | H = cp.Variable((Nu, Nw + 1)) 107 | 108 | # Define loss function 109 | # beta = 0.95 110 | Jx = np.zeros([(N + 1) * n, (N + 1) * n]) 111 | for i in range(N): 112 | Jx[i * n: (i + 1) * n, i * n: (i + 1) * n] = beta ** i * Q 113 | Jx[N * n:, N * n:] = beta ** N * Qf 114 | 115 | Ju = np.zeros([N * m, N * m]) 116 | for i in range(N): 117 | Ju[i * m: (i + 1) * m, i * m: (i + 1) * m] = beta ** i * R 118 | 119 | # This is only for var = 1 and mean = 0. Should be modified. 120 | mu_w = np.vstack([1] + [mu] * N) 121 | # M_w = mu_w @ mu_w.T + np.diag([0] + [1] * N * d) 122 | M_w = np.diag([1] + [sin_const ** 2 * (1 - np.exp(-2 * sigma ** 2)) / 2] * N * d) 123 | 124 | # Intermediate decision variables. Since CVXPY does not support quadratic obj of decision variable matrix. 125 | H_new_matrix = [] 126 | # for i in range(Nw+1): 127 | # H_new_matrix += [cp.Variable([Nu,1])] 128 | # H_new = cp.hstack(H_new_matrix) 129 | for i in range(Nu): 130 | H_new_matrix += [cp.Variable((1, Nw + 1))] 131 | H_new = cp.vstack(H_new_matrix) 132 | 133 | # print(H_new.shape) 134 | # Reformulate the quadratic term 135 | 136 | eigval, eigvec = np.linalg.eig(Ju + Bx.T @ Jx @ Bx) 137 | eigval_mat = np.diag(eigval) 138 | # print(Ju + Bx.T @ Jx @ Bx - eigvec @ eigval_mat @ np.linalg.inv(eigvec)) 139 | # Loss function 140 | loss_func = 0 141 | 142 | N_eig = np.shape(eigval)[0] 143 | I = np.diag([1] * (Nw + 1)) 144 | for i in range(N_eig): 145 | # Reformulate Tr(H.T @ (Ju + Bx.T @ Jx @ Bx)@ H ) @ M_w 146 | # print(np.shape(H_new_matrix[i].T)) 147 | loss_func += eigval[i] * M_w[i, i] * cp.quad_form(H_new_matrix[i].T, 148 | I) # When M_w is identity matrix. Otherwise reformulate system matrix or this line 149 | # loss_func += cp.trace(2 * Cx_tilde.T @ Jx @ Bx @ eigvec @ H_new @ M_w) 150 | loss_func += cp.trace(2 * Cx_tilde.T @ Jx @ Bx @ H @ M_w) 151 | # loss_func += cp.trace(2 * Cx_tilde.T @ Jx @ Bx @ H_cal_dec @ (Cy_tilde + Ey_tilde) @ M_w) 152 | loss_func += cp.trace(Cx_tilde.T @ Jx @ Cx_tilde @ M_w) 153 | # Reformulate mu_w.T @ (H.T @ (Ju + Bx.T @ Jx @ Bx)@ H ) @ mu_w 154 | # loss_func += eigval[0] * cp.quad_form(H_new_matrix[0].T, I) + 2 * mu_w.T @ Cx_tilde.T @ Jx @ Bx @ eigvec @ H_new @ mu_w 155 | loss_func += eigval[0] * cp.quad_form(H_new_matrix[0].T, I) + 2 * mu_w.T @ Cx_tilde.T @ Jx @ Bx @ H @ mu_w 156 | # loss_func += eigval[0] * cp.quad_form(H_new_matrix[0].T, I) + 2 * mu_w.T @ Cx_tilde.T @ Jx @ Bx @ H_cal_dec @ (Cy_tilde + Ey_tilde) @ mu_w 157 | loss_func += mu_w.T @ Cx_tilde.T @ Jx @ Cx_tilde @ mu_w 158 | 159 | return Jx, Ju, eigval, eigvec, H_cal_dec, H, H_new_matrix, H_new, loss_func 160 | 161 | 162 | def define_constraint(self, W_sample_matrix, W_sample_matrix_ext, eigvec, H_cal_dec, H, H_new, n, d, Bx, Cx_tilde, Cy_tilde, 163 | Ey_tilde, N, N_sample, sin_const, i_th_state, i_state_ub, epsilon): 164 | constraint = [] 165 | constraint += [H_new == np.linalg.inv(eigvec) @ H] 166 | # constraint += [H_new == eigvec.T @ H ] 167 | constraint += [H == H_cal_dec @ (Cy_tilde + Ey_tilde)] 168 | # constraint += [H_new == np.linalg.inv(eigvec) @ H_cal_dec @ (Cy_tilde + Ey_tilde) ] 169 | 170 | # i_th_state = 1 # 0 for first element, 1 for second element 171 | # i_state_ub = 0.05 172 | 173 | d_supp = np.vstack((sin_const * np.ones([N * d, 1]), sin_const * np.ones([N * d, 1]))) 174 | C_supp = np.vstack((np.diag([1] * N * d), np.diag([-1] * N * d))) 175 | # d_supp = np.vstack( ( 0 * np.ones([N*d, 1]), 0 * np.ones([N*d, 1]))) 176 | # C_supp = np.vstack( (np.diag([0]*N*d), np.diag([0]*N*d) )) 177 | # lambda_var = cp.Variable() 178 | lambda_var = cp.Variable(nonneg=True) 179 | 180 | gamma_shape = np.shape(d_supp)[0] 181 | gamma_matrix = [] 182 | for i in range(N_sample): 183 | for j in range(N): 184 | gamma_var = cp.Variable((gamma_shape, 1), nonneg=True) 185 | # gamma_var = cp.Variable([gamma_shape,1]) 186 | gamma_matrix += [gamma_var] 187 | # k in N, i in N_sample 188 | # bk + 189 | X_constraint = (Bx @ H_cal_dec @ (Cy_tilde + Ey_tilde) + Cx_tilde) @ W_sample_matrix_ext 190 | # si_var = cp.Variable((N_sample,1)) 191 | si_var = cp.Variable(N_sample) 192 | 193 | for i in range(N_sample): 194 | for j in range(N): 195 | # print(N_sample) 196 | constraint_temp = X_constraint[n * (j + 1) + i_th_state, i] + gamma_matrix[i * N + j].T @ ( 197 | d_supp - C_supp @ W_sample_matrix[:, [i]]) 198 | # constraint += [constraint_temp <= si_var[i,0]] 199 | constraint += [constraint_temp <= si_var[i]] 200 | 201 | ak_matrix = (Bx @ H_cal_dec @ (Cy_tilde + Ey_tilde) + Cx_tilde)[:, 1:] 202 | for i in range(N_sample): 203 | for j in range(N): 204 | # constraint_temp = C_supp.T @ gamma_matrix[i * N + j] - ak_matrix[n * (j+1) + i_th_state:n * (j+1)+i_th_state + 1,:].T 205 | constraint_temp = C_supp.T @ gamma_matrix[i * N + j] - ak_matrix[[n * (j + 1) + i_th_state], :].T 206 | # constraint += [cp.norm_inf(constraint_temp) <= lambda_var] 207 | constraint += [cp.norm(constraint_temp, p=np.inf) <= lambda_var] 208 | 209 | # for i in range(N_sample): 210 | # for j in range(N): 211 | # constraint += [gamma_matrix[i * N + j] >= 0] 212 | # constraint += [lambda_var * epsilon + 1/N_sample * cp.sum(si_var) <= i_state_ub] 213 | constraint += [lambda_var * epsilon + 1 / N_sample * cp.sum(si_var) <= i_state_ub] 214 | return lambda_var, gamma_matrix, si_var, constraint 215 | 216 | 217 | 218 | 219 | --------------------------------------------------------------------------------