├── .gitignore ├── FixedFinalTime ├── discretization.py ├── parameters.py └── scproblem.py ├── FreeFinalTime ├── discretization.py ├── parameters.py └── scproblem.py ├── LICENSE ├── Models ├── diffdrive_2d.py ├── diffdrive_2d_plot.py ├── rocket_landing_2d.py ├── rocket_landing_2d_plot.py ├── rocket_landing_3d.py └── rocket_landing_3d_plot.py ├── README.md ├── fixedfinaltime.py ├── freefinaltime.py ├── global_parameters.py ├── output └── trajectory │ └── 000 │ ├── U.npy │ ├── X.npy │ └── sigma.npy └── utils.py /.gitignore: -------------------------------------------------------------------------------- 1 | .idea 2 | __pycache__ 3 | 4 | -------------------------------------------------------------------------------- /FixedFinalTime/discretization.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from scipy.integrate import odeint 3 | 4 | 5 | class FirstOrderHold: 6 | def __init__(self, m, K, sigma): 7 | self.K = K 8 | self.m = m 9 | self.n_x = m.n_x 10 | self.n_u = m.n_u 11 | 12 | self.A_bar = np.zeros([m.n_x * m.n_x, K - 1]) 13 | self.B_bar = np.zeros([m.n_x * m.n_u, K - 1]) 14 | self.C_bar = np.zeros([m.n_x * m.n_u, K - 1]) 15 | self.z_bar = np.zeros([m.n_x, K - 1]) 16 | 17 | # vector indices for flat matrices 18 | x_end = m.n_x 19 | A_bar_end = m.n_x * (1 + m.n_x) 20 | B_bar_end = m.n_x * (1 + m.n_x + m.n_u) 21 | C_bar_end = m.n_x * (1 + m.n_x + m.n_u + m.n_u) 22 | z_bar_end = m.n_x * (1 + m.n_x + m.n_u + m.n_u + 1) 23 | self.x_ind = slice(0, x_end) 24 | self.A_bar_ind = slice(x_end, A_bar_end) 25 | self.B_bar_ind = slice(A_bar_end, B_bar_end) 26 | self.C_bar_ind = slice(B_bar_end, C_bar_end) 27 | self.z_bar_ind = slice(C_bar_end, z_bar_end) 28 | 29 | self.f, self.A, self.B = m.get_equations() 30 | 31 | # integration initial condition 32 | self.V0 = np.zeros((m.n_x * (1 + m.n_x + m.n_u + m.n_u + 1),)) 33 | self.V0[self.A_bar_ind] = np.eye(m.n_x).reshape(-1) 34 | 35 | self.sigma = sigma 36 | self.dt = 1. / (K - 1) * sigma 37 | 38 | def calculate_discretization(self, X, U): 39 | """ 40 | Calculate discretization for given states, inputs and total time. 41 | 42 | :param X: Matrix of states for all time points 43 | :param U: Matrix of inputs for all time points 44 | :return: The discretization matrices 45 | """ 46 | for k in range(self.K - 1): 47 | self.V0[self.x_ind] = X[:, k] 48 | V = np.array(odeint(self._ode_dVdt, self.V0, (0, self.dt), args=(U[:, k], U[:, k + 1]))[1, :]) 49 | 50 | # flatten matrices in column-major (Fortran) order for CVXPY 51 | Phi = V[self.A_bar_ind].reshape((self.n_x, self.n_x)) 52 | self.A_bar[:, k] = Phi.flatten(order='F') 53 | self.B_bar[:, k] = np.matmul(Phi, V[self.B_bar_ind].reshape((self.n_x, self.n_u))).flatten(order='F') 54 | self.C_bar[:, k] = np.matmul(Phi, V[self.C_bar_ind].reshape((self.n_x, self.n_u))).flatten(order='F') 55 | self.z_bar[:, k] = np.matmul(Phi, V[self.z_bar_ind]) 56 | 57 | return self.A_bar, self.B_bar, self.C_bar, self.z_bar 58 | 59 | def _ode_dVdt(self, V, t, u_t0, u_t1): 60 | """ 61 | ODE function to compute dVdt. 62 | 63 | :param V: Evaluation state V = [x, Phi_A, B_bar, C_bar, z_bar] 64 | :param t: Evaluation time 65 | :param u_t0: Input at start of interval 66 | :param u_t1: Input at end of interval 67 | :return: Derivative at current time and state dVdt 68 | """ 69 | alpha = (self.dt - t) / self.dt 70 | beta = t / self.dt 71 | x = V[self.x_ind] 72 | u = u_t0 + (t / self.dt) * (u_t1 - u_t0) 73 | 74 | # using \Phi_A(\tau_{k+1},\xi) = \Phi_A(\tau_{k+1},\tau_k)\Phi_A(\xi,\tau_k)^{-1} 75 | # and pre-multiplying with \Phi_A(\tau_{k+1},\tau_k) after integration 76 | Phi_A_xi = np.linalg.inv(V[self.A_bar_ind].reshape((self.n_x, self.n_x))) 77 | 78 | A_subs = self.A(x, u) 79 | B_subs = self.B(x, u) 80 | f_subs = self.f(x, u) 81 | 82 | dVdt = np.zeros_like(V) 83 | dVdt[self.x_ind] = f_subs.T 84 | dVdt[self.A_bar_ind] = np.matmul(A_subs, V[self.A_bar_ind].reshape((self.n_x, self.n_x))).reshape(-1) 85 | dVdt[self.B_bar_ind] = np.matmul(Phi_A_xi, B_subs).reshape(-1) * alpha 86 | dVdt[self.C_bar_ind] = np.matmul(Phi_A_xi, B_subs).reshape(-1) * beta 87 | z_t = np.squeeze(f_subs) - np.matmul(A_subs, x) - np.matmul(B_subs, u) 88 | 89 | dVdt[self.z_bar_ind] = np.matmul(Phi_A_xi, z_t) 90 | return dVdt 91 | 92 | def integrate_nonlinear_piecewise(self, X_l, U): 93 | """ 94 | Piecewise integration to verfify accuracy of linearization. 95 | :param X_l: Linear state evolution 96 | :param U: Linear input evolution 97 | :return: The piecewise integrated dynamics 98 | """ 99 | X_nl = np.zeros_like(X_l) 100 | X_nl[:, 0] = X_l[:, 0] 101 | 102 | for k in range(self.K - 1): 103 | X_nl[:, k + 1] = odeint(self._dx, X_l[:, k], (0, self.dt), args=(U[:, k], U[:, k + 1]))[1, :] 104 | 105 | return X_nl 106 | 107 | def integrate_nonlinear_full(self, x0, U): 108 | """ 109 | Simulate nonlinear behavior given an initial state and an input over time. 110 | :param x0: Initial state 111 | :param U: Linear input evolution 112 | :return: The full integrated dynamics 113 | """ 114 | X_nl = np.zeros([x0.size, self.K]) 115 | X_nl[:, 0] = x0 116 | 117 | for k in range(self.K - 1): 118 | X_nl[:, k + 1] = odeint(self._dx, X_nl[:, k], (0, self.dt), args=(U[:, k], U[:, k + 1]))[1, :] 119 | 120 | return X_nl 121 | 122 | def _dx(self, x, t, u_t0, u_t1): 123 | u = u_t0 + (t / self.dt) * (u_t1 - u_t0) 124 | 125 | return np.squeeze(self.f(x, u)) 126 | -------------------------------------------------------------------------------- /FixedFinalTime/parameters.py: -------------------------------------------------------------------------------- 1 | from global_parameters import * 2 | 3 | # Weight constants 4 | w_nu = 1e5 # virtual control 5 | # initial trust region radius 6 | tr_radius = 5 7 | # trust region variables 8 | rho_0 = 0.0 9 | rho_1 = 0.25 10 | rho_2 = 0.9 11 | alpha = 2.0 12 | beta = 3.2 13 | -------------------------------------------------------------------------------- /FixedFinalTime/scproblem.py: -------------------------------------------------------------------------------- 1 | import cvxpy as cvx 2 | 3 | 4 | class SCProblem: 5 | """ 6 | Defines a standard Successive Convexification problem and adds the model specific constraints and objectives. 7 | 8 | :param m: The model object 9 | :param K: Number of discretization points 10 | """ 11 | 12 | def __init__(self, m, K): 13 | # Variables: 14 | self.var = dict() 15 | self.var['X'] = cvx.Variable((m.n_x, K)) 16 | self.var['U'] = cvx.Variable((m.n_u, K)) 17 | self.var['nu'] = cvx.Variable((m.n_x, K - 1)) 18 | 19 | # Parameters: 20 | self.par = dict() 21 | self.par['A_bar'] = cvx.Parameter((m.n_x * m.n_x, K - 1)) 22 | self.par['B_bar'] = cvx.Parameter((m.n_x * m.n_u, K - 1)) 23 | self.par['C_bar'] = cvx.Parameter((m.n_x * m.n_u, K - 1)) 24 | self.par['z_bar'] = cvx.Parameter((m.n_x, K - 1)) 25 | 26 | self.par['X_last'] = cvx.Parameter((m.n_x, K)) 27 | self.par['U_last'] = cvx.Parameter((m.n_u, K)) 28 | 29 | self.par['weight_nu'] = cvx.Parameter(nonneg=True) 30 | self.par['tr_radius'] = cvx.Parameter(nonneg=True) 31 | 32 | # Constraints: 33 | constraints = [] 34 | 35 | # Model: 36 | constraints += m.get_constraints(self.var['X'], self.var['U'], self.par['X_last'], self.par['U_last']) 37 | 38 | # Dynamics: 39 | constraints += [ 40 | self.var['X'][:, k + 1] == 41 | cvx.reshape(self.par['A_bar'][:, k], (m.n_x, m.n_x)) * self.var['X'][:, k] 42 | + cvx.reshape(self.par['B_bar'][:, k], (m.n_x, m.n_u)) * self.var['U'][:, k] 43 | + cvx.reshape(self.par['C_bar'][:, k], (m.n_x, m.n_u)) * self.var['U'][:, k + 1] 44 | + self.par['z_bar'][:, k] 45 | + self.var['nu'][:, k] 46 | for k in range(K - 1) 47 | ] 48 | 49 | # Trust region: 50 | du = self.var['U'] - self.par['U_last'] 51 | dx = self.var['X'] - self.par['X_last'] 52 | constraints += [cvx.norm(dx, 1) + cvx.norm(du, 1) <= self.par['tr_radius']] 53 | 54 | # Objective: 55 | model_objective = m.get_objective(self.var['X'], self.var['U'], self.par['X_last'], self.par['U_last']) 56 | sc_objective = cvx.Minimize( 57 | self.par['weight_nu'] * cvx.norm(self.var['nu'], 1) 58 | ) 59 | 60 | objective = sc_objective if model_objective is None else sc_objective + model_objective 61 | 62 | self.prob = cvx.Problem(objective, constraints) 63 | 64 | def set_parameters(self, **kwargs): 65 | """ 66 | All parameters have to be filled before calling solve(). 67 | """ 68 | 69 | for key in kwargs: 70 | if key in self.par: 71 | self.par[key].value = kwargs[key] 72 | else: 73 | print(f'Parameter \'{key}\' does not exist.') 74 | 75 | def print_available_parameters(self): 76 | print('Parameter names:') 77 | for key in self.par: 78 | print(f'\t {key}') 79 | print('\n') 80 | 81 | def print_available_variables(self): 82 | print('Variable names:') 83 | for key in self.var: 84 | print(f'\t {key}') 85 | print('\n') 86 | 87 | def get_variable(self, name): 88 | """ 89 | :param name: Name of the variable. 90 | :return The value of the variable. 91 | """ 92 | 93 | if name in self.var: 94 | return self.var[name].value 95 | else: 96 | print(f'Variable \'{name}\' does not exist.') 97 | return None 98 | 99 | def solve(self, **kwargs): 100 | error = False 101 | try: 102 | self.prob.solve(**kwargs) 103 | except cvx.SolverError: 104 | error = True 105 | 106 | return error 107 | -------------------------------------------------------------------------------- /FreeFinalTime/discretization.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from scipy.integrate import odeint 3 | 4 | 5 | class FirstOrderHold: 6 | def __init__(self, m, K): 7 | self.K = K 8 | self.m = m 9 | self.n_x = m.n_x 10 | self.n_u = m.n_u 11 | 12 | self.A_bar = np.zeros([m.n_x * m.n_x, K - 1]) 13 | self.B_bar = np.zeros([m.n_x * m.n_u, K - 1]) 14 | self.C_bar = np.zeros([m.n_x * m.n_u, K - 1]) 15 | self.S_bar = np.zeros([m.n_x, K - 1]) 16 | self.z_bar = np.zeros([m.n_x, K - 1]) 17 | 18 | # vector indices for flat matrices 19 | x_end = m.n_x 20 | A_bar_end = m.n_x * (1 + m.n_x) 21 | B_bar_end = m.n_x * (1 + m.n_x + m.n_u) 22 | C_bar_end = m.n_x * (1 + m.n_x + m.n_u + m.n_u) 23 | S_bar_end = m.n_x * (1 + m.n_x + m.n_u + m.n_u + 1) 24 | z_bar_end = m.n_x * (1 + m.n_x + m.n_u + m.n_u + 2) 25 | self.x_ind = slice(0, x_end) 26 | self.A_bar_ind = slice(x_end, A_bar_end) 27 | self.B_bar_ind = slice(A_bar_end, B_bar_end) 28 | self.C_bar_ind = slice(B_bar_end, C_bar_end) 29 | self.S_bar_ind = slice(C_bar_end, S_bar_end) 30 | self.z_bar_ind = slice(S_bar_end, z_bar_end) 31 | 32 | self.f, self.A, self.B = m.get_equations() 33 | 34 | # integration initial condition 35 | self.V0 = np.zeros((m.n_x * (1 + m.n_x + m.n_u + m.n_u + 2),)) 36 | self.V0[self.A_bar_ind] = np.eye(m.n_x).reshape(-1) 37 | 38 | self.dt = 1. / (K - 1) 39 | 40 | def calculate_discretization(self, X, U, sigma): 41 | """ 42 | Calculate discretization for given states, inputs and total time. 43 | 44 | :param X: Matrix of states for all time points 45 | :param U: Matrix of inputs for all time points 46 | :param sigma: Total time 47 | :return: The discretization matrices 48 | """ 49 | for k in range(self.K - 1): 50 | self.V0[self.x_ind] = X[:, k] 51 | V = np.array(odeint(self._ode_dVdt, self.V0, (0, self.dt), args=(U[:, k], U[:, k + 1], sigma))[1, :]) 52 | 53 | # using \Phi_A(\tau_{k+1},\xi) = \Phi_A(\tau_{k+1},\tau_k)\Phi_A(\xi,\tau_k)^{-1} 54 | # flatten matrices in column-major (Fortran) order for CVXPY 55 | Phi = V[self.A_bar_ind].reshape((self.n_x, self.n_x)) 56 | self.A_bar[:, k] = Phi.flatten(order='F') 57 | self.B_bar[:, k] = np.matmul(Phi, V[self.B_bar_ind].reshape((self.n_x, self.n_u))).flatten(order='F') 58 | self.C_bar[:, k] = np.matmul(Phi, V[self.C_bar_ind].reshape((self.n_x, self.n_u))).flatten(order='F') 59 | self.S_bar[:, k] = np.matmul(Phi, V[self.S_bar_ind]) 60 | self.z_bar[:, k] = np.matmul(Phi, V[self.z_bar_ind]) 61 | 62 | return self.A_bar, self.B_bar, self.C_bar, self.S_bar, self.z_bar 63 | 64 | def _ode_dVdt(self, V, t, u_t0, u_t1, sigma): 65 | """ 66 | ODE function to compute dVdt. 67 | 68 | :param V: Evaluation state V = [x, Phi_A, B_bar, C_bar, S_bar, z_bar] 69 | :param t: Evaluation time 70 | :param u_t0: Input at start of interval 71 | :param u_t1: Input at end of interval 72 | :param sigma: Total time 73 | :return: Derivative at current time and state dVdt 74 | """ 75 | alpha = (self.dt - t) / self.dt 76 | beta = t / self.dt 77 | x = V[self.x_ind] 78 | u = u_t0 + (t / self.dt) * (u_t1 - u_t0) 79 | 80 | # using \Phi_A(\tau_{k+1},\xi) = \Phi_A(\tau_{k+1},\tau_k)\Phi_A(\xi,\tau_k)^{-1} 81 | # and pre-multiplying with \Phi_A(\tau_{k+1},\tau_k) after integration 82 | Phi_A_xi = np.linalg.inv(V[self.A_bar_ind].reshape((self.n_x, self.n_x))) 83 | 84 | A_subs = sigma * self.A(x, u) 85 | B_subs = sigma * self.B(x, u) 86 | f_subs = self.f(x, u) 87 | 88 | dVdt = np.zeros_like(V) 89 | dVdt[self.x_ind] = sigma * f_subs.transpose() 90 | dVdt[self.A_bar_ind] = np.matmul(A_subs, V[self.A_bar_ind].reshape((self.n_x, self.n_x))).reshape(-1) 91 | dVdt[self.B_bar_ind] = np.matmul(Phi_A_xi, B_subs).reshape(-1) * alpha 92 | dVdt[self.C_bar_ind] = np.matmul(Phi_A_xi, B_subs).reshape(-1) * beta 93 | dVdt[self.S_bar_ind] = np.matmul(Phi_A_xi, f_subs).transpose() 94 | z_t = -np.matmul(A_subs, x) - np.matmul(B_subs, u) 95 | dVdt[self.z_bar_ind] = np.matmul(Phi_A_xi, z_t) 96 | 97 | return dVdt 98 | 99 | def integrate_nonlinear_piecewise(self, X_l, U, sigma): 100 | """ 101 | Piecewise integration to verfify accuracy of linearization. 102 | :param X_l: Linear state evolution 103 | :param U: Linear input evolution 104 | :param sigma: Total time 105 | :return: The piecewise integrated dynamics 106 | """ 107 | X_nl = np.zeros_like(X_l) 108 | X_nl[:, 0] = X_l[:, 0] 109 | 110 | for k in range(self.K - 1): 111 | X_nl[:, k + 1] = odeint(self._dx, X_l[:, k], 112 | (0, self.dt * sigma), 113 | args=(U[:, k], U[:, k + 1], sigma))[1, :] 114 | 115 | return X_nl 116 | 117 | def integrate_nonlinear_full(self, x0, U, sigma): 118 | """ 119 | Simulate nonlinear behavior given an initial state and an input over time. 120 | :param x0: Initial state 121 | :param U: Linear input evolution 122 | :param sigma: Total time 123 | :return: The full integrated dynamics 124 | """ 125 | X_nl = np.zeros([x0.size, self.K]) 126 | X_nl[:, 0] = x0 127 | 128 | for k in range(self.K - 1): 129 | X_nl[:, k + 1] = odeint(self._dx, X_nl[:, k], 130 | (0, self.dt * sigma), 131 | args=(U[:, k], U[:, k + 1], sigma))[1, :] 132 | 133 | return X_nl 134 | 135 | def _dx(self, x, t, u_t0, u_t1, sigma): 136 | u = u_t0 + (t / (self.dt * sigma)) * (u_t1 - u_t0) 137 | 138 | return np.squeeze(self.f(x, u)) 139 | -------------------------------------------------------------------------------- /FreeFinalTime/parameters.py: -------------------------------------------------------------------------------- 1 | from global_parameters import * 2 | 3 | # Weight constants 4 | w_nu = 1e5 # virtual control 5 | w_sigma = 10 # flight time 6 | # initial trust region radius 7 | tr_radius = 5 8 | # trust region variables 9 | rho_0 = 0.0 10 | rho_1 = 0.25 11 | rho_2 = 0.9 12 | alpha = 2.0 13 | beta = 3.2 14 | -------------------------------------------------------------------------------- /FreeFinalTime/scproblem.py: -------------------------------------------------------------------------------- 1 | import cvxpy as cvx 2 | 3 | 4 | class SCProblem: 5 | """ 6 | Defines a standard Successive Convexification problem and adds the model specific constraints and objectives. 7 | 8 | :param m: The model object 9 | :param K: Number of discretization points 10 | """ 11 | 12 | def __init__(self, m, K): 13 | # Variables: 14 | self.var = dict() 15 | self.var['X'] = cvx.Variable((m.n_x, K)) 16 | self.var['U'] = cvx.Variable((m.n_u, K)) 17 | self.var['nu'] = cvx.Variable((m.n_x, K - 1)) 18 | self.var['sigma'] = cvx.Variable(nonneg=True) 19 | 20 | # Parameters: 21 | self.par = dict() 22 | self.par['A_bar'] = cvx.Parameter((m.n_x * m.n_x, K - 1)) 23 | self.par['B_bar'] = cvx.Parameter((m.n_x * m.n_u, K - 1)) 24 | self.par['C_bar'] = cvx.Parameter((m.n_x * m.n_u, K - 1)) 25 | self.par['S_bar'] = cvx.Parameter((m.n_x, K - 1)) 26 | self.par['z_bar'] = cvx.Parameter((m.n_x, K - 1)) 27 | 28 | self.par['X_last'] = cvx.Parameter((m.n_x, K)) 29 | self.par['U_last'] = cvx.Parameter((m.n_u, K)) 30 | self.par['sigma_last'] = cvx.Parameter(nonneg=True) 31 | 32 | self.par['weight_sigma'] = cvx.Parameter(nonneg=True) 33 | self.par['weight_nu'] = cvx.Parameter(nonneg=True) 34 | self.par['tr_radius'] = cvx.Parameter(nonneg=True) 35 | 36 | # Constraints: 37 | constraints = [] 38 | 39 | # Model: 40 | constraints += m.get_constraints(self.var['X'], self.var['U'], self.par['X_last'], self.par['U_last']) 41 | 42 | # Dynamics: 43 | constraints += [ 44 | self.var['X'][:, k + 1] == 45 | cvx.reshape(self.par['A_bar'][:, k], (m.n_x, m.n_x)) * self.var['X'][:, k] 46 | + cvx.reshape(self.par['B_bar'][:, k], (m.n_x, m.n_u)) * self.var['U'][:, k] 47 | + cvx.reshape(self.par['C_bar'][:, k], (m.n_x, m.n_u)) * self.var['U'][:, k + 1] 48 | + self.par['S_bar'][:, k] * self.var['sigma'] 49 | + self.par['z_bar'][:, k] 50 | + self.var['nu'][:, k] 51 | for k in range(K - 1) 52 | ] 53 | 54 | # Trust region: 55 | du = self.var['U'] - self.par['U_last'] 56 | dx = self.var['X'] - self.par['X_last'] 57 | ds = self.var['sigma'] - self.par['sigma_last'] 58 | constraints += [cvx.norm(dx, 1) + cvx.norm(du, 1) + cvx.norm(ds, 1) <= self.par['tr_radius']] 59 | 60 | # Objective: 61 | model_objective = m.get_objective(self.var['X'], self.var['U'], self.par['X_last'], self.par['U_last']) 62 | sc_objective = cvx.Minimize( 63 | self.par['weight_sigma'] * self.var['sigma'] 64 | + self.par['weight_nu'] * cvx.norm(self.var['nu'], 1) 65 | ) 66 | 67 | objective = sc_objective if model_objective is None else sc_objective + model_objective 68 | 69 | self.prob = cvx.Problem(objective, constraints) 70 | 71 | def set_parameters(self, **kwargs): 72 | """ 73 | All parameters have to be filled before calling solve(). 74 | """ 75 | 76 | for key in kwargs: 77 | if key in self.par: 78 | self.par[key].value = kwargs[key] 79 | else: 80 | print(f'Parameter \'{key}\' does not exist.') 81 | 82 | def print_available_parameters(self): 83 | print('Parameter names:') 84 | for key in self.par: 85 | print(f'\t {key}') 86 | print('\n') 87 | 88 | def print_available_variables(self): 89 | print('Variable names:') 90 | for key in self.var: 91 | print(f'\t {key}') 92 | print('\n') 93 | 94 | def get_variable(self, name): 95 | """ 96 | :param name: Name of the variable. 97 | :return The value of the variable. 98 | """ 99 | 100 | if name in self.var: 101 | return self.var[name].value 102 | else: 103 | print(f'Variable \'{name}\' does not exist.') 104 | return None 105 | 106 | def solve(self, **kwargs): 107 | error = False 108 | try: 109 | self.prob.solve(**kwargs) 110 | except cvx.SolverError: 111 | error = True 112 | 113 | return error 114 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2018 Sven Niederberger 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 | -------------------------------------------------------------------------------- /Models/diffdrive_2d.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cvxpy as cvx 3 | import sympy as sp 4 | 5 | from global_parameters import K 6 | 7 | 8 | class Model: 9 | """ 10 | A 2D path planning problem. 11 | """ 12 | n_x = 3 13 | n_u = 2 14 | 15 | v_max = 1 # m/s 16 | w_max = np.pi / 6 # rad/s 17 | upper_bound = 10. # m 18 | lower_bound = -10. # m 19 | 20 | robot_radius = 0.5 # m 21 | 22 | r_init = np.array([-8., -8., 0.]) 23 | r_final = np.array([8., 8., 0.]) 24 | 25 | t_f_guess = 30 # s 26 | 27 | # slack variables for artificial infeasibility 28 | s_prime = [] 29 | 30 | def __init__(self): 31 | self.x_init = self.r_init 32 | self.x_final = self.r_final 33 | 34 | self.r_scale = self.upper_bound - self.lower_bound 35 | 36 | # cylindrical obstacles [(x,y),r] 37 | self.obstacles = [ 38 | [[5., 4.], 3.], 39 | [[-5., -4.], 3.], 40 | [[0., 0.], 2.], 41 | ] 42 | 43 | for _ in self.obstacles: 44 | self.s_prime.append(cvx.Variable((K, 1), nonneg=True)) 45 | 46 | def nondimensionalize(self): 47 | """ nondimensionalize all parameters and boundaries """ 48 | 49 | self.v_max /= self.r_scale 50 | self.upper_bound /= self.r_scale 51 | self.lower_bound /= self.r_scale 52 | self.robot_radius /= self.r_scale 53 | 54 | self.x_init = self.x_nondim(self.x_init) 55 | self.x_final = self.x_nondim(self.x_final) 56 | 57 | for j in range(len(self.obstacles)): 58 | self.obstacles[j][0][0] /= self.r_scale 59 | self.obstacles[j][0][1] /= self.r_scale 60 | self.obstacles[j][1] /= self.r_scale 61 | 62 | def x_nondim(self, x): 63 | """ nondimensionalize a single x row """ 64 | x[0:2] /= self.r_scale 65 | return x 66 | 67 | def u_nondim(self, u): 68 | """ nondimensionalize u""" 69 | u[0, :] /= self.r_scale 70 | return u 71 | 72 | def redimensionalize(self): 73 | """ redimensionalize all parameters """ 74 | self.v_max *= self.r_scale 75 | self.upper_bound *= self.r_scale 76 | self.lower_bound *= self.r_scale 77 | self.robot_radius *= self.r_scale 78 | 79 | self.x_init = self.x_nondim(self.x_init) 80 | self.x_final = self.x_nondim(self.x_final) 81 | 82 | for j in range(len(self.obstacles)): 83 | self.obstacles[j][0][0] *= self.r_scale 84 | self.obstacles[j][0][1] *= self.r_scale 85 | self.obstacles[j][1] *= self.r_scale 86 | 87 | def x_redim(self, x): 88 | """ redimensionalize x, assumed to have the shape of a solution """ 89 | x[0:2, :] *= self.r_scale 90 | return x 91 | 92 | def u_redim(self, u): 93 | """ redimensionalize u """ 94 | u[0, :] *= self.r_scale 95 | return u 96 | 97 | def get_equations(self): 98 | """ 99 | :return: Functions to calculate A, B and f given state x and input u 100 | """ 101 | f = sp.zeros(3, 1) 102 | 103 | x = sp.Matrix(sp.symbols('x y theta', real=True)) 104 | u = sp.Matrix(sp.symbols('v w', real=True)) 105 | 106 | f[0, 0] = u[0, 0] * sp.cos(x[2, 0]) 107 | f[1, 0] = u[0, 0] * sp.sin(x[2, 0]) 108 | f[2, 0] = u[1, 0] 109 | 110 | f = sp.simplify(f) 111 | A = sp.simplify(f.jacobian(x)) 112 | B = sp.simplify(f.jacobian(u)) 113 | 114 | f_func = sp.lambdify((x, u), f, 'numpy') 115 | A_func = sp.lambdify((x, u), A, 'numpy') 116 | B_func = sp.lambdify((x, u), B, 'numpy') 117 | 118 | return f_func, A_func, B_func 119 | 120 | def initialize_trajectory(self, X, U): 121 | """ 122 | Initialize the trajectory. 123 | 124 | :param X: Numpy array of states to be initialized 125 | :param U: Numpy array of inputs to be initialized 126 | :return: The initialized X and U 127 | """ 128 | K = X.shape[1] 129 | 130 | for k in range(K): 131 | alpha1 = (K - k) / K 132 | alpha2 = k / K 133 | X[:, k] = self.x_init * alpha1 + self.x_final * alpha2 134 | 135 | U[:, :] = 0 136 | 137 | return X, U 138 | 139 | def get_objective(self, X_v, U_v, X_last_p, U_last_p): 140 | """ 141 | Get model specific objective to be minimized. 142 | 143 | :param X_v: cvx variable for current states 144 | :param U_v: cvx variable for current inputs 145 | :param X_last_p: cvx parameter for last states 146 | :param U_last_p: cvx parameter for last inputs 147 | :return: A cvx objective function. 148 | """ 149 | 150 | slack = 0 151 | for j in range(len(self.obstacles)): 152 | slack += cvx.sum(self.s_prime[j]) 153 | 154 | objective = cvx.Minimize(1e5 * slack) 155 | # objective += cvx.Minimize(cvx.sum(cvx.square(U_v))) 156 | return objective 157 | 158 | def get_constraints(self, X_v, U_v, X_last_p, U_last_p): 159 | """ 160 | Get model specific constraints. 161 | 162 | :param X_v: cvx variable for current states 163 | :param U_v: cvx variable for current inputs 164 | :param X_last_p: cvx parameter for last states 165 | :param U_last_p: cvx parameter for last inputs 166 | :return: A list of cvx constraints 167 | """ 168 | # Boundary conditions: 169 | constraints = [ 170 | X_v[:, 0] == self.x_init, 171 | X_v[:, -1] == self.x_final, 172 | 173 | U_v[:, 0] == 0, 174 | U_v[:, -1] == 0 175 | ] 176 | 177 | # Input conditions: 178 | constraints += [ 179 | 0 <= U_v[0, :], 180 | U_v[0, :] <= self.v_max, 181 | cvx.abs(U_v[1, :]) <= self.w_max, 182 | ] 183 | 184 | # State conditions: 185 | constraints += [ 186 | X_v[0:2, :] <= self.upper_bound - self.robot_radius, 187 | X_v[0:2, :] >= self.lower_bound + self.robot_radius, 188 | ] 189 | 190 | # linearized obstacles 191 | for j, obst in enumerate(self.obstacles): 192 | p = obst[0] 193 | r = obst[1] + self.robot_radius 194 | 195 | lhs = [(X_last_p[0:2, k] - p) / (cvx.norm((X_last_p[0:2, k] - p)) + 1e-6) * (X_v[0:2, k] - p) 196 | for k in range(K)] 197 | constraints += [r - cvx.vstack(lhs) <= self.s_prime[j]] 198 | return constraints 199 | 200 | def get_linear_cost(self): 201 | cost = 0 202 | for j in range(len(self.obstacles)): 203 | cost += np.sum(self.s_prime[j].value) 204 | return cost 205 | 206 | def get_nonlinear_cost(self, X, U=None): 207 | cost = 0 208 | for obst in self.obstacles: 209 | vector_to_obstacle = X[0:2, :].T - obst[0] 210 | dist_to_obstacle = np.linalg.norm(vector_to_obstacle, 2, axis=1) 211 | is_violated = dist_to_obstacle < obst[1] + self.robot_radius 212 | violation = obst[1] + self.robot_radius - dist_to_obstacle 213 | cost += np.sum(is_violated * violation) 214 | return cost 215 | -------------------------------------------------------------------------------- /Models/diffdrive_2d_plot.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | from matplotlib import gridspec 4 | from matplotlib import collections as mc 5 | from mpl_toolkits import mplot3d 6 | from Models.diffdrive_2d import Model 7 | 8 | m = Model() 9 | 10 | 11 | def my_plot(fig, figures_i): 12 | X_i = X[figures_i, :, :] 13 | U_i = U[figures_i, :, :] 14 | K = X_i.shape[1] 15 | 16 | gs1 = gridspec.GridSpec(nrows=1, ncols=1, left=0.05, right=0.95, top=0.95, bottom=0.35) 17 | gs2 = gridspec.GridSpec(nrows=1, ncols=2, left=0.1, right=0.9, top=0.3, bottom=0.05) 18 | 19 | ax = fig.add_subplot(gs1[0, 0]) 20 | ax.set_xlim(m.lower_bound, m.upper_bound) 21 | ax.set_ylim(m.lower_bound, m.upper_bound) 22 | ax.set_xlabel('X [m]') 23 | ax.set_ylabel('Y [m]') 24 | ax.set_aspect('equal') 25 | 26 | ax.set_title("Iteration " + str(figures_i)) 27 | 28 | ax.plot(X_i[0, :], X_i[1, :], color='lightgrey', zorder=0) 29 | 30 | for k in range(K): 31 | heading = X_i[2, k] 32 | 33 | x = X_i[0, k] 34 | y = X_i[1, k] 35 | dx = np.cos(heading) * m.robot_radius 36 | dy = np.sin(heading) * m.robot_radius 37 | 38 | ax.arrow(x, y, dx, dy, color='green', head_width=0.2, length_includes_head=True) 39 | 40 | robot = plt.Circle((x, y), m.robot_radius, color='gray', fill=False) 41 | ax.add_artist(robot) 42 | 43 | for obst in m.obstacles: 44 | x, y = obst[0] 45 | r = obst[1] 46 | 47 | obstacle1 = plt.Circle((x, y), r, color='black', fill=False) 48 | ax.add_artist(obstacle1) 49 | 50 | ax = fig.add_subplot(gs2[0, 0]) 51 | x = np.linspace(0, sigma[figures_i], K) 52 | ax.plot(x, U_i[0, :]) 53 | ax.set_xlabel('time [s]') 54 | ax.set_ylabel('velocity [m/s]') 55 | 56 | ax = fig.add_subplot(gs2[0, 1]) 57 | ax.plot(x, np.rad2deg(U_i[1, :])) 58 | ax.set_xlabel('time [s]') 59 | ax.set_ylabel('angular velocity [°/s]') 60 | 61 | 62 | def key_press_event(event): 63 | global figures_i, figures_N 64 | 65 | fig = event.canvas.figure 66 | 67 | if event.key == 'q' or event.key == 'escape': 68 | plt.close(event.canvas.figure) 69 | return 70 | if event.key == 'right': 71 | figures_i += 1 72 | figures_i %= figures_N 73 | elif event.key == 'left': 74 | figures_i -= 1 75 | figures_i %= figures_N 76 | fig.clear() 77 | my_plot(fig, figures_i) 78 | plt.draw() 79 | 80 | 81 | def plot(X_in, U_in, sigma_in): 82 | global figures_i, figures_N 83 | figures_N = X_in.shape[0] 84 | figures_i = figures_N - 1 85 | 86 | global X, U, sigma 87 | X = X_in 88 | U = U_in 89 | sigma = sigma_in 90 | 91 | fig = plt.figure(figsize=(10, 12)) 92 | my_plot(fig, figures_i) 93 | cid = fig.canvas.mpl_connect('key_press_event', key_press_event) 94 | plt.show() 95 | 96 | 97 | if __name__ == "__main__": 98 | import os 99 | 100 | folder_number = str(int(max(os.listdir('output/trajectory/')))).zfill(3) 101 | 102 | X_in = np.load(f"output/trajectory/{folder_number}/X.npy") 103 | U_in = np.load(f"output/trajectory/{folder_number}/U.npy") 104 | sigma_in = np.load(f"output/trajectory/{folder_number}/sigma.npy") 105 | 106 | plot(X_in, U_in, sigma_in) 107 | -------------------------------------------------------------------------------- /Models/rocket_landing_2d.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cvxpy as cvx 3 | import sympy as sp 4 | 5 | from global_parameters import K 6 | 7 | 8 | class Model: 9 | """ 10 | A 2D path rocket landing problem. 11 | """ 12 | n_x = 6 13 | n_u = 2 14 | 15 | m = 2 16 | I = 1e-2 17 | 18 | r_init = [4., 4.] 19 | v_init = [-2., -1.] 20 | t_init = [np.deg2rad(0.), ] 21 | w_init = [np.deg2rad(0.), ] 22 | 23 | r_final = [0., 0.] 24 | v_final = [0., 0.] 25 | t_final = [np.deg2rad(0.), ] 26 | w_final = [np.deg2rad(0.), ] 27 | 28 | t_f_guess = 10 # s 29 | 30 | t_max = np.deg2rad(60) 31 | w_max = np.deg2rad(60) 32 | max_gimbal = np.deg2rad(7) 33 | T_max = 5 34 | T_min = T_max * 0.4 35 | r_T = 1e-2 36 | g = 1 37 | 38 | def __init__(self): 39 | self.r_scale = np.linalg.norm(self.r_init) 40 | self.m_scale = self.m 41 | 42 | self.x_init = np.concatenate((self.r_init, self.v_init, self.t_init, self.w_init)) 43 | self.x_final = np.concatenate((self.r_final, self.v_final, self.t_final, self.w_final)) 44 | 45 | def nondimensionalize(self): 46 | """ nondimensionalize all parameters and boundaries """ 47 | self.r_init /= self.r_scale 48 | self.v_init /= self.r_scale 49 | self.r_T /= self.r_scale 50 | self.g /= self.r_scale 51 | self.I /= self.m_scale * self.r_scale ** 2 52 | self.m /= self.m_scale 53 | self.T_min /= self.m_scale * self.r_scale 54 | self.T_max /= self.m_scale * self.r_scale 55 | 56 | self.x_init = self.x_nondim(self.x_init) 57 | self.x_final = self.x_nondim(self.x_final) 58 | 59 | def x_nondim(self, x): 60 | """ nondimensionalize a single x row """ 61 | x[0:4] /= self.r_scale 62 | return x 63 | 64 | def u_nondim(self, u): 65 | """ nondimensionalize u""" 66 | u[1, :] /= self.m_scale * self.r_scale 67 | return u 68 | 69 | def redimensionalize(self): 70 | """ redimensionalize all parameters """ 71 | self.r_init *= self.r_scale 72 | self.v_init *= self.r_scale 73 | self.r_T *= self.r_scale 74 | self.g *= self.r_scale 75 | self.I *= self.m_scale * self.r_scale ** 2 76 | self.m *= self.m_scale 77 | self.T_min *= self.m_scale * self.r_scale 78 | self.T_max *= self.m_scale * self.r_scale 79 | 80 | self.x_init = self.x_redim(self.x_init) 81 | self.x_final = self.x_redim(self.x_final) 82 | 83 | def x_redim(self, x): 84 | """ redimensionalize x, assumed to have the shape of a solution """ 85 | x[0:4] *= self.r_scale 86 | return x 87 | 88 | def u_redim(self, u): 89 | """ redimensionalize u """ 90 | u[1, :] *= self.m_scale * self.r_scale 91 | return u 92 | 93 | def get_equations(self): 94 | """ 95 | :return: Functions to calculate A, B and f given state x and input u 96 | """ 97 | f = sp.zeros(6, 1) 98 | 99 | x = sp.Matrix(sp.symbols('rx ry vx vy t w', real=True)) 100 | u = sp.Matrix(sp.symbols('gimbal T', real=True)) 101 | 102 | f[0, 0] = x[2, 0] 103 | f[1, 0] = x[3, 0] 104 | f[2, 0] = 1 / self.m * sp.sin(x[4, 0] + u[0, 0]) * u[1, 0] 105 | f[3, 0] = 1 / self.m * (sp.cos(x[4, 0] + u[0, 0]) * u[1, 0] - self.m * self.g) 106 | f[4, 0] = x[5, 0] 107 | f[5, 0] = 1 / self.I * (-sp.sin(u[0, 0]) * u[1, 0] * self.r_T) 108 | 109 | f = sp.simplify(f) 110 | A = sp.simplify(f.jacobian(x)) 111 | B = sp.simplify(f.jacobian(u)) 112 | 113 | f_func = sp.lambdify((x, u), f, 'numpy') 114 | A_func = sp.lambdify((x, u), A, 'numpy') 115 | B_func = sp.lambdify((x, u), B, 'numpy') 116 | 117 | return f_func, A_func, B_func 118 | 119 | def initialize_trajectory(self, X, U): 120 | """ 121 | Initialize the trajectory. 122 | 123 | :param X: Numpy array of states to be initialized 124 | :param U: Numpy array of inputs to be initialized 125 | :return: The initialized X and U 126 | """ 127 | 128 | for k in range(K): 129 | alpha1 = (K - k) / K 130 | alpha2 = k / K 131 | 132 | X[:, k] = alpha1 * self.x_init + alpha2 * self.x_final 133 | U[0, :] = 0 134 | U[1, :] = (self.T_max - self.T_min) / 2 135 | 136 | return X, U 137 | 138 | def get_objective(self, X_v, U_v, X_last_p, U_last_p): 139 | """ 140 | Get model specific objective to be minimized. 141 | 142 | :param X_v: cvx variable for current states 143 | :param U_v: cvx variable for current inputs 144 | :param X_last_p: cvx parameter for last states 145 | :param U_last_p: cvx parameter for last inputs 146 | :return: A cvx objective function. 147 | """ 148 | objective = None 149 | return objective 150 | 151 | def get_constraints(self, X_v, U_v, X_last_p, U_last_p): 152 | """ 153 | Get model specific constraints. 154 | 155 | :param X_v: cvx variable for current states 156 | :param U_v: cvx variable for current inputs 157 | :param X_last_p: cvx parameter for last states 158 | :param U_last_p: cvx parameter for last inputs 159 | :return: A list of cvx constraints 160 | """ 161 | 162 | constraints = [ 163 | # Boundary conditions: 164 | X_v[0:2, 0] == self.x_init[0:2], 165 | X_v[2:4, 0] == self.x_init[2:4], 166 | X_v[4, 0] == self.x_init[4], 167 | X_v[5, 0] == self.x_init[5], 168 | 169 | X_v[:, -1] == self.x_final, 170 | 171 | # State constraints: 172 | cvx.abs(X_v[4, :]) <= self.t_max, 173 | cvx.abs(X_v[5, :]) <= self.w_max, 174 | X_v[1, :] >= 0, 175 | 176 | # Control constraints: 177 | cvx.abs(U_v[0, :]) <= self.max_gimbal, 178 | U_v[1, :] >= self.T_min, 179 | U_v[1, :] <= self.T_max, 180 | ] 181 | return constraints 182 | 183 | def get_linear_cost(self): 184 | return 0 185 | 186 | def get_nonlinear_cost(self, X, U=None): 187 | return 0 188 | -------------------------------------------------------------------------------- /Models/rocket_landing_2d_plot.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | 4 | # vector scaling 5 | attitude_scale = 15 6 | thrust_scale = 100 7 | 8 | 9 | def my_plot(fig, figures_i): 10 | ax = fig.add_subplot(111) 11 | 12 | X_i = X[figures_i, :, :] 13 | U_i = U[figures_i, :, :] 14 | K = X_i.shape[1] 15 | 16 | ax.set_xlabel('X, east') 17 | ax.set_ylabel('Y, up') 18 | 19 | ax.plot(X_i[0, :], X_i[1, :], color='lightgrey', zorder=0) 20 | 21 | rx = X_i[0, :] 22 | ry = X_i[1, :] 23 | 24 | dx = np.sin(X_i[4, :]) 25 | dy = np.cos(X_i[4, :]) 26 | 27 | Fx = -np.sin(X_i[4, :] + U_i[0, :]) * U_i[1, :] 28 | Fy = -np.cos(X_i[4, :] + U_i[0, :]) * U_i[1, :] 29 | 30 | # attitude vector 31 | ax.quiver(rx, ry, dx, dy, color='blue', width=0.003, scale=attitude_scale, headwidth=1, headlength=0) 32 | 33 | # force vector 34 | ax.quiver(rx, ry, Fx, Fy, color='red', width=0.002, scale=thrust_scale, headwidth=1, headlength=0) 35 | 36 | ax.set_title("Iteration " + str(figures_i)) 37 | 38 | 39 | def key_press_event(event): 40 | global figures_i, figures_N 41 | 42 | fig = event.canvas.figure 43 | 44 | if event.key == 'q' or event.key == 'escape': 45 | plt.close(event.canvas.figure) 46 | return 47 | if event.key == 'right': 48 | figures_i += 1 49 | figures_i %= figures_N 50 | elif event.key == 'left': 51 | figures_i -= 1 52 | figures_i %= figures_N 53 | fig.clear() 54 | my_plot(fig, figures_i) 55 | plt.draw() 56 | 57 | 58 | def plot(X_in, U_in, sigma_in): 59 | global figures_i, figures_N 60 | figures_N = X_in.shape[0] 61 | figures_i = figures_N - 1 62 | 63 | global X, U, sigma 64 | X = X_in 65 | U = U_in 66 | sigma = sigma_in 67 | 68 | fig = plt.figure(figsize=(10, 12)) 69 | my_plot(fig, figures_i) 70 | cid = fig.canvas.mpl_connect('key_press_event', key_press_event) 71 | plt.show() 72 | 73 | 74 | if __name__ == "__main__": 75 | import os 76 | 77 | folder_number = str(int(max(os.listdir('output/trajectory/')))).zfill(3) 78 | 79 | X_in = np.load(f"output/trajectory/{folder_number}/X.npy") 80 | U_in = np.load(f"output/trajectory/{folder_number}/U.npy") 81 | sigma_in = np.load(f"output/trajectory/{folder_number}/sigma.npy") 82 | 83 | plot(X_in, U_in, sigma_in) 84 | -------------------------------------------------------------------------------- /Models/rocket_landing_3d.py: -------------------------------------------------------------------------------- 1 | import sympy as sp 2 | import numpy as np 3 | import cvxpy as cvx 4 | from utils import euler_to_quat 5 | from global_parameters import K 6 | 7 | 8 | def skew(v): 9 | return sp.Matrix([ 10 | [0, -v[2], v[1]], 11 | [v[2], 0, -v[0]], 12 | [-v[1], v[0], 0] 13 | ]) 14 | 15 | 16 | def dir_cosine(q): 17 | return sp.Matrix([ 18 | [1 - 2 * (q[2] ** 2 + q[3] ** 2), 2 * (q[1] * q[2] + q[0] * q[3]), 2 * (q[1] * q[3] - q[0] * q[2])], 19 | [2 * (q[1] * q[2] - q[0] * q[3]), 1 - 2 * (q[1] ** 2 + q[3] ** 2), 2 * (q[2] * q[3] + q[0] * q[1])], 20 | [2 * (q[1] * q[3] + q[0] * q[2]), 2 * (q[2] * q[3] - q[0] * q[1]), 1 - 2 * (q[1] ** 2 + q[2] ** 2)] 21 | ]) 22 | 23 | 24 | def omega(w): 25 | return sp.Matrix([ 26 | [0, -w[0], -w[1], -w[2]], 27 | [w[0], 0, w[2], -w[1]], 28 | [w[1], -w[2], 0, w[0]], 29 | [w[2], w[1], -w[0], 0], 30 | ]) 31 | 32 | 33 | class Model: 34 | """ 35 | A 6 degree of freedom rocket landing problem. 36 | """ 37 | n_x = 14 38 | n_u = 3 39 | 40 | # Mass 41 | m_wet = 30000. # 30000 kg 42 | m_dry = 22000. # 22000 kg 43 | 44 | # Flight time guess 45 | t_f_guess = 15. # 10 s 46 | 47 | # State constraints 48 | r_I_init = np.array((0., 200., 200.)) # 2000 m, 200 m, 200 m 49 | v_I_init = np.array((-50., -100., -50.)) # -300 m/s, 50 m/s, 50 m/s 50 | q_B_I_init = euler_to_quat((0, 0, 0)) 51 | w_B_init = np.deg2rad(np.array((0., 0., 0.))) 52 | 53 | r_I_final = np.array((0., 0., 0.)) 54 | v_I_final = np.array((0., 0., -5.)) 55 | q_B_I_final = euler_to_quat((0, 0, 0)) 56 | w_B_final = np.deg2rad(np.array((0., 0., 0.))) 57 | 58 | w_B_max = np.deg2rad(90) 59 | 60 | # Angles 61 | max_gimbal = 7 62 | max_angle = 70 63 | glidelslope_angle = 20 64 | 65 | tan_delta_max = np.tan(np.deg2rad(max_gimbal)) 66 | cos_delta_max = np.tan(np.deg2rad(max_gimbal)) 67 | cos_theta_max = np.cos(np.deg2rad(max_angle)) 68 | tan_gamma_gs = np.tan(np.deg2rad(glidelslope_angle)) 69 | 70 | # Thrust limits 71 | T_max = 800000. # 800000 [kg*m/s^2] 72 | T_min = T_max * 0.4 73 | 74 | # Angular moment of inertia 75 | J_B = np.diag([4000000., 4000000., 100000.]) # 100000 [kg*m^2], 4000000 [kg*m^2], 4000000 [kg*m^2] 76 | 77 | # Gravity 78 | g_I = np.array((0., 0., -9.81)) # -9.81 [m/s^2] 79 | 80 | # Fuel consumption 81 | alpha_m = 1 / (282 * 9.81) # 1 / (282 * 9.81) [s/m] 82 | 83 | # Vector from thrust point to CoM 84 | r_T_B = np.array([0., 0., -14.]) # -20 m 85 | 86 | def set_random_initial_state(self): 87 | self.r_I_init[2] = 500 88 | self.r_I_init[0:2] = np.random.uniform(-300, 300, size=2) 89 | 90 | self.v_I_init[2] = np.random.uniform(-100, -60) 91 | self.v_I_init[0:2] = np.random.uniform(-0.5, -0.2, size=2) * self.r_I_init[0:2] 92 | 93 | self.q_B_I_init = euler_to_quat((np.random.uniform(-30, 30), 94 | np.random.uniform(-30, 30), 95 | 0)) 96 | self.w_B_init = np.deg2rad((np.random.uniform(-20, 20), 97 | np.random.uniform(-20, 20), 98 | 0)) 99 | 100 | # ------------------------------------------ Start normalization stuff 101 | def __init__(self): 102 | """ 103 | A large r_scale for a small scale problem will 104 | ead to numerical problems as parameters become excessively small 105 | and (it seems) precision is lost in the dynamics. 106 | """ 107 | 108 | self.set_random_initial_state() 109 | 110 | self.x_init = np.concatenate(((self.m_wet,), self.r_I_init, self.v_I_init, self.q_B_I_init, self.w_B_init)) 111 | self.x_final = np.concatenate(((self.m_dry,), self.r_I_final, self.v_I_final, self.q_B_I_final, self.w_B_final)) 112 | 113 | self.r_scale = np.linalg.norm(self.r_I_init) 114 | self.m_scale = self.m_wet 115 | 116 | # slack variable for linear constraint relaxation 117 | self.s_prime = cvx.Variable((K, 1), nonneg=True) 118 | 119 | # slack variable for lossless convexification 120 | # self.gamma = cvx.Variable(K, nonneg=True) 121 | 122 | def nondimensionalize(self): 123 | """ nondimensionalize all parameters and boundaries """ 124 | 125 | self.alpha_m *= self.r_scale # s 126 | self.r_T_B /= self.r_scale # 1 127 | self.g_I /= self.r_scale # 1/s^2 128 | self.J_B /= (self.m_scale * self.r_scale ** 2) # 1 129 | 130 | self.x_init = self.x_nondim(self.x_init) 131 | self.x_final = self.x_nondim(self.x_final) 132 | 133 | self.T_max = self.u_nondim(self.T_max) 134 | self.T_min = self.u_nondim(self.T_min) 135 | 136 | self.m_wet /= self.m_scale 137 | self.m_dry /= self.m_scale 138 | 139 | def x_nondim(self, x): 140 | """ nondimensionalize a single x row """ 141 | 142 | x[0] /= self.m_scale 143 | x[1:4] /= self.r_scale 144 | x[4:7] /= self.r_scale 145 | 146 | return x 147 | 148 | def u_nondim(self, u): 149 | """ nondimensionalize u, or in general any force in Newtons""" 150 | return u / (self.m_scale * self.r_scale) 151 | 152 | def redimensionalize(self): 153 | """ redimensionalize all parameters """ 154 | 155 | self.alpha_m /= self.r_scale # s 156 | self.r_T_B *= self.r_scale 157 | self.g_I *= self.r_scale 158 | self.J_B *= (self.m_scale * self.r_scale ** 2) 159 | 160 | self.T_max = self.u_redim(self.T_max) 161 | self.T_min = self.u_redim(self.T_min) 162 | 163 | self.m_wet *= self.m_scale 164 | self.m_dry *= self.m_scale 165 | 166 | def x_redim(self, x): 167 | """ redimensionalize x, assumed to have the shape of a solution """ 168 | 169 | x[0, :] *= self.m_scale 170 | x[1:4, :] *= self.r_scale 171 | x[4:7, :] *= self.r_scale 172 | 173 | return x 174 | 175 | def u_redim(self, u): 176 | """ redimensionalize u """ 177 | return u * (self.m_scale * self.r_scale) 178 | 179 | # ------------------------------------------ End normalization stuff 180 | 181 | def get_equations(self): 182 | """ 183 | :return: Functions to calculate A, B and f given state x and input u 184 | """ 185 | f = sp.zeros(14, 1) 186 | 187 | x = sp.Matrix(sp.symbols('m rx ry rz vx vy vz q0 q1 q2 q3 wx wy wz', real=True)) 188 | u = sp.Matrix(sp.symbols('ux uy uz', real=True)) 189 | 190 | g_I = sp.Matrix(self.g_I) 191 | r_T_B = sp.Matrix(self.r_T_B) 192 | J_B = sp.Matrix(self.J_B) 193 | 194 | C_B_I = dir_cosine(x[7:11, 0]) 195 | C_I_B = C_B_I.transpose() 196 | 197 | f[0, 0] = - self.alpha_m * u.norm() 198 | f[1:4, 0] = x[4:7, 0] 199 | f[4:7, 0] = 1 / x[0, 0] * C_I_B * u + g_I 200 | f[7:11, 0] = 1 / 2 * omega(x[11:14, 0]) * x[7: 11, 0] 201 | f[11:14, 0] = J_B ** -1 * (skew(r_T_B) * u) - skew(x[11:14, 0]) * x[11:14, 0] 202 | 203 | f = sp.simplify(f) 204 | A = sp.simplify(f.jacobian(x)) 205 | B = sp.simplify(f.jacobian(u)) 206 | 207 | f_func = sp.lambdify((x, u), f, 'numpy') 208 | A_func = sp.lambdify((x, u), A, 'numpy') 209 | B_func = sp.lambdify((x, u), B, 'numpy') 210 | 211 | return f_func, A_func, B_func 212 | 213 | def initialize_trajectory(self, X, U): 214 | """ 215 | Initialize the trajectory. 216 | 217 | :param X: Numpy array of states to be initialized 218 | :param U: Numpy array of inputs to be initialized 219 | :return: The initialized X and U 220 | """ 221 | 222 | for k in range(K): 223 | alpha1 = (K - k) / K 224 | alpha2 = k / K 225 | 226 | m_k = (alpha1 * self.x_init[0] + alpha2 * self.x_final[0],) 227 | r_I_k = alpha1 * self.x_init[1:4] + alpha2 * self.x_final[1:4] 228 | v_I_k = alpha1 * self.x_init[4:7] + alpha2 * self.x_final[4:7] 229 | q_B_I_k = np.array([1, 0, 0, 0]) 230 | w_B_k = alpha1 * self.x_init[11:14] + alpha2 * self.x_final[11:14] 231 | 232 | X[:, k] = np.concatenate((m_k, r_I_k, v_I_k, q_B_I_k, w_B_k)) 233 | U[:, k] = (self.T_max - self.T_min) / 2 * np.array([0, 0, 1]) 234 | 235 | return X, U 236 | 237 | def get_objective(self, X_v, U_v, X_last_p, U_last_p): 238 | """ 239 | Get model specific objective to be minimized. 240 | 241 | :param X_v: cvx variable for current states 242 | :param U_v: cvx variable for current inputs 243 | :param X_last_p: cvx parameter for last states 244 | :param U_last_p: cvx parameter for last inputs 245 | :return: A cvx objective function. 246 | """ 247 | return cvx.Minimize(1e5 * cvx.sum(self.s_prime)) 248 | 249 | def get_constraints(self, X_v, U_v, X_last_p, U_last_p): 250 | """ 251 | Get model specific constraints. 252 | 253 | :param X_v: cvx variable for current states 254 | :param U_v: cvx variable for current inputs 255 | :param X_last_p: cvx parameter for last states 256 | :param U_last_p: cvx parameter for last inputs 257 | :return: A list of cvx constraints 258 | """ 259 | # Boundary conditions: 260 | constraints = [ 261 | X_v[0, 0] == self.x_init[0], 262 | X_v[1:4, 0] == self.x_init[1:4], 263 | X_v[4:7, 0] == self.x_init[4:7], 264 | X_v[7:11, 0] == self.x_init[7:11], 265 | X_v[11:14, 0] == self.x_init[11:14], 266 | 267 | # X_[0, -1] == self.x_final[0], # final mass is free 268 | X_v[1:, -1] == self.x_final[1:], 269 | # U_v[1:3, -1] == 0, 270 | ] 271 | 272 | constraints += [ 273 | # State constraints: 274 | X_v[0, :] >= self.m_dry, # minimum mass 275 | cvx.norm(X_v[1: 3, :], axis=0) <= X_v[3, :] / self.tan_gamma_gs, # glideslope 276 | cvx.norm(X_v[8:10, :], axis=0) <= np.sqrt((1 - self.cos_theta_max) / 2), # maximum angle 277 | cvx.norm(X_v[11: 14, :], axis=0) <= self.w_B_max, # maximum angular velocity 278 | 279 | # Control constraints: 280 | cvx.norm(U_v[0:2, :], axis=0) <= self.tan_delta_max * U_v[2, :], # gimbal angle constraint 281 | # self.cos_delta_max * self.gamma <= U_v[2, :], 282 | 283 | cvx.norm(U_v, axis=0) <= self.T_max, # upper thrust constraint 284 | # U_v[2, :] >= self.T_min # simple lower thrust constraint 285 | 286 | # # Lossless convexification: 287 | # self.gamma <= self.T_max, 288 | # self.T_min <= self.gamma, 289 | # cvx.norm(U_v, axis=0) <= self.gamma 290 | ] 291 | 292 | # linearized lower thrust constraint 293 | lhs = [U_last_p[:, k] / (cvx.norm(U_last_p[:, k])) * U_v[:, k] for k in range(K)] 294 | constraints += [ 295 | self.T_min - cvx.vstack(lhs) <= self.s_prime 296 | ] 297 | 298 | return constraints 299 | 300 | def get_linear_cost(self): 301 | cost = np.sum(self.s_prime.value) 302 | return cost 303 | 304 | def get_nonlinear_cost(self, X=None, U=None): 305 | magnitude = np.linalg.norm(U, 2, axis=0) 306 | is_violated = magnitude < self.T_min 307 | violation = self.T_min - magnitude 308 | cost = np.sum(is_violated * violation) 309 | return cost 310 | -------------------------------------------------------------------------------- /Models/rocket_landing_3d_plot.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | from mpl_toolkits.mplot3d import art3d 4 | 5 | figures_i = 0 6 | 7 | # vector scaling 8 | thrust_scale = 0.00002 9 | attitude_scale = 20 10 | 11 | 12 | def key_press_event(event): 13 | global figures_i 14 | fig = event.canvas.figure 15 | 16 | if event.key == 'q' or event.key == 'escape': 17 | plt.close(event.canvas.figure) 18 | return 19 | 20 | if event.key == 'right': 21 | figures_i = (figures_i + 1) % figures_N 22 | elif event.key == 'left': 23 | figures_i = (figures_i - 1) % figures_N 24 | 25 | fig.clear() 26 | my_plot(fig, figures_i) 27 | plt.draw() 28 | 29 | 30 | def my_plot(fig, figures_i): 31 | ax = fig.add_subplot(111, projection='3d') 32 | 33 | X_i = X[figures_i, :, :] 34 | U_i = U[figures_i, :, :] 35 | K = X_i.shape[1] 36 | 37 | ax.set_xlabel('X, east') 38 | ax.set_ylabel('Y, north') 39 | ax.set_zlabel('Z, up') 40 | 41 | for k in range(K): 42 | rx, ry, rz = X_i[1:4, k] 43 | qw, qx, qy, qz = X_i[7:11, k] 44 | 45 | CBI = np.array([ 46 | [1 - 2 * (qy ** 2 + qz ** 2), 2 * (qx * qy + qw * qz), 2 * (qx * qz - qw * qy)], 47 | [2 * (qx * qy - qw * qz), 1 - 2 * (qx ** 2 + qz ** 2), 2 * (qy * qz + qw * qx)], 48 | [2 * (qx * qz + qw * qy), 2 * (qy * qz - qw * qx), 1 - 2 * (qx ** 2 + qy ** 2)] 49 | ]) 50 | 51 | dx, dy, dz = np.dot(np.transpose(CBI), np.array([0., 0., 1.])) 52 | Fx, Fy, Fz = np.dot(np.transpose(CBI), U_i[:, k]) 53 | 54 | # attitude vector 55 | ax.quiver(rx, ry, rz, dx, dy, dz, length=attitude_scale, arrow_length_ratio=0.0, color='blue') 56 | 57 | # thrust vector 58 | ax.quiver(rx, ry, rz, -Fx, -Fy, -Fz, length=thrust_scale, arrow_length_ratio=0.0, color='red') 59 | 60 | scale = X_i[3, 0] 61 | ax.auto_scale_xyz([-scale / 2, scale / 2], [-scale / 2, scale / 2], [0, scale]) 62 | 63 | pad = plt.Circle((0, 0), 20, color='lightgray') 64 | ax.add_patch(pad) 65 | art3d.pathpatch_2d_to_3d(pad) 66 | 67 | ax.set_title("Iteration " + str(figures_i)) 68 | ax.plot(X_i[1, :], X_i[2, :], X_i[3, :], color='lightgrey') 69 | ax.set_aspect('equal') 70 | 71 | def plot(X_in, U_in, sigma_in): 72 | global figures_N 73 | figures_N = X_in.shape[0] 74 | figures_i = figures_N - 1 75 | 76 | global X, U 77 | X = X_in 78 | U = U_in 79 | 80 | fig = plt.figure(figsize=(10, 12)) 81 | my_plot(fig, figures_i) 82 | cid = fig.canvas.mpl_connect('key_press_event', key_press_event) 83 | plt.show() 84 | 85 | 86 | if __name__ == "__main__": 87 | import os 88 | 89 | folder_number = str(int(max(os.listdir('output/trajectory/')))).zfill(3) 90 | 91 | X_in = np.load(f"output/trajectory/{folder_number}/X.npy") 92 | U_in = np.load(f"output/trajectory/{folder_number}/U.npy") 93 | sigma_in = np.load(f"output/trajectory/{folder_number}/sigma.npy") 94 | 95 | plot(X_in, U_in, sigma_in) 96 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # SuccessiveConvexification 2 | Implementation of [Successive Convexification: A Superlinearly Convergent Algorithm for Non-convex Optimal Control Problems 3 | ](https://arxiv.org/abs/1804.06539) by Yuanqi Mao, Michael Szmuk and Behcet Acikmese 4 | 5 | Reqires `matplotlib`, `numpy`, `scipy` and `cvxpy`. 6 | 7 | 8 | This framework provides an easy way to implement your own models. 9 | 10 | Fixed- and free-final-time optimization is possible. 11 | 12 | The following models are currently implemented: 13 | 14 | - Rocket trajectory model with free-final-time based on 15 | [Successive Convexification for 6-DoF Mars Rocket Powered Landing with Free-Final-Time](https://arxiv.org/abs/1802.03827) 16 | by Michael Szmuk and Behçet Açıkmeşe. 17 | 18 | ![](https://i.imgur.com/W6E0rgL.png) 19 | 20 | [Video example of generated trajectories](https://gfycat.com/InbornCoarseArcticseal) 21 | 22 | - Differential drive robot path planning with free-final-time and non-convex obstacle constraints: 23 | ![](https://i.imgur.com/xNaD5eP.png) 24 | 25 | - 2D rocket landing problem 26 | [Feed-forward input tested in a box2d physics simulation](https://gfycat.com/DaringPortlyBlacklab) 27 | -------------------------------------------------------------------------------- /fixedfinaltime.py: -------------------------------------------------------------------------------- 1 | from time import time 2 | import numpy as np 3 | 4 | from FixedFinalTime.parameters import * 5 | from FixedFinalTime.discretization import FirstOrderHold 6 | from FixedFinalTime.scproblem import SCProblem 7 | from utils import format_line, save_arrays 8 | 9 | # from Models.diffdrive_2d import Model 10 | # from Models.diffdrive_2d_plot import plot 11 | from Models.rocket_landing_3d import Model 12 | from Models.rocket_landing_3d_plot import plot 13 | 14 | """ 15 | Python implementation of the Successive Convexification algorithm. 16 | 17 | Rocket trajectory model based on the 18 | 'Successive Convexification for 6-DoF Mars Rocket Powered Landing with Free-Final-Time' paper 19 | by Michael Szmuk and Behçet Açıkmeşe. 20 | 21 | Implementation by Sven Niederberger (s-niederberger@outlook.com) 22 | """ 23 | 24 | m = Model() 25 | m.nondimensionalize() 26 | 27 | # state and input 28 | X = np.empty(shape=[m.n_x, K]) 29 | U = np.empty(shape=[m.n_u, K]) 30 | 31 | # INITIALIZATION-------------------------------------------------------------------------------------------------------- 32 | sigma = m.t_f_guess 33 | X, U = m.initialize_trajectory(X, U) 34 | 35 | # START SUCCESSIVE CONVEXIFICATION-------------------------------------------------------------------------------------- 36 | all_X = [m.x_redim(X.copy())] 37 | all_U = [m.u_redim(U.copy())] 38 | 39 | integrator = FirstOrderHold(m, K, sigma) 40 | problem = SCProblem(m, K) 41 | 42 | last_nonlinear_cost = None 43 | converged = False 44 | for it in range(iterations): 45 | t0_it = time() 46 | print('-' * 50) 47 | print('-' * 18 + f' Iteration {str(it + 1).zfill(2)} ' + '-' * 18) 48 | print('-' * 50) 49 | 50 | t0_tm = time() 51 | A_bar, B_bar, C_bar, z_bar = integrator.calculate_discretization(X, U) 52 | print(format_line('Time for transition matrices', time() - t0_tm, 's')) 53 | 54 | problem.set_parameters(A_bar=A_bar, B_bar=B_bar, C_bar=C_bar, z_bar=z_bar, 55 | X_last=X, U_last=U, 56 | weight_nu=w_nu, tr_radius=tr_radius) 57 | 58 | while True: 59 | error = problem.solve(verbose=verbose_solver, solver=solver, max_iters=200) 60 | print(format_line('Solver Error', error)) 61 | 62 | # get solution 63 | new_X = problem.get_variable('X') 64 | new_U = problem.get_variable('U') 65 | 66 | X_nl = integrator.integrate_nonlinear_piecewise(new_X, new_U) 67 | 68 | linear_cost_dynamics = np.linalg.norm(problem.get_variable('nu'), 1) 69 | nonlinear_cost_dynamics = np.linalg.norm(new_X - X_nl, 1) 70 | 71 | linear_cost_constraints = m.get_linear_cost() 72 | nonlinear_cost_constraints = m.get_nonlinear_cost(X=new_X, U=new_U) 73 | 74 | linear_cost = linear_cost_dynamics + linear_cost_constraints # J 75 | nonlinear_cost = nonlinear_cost_dynamics + nonlinear_cost_constraints # L 76 | 77 | if last_nonlinear_cost is None: 78 | last_nonlinear_cost = nonlinear_cost 79 | X = new_X 80 | U = new_U 81 | break 82 | 83 | actual_change = last_nonlinear_cost - nonlinear_cost # delta_J 84 | predicted_change = last_nonlinear_cost - linear_cost # delta_L 85 | 86 | print('') 87 | print(format_line('Virtual Control Cost', linear_cost_dynamics)) 88 | print(format_line('Constraint Cost', linear_cost_constraints)) 89 | print('') 90 | print(format_line('Actual change', actual_change)) 91 | print(format_line('Predicted change', predicted_change)) 92 | print('') 93 | 94 | if abs(predicted_change) < 1e-6: 95 | converged = True 96 | break 97 | else: 98 | rho = actual_change / predicted_change 99 | if rho < rho_0: 100 | # reject solution 101 | tr_radius /= alpha 102 | print(f'Trust region too large. Solving again with radius={tr_radius}') 103 | else: 104 | # accept solution 105 | X = new_X 106 | U = new_U 107 | 108 | print('Solution accepted.') 109 | 110 | if rho < rho_1: 111 | print('Decreasing radius.') 112 | tr_radius /= alpha 113 | elif rho >= rho_2: 114 | print('Increasing radius.') 115 | tr_radius *= beta 116 | 117 | last_nonlinear_cost = nonlinear_cost 118 | break 119 | 120 | problem.set_parameters(tr_radius=tr_radius) 121 | 122 | print('-' * 50) 123 | 124 | print('') 125 | print(format_line('Time for iteration', time() - t0_it, 's')) 126 | print('') 127 | 128 | all_X.append(m.x_redim(X.copy())) 129 | all_U.append(m.u_redim(U.copy())) 130 | 131 | if converged: 132 | print(f'Converged after {it + 1} iterations.') 133 | break 134 | 135 | all_X = np.stack(all_X) 136 | all_U = np.stack(all_U) 137 | all_sigma = np.ones(K) * sigma 138 | 139 | if not converged: 140 | print('Maximum number of iterations reached without convergence.') 141 | 142 | # save trajectory to file for visualization 143 | save_arrays('output/trajectory/', {'X': all_X, 'U': all_U, 'sigma': all_sigma}) 144 | 145 | # plot trajectory 146 | plot(all_X, all_U, all_sigma) 147 | -------------------------------------------------------------------------------- /freefinaltime.py: -------------------------------------------------------------------------------- 1 | from time import time 2 | import numpy as np 3 | 4 | from FreeFinalTime.parameters import * 5 | from FreeFinalTime.discretization import FirstOrderHold 6 | from FreeFinalTime.scproblem import SCProblem 7 | from utils import format_line, save_arrays 8 | 9 | # from Models.diffdrive_2d import Model 10 | # from Models.diffdrive_2d_plot import plot 11 | from Models.rocket_landing_3d import Model 12 | from Models.rocket_landing_3d_plot import plot 13 | 14 | """ 15 | Python implementation of the Successive Convexification algorithm. 16 | 17 | Rocket trajectory model based on the 18 | 'Successive Convexification for 6-DoF Mars Rocket Powered Landing with Free-Final-Time' paper 19 | by Michael Szmuk and Behçet Açıkmeşe. 20 | 21 | Implementation by Sven Niederberger (s-niederberger@outlook.com) 22 | """ 23 | 24 | m = Model() 25 | m.nondimensionalize() 26 | 27 | # state and input 28 | X = np.empty(shape=[m.n_x, K]) 29 | U = np.empty(shape=[m.n_u, K]) 30 | 31 | # INITIALIZATION-------------------------------------------------------------------------------------------------------- 32 | sigma = m.t_f_guess 33 | X, U = m.initialize_trajectory(X, U) 34 | 35 | # START SUCCESSIVE CONVEXIFICATION-------------------------------------------------------------------------------------- 36 | all_X = [m.x_redim(X.copy())] 37 | all_U = [m.u_redim(U.copy())] 38 | all_sigma = [sigma] 39 | 40 | integrator = FirstOrderHold(m, K) 41 | problem = SCProblem(m, K) 42 | 43 | last_nonlinear_cost = None 44 | converged = False 45 | for it in range(iterations): 46 | t0_it = time() 47 | print('-' * 50) 48 | print('-' * 18 + f' Iteration {str(it + 1).zfill(2)} ' + '-' * 18) 49 | print('-' * 50) 50 | 51 | t0_tm = time() 52 | A_bar, B_bar, C_bar, S_bar, z_bar = integrator.calculate_discretization(X, U, sigma) 53 | print(format_line('Time for transition matrices', time() - t0_tm, 's')) 54 | 55 | problem.set_parameters(A_bar=A_bar, B_bar=B_bar, C_bar=C_bar, S_bar=S_bar, z_bar=z_bar, 56 | X_last=X, U_last=U, sigma_last=sigma, 57 | weight_nu=w_nu, weight_sigma=w_sigma, tr_radius=tr_radius) 58 | 59 | while True: 60 | error = problem.solve(verbose=verbose_solver, solver=solver, max_iters=200) 61 | print(format_line('Solver Error', error)) 62 | 63 | # get solution 64 | new_X = problem.get_variable('X') 65 | new_U = problem.get_variable('U') 66 | new_sigma = problem.get_variable('sigma') 67 | 68 | X_nl = integrator.integrate_nonlinear_piecewise(new_X, new_U, new_sigma) 69 | 70 | linear_cost_dynamics = np.linalg.norm(problem.get_variable('nu'), 1) 71 | nonlinear_cost_dynamics = np.linalg.norm(new_X - X_nl, 1) 72 | 73 | linear_cost_constraints = m.get_linear_cost() 74 | nonlinear_cost_constraints = m.get_nonlinear_cost(X=new_X, U=new_U) 75 | 76 | linear_cost = linear_cost_dynamics + linear_cost_constraints # J 77 | nonlinear_cost = nonlinear_cost_dynamics + nonlinear_cost_constraints # L 78 | 79 | if last_nonlinear_cost is None: 80 | last_nonlinear_cost = nonlinear_cost 81 | X = new_X 82 | U = new_U 83 | sigma = new_sigma 84 | break 85 | 86 | actual_change = last_nonlinear_cost - nonlinear_cost # delta_J 87 | predicted_change = last_nonlinear_cost - linear_cost # delta_L 88 | 89 | print('') 90 | print(format_line('Virtual Control Cost', linear_cost_dynamics)) 91 | print(format_line('Constraint Cost', linear_cost_constraints)) 92 | print('') 93 | print(format_line('Actual change', actual_change)) 94 | print(format_line('Predicted change', predicted_change)) 95 | print('') 96 | print(format_line('Final time', sigma)) 97 | print('') 98 | 99 | if abs(predicted_change) < 1e-4: 100 | converged = True 101 | break 102 | else: 103 | rho = actual_change / predicted_change 104 | if rho < rho_0: 105 | # reject solution 106 | tr_radius /= alpha 107 | print(f'Trust region too large. Solving again with radius={tr_radius}') 108 | else: 109 | # accept solution 110 | X = new_X 111 | U = new_U 112 | sigma = new_sigma 113 | 114 | print('Solution accepted.') 115 | 116 | if rho < rho_1: 117 | print('Decreasing radius.') 118 | tr_radius /= alpha 119 | elif rho >= rho_2: 120 | print('Increasing radius.') 121 | tr_radius *= beta 122 | 123 | last_nonlinear_cost = nonlinear_cost 124 | break 125 | 126 | problem.set_parameters(tr_radius=tr_radius) 127 | 128 | print('-' * 50) 129 | 130 | print('') 131 | print(format_line('Time for iteration', time() - t0_it, 's')) 132 | print('') 133 | 134 | all_X.append(m.x_redim(X.copy())) 135 | all_U.append(m.u_redim(U.copy())) 136 | all_sigma.append(sigma) 137 | 138 | if converged: 139 | print(f'Converged after {it + 1} iterations.') 140 | break 141 | 142 | all_X = np.stack(all_X) 143 | all_U = np.stack(all_U) 144 | all_sigma = np.array(all_sigma) 145 | if not converged: 146 | print('Maximum number of iterations reached without convergence.') 147 | 148 | # save trajectory to file for visualization 149 | save_arrays('output/trajectory/', {'X': all_X, 'U': all_U, 'sigma': all_sigma}) 150 | 151 | # plot trajectory 152 | plot(all_X, all_U, all_sigma) 153 | -------------------------------------------------------------------------------- /global_parameters.py: -------------------------------------------------------------------------------- 1 | # Trajectory points 2 | K = 30 3 | 4 | # Max solver iterations 5 | iterations = 50 6 | 7 | solver = ['ECOS', 'MOSEK'][0] 8 | verbose_solver = False 9 | -------------------------------------------------------------------------------- /output/trajectory/000/U.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/EmbersArc/SCvx/6476c392d5f2d7498d4592c14417618ee828966d/output/trajectory/000/U.npy -------------------------------------------------------------------------------- /output/trajectory/000/X.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/EmbersArc/SCvx/6476c392d5f2d7498d4592c14417618ee828966d/output/trajectory/000/X.npy -------------------------------------------------------------------------------- /output/trajectory/000/sigma.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/EmbersArc/SCvx/6476c392d5f2d7498d4592c14417618ee828966d/output/trajectory/000/sigma.npy -------------------------------------------------------------------------------- /utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | 4 | 5 | def euler_to_quat(a): 6 | a = np.deg2rad(a) 7 | 8 | cy = np.cos(a[1] * 0.5) 9 | sy = np.sin(a[1] * 0.5) 10 | cr = np.cos(a[0] * 0.5) 11 | sr = np.sin(a[0] * 0.5) 12 | cp = np.cos(a[2] * 0.5) 13 | sp = np.sin(a[2] * 0.5) 14 | 15 | q = np.zeros(4) 16 | 17 | q[0] = cy * cr * cp + sy * sr * sp 18 | q[1] = cy * sr * cp - sy * cr * sp 19 | q[3] = cy * cr * sp + sy * sr * cp 20 | q[2] = sy * cr * cp - cy * sr * sp 21 | 22 | return q 23 | 24 | 25 | def format_line(name, value, unit=''): 26 | """ 27 | Formats a line e.g. 28 | {Name:} {value}{unit} 29 | """ 30 | name += ':' 31 | if isinstance(value, (float, np.ndarray)): 32 | value = f'{value:{0}.{4}}' 33 | 34 | return f'{name.ljust(40)}{value}{unit}' 35 | 36 | 37 | def save_arrays(path, a_dict): 38 | """ 39 | :param path: Output path 40 | :param a_dict: A dict containing the name of the array as key. 41 | """ 42 | path = path.rstrip('/') 43 | 44 | if not os.path.isdir(path): 45 | os.mkdir(path) 46 | 47 | if len(os.listdir(path)) == 0: 48 | folder_number = '000' 49 | else: 50 | folder_number = str(int(max(os.listdir(path))) + 1).zfill(3) 51 | 52 | os.mkdir(f'{path}/{folder_number}') 53 | 54 | for key in a_dict: 55 | np.save(f'{path}/{folder_number}/{key}.npy', a_dict[key]) 56 | --------------------------------------------------------------------------------