├── .gitignore ├── trajectory ├── U.p ├── X.p └── plot.py ├── LICENSE ├── parameters.py ├── README.md ├── dynamics_functions.py ├── main.py └── dynamics_generation.py /.gitignore: -------------------------------------------------------------------------------- 1 | .idea 2 | __pycache__ 3 | *.m~ 4 | pdf 5 | old 6 | -------------------------------------------------------------------------------- /trajectory/U.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jonnyhyman/SuccessiveConvexification/HEAD/trajectory/U.p -------------------------------------------------------------------------------- /trajectory/X.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jonnyhyman/SuccessiveConvexification/HEAD/trajectory/X.p -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2018 EmbersArc 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 | -------------------------------------------------------------------------------- /parameters.py: -------------------------------------------------------------------------------- 1 | from dynamics_functions import Dynamics 2 | import numpy as np 3 | 4 | # Trajectory points 5 | K = 70 6 | dt = 1 / (K - 1) 7 | 8 | # Max solver iterations 9 | iterations = 15 10 | 11 | # increase w_delta for iterations above 'start' 12 | # this puts major pressure on the dynamics trust region to converge 13 | force_converge = { 14 | 'active' : False, 15 | 'start' : 4, 16 | 'amount' : 1e3 17 | } 18 | 19 | # Mass 20 | m_wet = 2.0 21 | m_dry = 1.0 22 | 23 | # Flight time guess 24 | t_f_guess = 5. 25 | 26 | # # Weight constants 27 | w_nu = 1e5 28 | w_delta = 1e-3 29 | w_delta_sigma = 1e-1 30 | 31 | # Exit conditions 32 | nu_tol = 1e-8 33 | delta_tol = 1e-3 34 | 35 | # State constraints 36 | r_I_init = np.array([20., 5., 5.]) 37 | v_I_init = np.array([-4, -3, -2]) 38 | q_B_I_init = np.array([1.0, 0.0, 0.0, 0.0]) 39 | w_B_init = np.array([0., 0., 0.]) 40 | 41 | r_I_final = np.array([0., 0., 0.]) 42 | v_I_final = np.array([0., 0., 0.]) 43 | q_B_I_final = np.array([1.0, 0.0, 0.0, 0.0]) 44 | w_B_final = np.array([0., 0., 0.]) 45 | 46 | w_B_max = np.deg2rad(60) 47 | 48 | # Angles 49 | cos_delta_max = np.cos(np.deg2rad(20)) 50 | cos_theta_max = np.cos(np.deg2rad(90)) 51 | tan_gamma_gs = np.tan(np.deg2rad(20)) 52 | 53 | # Angular moment of inertia 54 | J_B_I = 1e-2 * np.eye(3) 55 | 56 | # Vector from thrust point to CoM 57 | r_T_B = np.array((-1e-2, 0., 0.)) 58 | 59 | # Gravity 60 | g_I = np.array((-1., 0., 0.)) 61 | 62 | # Thrust limits 63 | T_min = 2.0 64 | T_max = 5.0 65 | 66 | # Fuel consumption 67 | alpha_m = 0.01 68 | 69 | parms = { 70 | 'alpha': alpha_m, 71 | 'g_I' : g_I, 72 | 73 | 'rTB' : r_T_B, 74 | 'J' : J_B_I, 75 | } 76 | 77 | # Linearized state matrices: 78 | matrix_functions = Dynamics() 79 | matrix_functions.set_parameters(parms) 80 | 81 | # Add to local namespace so that the main program can see them here 82 | A = matrix_functions.A 83 | B = matrix_functions.B 84 | f = matrix_functions.f 85 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # SuccessiveConvexification 2 | 3 | Implementation of [Successive Convexification for 6-DoF Mars Rocket Powered Landing with Free-Final-Time](https://arxiv.org/abs/1802.03827) 4 | 5 | Reqires `python3`, `matplotlib`, `numpy`, `scipy`, `sympy` and `cvxpy 1.0.0`. 6 | 7 | Since [cvxpy 1.0.0](https://github.com/cvxgrp/cvxpy/tree/1.0) only supports Mac and Linux, Windows users are reccomended to use the [Windows Subsystem for Linux (WSL) and Ubuntu](https://en.wikipedia.org/wiki/Windows_Subsystem_for_Linux). This is confirmed to work with reasonable speed (this is my development environment). 8 | 9 | 10 | This branch: 11 | - Erases MATLAB (.m) code for symbolic dynamics code generation 12 | - Replaced with `sympy` symbolic code generation 13 | 14 | 15 | - Erases visualization code - for no reason other than I haven't yet used it really :) 16 | - Replaced with simple matplotlib plots 17 | 18 | 19 | - Adds a tremendous amount of documentation and pythonic operations 20 | - Mostly conforms to PEP8 21 | 22 | - Due to intense hatred of variables ending with `_`, replaced all cvx variables ending with `_` with proper names 23 | - v instead of _ denotes a cvx Variable 24 | - parm instead of _ denotes a cvx Parameter 25 | 26 | 27 | - Replaced a great number of () with [] to clarify definitions of arrays versus function calls 28 | 29 | - Replaced all dynamics functions in parameters with the sympy generated ones in dynamics_functions.py 30 | 31 | ---- 32 | 33 | # How to Run 34 | 35 | Solve dynamics matrices and generate all functions : `python dynamics_generation.py` 36 | 37 | Solve successive convex optimization problems : `python main.py` 38 | 39 | Plot solution variables : `python ./trajectory/plot.py` 40 | 41 | 42 | [Short Video Visualization by Sven Niederberger - from the master branch](https://gfycat.com/HideousUniqueEthiopianwolf) 43 | 44 | ---- 45 | 46 | Roadmap: 47 | 48 | - Lift and Drag Dynamics With Aerodynamic Controls, in the spirit of [this paper by Liu](https://arc.aiaa.org/doi/abs/10.2514/6.2017-1732) 49 | 50 | - Automatic scaling of inputs from real-world dimensional scenarios to dimensionless solution inputs, and the reverse for the output 51 | 52 | - C++ implementation likely to be forked from EmbersArc / Sven Niederberger's [WIP C++ implementation](https://github.com/EmbersArc/SuccessiveConvexificationCpp) 53 | 54 | - Real-time performance evaluation in a realistic spaceflight simulation 55 | 56 | 57 | 58 | History of this project: 59 | 60 | - After the paper came out, I immediately started working on a python implementation. I worked for about 2 weeks until I came across [Sven Niederberger's](https://github.com/EmbersArc) implementation on github! I ditched my implementation although it was nearly convergent. 61 | -------------------------------------------------------------------------------- /trajectory/plot.py: -------------------------------------------------------------------------------- 1 | from mpl_toolkits.mplot3d import Axes3D 2 | import matplotlib.pyplot as plt 3 | import numpy as np 4 | import pickle 5 | 6 | X_in = pickle.load(open("X.p", "rb")) 7 | U_in = pickle.load(open("U.p", "rb")) 8 | 9 | def plot_X(): 10 | 11 | ''' state variable 12 | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 13 | x = [m, r0, r1, r2, v0, v1, v2, q0, q1, q2, q3, w0, w1, w2].T 14 | 15 | control variable, thrust 16 | u = [T0, T1, T2] in body axes 17 | ''' 18 | 19 | x = np.array(X_in) 20 | u = np.array(U_in) 21 | 22 | T = np.array([np.linalg.norm(uk) for uk in u]) 23 | 24 | m = x[:,0] 25 | r = x[:,1:4] 26 | v = x[:,4:7] 27 | q = x[:,7:11] 28 | w = x[:,11:14] 29 | 30 | vm = np.array([np.linalg.norm(vk) for vk in v]) 31 | 32 | qm = np.array([1. - 2.*(qk[2]**2 + qk[3]**2) for qk in q]) 33 | qm = np.arccos(qm) 34 | qm = np.rad2deg(qm) 35 | 36 | dm = np.array([uk[0]/np.linalg.norm(uk) for uk in u]) 37 | dm = np.arccos(dm) 38 | dm = np.rad2deg(dm) 39 | 40 | wm = np.array([wk for wk in w]) 41 | wm = np.rad2deg(wm) 42 | 43 | plt.figure(1) 44 | 45 | plt.subplot(321) 46 | plt.plot( r[:,1], r[:,0], 'k', r[:,1], r[:,0],'g.', label='Up/East') 47 | plt.quiver( r[:,1], r[:,0], u[:,1], u[:,0], label='Control Vectors', 48 | scale=50,color='b',headwidth =2,pivot='tip') 49 | plt.legend() 50 | 51 | plt.subplot(322) 52 | plt.plot(qm, 'k', qm, 'b.', label='Tilt Angle [deg]') 53 | plt.plot((90,)*len(qm)) 54 | plt.legend() 55 | 56 | plt.subplot(323) 57 | plt.plot( T, 'k', T, 'c.', label='Thrust Magnitude') 58 | plt.legend() 59 | 60 | plt.subplot(324) 61 | plt.plot(wm, 'k', wm, 'b.', label='Angular Rate [deg/UT]') 62 | plt.plot((+60,)*len(wm)) 63 | plt.legend() 64 | 65 | plt.subplot(325) 66 | plt.plot( m, 'k', m, 'm.', label='Mass') 67 | plt.legend() 68 | 69 | plt.subplot(326) 70 | plt.plot(dm,label='Thrust Pointing Angle [deg]') 71 | plt.plot((20,)*len(dm)) 72 | plt.legend() 73 | plt.show() 74 | 75 | 76 | def cIB(q): 77 | q0, q1, q2, q3 = q 78 | 79 | cIB_m = np.zeros((3,3)) 80 | 81 | cIB_m[0,0] = 1-2*(q2**2 + q3**2) 82 | cIB_m[0,1] = 2*(q1*q2 + q0*q3) 83 | cIB_m[0,2] = 2*(q1*q3 - q0*q2) 84 | 85 | cIB_m[1,0] = 2*(q1*q2 - q0*q3) 86 | cIB_m[1,1] = 1-2*(q1**2 + q3**2) 87 | cIB_m[1,2] = 2*(q2*q3 + q0*q1) 88 | 89 | cIB_m[2,0] = 2*(q1*q3 + q0*q2) 90 | cIB_m[2,1] = 2*(q2*q3 - q0*q1) 91 | cIB_m[2,2] = 1-2*(q1**2 + q2**2) 92 | 93 | return cIB_m 94 | 95 | def plot_3D(): 96 | 97 | x = np.array(X_in) 98 | u = np.array(U_in) 99 | 100 | r = x[:,1:4] 101 | v = x[:,4:7] 102 | 103 | fig = plt.figure() 104 | ax = fig.gca(projection='3d') 105 | 106 | colors = [] 107 | for i, _ in enumerate(r[:, 0]): 108 | 109 | vnorm = np.linalg.norm(v[i, :]) / 6.16 110 | 111 | colori = plt.cm.plasma(vnorm) 112 | colors.append(colori) 113 | 114 | ax.plot( 115 | r[i : i+2, 1], 116 | r[i : i+2, 2], 117 | r[i : i+2, 0], 118 | color = colori, 119 | ) 120 | 121 | 122 | ax.quiver( r[:,1], r[:,2], r[:,0], 123 | u[:,1], u[:,2], u[:,0], 124 | 125 | pivot='tip', 126 | colors=colors, 127 | arrow_length_ratio=0) 128 | 129 | ax.set_aspect('equal') 130 | plt.show() 131 | 132 | plot_3D() 133 | plot_X() 134 | -------------------------------------------------------------------------------- /dynamics_functions.py: -------------------------------------------------------------------------------- 1 | """ This was generated by dynamics_generation.py """ 2 | 3 | from numpy import sqrt 4 | import numpy as np 5 | 6 | class Dynamics: 7 | 8 | def set_parameters(self, parms): 9 | for name, val in parms.items(): 10 | setattr(self, name, val) 11 | 12 | def A(self, x, u, s): 13 | m, r0, r1, r2, v0, v1, v2, q0, q1, q2, q3, w0, w1, w2 = x 14 | u0, u1, u2 = u 15 | 16 | J = self.J 17 | alpha = self.alpha 18 | gx, gy, gz = self.g_I 19 | rTB0, rTB1, rTB2 = self.rTB 20 | 21 | Am = np.zeros((14, 14)) 22 | Am[1, 4]=s 23 | Am[2, 5]=s 24 | Am[3, 6]=s 25 | Am[4, 0]=s*(-1.0*u0*(-2*q2**2 - 2*q3**2 + 1)/m**2 - 1.0*u1*(2*q0*q3 + 2*q1*q2)/m**2 - 1.0*u2*(-2*q0*q2 + 2*q1*q3)/m**2) 26 | Am[4, 7]=s*(-2.0*q2*u2/m + 2.0*q3*u1/m) 27 | Am[4, 8]=s*(2.0*q2*u1/m + 2.0*q3*u2/m) 28 | Am[4, 9]=s*(-2.0*q0*u2/m + 2.0*q1*u1/m - 4.0*q2*u0/m) 29 | Am[4, 10]=s*(2.0*q0*u1/m + 2.0*q1*u2/m - 4.0*q3*u0/m) 30 | Am[5, 0]=s*(-1.0*u0*(-2*q0*q3 + 2*q1*q2)/m**2 - 1.0*u1*(-2*q1**2 - 2*q3**2 + 1)/m**2 - 1.0*u2*(2*q0*q1 + 2*q2*q3)/m**2) 31 | Am[5, 7]=s*(2.0*q1*u2/m - 2.0*q3*u0/m) 32 | Am[5, 8]=s*(2.0*q0*u2/m - 4.0*q1*u1/m + 2.0*q2*u0/m) 33 | Am[5, 9]=s*(2.0*q1*u0/m + 2.0*q3*u2/m) 34 | Am[5, 10]=s*(-2.0*q0*u0/m + 2.0*q2*u2/m - 4.0*q3*u1/m) 35 | Am[6, 0]=s*(-1.0*u0*(2*q0*q2 + 2*q1*q3)/m**2 - 1.0*u1*(-2*q0*q1 + 2*q2*q3)/m**2 - 1.0*u2*(-2*q1**2 - 2*q2**2 + 1)/m**2) 36 | Am[6, 7]=s*(-2.0*q1*u1/m + 2.0*q2*u0/m) 37 | Am[6, 8]=s*(-2.0*q0*u1/m - 4.0*q1*u2/m + 2.0*q3*u0/m) 38 | Am[6, 9]=s*(2.0*q0*u0/m - 4.0*q2*u2/m + 2.0*q3*u1/m) 39 | Am[6, 10]=s*(2.0*q1*u0/m + 2.0*q2*u1/m) 40 | Am[7, 8]=-0.5*s*w0 41 | Am[7, 9]=-0.5*s*w1 42 | Am[7, 10]=-0.5*s*w2 43 | Am[7, 11]=-0.5*q1*s 44 | Am[7, 12]=-0.5*q2*s 45 | Am[7, 13]=-0.5*q3*s 46 | Am[8, 7]=0.5*s*w0 47 | Am[8, 9]=0.5*s*w2 48 | Am[8, 10]=-0.5*s*w1 49 | Am[8, 11]=0.5*q0*s 50 | Am[8, 12]=-0.5*q3*s 51 | Am[8, 13]=0.5*q2*s 52 | Am[9, 7]=0.5*s*w1 53 | Am[9, 8]=-0.5*s*w2 54 | Am[9, 10]=0.5*s*w0 55 | Am[9, 11]=0.5*q3*s 56 | Am[9, 12]=0.5*q0*s 57 | Am[9, 13]=-0.5*q1*s 58 | Am[10, 7]=0.5*s*w2 59 | Am[10, 8]=0.5*s*w1 60 | Am[10, 9]=-0.5*s*w0 61 | Am[10, 11]=-0.5*q2*s 62 | Am[10, 12]=0.5*q1*s 63 | Am[10, 13]=0.5*q0*s 64 | Am[11, 12]=s*(J[1,1]*w2 - J[2,2]*w2)/J[0,0] 65 | Am[11, 13]=s*(J[1,1]*w1 - J[2,2]*w1)/J[0,0] 66 | Am[12, 11]=s*(-J[0,0]*w2 + J[2,2]*w2)/J[1,1] 67 | Am[12, 13]=s*(-J[0,0]*w0 + J[2,2]*w0)/J[1,1] 68 | Am[13, 11]=s*(J[0,0]*w1 - J[1,1]*w1)/J[2,2] 69 | Am[13, 12]=s*(J[0,0]*w0 - J[1,1]*w0)/J[2,2] 70 | return Am 71 | 72 | def B(self, x, u, s): 73 | m, r0, r1, r2, v0, v1, v2, q0, q1, q2, q3, w0, w1, w2 = x 74 | u0, u1, u2 = u 75 | 76 | J = self.J 77 | alpha = self.alpha 78 | gx, gy, gz = self.g_I 79 | rTB0, rTB1, rTB2 = self.rTB 80 | 81 | Bm = np.zeros((14, 3)) 82 | Bm[0, 0]=-1.0*alpha*s*u0**1.0/sqrt(u0**2.0 + u1**2.0 + u2**2.0) 83 | Bm[0, 1]=-1.0*alpha*s*u1**1.0/sqrt(u0**2.0 + u1**2.0 + u2**2.0) 84 | Bm[0, 2]=-1.0*alpha*s*u2**1.0/sqrt(u0**2.0 + u1**2.0 + u2**2.0) 85 | Bm[4, 0]=1.0*s*(-2*q2**2 - 2*q3**2 + 1)/m 86 | Bm[4, 1]=1.0*s*(2*q0*q3 + 2*q1*q2)/m 87 | Bm[4, 2]=1.0*s*(-2*q0*q2 + 2*q1*q3)/m 88 | Bm[5, 0]=1.0*s*(-2*q0*q3 + 2*q1*q2)/m 89 | Bm[5, 1]=1.0*s*(-2*q1**2 - 2*q3**2 + 1)/m 90 | Bm[5, 2]=1.0*s*(2*q0*q1 + 2*q2*q3)/m 91 | Bm[6, 0]=1.0*s*(2*q0*q2 + 2*q1*q3)/m 92 | Bm[6, 1]=1.0*s*(-2*q0*q1 + 2*q2*q3)/m 93 | Bm[6, 2]=1.0*s*(-2*q1**2 - 2*q2**2 + 1)/m 94 | Bm[11, 1]=-rTB2*s/J[0,0] 95 | Bm[11, 2]=rTB1*s/J[0,0] 96 | Bm[12, 0]=rTB2*s/J[1,1] 97 | Bm[12, 2]=-rTB0*s/J[1,1] 98 | Bm[13, 0]=-rTB1*s/J[2,2] 99 | Bm[13, 1]=rTB0*s/J[2,2] 100 | return Bm 101 | 102 | def f(self, x, u): 103 | m, r0, r1, r2, v0, v1, v2, q0, q1, q2, q3, w0, w1, w2 = x 104 | u0, u1, u2 = u 105 | 106 | J = self.J 107 | alpha = self.alpha 108 | gx, gy, gz = self.g_I 109 | rTB0, rTB1, rTB2 = self.rTB 110 | 111 | fm = np.zeros((14,)) 112 | fm[0]=-alpha*sqrt(u0**2.0 + u1**2.0 + u2**2.0) 113 | fm[1]=v0 114 | fm[2]=v1 115 | fm[3]=v2 116 | fm[4]=gx + 1.0*u0*(-2*q2**2 - 2*q3**2 + 1)/m + 1.0*u1*(2*q0*q3 + 2*q1*q2)/m + 1.0*u2*(-2*q0*q2 + 2*q1*q3)/m 117 | fm[5]=gy + 1.0*u0*(-2*q0*q3 + 2*q1*q2)/m + 1.0*u1*(-2*q1**2 - 2*q3**2 + 1)/m + 1.0*u2*(2*q0*q1 + 2*q2*q3)/m 118 | fm[6]=gz + 1.0*u0*(2*q0*q2 + 2*q1*q3)/m + 1.0*u1*(-2*q0*q1 + 2*q2*q3)/m + 1.0*u2*(-2*q1**2 - 2*q2**2 + 1)/m 119 | fm[7]=-0.5*q1*w0 - 0.5*q2*w1 - 0.5*q3*w2 120 | fm[8]=0.5*q0*w0 + 0.5*q2*w2 - 0.5*q3*w1 121 | fm[9]=0.5*q0*w1 - 0.5*q1*w2 + 0.5*q3*w0 122 | fm[10]=0.5*q0*w2 + 0.5*q1*w1 - 0.5*q2*w0 123 | fm[11]=(J[1,1]*w1*w2 - J[2,2]*w1*w2 + rTB1*u2 - rTB2*u1)/J[0,0] 124 | fm[12]=(-J[0,0]*w0*w2 + J[2,2]*w0*w2 - rTB0*u2 + rTB2*u0)/J[1,1] 125 | fm[13]=(J[0,0]*w0*w1 - J[1,1]*w0*w1 + rTB0*u1 - rTB1*u0)/J[2,2] 126 | return fm 127 | -------------------------------------------------------------------------------- /main.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Ackimese & Szmuk : Successive Convexification for 6-DoF Mars Landing 3 | -> https://arxiv.org/abs/1802.03827 4 | 5 | FORKED FROM Sven Niederberger's implementation 6 | -> https://github.com/EmbersArc/SuccessiveConvexification 7 | ''' 8 | 9 | from scipy.integrate import odeint 10 | from parameters import * 11 | from time import time 12 | import cvxpy as cvx 13 | import pickle 14 | 15 | 16 | ''' 17 | state variable 18 | 19 | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 20 | x = [m, r0, r1, r2, v0, v1, v2, q0, q1, q2, q3, w0, w1, w2].T 21 | 22 | control variable, thrust 23 | 24 | u = [T0, T1, T2] in body axes 25 | ''' 26 | 27 | Xi = np.empty(shape=[K, 14]) # axis 0 = discretization step, 28 | Ui = np.empty(shape=[K, 3]) # axis 1 = state / control variable axis 29 | 30 | x_init = np.concatenate([ [m_wet,], 31 | r_I_init, v_I_init, q_B_I_init, w_B_init]) 32 | x_final = np.concatenate([ [m_dry,], 33 | r_I_final, v_I_final, q_B_I_final, w_B_final]) 34 | 35 | # SETUP --------------------------------------------------------------------- 36 | 37 | # Variables: 38 | Xv = cvx.Variable((K, 14)) 39 | Uv = cvx.Variable((K, 3) ) 40 | nuv = cvx.Variable((14 * (K - 1))) 41 | delta = cvx.Variable(K) 42 | sigmav = cvx.Variable() 43 | delta_sv = cvx.Variable() 44 | 45 | # Parameters: 46 | A_bar_parm = cvx.Parameter((K, 14 * 14)) 47 | B_bar_parm = cvx.Parameter((K, 14 * 3)) 48 | C_bar_parm = cvx.Parameter((K, 14 * 3)) 49 | S_bar_parm = cvx.Parameter((K, 14)) 50 | z_bar_parm = cvx.Parameter((K, 14)) 51 | 52 | X_last_parm = cvx.Parameter((K, 14)) 53 | U_last_parm = cvx.Parameter((K, 3)) 54 | s_last_parm = cvx.Parameter(nonneg=True) 55 | w_delta_parm = cvx.Parameter(nonneg=True) 56 | w_nu_parm = cvx.Parameter(nonneg=True) 57 | w_delta_sigma_parm = cvx.Parameter(nonneg=True) 58 | 59 | # Boundary conditions: 60 | constraints = [ 61 | Xv[0, 0] == x_init[0], 62 | Xv[0, 1:4] == x_init[1:4], 63 | Xv[0, 4:7] == x_init[4:7], 64 | # Xv[0, 7:11] == x_init[7:11], # initial attitude is free 65 | Xv[0, 11:14] == x_init[11:14], 66 | 67 | # Xv[0, 0] == x_final[0], # final mass is free 68 | Xv[K - 1, 1:4] == x_final[1:4], 69 | Xv[K - 1, 4:7] == x_final[4:7], 70 | Xv[K - 1, 7:11] == x_final[7:11], 71 | Xv[K - 1, 11:14] == x_final[11:14], 72 | 73 | Uv[K - 1, 1] == 0, 74 | Uv[K - 1, 2] == 0 75 | ] 76 | 77 | # Dynamics: 78 | for k in range(K - 1): 79 | constraints += [ 80 | Xv[k + 1, :] == 81 | cvx.reshape(A_bar_parm[k, :], (14, 14)) * Xv[k, :] 82 | + cvx.reshape(B_bar_parm[k, :], (14, 3)) * Uv[k, :] 83 | + cvx.reshape(C_bar_parm[k, :], (14, 3)) * Uv[k + 1, :] 84 | + S_bar_parm[k, :] * sigmav 85 | + z_bar_parm[k, :] 86 | + nuv[k * 14:(k + 1) * 14] 87 | ] 88 | 89 | # State constraints: 90 | constraints += [Xv[:, 0] >= m_dry] 91 | for k in range(K): 92 | constraints += [ 93 | tan_gamma_gs * cvx.norm(Xv[k, 2: 4]) <= Xv[k, 1], 94 | cos_theta_max <= 1 - 2 * cvx.sum_squares(Xv[k, 9:11]), 95 | cvx.norm(Xv[k, 11: 14]) <= w_B_max 96 | ] 97 | 98 | # Control constraints: 99 | for k in range(K): 100 | B_g = U_last_parm[k, :] / cvx.norm(U_last_parm[k, :]) 101 | constraints += [ 102 | T_min <= B_g * Uv[k, :], 103 | cvx.norm(Uv[k, :]) <= T_max, 104 | cos_delta_max * cvx.norm(Uv[k, :]) <= Uv[k, 0] 105 | ] 106 | 107 | # Trust regions: 108 | for k in range(K): 109 | dx = Xv[k, :] - X_last_parm[k, :] 110 | du = Uv[k, :] - U_last_parm[k, :] 111 | constraints += [ 112 | cvx.sum_squares(dx) + cvx.sum_squares(du) <= delta[k] 113 | ] 114 | 115 | ds = sigmav - s_last_parm 116 | constraints += [cvx.norm(ds, 2) <= delta_sv] 117 | 118 | # Objective: 119 | objective = cvx.Minimize( 120 | sigmav # minimize flight time 121 | #-Xv[0,-1] # minimize fuel expenditure / maximize terminal fuel 122 | + w_nu_parm * cvx.norm(nuv, 1) # virtual control 123 | + w_delta_parm * cvx.norm(delta) # trust region on dynamics 124 | + w_delta_sigma_parm * cvx.norm(delta_sv, 1) # trust region on sigma 125 | ) 126 | 127 | prob = cvx.Problem(objective, constraints) 128 | 129 | if not prob.is_dcp(): 130 | raise(Exception("Problem does not follow DCP rules")) 131 | 132 | # Initialization ------------------------------------------------------------ 133 | 134 | sigma = t_f_guess 135 | 136 | for k in range(K): 137 | 138 | alpha1 = (K - k) / K 139 | alpha2 = (k / K) 140 | 141 | # Linear interpolate between start and end values for linearization guess 142 | 143 | mk = [alpha1 * x_init[0] + alpha2 * x_final[0], ] 144 | rk = alpha1 * x_init[1:4] + alpha2 * x_final[1:4] 145 | vk = alpha1 * x_init[4:7] + alpha2 * x_final[4:7] 146 | wk = alpha1 * x_init[11:14] + alpha2 * x_final[11:14] 147 | qk = np.array([1.0, 0.0, 0.0, 0.0]) 148 | 149 | Xi[k, :] = np.concatenate([mk, rk, vk, qk, wk]) 150 | Ui[k, :] = mk * -g_I # hover 151 | 152 | 153 | """ The start/end indexes of each variable in the vector V = XABCSz """ 154 | idx = [ 14 ] # end of x (14,1) 155 | idx += [idx[0] + (14 * 14)] # end of A (14,14) 156 | idx += [idx[1] + (14 * 3)] # end of B (14,3) 157 | idx += [idx[2] + (14 * 3)] # end of C (14,3) 158 | idx += [idx[3] + (14 * 1)] # end of S (14,1) 159 | idx += [idx[4] + (14 * 1)] # end of z (14,1) 160 | 161 | 162 | def ode_dVdt(V, t, u_t, u_t1, sigma): 163 | ''' integrate the problem vector, which is defined as: 164 | 165 | V = [x(14), 166 | Phi_A(14x14), B_bar(14x3), C_bar(14x3), S_bar(14), z_bar(14)] 167 | 168 | V has no correlation to v (velocity) except that it contains v inside 169 | ''' 170 | 171 | dV_dt = np.zeros( (idx[-1],) ) 172 | 173 | alpha = (t / dt) 174 | beta = (1 - t / dt) 175 | 176 | # Gather x and u 177 | x = V[0 : idx[0]] 178 | u = u_t + alpha * (u_t1 - u_t) 179 | 180 | ''' Since PhiA(t) * A(t) = Phi(t_end), PhiA(t) = Phi(t_end) * A.inverse ''' 181 | Phi_A_xi = np.linalg.inv(V[idx[0] : idx[1]].reshape((14, 14))) 182 | Phi = V[idx[0] : idx[1]].reshape((14, 14)) 183 | 184 | z_t = - np.matmul(A(x, u, sigma), x) 185 | z_t += - np.matmul(B(x, u, sigma), u) 186 | 187 | dV_dt[0 : idx[0]] = sigma * f(x, u) 188 | dV_dt[idx[0] : idx[1]] = np.matmul(A(x, u, sigma), Phi).reshape(-1) 189 | dV_dt[idx[1] : idx[2]] = np.matmul(Phi_A_xi, B(x, u, sigma)).reshape(-1) * alpha 190 | dV_dt[idx[2] : idx[3]] = np.matmul(Phi_A_xi, B(x, u, sigma)).reshape(-1) * beta 191 | dV_dt[idx[3] : idx[4]] = np.matmul(Phi_A_xi, f(x, u)) 192 | dV_dt[idx[4] : idx[5]] = np.matmul(Phi_A_xi, z_t) 193 | 194 | return dV_dt 195 | 196 | 197 | # Successive Convexification ------------------------------------------------- 198 | for iteration in range(iterations): 199 | 200 | print("Iteration", iteration + 1) 201 | 202 | start_time = time() 203 | 204 | A_bar = np.zeros([K, 14, 14]) 205 | B_bar = np.zeros([K, 14, 3]) 206 | C_bar = np.zeros([K, 14, 3]) 207 | S_bar = np.zeros([K, 14]) 208 | z_bar = np.zeros([K, 14]) 209 | 210 | for k in range(0, K - 1): 211 | 212 | # find A_bar, B_bar, C_bar, Sigma_bar = S_bar, and z_bar 213 | 214 | V0 = np.zeros( (idx[-1],) ) 215 | V0[0 : idx[0]] = Xi[k, :] # X at initial step 216 | V0[idx[0] : idx[1]] = np.eye(14).reshape(-1) # PhiA at initial step 217 | 218 | V = odeint( 219 | ode_dVdt, # dV/dt function 220 | V0, # initial value of the V container vector 221 | (0, dt), # integrate over the time width of a step 222 | args = (Ui[k, :], Ui[k + 1, :], sigma) 223 | ) 224 | 225 | V = np.array(V)[1, :] 226 | 227 | A_bar[k, :, :] = V[idx[0] : idx[1]].reshape((14, 14)) 228 | B_bar[k, :, :] = V[idx[1] : idx[2]].reshape((14, 3)) 229 | C_bar[k, :, :] = V[idx[2] : idx[3]].reshape((14, 3)) 230 | S_bar[k, :] = V[idx[3] : idx[4]] 231 | z_bar[k, :] = V[idx[4] : idx[5]] 232 | 233 | X_last_parm.value = Xi 234 | U_last_parm.value = Ui 235 | s_last_parm.value = sigma 236 | 237 | A_bar_parm.value = A_bar.reshape((K, 14 * 14), order='F') 238 | B_bar_parm.value = B_bar.reshape((K, 14 * 3), order='F') 239 | C_bar_parm.value = C_bar.reshape((K, 14 * 3), order='F') 240 | S_bar_parm.value = S_bar 241 | z_bar_parm.value = z_bar 242 | 243 | # weights are defined and imported from parameters.py 244 | 245 | if force_converge['active']: 246 | if iteration < force_converge['start']: 247 | w_delta_parm.value = w_delta 248 | else: 249 | w_delta_parm.value = w_delta * force_converge['amount'] 250 | else: 251 | w_delta_parm.value = w_delta 252 | 253 | w_nu_parm.value = w_nu 254 | w_delta_sigma_parm.value = w_delta_sigma 255 | 256 | print("Solving problem. ", time() - start_time, "sec to integrate") 257 | 258 | prob.solve(verbose=True, solver='ECOS') 259 | # CVX ---------------------------------------------------------------------------------------------------------- 260 | 261 | Xi = Xv.value 262 | Ui = Uv.value 263 | sigma = sigmav.value 264 | 265 | delta_norm = np.linalg.norm(delta.value) 266 | nu_norm = np.linalg.norm(nuv.value, ord=1) 267 | 268 | print("Flight time:", sigmav.value, end = ' | ') 269 | print("Delta_norm:", delta_norm, end = ' | ') 270 | print("Nu_norm:", nu_norm) 271 | 272 | if delta_norm < delta_tol and nu_norm < nu_tol: 273 | print("Converged after", iteration + 1, "iterations!") 274 | break 275 | 276 | pickle.dump(X, open("trajectory/X.p", "wb")) 277 | pickle.dump(U, open("trajectory/U.p", "wb")) 278 | 279 | print('Saved. Program complete!') 280 | -------------------------------------------------------------------------------- /dynamics_generation.py: -------------------------------------------------------------------------------- 1 | from sympy import symbols, Matrix, Function 2 | from time import time 3 | import numpy as np 4 | import sympy 5 | 6 | class Dynamics(object): 7 | 8 | ''' The Dynamics class defines all dynamic functions symbolically. 9 | 10 | It can be evaluted if "constants" is not None, 11 | otherwise will generate a .py file with all the definitions 12 | ''' 13 | 14 | def __init__(self, constants = None): 15 | 16 | start = time() 17 | 18 | 19 | # --------------------------------------------------------- setup 20 | 21 | # Variable Symbols 22 | m = symbols('m') 23 | r = Matrix(symbols('r0 r1 r2')) # aka rI 24 | v = Matrix(symbols('v0 v1 v2')) # aka vI 25 | q = Matrix(symbols('q0 q1 q2 q3')) # aka qBI 26 | w = Matrix(symbols('w0 w1 w2')) # aka wB 27 | 28 | self.x = [ 29 | [m], 30 | [r[0]], [r[1]], [r[2]], 31 | [v[0]], [v[1]], [v[2]], 32 | [q[0]], [q[1]], [q[2]], [q[3]], 33 | [w[0]], [w[1]], [w[2]] 34 | ] 35 | 36 | self.u = Matrix(symbols('u0 u1 u2', positive=True)) 37 | self.s = symbols('s', positive=True) # dtime/dtau 38 | 39 | # Constants 40 | if constants is not None: 41 | 42 | generate_code = False 43 | 44 | alpha = constants['alpha'] 45 | rTB = Matrix(constants['rTB']) 46 | J = Matrix(constants['J']) 47 | g = Matrix(constants['g']) 48 | 49 | else: 50 | 51 | alpha = symbols('alpha') 52 | rTB = Matrix(symbols('rTB0 rTB1 rTB2')) 53 | g = Matrix(symbols('gx gy gz')) 54 | 55 | J = sympy.zeros(3,3) 56 | J[0,0] = symbols('J00') 57 | J[1,1] = symbols('J11') 58 | J[2,2] = symbols('J22') 59 | 60 | generate_code = True 61 | 62 | # --------------------------------------------------------- dx / dt 63 | # f(x) = first derivative of x with respect to t 64 | self.f = sympy.zeros(14,1) 65 | 66 | u_mag = sympy.sqrt(self.u[0]**2. + self.u[1]**2. + self.u[2]**2.) 67 | 68 | self.f[0] = (-alpha) * u_mag # dm / dt = -alpha * ||u|| 69 | self.f[1:4,:] = (v) # dr/dt = velocity 70 | 71 | # dv/dt = gravity + (pointed thrust)/mass 72 | self.f[4:7,:] = (1./m) * self.cIB(q) * self.u + g 73 | self.f[7:11,:] = (1./2.) * self.Om(w) * q 74 | 75 | # J.T == J.inv() due to orthogonality 76 | self.f[11:14,:] = J.pinv_solve(rTB.cross(self.u) - w.cross(J*w)) 77 | 78 | self.A = self.s * self.f.jacobian(self.x) # df/dx = (17b) 79 | self.B = self.s * self.f.jacobian(self.u) # df/du = (17c) 80 | 81 | print(' >> Dynamics class init took',time() - start,'sec') 82 | 83 | # --------------------------------------------------------- code gen 84 | 85 | if generate_code: 86 | self.generate_functions() 87 | 88 | @staticmethod 89 | def Om(w, numpy=False): 90 | w0, w1, w2 = w 91 | 92 | if numpy: 93 | Omega = np.zeros((4,4)) 94 | else: 95 | Omega = sympy.zeros(4,4) 96 | 97 | Omega[0,0] = 0 98 | Omega[0,1] = -w0 99 | Omega[0,2] = -w1 100 | Omega[0,3] = -w2 101 | Omega[1,0] = +w0 102 | Omega[1,1] = 0 103 | Omega[1,2] = +w2 104 | Omega[1,3] = -w1 105 | Omega[2,0] = +w1 106 | Omega[2,1] = -w2 107 | Omega[2,2] = 0 108 | Omega[2,3] = +w0 109 | Omega[3,0] = +w2 110 | Omega[3,1] = +w1 111 | Omega[3,2] = -w0 112 | Omega[3,3] = 0 113 | 114 | return Omega 115 | 116 | @staticmethod 117 | def cIB(q, numpy=False): 118 | q0, q1, q2, q3 = q 119 | 120 | if numpy: 121 | cIB_m = np.zeros((3,3)) 122 | else: 123 | cIB_m = sympy.zeros(3,3) 124 | 125 | cIB_m[0,0] = 1-2*(q2**2 + q3**2) 126 | cIB_m[0,1] = 2*(q1*q2 + q0*q3) 127 | cIB_m[0,2] = 2*(q1*q3 - q0*q2) 128 | 129 | cIB_m[1,0] = 2*(q1*q2 - q0*q3) 130 | cIB_m[1,1] = 1-2*(q1**2 + q3**2) 131 | cIB_m[1,2] = 2*(q2*q3 + q0*q1) 132 | 133 | cIB_m[2,0] = 2*(q1*q3 + q0*q2) 134 | cIB_m[2,1] = 2*(q2*q3 - q0*q1) 135 | cIB_m[2,2] = 1-2*(q1**2 + q2**2) 136 | 137 | return cIB_m 138 | 139 | def get(self, name, xi, ui, si): 140 | 141 | substitutions = [] 142 | for n, sym in enumerate(self.x): 143 | substitutions.append( (sym[0], xi[n,0]) ) 144 | 145 | for n, sym in enumerate(self.u): 146 | substitutions.append( (sym, ui[n]) ) 147 | 148 | substitutions.append((self.s, si)) 149 | 150 | function = getattr(self, name) 151 | matrix = function.subs(substitutions) 152 | 153 | return np.array(matrix).astype(np.float64) 154 | 155 | def generate_functions(self): 156 | ''' Code generation of the symbolic dynamic functions ''' 157 | 158 | tab = ' ' # 4 spaces 159 | functions = {'A':self.A, 'B':self.B, 'f':self.f} 160 | 161 | with open('dynamics_functions.py','w') as f: 162 | 163 | f.write('""" This was generated by dynamics_generation.py """' + '\n\n') 164 | f.write('from numpy import sqrt' + '\n') 165 | f.write('import numpy as np' + '\n\n') 166 | 167 | f.write('class Dynamics:' + '\n\n') 168 | 169 | set_parameters_string = tab 170 | 171 | set_parameters_string += 'def set_parameters(self, parms):' + '\n' 172 | set_parameters_string += tab 173 | set_parameters_string += tab 174 | 175 | set_parameters_string += 'for name, val in parms.items():' + '\n' 176 | set_parameters_string += tab 177 | set_parameters_string += tab 178 | set_parameters_string += tab 179 | set_parameters_string += 'setattr(self, name, val)' 180 | 181 | f.write(set_parameters_string + '\n') 182 | 183 | for name, matrix in functions.items(): 184 | 185 | if name != 'f': 186 | f.write('\n') 187 | f.write(tab) 188 | f.write('def ' + name + '(self, x, u, s):' + '\n') 189 | 190 | else: 191 | f.write('\n') 192 | f.write(tab) 193 | f.write('def ' + name + '(self, x, u):' + '\n') 194 | 195 | variables_unpacking = [ 196 | 'm, r0, r1, r2, v0, v1, v2, q0, q1, q2, q3, w0, w1, w2 = x', 197 | 'u0, u1, u2 = u', 198 | ] 199 | 200 | constants_unpacking = [ 201 | "J = self.J", 202 | "alpha = self.alpha", 203 | "gx, gy, gz = self.g_I", 204 | "rTB0, rTB1, rTB2 = self.rTB", 205 | ] 206 | 207 | for v in variables_unpacking: 208 | f.write(tab + tab + v + '\n') 209 | 210 | f.write('\n') 211 | 212 | for c in constants_unpacking: 213 | f.write(tab + tab + c + '\n') 214 | 215 | f.write('\n' + tab) 216 | 217 | 218 | if name != 'f': 219 | f.write(tab + name + 'm') 220 | f.write(' = np.zeros(' + str(matrix.shape) + ')\n') 221 | 222 | else: 223 | f.write(tab + name + 'm') 224 | f.write(' = np.zeros((' + str(matrix.shape[0]) + ',))\n') 225 | 226 | for n in range(matrix.shape[0]): 227 | for m in range(matrix.shape[1]): 228 | 229 | value = str(matrix[n,m]) 230 | 231 | if value != '0': 232 | 233 | genline = tab 234 | genline += name + 'm' 235 | 236 | if name != 'f': 237 | genline += '[' + str(n) + ', ' + str(m) +']=' 238 | genline += str(value) 239 | else: 240 | genline += '[' + str(n) + ']=' 241 | genline += str(value) 242 | 243 | # J matrix should be indexed instead of unpacked 244 | for i in range(3): 245 | for j in range(3): 246 | genline = genline.replace( 247 | 'J'+str(i)+str(j), 248 | 'J['+str(i)+','+str(j)+']', 249 | ) 250 | 251 | f.write(tab + genline + '\n') 252 | 253 | f.write(tab + tab + 'return ' + name + 'm' + '\n') 254 | 255 | print('Done writing to file') 256 | 257 | def runtime_tests(): 258 | d = Dynamics(constants) 259 | start = time() 260 | A = d.get('A', x, u, s) 261 | B = d.get('B', x, u, s) 262 | f = d.get('f', x, u, s) 263 | print('took',time()-start,'sec to run A, B and f') 264 | 265 | print('A',A.shape, A) 266 | print('B',B.shape, B) 267 | print('f',f.shape, f) 268 | 269 | print('Ax', A*x) 270 | print('Bu', B*u) 271 | 272 | def function_tests(): 273 | 274 | import dynamics_functions as funk 275 | 276 | f = funk.Dynamics() 277 | f.set_parameters(constants) 278 | 279 | # If these can be assigned, all is well in set_parameters 280 | a = (f.alpha),(f.rTB),(f.g_I),(f.J) 281 | 282 | # If this executes, all is well in the dynamics functions 283 | x = np.random.random((14,1)) 284 | u = np.random.random((3,1)) 285 | s = 1 286 | 287 | f.A(x,u,s) 288 | f.B(x,u,s) 289 | f.f(x,u) 290 | 291 | print('Tests passed') 292 | 293 | if __name__ == '__main__': 294 | 295 | import numpy as np 296 | 297 | constants = {} 298 | constants['alpha'] = 0.1 299 | constants['rTB'] = -1e-2 * np.array([1,0,0]) 300 | constants['J'] = 1e-2 * np.eye(3) 301 | constants['g_I'] = np.array([ -1, 0, 0]) # inertial frame 302 | 303 | d = Dynamics() 304 | 305 | function_tests() 306 | --------------------------------------------------------------------------------