├── .gitattributes ├── BSCP.py ├── PTR.py ├── PTR_tf_free.py ├── README.md ├── Scaling.py ├── __pycache__ ├── PTR.cpython-38.pyc ├── PTR.cpython-39.pyc ├── PTR_tf_free.cpython-38.pyc ├── PTR_tffree.cpython-38.pyc ├── SCP_buffer_deviation.cpython-310.pyc ├── SCP_buffer_deviation.cpython-38.pyc ├── Scaling.cpython-310.pyc ├── Scaling.cpython-38.pyc ├── Scaling.cpython-39.pyc ├── Scvx.cpython-310.pyc ├── Scvx.cpython-38.pyc ├── Scvx_fixedtime.cpython-38.pyc ├── Scvx_from_github.cpython-38.pyc ├── Scvx_tf_free.cpython-310.pyc ├── Scvx_tf_free.cpython-38.pyc ├── Scvx_tf_free_buffer.cpython-310.pyc ├── Scvx_tf_free_var.cpython-38.pyc ├── constraints.cpython-38.pyc ├── cost.cpython-38.pyc ├── cylinder.cpython-38.pyc ├── forward.cpython-38.pyc ├── model.cpython-38.pyc ├── shape.cpython-38.pyc └── utils.cpython-38.pyc ├── constraints ├── Aircraft3dofConstraints.py ├── Aircraft3dofConstraintsNondimension.py ├── AircraftKinematicsConstraints.py ├── Landing2DConstraints.py ├── Landing3DConstraints.py ├── LinearConstraints.py ├── QuadRotorPointMassConstraints.py ├── QuadRotorSmallAngleConstraints.py ├── UnicycleConstraints.py ├── __pycache__ │ ├── Aircraft3dofConstraints.cpython-310.pyc │ ├── Aircraft3dofConstraints.cpython-38.pyc │ ├── AircraftKinematicsConstraints.cpython-38.pyc │ ├── Landing2DConstraints.cpython-38.pyc │ ├── Landing3DConstraints.cpython-38.pyc │ ├── LinearConstraints.cpython-38.pyc │ ├── QuadRotorPointMassConstraints.cpython-38.pyc │ ├── QuadRotorSmallAngleConstraints.cpython-38.pyc │ ├── UnicycleConstraints.cpython-310.pyc │ ├── UnicycleConstraints.cpython-38.pyc │ ├── UnicycleConstraints.cpython-39.pyc │ ├── constraints.cpython-310.pyc │ ├── constraints.cpython-38.pyc │ └── constraints.cpython-39.pyc └── constraints.py ├── cost ├── Aircraft3dofCost.py ├── AircraftKinematicsCost.py ├── FinaltimeFreeCost.py ├── Landing2DCost.py ├── Landing3DCost.py ├── LinearCost.py ├── QuadRotorPointMassCost.py ├── QuadRotorSmallAngleCost.py ├── UnicycleCost.py ├── __pycache__ │ ├── Aircraft3dofCost.cpython-38.pyc │ ├── AircraftKinematicsCost.cpython-38.pyc │ ├── FinaltimeFreeCost.cpython-310.pyc │ ├── FinaltimeFreeCost.cpython-38.pyc │ ├── Landing2DCost.cpython-38.pyc │ ├── Landing3DCost.cpython-38.pyc │ ├── LinearCost.cpython-38.pyc │ ├── QuadRotorPointMassCost.cpython-38.pyc │ ├── QuadRotorSmallAngleCost.cpython-38.pyc │ ├── UnicycleCost.cpython-310.pyc │ ├── UnicycleCost.cpython-38.pyc │ ├── UnicycleCost.cpython-39.pyc │ ├── cost.cpython-310.pyc │ ├── cost.cpython-38.pyc │ └── cost.cpython-39.pyc └── cost.py ├── forward.py ├── images ├── Landing2D.gif ├── Landing3D.gif ├── airplane_landing_0222_01.gif ├── airplane_landing_0222_02.gif ├── airplane_landing_0222_03.gif ├── quadrotor.gif └── unicycle.gif ├── model ├── Aircraft3dofApproxModel.py ├── Aircraft3dofModel.py ├── Aircraft3dofModelNondimension.py ├── Aircraft3dofModel_tmp.py ├── AircraftKinematicsModel.py ├── HypersonicEntry3DofPolar.py ├── Landing2DModel.py ├── Landing3DModel.py ├── Landing3DModel_UEN.py ├── LinearModel.py ├── QuadRotorPointMassModel.py ├── QuadRotorSmallAngleModel.py ├── Reentry.py ├── UnicycleModel.py ├── __pycache__ │ ├── Aircraft3dofApproxModel.cpython-38.pyc │ ├── Aircraft3dofModel.cpython-310.pyc │ ├── Aircraft3dofModel.cpython-38.pyc │ ├── Aircraft3dofModel_tmp.cpython-310.pyc │ ├── AircraftKinematicsModel.cpython-38.pyc │ ├── HypersonicEntry3DofPolar.cpython-38.pyc │ ├── Landing2DModel.cpython-38.pyc │ ├── Landing3DModel.cpython-38.pyc │ ├── Landing3DModel_UEN.cpython-38.pyc │ ├── LinearModel.cpython-38.pyc │ ├── QuadRotorPointMassModel.cpython-38.pyc │ ├── QuadRotorSmallAngleModel.cpython-38.pyc │ ├── Reentry.cpython-310.pyc │ ├── Reentry.cpython-38.pyc │ ├── UnicycleModel.cpython-310.pyc │ ├── UnicycleModel.cpython-38.pyc │ ├── UnicycleModel.cpython-39.pyc │ ├── model.cpython-310.pyc │ ├── model.cpython-38.pyc │ └── model.cpython-39.pyc └── model.py ├── notebooks ├── (need_to_update)test_RocketLanding3D.ipynb ├── (need_to_update)test_quadrotor_pointmass.ipynb ├── (need_to_update)test_quadrotor_smallangle.ipynb ├── .ipynb_checkpoints │ ├── test_unicycle-checkpoint.ipynb │ └── test_unicycle_final_time_free-checkpoint.ipynb ├── dynamics_sympy │ ├── .ipynb_checkpoints │ │ ├── 6Drocket-checkpoint.ipynb │ │ ├── air_density_model-checkpoint.ipynb │ │ ├── aircraft_pointmass-checkpoint.ipynb │ │ ├── aircraft_reentry-checkpoint.ipynb │ │ └── spacecraft_reentry-checkpoint.ipynb │ ├── 6Drocket.ipynb │ ├── air_density_model.ipynb │ ├── aircraft_pointmass.ipynb │ ├── aircraft_reentry.ipynb │ └── spacecraft_reentry.ipynb ├── test_DLTV │ ├── .ipynb_checkpoints │ │ ├── RK4-checkpoint.ipynb │ │ ├── test_entry_linearization-checkpoint.ipynb │ │ └── test_reentry-checkpoint.ipynb │ ├── RK4.ipynb │ ├── test_entry_linearization.ipynb │ └── test_reentry.ipynb ├── test_unicycle.ipynb └── test_unicycle_final_time_free.ipynb ├── sensor ├── QuadRotorSensor.py ├── __pycache__ │ ├── QuadRotorPointMassObs.cpython-38.pyc │ ├── QuadRotorSensor.cpython-38.pyc │ ├── observation.cpython-38.pyc │ ├── sensor.cpython-38.pyc │ └── utils_sensor.cpython-38.pyc └── sensor.py └── utils ├── __pycache__ ├── utils_obs.cpython-38.pyc └── utils_plot.cpython-38.pyc ├── utils_obs.py └── utils_plot.py /.gitattributes: -------------------------------------------------------------------------------- 1 | notebooks/** linguist-vendored 2 | -------------------------------------------------------------------------------- /PTR_tf_free.py: -------------------------------------------------------------------------------- 1 | from __future__ import division 2 | import matplotlib.pyplot as plt 3 | from scipy.integrate import odeint 4 | from scipy.integrate import solve_ivp 5 | import numpy as np 6 | import cvxpy as cvx 7 | import time 8 | import random 9 | def print_np(x): 10 | print ("Type is %s" % (type(x))) 11 | print ("Shape is %s" % (x.shape,)) 12 | # print ("Values are: \n%s" % (x)) 13 | 14 | import cost 15 | import model 16 | import IPython 17 | from PTR import PTR 18 | 19 | from Scaling import TrajectoryScaling 20 | 21 | class PTR_tf_free(PTR): 22 | def __init__(self,name,horizon,tf,maxIter,Model,Cost,Const,Scaling=None,type_discretization='zoh', 23 | w_c=1,w_vc=1e4,w_tr=1e-3,w_rate=0, 24 | tol_vc=1e-10,tol_tr=1e-3,tol_bc=1e-3, 25 | flag_policyopt=False,verbosity=True): 26 | self.name = name 27 | self.model = Model 28 | self.const = Const 29 | self.cost = Cost 30 | self.N = horizon 31 | self.tf = tf 32 | self.delT = tf/horizon 33 | if Scaling is None : 34 | self.Scaling = TrajectoryScaling() 35 | self.Scaling.S_sigma = 1 36 | self.flag_update_scale = True 37 | else : 38 | self.Scaling = Scaling 39 | self.flag_update_scale = False 40 | 41 | # cost optimization 42 | self.verbosity = verbosity 43 | self.w_c = w_c 44 | self.w_vc = w_vc 45 | self.w_tr = w_tr 46 | self.w_rate = w_rate 47 | # self.tol_fun = 1e-6 48 | self.tol_tr = tol_tr 49 | self.tol_vc = tol_vc 50 | self.tol_bc = tol_bc 51 | self.maxIter = maxIter 52 | self.last_head = True 53 | self.type_discretization = type_discretization 54 | self.flag_policyopt = flag_policyopt 55 | self.initialize() 56 | 57 | def cvxopt(self): 58 | # TODO - we can get rid of most of loops here 59 | 60 | # state & input & horizon size 61 | ix = self.model.ix 62 | iu = self.model.iu 63 | N = self.N 64 | 65 | if self.flag_update_scale is True : 66 | self.Scaling.update_scaling_from_traj(self.x,self.u) 67 | Sx,iSx,sx,Su,iSu,su = self.Scaling.get_scaling() 68 | 69 | S_sigma = self.Scaling.S_sigma 70 | 71 | x_cvx = cvx.Variable((N+1,ix)) 72 | u_cvx = cvx.Variable((N+1,iu)) 73 | vc = cvx.Variable((N,ix)) 74 | sigma = cvx.Variable(nonneg=True) 75 | 76 | # initial & final boundary condition 77 | constraints = [] 78 | constraints.append(Sx@x_cvx[0]+sx == self.xi) 79 | constraints.append(Sx@x_cvx[-1]+sx == self.xf) 80 | 81 | # state and input contraints 82 | for i in range(0,N+1) : 83 | h = self.const.forward(Sx@x_cvx[i]+sx,Su@u_cvx[i]+su,self.x[i],self.u[i],i==N) 84 | constraints += h 85 | 86 | # model constraints 87 | for i in range(0,N) : 88 | if self.type_discretization == 'zoh' : 89 | constraints.append(Sx@x_cvx[i+1]+sx == self.A[i]@(Sx@x_cvx[i]+sx)+self.B[i]@(Su@u_cvx[i]+su) 90 | +sigma*S_sigma*self.s[i] 91 | +self.z[i] 92 | +vc[i]) 93 | elif self.type_discretization == 'foh' : 94 | constraints.append(Sx@x_cvx[i+1]+sx == self.A[i]@(Sx@x_cvx[i]+sx)+self.Bm[i]@(Su@u_cvx[i]+su) 95 | +self.Bp[i]@(Su@u_cvx[i+1]+su) 96 | +sigma*S_sigma*self.s[i] 97 | +self.z[i] 98 | +vc[i] 99 | ) 100 | 101 | # cost 102 | objective = [] 103 | objective_vc = [] 104 | objective_tr = [] 105 | objective_rate = [] 106 | objective.append(self.w_c * self.cost.estimate_cost_cvx(sigma*S_sigma)) 107 | for i in range(0,N+1) : 108 | if i < N : 109 | objective_vc.append(self.w_vc * cvx.norm(vc[i],1)) 110 | objective_rate.append(self.w_rate * cvx.quad_form(u_cvx[i+1]-u_cvx[i],np.eye(iu))) 111 | objective_tr.append( self.w_tr * (cvx.quad_form(x_cvx[i] - 112 | iSx@(self.x[i]-sx),np.eye(ix)) + 113 | cvx.quad_form(u_cvx[i]-iSu@(self.u[i]-su),np.eye(iu))) ) 114 | objective_tr.append(self.w_tr*(sigma-self.tf/S_sigma)**2) 115 | 116 | l = cvx.sum(objective) 117 | l_vc = cvx.sum(objective_vc) 118 | l_tr = cvx.sum(objective_tr) 119 | l_rate = cvx.sum(objective_rate) 120 | 121 | l_all = l + l_vc + l_tr + l_rate 122 | prob = cvx.Problem(cvx.Minimize(l_all), constraints) 123 | 124 | error = False 125 | # prob.solve(verbose=False,solver=cvx.MOSEK) 126 | # prob.solve(verbose=False,solver=cvx.CPLEX) 127 | # prob.solve(verbose=False,solver=cvx.GUROBI) 128 | prob.solve(verbose=False,solver=cvx.ECOS) 129 | # prob.solve(verbose=False,solver=cvx.SCS) 130 | 131 | if prob.status == cvx.OPTIMAL_INACCURATE : 132 | print("WARNING: inaccurate solution") 133 | 134 | try : 135 | x_bar = np.zeros_like(self.x) 136 | u_bar = np.zeros_like(self.u) 137 | for i in range(N+1) : 138 | x_bar[i] = Sx@x_cvx[i].value + sx 139 | u_bar[i] = Su@u_cvx[i].value + su 140 | sigma_bar = sigma.value * S_sigma 141 | except ValueError : 142 | print(prob.status,"FAIL: ValueError") 143 | error = True 144 | except TypeError : 145 | print(prob.status,"FAIL: TypeError") 146 | error = True 147 | # print("x_min {:f} x_max {:f} u_min {:f} u _max{:f}".format(np.min(x_cvx.value), 148 | # np.max(x_cvx.value), 149 | # np.min(u_cvx.value), 150 | # np.max(u_cvx.value))) 151 | # sigma_bar = self.tf 152 | return prob.status,l.value,l_vc.value,l_tr.value,x_bar,u_bar,sigma_bar,vc.value,error 153 | 154 | def run(self,x0,u0,xi,xf): 155 | # initial trajectory 156 | self.x0 = x0 157 | 158 | # save trajectory 159 | x_traj = [] 160 | u_traj = [] 161 | T_traj = [] 162 | 163 | # initial input 164 | self.u0 = u0 165 | self.u = u0 166 | 167 | # initial condition 168 | self.xi = xi 169 | 170 | # final condition 171 | self.xf = xf 172 | 173 | # state & input & horizon size 174 | ix = self.model.ix 175 | iu = self.model.iu 176 | N = self.N 177 | 178 | # timer setting 179 | # trace for iteration 180 | # timer, counters, constraints 181 | # timer begin!! 182 | 183 | # generate initial trajectory 184 | diverge = False 185 | stop = False 186 | 187 | self.x = self.x0 188 | self.c = self.tf 189 | self.cvc = 0 190 | self.ctr = 0 191 | 192 | # iterations starts!! 193 | total_num_iter = 0 194 | flag_boundary = False 195 | for iteration in range(self.maxIter) : 196 | 197 | # differentiate dynamics and cost 198 | start = time.time() 199 | self.A,self.B,self.Bm,self.Bp,self.s,self.z,self.x_prop,self.x_prop_n = self.get_linearized_matrices(self.x,self.u,self.delT,self.tf) 200 | time_derivs = (time.time() - start) 201 | 202 | # step2. cvxopt 203 | prob_status,l,l_vc,l_tr,self.xnew,self.unew,self.tfnew,self.vcnew,error = self.cvxopt() 204 | if error == True : 205 | total_num_iter = 1e5 206 | break 207 | 208 | # step3. evaluate step 209 | reduction = self.c + self.cvc + self.ctr - l - l_vc - l_tr 210 | # self.xfwd,self.ufwd = self.forward_single(self.xnew[0,:],self.unew,self.tfnew,iteration) 211 | self.xfwd,self.ufwd = self.forward_multiple(self.xnew,self.unew,self.tfnew,iteration) 212 | 213 | # check the boundary condtion 214 | bc_error_norm = np.max(np.linalg.norm(self.xfwd-self.xnew,axis=1)) 215 | 216 | if bc_error_norm >= self.tol_bc : 217 | flag_boundary = False 218 | else : 219 | flag_boundary = True 220 | 221 | 222 | # step4. accept step, draw graphics, print status 223 | if self.verbosity == True and self.last_head == True: 224 | self.last_head = False 225 | print("iteration total_cost cost ||vc|| ||tr|| reduction w_tr dynamics") 226 | # accept changes 227 | self.x = self.xnew 228 | self.u = self.unew 229 | self.tf = self.tfnew 230 | 231 | self.delT = self.tf/self.N 232 | self.vc = self.vcnew 233 | self.c = l 234 | self.cvc = l_vc 235 | self.ctr = l_tr 236 | 237 | x_traj.append(self.x) 238 | u_traj.append(self.u) 239 | T_traj.append(self.tf) 240 | 241 | if self.verbosity == True: 242 | print("%-12d%-18.3f%-12.3f%-12.3g%-12.3g%-12.3g%-12.3f%-1d(%2.3g)" % ( iteration+1,self.c+self.cvc+self.ctr, 243 | self.c,self.cvc/self.w_vc,self.ctr/self.w_tr, 244 | reduction,self.w_tr,flag_boundary,bc_error_norm)) 245 | if flag_boundary == True and \ 246 | self.ctr/self.w_tr < self.tol_tr and self.cvc/self.w_vc < self.tol_vc : 247 | if self.verbosity == True: 248 | print("SUCCEESS: virtual control and trust region < tol") 249 | total_num_iter = iteration+1 250 | break 251 | if iteration == self.maxIter - 1 : 252 | print("NOT ENOUGH : reached to max iteration") 253 | total_num_iter = iteration+1 254 | 255 | return self.xfwd,self.ufwd,self.x,self.u,self.tf,total_num_iter,flag_boundary,l,l_vc,l_tr,x_traj,u_traj,T_traj 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Sequential convex programming 2 | This reposit is the implementation of sequential convex programming in Python. The implementation depends on numpy and cvxopt. You can start running the code in notebooks/test_unicycle.ipynb or notebooks/test_RocketLanding3D.ipynb 3 | 4 | ## References 5 | * Successive Convexification for 6-DoF Mars 6 | Rocket Powered Landing with Free-Final-Time (https://arxiv.org/pdf/1802.03827.pdf) 7 | * Successive Convexification of Non-Convex Optimal Control Problems with State Constraints (https://www.sciencedirect.com/science/article/pii/S2405896317312405) 8 | * Successive convexification of non-convex optimal control problems and its convergence properties (https://ieeexplore.ieee.org/abstract/document/7798816) 9 | 10 | ## TODO 11 | * final time free 12 | * Quadratic approximation for the cost 13 | * Integration with odeFun 14 | 15 | ## Obstacle avoidance for a quadrotor 16 | 17 | 18 | ## 3D Landing 19 | 20 | 21 | ## Unicycle Driving 22 | -------------------------------------------------------------------------------- /Scaling.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import time 3 | import random 4 | import IPython 5 | def print_np(x): 6 | print ("Type is %s" % (type(x))) 7 | print ("Shape is %s" % (x.shape,)) 8 | # print ("Values are: \n%s" % (x)) 9 | 10 | class TrajectoryScaling(object): 11 | def __init__(self,x_min=None,x_max=None,u_min=None,u_max=None,tf=None) : 12 | if x_min is not None : 13 | self.Sx,self.iSx,self.sx,self.Su,self.iSu,self.su = self.compute_scaling(x_min,x_max,u_min,u_max) 14 | self.S_sigma=tf 15 | 16 | def get_scaling(self) : 17 | return self.Sx,self.iSx,self.sx,self.Su,self.iSu,self.su 18 | 19 | def update_scaling_from_traj(self,x,u) : 20 | x_max = np.max(x,0) 21 | x_min = np.min(x,0) 22 | u_max = np.max(u,0) 23 | u_min = np.min(u,0) 24 | self.Sx,self.iSx,self.sx,self.Su,self.iSu,self.su = self.compute_scaling(x_min,x_max,u_min,u_max) 25 | 26 | def compute_scaling(self,x_min,x_max,u_min,u_max) : 27 | # Scaling 28 | tol_zero = np.sqrt(np.finfo(float).eps) 29 | x_intrvl = [0,1] 30 | u_intrvl = [0,1] 31 | x_width = x_intrvl[1] - x_intrvl[0] 32 | u_width = u_intrvl[1] - u_intrvl[0] 33 | 34 | Sx = (x_max - x_min) / x_width 35 | Sx[np.where(Sx < tol_zero)] = 1 36 | Sx = np.diag(Sx) 37 | iSx = np.linalg.inv(Sx) 38 | sx = x_min - x_intrvl[0] * np.diag(Sx) 39 | 40 | Su = (u_max - u_min) / u_width 41 | Su[np.where(Su < tol_zero)] = 1 42 | Su = np.diag(Su) 43 | iSu = np.linalg.inv(Su) 44 | su = u_min - u_intrvl[0] * np.diag(Su) 45 | 46 | return Sx,iSx,sx,Su,iSu,su -------------------------------------------------------------------------------- /__pycache__/PTR.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/__pycache__/PTR.cpython-38.pyc -------------------------------------------------------------------------------- /__pycache__/PTR.cpython-39.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/__pycache__/PTR.cpython-39.pyc -------------------------------------------------------------------------------- /__pycache__/PTR_tf_free.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/__pycache__/PTR_tf_free.cpython-38.pyc -------------------------------------------------------------------------------- /__pycache__/PTR_tffree.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/__pycache__/PTR_tffree.cpython-38.pyc -------------------------------------------------------------------------------- /__pycache__/SCP_buffer_deviation.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/__pycache__/SCP_buffer_deviation.cpython-310.pyc -------------------------------------------------------------------------------- /__pycache__/SCP_buffer_deviation.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/__pycache__/SCP_buffer_deviation.cpython-38.pyc -------------------------------------------------------------------------------- /__pycache__/Scaling.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/__pycache__/Scaling.cpython-310.pyc -------------------------------------------------------------------------------- /__pycache__/Scaling.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/__pycache__/Scaling.cpython-38.pyc -------------------------------------------------------------------------------- /__pycache__/Scaling.cpython-39.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/__pycache__/Scaling.cpython-39.pyc -------------------------------------------------------------------------------- /__pycache__/Scvx.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/__pycache__/Scvx.cpython-310.pyc -------------------------------------------------------------------------------- /__pycache__/Scvx.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/__pycache__/Scvx.cpython-38.pyc -------------------------------------------------------------------------------- /__pycache__/Scvx_fixedtime.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/__pycache__/Scvx_fixedtime.cpython-38.pyc -------------------------------------------------------------------------------- /__pycache__/Scvx_from_github.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/__pycache__/Scvx_from_github.cpython-38.pyc -------------------------------------------------------------------------------- /__pycache__/Scvx_tf_free.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/__pycache__/Scvx_tf_free.cpython-310.pyc -------------------------------------------------------------------------------- /__pycache__/Scvx_tf_free.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/__pycache__/Scvx_tf_free.cpython-38.pyc -------------------------------------------------------------------------------- /__pycache__/Scvx_tf_free_buffer.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/__pycache__/Scvx_tf_free_buffer.cpython-310.pyc -------------------------------------------------------------------------------- /__pycache__/Scvx_tf_free_var.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/__pycache__/Scvx_tf_free_var.cpython-38.pyc -------------------------------------------------------------------------------- /__pycache__/constraints.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/__pycache__/constraints.cpython-38.pyc -------------------------------------------------------------------------------- /__pycache__/cost.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/__pycache__/cost.cpython-38.pyc -------------------------------------------------------------------------------- /__pycache__/cylinder.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/__pycache__/cylinder.cpython-38.pyc -------------------------------------------------------------------------------- /__pycache__/forward.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/__pycache__/forward.cpython-38.pyc -------------------------------------------------------------------------------- /__pycache__/model.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/__pycache__/model.cpython-38.pyc -------------------------------------------------------------------------------- /__pycache__/shape.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/__pycache__/shape.cpython-38.pyc -------------------------------------------------------------------------------- /__pycache__/utils.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/__pycache__/utils.cpython-38.pyc -------------------------------------------------------------------------------- /constraints/Aircraft3dofConstraints.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | import scipy as sp 4 | import scipy.linalg 5 | import time 6 | import random 7 | def print_np(x): 8 | print ("Type is %s" % (type(x))) 9 | print ("Shape is %s" % (x.shape,)) 10 | print ("Values are: \n%s" % (x)) 11 | 12 | 13 | from constraints import OptimalcontrolConstraints 14 | 15 | class Aircraft3dof(OptimalcontrolConstraints): 16 | def __init__(self,name,ix,iu): 17 | super().__init__(name,ix,iu) 18 | self.idx_bc_f = slice(0, ix) 19 | self.CL_min = -0.31 20 | self.CL_max = 1.52 21 | self.phi_min = -np.deg2rad(15) 22 | self.phi_max = np.deg2rad(15) 23 | self.T_min = 0 24 | self.T_max = 1#1126.3 * 1e3 25 | self.v_min = 95 26 | self.v_max = 270 27 | self.gamma_min = -np.deg2rad(30) 28 | self.gamma_max = np.deg2rad(30) * 0 29 | 30 | 31 | def forward(self,x,u,xbar=None,ubar=None,final=False): 32 | # state & input 33 | rx = x[0] 34 | ry = x[1] 35 | rz = x[2] 36 | v = x[3] # speed 37 | gamma = x[4] # path angle 38 | psi = x[5] # velocity heading 39 | 40 | CL = u[0] # lift coefficient 41 | phi = u[1] # bank angle 42 | thrust = u[2] # thrust 43 | 44 | # # scale 45 | T_min = self.T_min 46 | T_max = self.T_max 47 | v_max = self.v_max 48 | v_min = self.v_min 49 | 50 | h = [] 51 | h.append(CL>=self.CL_min) 52 | h.append(CL<=self.CL_max) 53 | h.append(phi>=self.phi_min) 54 | h.append(phi<=self.phi_max) 55 | h.append(thrust>=T_min) 56 | h.append(thrust<=T_max) 57 | h.append(v>=v_min) 58 | h.append(v<=v_max) 59 | h.append(gamma>=self.gamma_min) 60 | h.append(gamma<=self.gamma_max) 61 | h.append(rz>=0) 62 | return h 63 | 64 | 65 | 66 | class Aircraft3dofObstacleAvoidance(Aircraft3dof): 67 | def __init__(self,name,ix,iu,c,H): 68 | super().__init__(name,ix,iu) 69 | self.c = c 70 | self.H = H 71 | 72 | 73 | def forward(self,x,u,xbar=None,ubar=None,final=False): 74 | h = super().forward(x,u,xbar,ubar,final) 75 | 76 | # obstacle avoidance 77 | def get_obs_const(c1,H1) : 78 | return (1 - np.linalg.norm(H1@(xbar[0:2]-c1)) - 79 | (H1.T@H1@(xbar[0:2]-c1)/np.linalg.norm(H1@(xbar[0:2]-c1))).T@(x[0:2]-xbar[0:2])\ 80 | <=0) 81 | if self.H is not None : 82 | for c1,H1 in zip(self.c,self.H) : 83 | h.append(get_obs_const(c1,H1)) 84 | return h 85 | 86 | class Aircraft3dofActuatorFOS(Aircraft3dof): 87 | def __init__(self,name,ix,iu): 88 | super().__init__(name,ix,iu) 89 | 90 | def forward(self,x,u,xbar=None,ubar=None,final=False): 91 | # state & input 92 | rx = x[0] 93 | ry = x[1] 94 | rz = x[2] 95 | v = x[3] # speed 96 | gamma = x[4] # path angle 97 | psi = x[5] # velocity heading 98 | thrust = x[6] 99 | 100 | CL = u[0] # lift coefficient 101 | phi = u[1] # bank angle 102 | thrust_cmd = u[2] # thrust 103 | 104 | # # scale 105 | T_min = self.T_min 106 | T_max = self.T_max 107 | v_max = self.v_max 108 | v_min = self.v_min 109 | 110 | h = [] 111 | h.append(CL>=self.CL_min) 112 | h.append(CL<=self.CL_max) 113 | h.append(phi>=self.phi_min) 114 | h.append(phi<=self.phi_max) 115 | h.append(thrust>=T_min) 116 | h.append(thrust<=T_max) 117 | h.append(v>=v_min) 118 | h.append(v<=v_max) 119 | h.append(gamma>=self.gamma_min) 120 | h.append(gamma<=self.gamma_max) 121 | h.append(rz>=0) 122 | h.append(thrust_cmd>=0) 123 | return h 124 | 125 | class Aircraft3dofStateTriggered(OptimalcontrolConstraints): 126 | def __init__(self,name,ix,iu): 127 | super().__init__(name,ix,iu) 128 | self.idx_bc_f = slice(0, ix) 129 | self.CL_min = -0.31 130 | self.CL_max = 1.52 131 | self.phi_min = -np.deg2rad(15) 132 | self.phi_max = np.deg2rad(15) 133 | self.T_min = 0 134 | self.T_max = 1#1126.3 * 1e3 135 | self.v_min = 95 136 | self.v_max = 270 137 | self.gamma_min = -np.deg2rad(30) 138 | self.gamma_max = np.deg2rad(30) * 0 139 | self.scl_v = 1 140 | self.scl_f = 1 141 | self.ih = 11 142 | 143 | self.z_trigger = 2000 144 | self.y_min_trigger = -65000 145 | self.y_max_trigger = -55000 146 | 147 | def set_scale(self,scl_v,scl_f) : 148 | self.scl_v = scl_v 149 | self.scl_f = scl_f 150 | 151 | def forward(self,x,u,xbar=None,ubar=None,final=False): 152 | # state & input 153 | rx = x[0] 154 | ry = x[1] 155 | rz = x[2] 156 | v = x[3] # speed 157 | gamma = x[4] # path angle 158 | psi = x[5] # velocity heading 159 | 160 | CL = u[0] # lift coefficient 161 | phi = u[1] # bank angle 162 | thrust = u[2] # thrust 163 | 164 | # # scale 165 | T_min = self.T_min / self.scl_f 166 | T_max = self.T_max / self.scl_f 167 | v_max = self.v_max / self.scl_v 168 | v_min = self.v_min / self.scl_v 169 | 170 | h = [] 171 | h.append(CL>=self.CL_min) 172 | h.append(CL<=self.CL_max) 173 | h.append(phi>=self.phi_min) 174 | h.append(phi<=self.phi_max) 175 | h.append(thrust>=T_min) 176 | h.append(thrust<=T_max) 177 | h.append(v>=v_min) 178 | h.append(v<=v_max) 179 | h.append(gamma>=self.gamma_min) 180 | h.append(gamma<=self.gamma_max) 181 | h.append(rz>=0) 182 | 183 | # state-triggered 184 | rybar = xbar[1] 185 | rzbar = xbar[2] 186 | if rzbar < self.z_trigger : 187 | h1bar = -(rzbar-self.z_trigger) * (rybar-self.y_max_trigger) 188 | rh1_rz = -(rybar-self.y_max_trigger) * (rz-rzbar) 189 | rh1_ry = -(rzbar-self.z_trigger) * (ry-rybar) 190 | h.append(h1bar+rh1_rz+rh1_ry <= 0) 191 | 192 | h2bar = -(rzbar-self.z_trigger) * (-rybar+self.y_min_trigger) 193 | rh2_rz = (rybar-self.y_min_trigger) * (rz-rzbar) 194 | rh2_ry = (rzbar-self.z_trigger) * (ry-rybar) 195 | h.append(h2bar+rh2_rz+rh2_ry <= 0) 196 | return h 197 | 198 | 199 | def bc_final(self,x_cvx,xf): 200 | h = [] 201 | h.append(x_cvx == xf) 202 | 203 | return h 204 | 205 | 206 | class Aircraft3dofApprox(OptimalcontrolConstraints): 207 | def __init__(self,name,ix,iu): 208 | super().__init__(name,ix,iu) 209 | self.idx_bc_f = slice(0, ix) 210 | self.CL_min = -0.31 211 | self.CL_max = 1.52 212 | self.phi_min = -np.deg2rad(15) 213 | self.phi_max = np.deg2rad(15) 214 | self.T_min = 0 215 | self.T_max = 1#1126.3 * 1e3 216 | self.v_min = 95 217 | self.v_max = 270 218 | self.gamma_min = -np.deg2rad(20) 219 | self.gamma_max = np.deg2rad(20)*0 220 | self.scl_v = 1 221 | self.scl_f = 1 222 | self.ih = 11 223 | 224 | def set_scale(self,scl_v,scl_f) : 225 | self.scl_v = scl_v 226 | self.scl_f = scl_f 227 | 228 | def forward(self,x,u,xbar=None,ubar=None,final=False): 229 | # state & input 230 | rx = x[0] 231 | ry = x[1] 232 | rz = x[2] 233 | v = x[3] # speed 234 | gamma = x[4] # path angle 235 | psi = x[5] # velocity heading 236 | 237 | alpha = u[0] # lift coefficient 238 | phi = u[1] # bank angle 239 | thrust = u[2] # thrust 240 | 241 | CLalpha = 4.2 242 | CL0 = 0.4225 243 | CL = CL0 + CLalpha * alpha 244 | 245 | # # scale 246 | T_min = self.T_min / self.scl_f 247 | T_max = self.T_max / self.scl_f 248 | v_max = self.v_max / self.scl_v 249 | v_min = self.v_min / self.scl_v 250 | 251 | h = [] 252 | h.append(CL>=self.CL_min) 253 | h.append(CL<=self.CL_max) 254 | h.append(phi>=self.phi_min) 255 | h.append(phi<=self.phi_max) 256 | h.append(thrust>=T_min) 257 | h.append(thrust<=T_max) 258 | h.append(v>=v_min) 259 | h.append(v<=v_max) 260 | h.append(gamma>=self.gamma_min) 261 | h.append(gamma<=self.gamma_max) 262 | h.append(rz>=0) 263 | return h 264 | 265 | def bc_final(self,x_cvx,xf): 266 | h = [] 267 | h.append(x_cvx == xf) 268 | 269 | return h 270 | 271 | 272 | 273 | -------------------------------------------------------------------------------- /constraints/Aircraft3dofConstraintsNondimension.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | import scipy as sp 4 | import scipy.linalg 5 | import time 6 | import random 7 | def print_np(x): 8 | print ("Type is %s" % (type(x))) 9 | print ("Shape is %s" % (x.shape,)) 10 | print ("Values are: \n%s" % (x)) 11 | 12 | 13 | from constraints import OptimalcontrolConstraints 14 | 15 | class Aircraft3dofNondimension(OptimalcontrolConstraints): 16 | def __init__(self,name,ix,iu): 17 | super().__init__(name,ix,iu) 18 | self.idx_bc_f = slice(0, ix) 19 | self.CL_min = -0.31 20 | self.CL_max = 1.52 21 | self.phi_min = -np.deg2rad(15) 22 | self.phi_max = np.deg2rad(15) 23 | self.T_min = 0 24 | self.T_max = 1126.3 * 1e3 25 | 26 | self.v_min = 95 27 | self.v_max = 270 28 | 29 | self.gamma_min = -np.deg2rad(30) 30 | self.gamma_max = np.deg2rad(30) * 0 31 | 32 | self.ih = 11 33 | 34 | def set_scale(self,scl_v,scl_f) : 35 | self.scl_v = scl_v 36 | self.scl_f = scl_f 37 | 38 | def forward(self,x,u,xbar=None,ubar=None,final=False): 39 | # state & input 40 | rx = x[0] 41 | ry = x[1] 42 | rz = x[2] 43 | v = x[3] # speed 44 | gamma = x[4] # path angle 45 | psi = x[5] # velocity heading 46 | 47 | CL = u[0] # lift coefficient 48 | phi = u[1] # bank angle 49 | thrust = u[2] # thrust 50 | 51 | # # scale 52 | T_min = self.T_min / self.scl_f 53 | T_max = self.T_max / self.scl_f 54 | v_max = self.v_max / self.scl_v 55 | v_min = self.v_min / self.scl_v 56 | 57 | h = [] 58 | h.append(CL>=self.CL_min) 59 | h.append(CL<=self.CL_max) 60 | h.append(phi>=self.phi_min) 61 | h.append(phi<=self.phi_max) 62 | h.append(thrust>=T_min) 63 | h.append(thrust<=T_max) 64 | h.append(v>=v_min) 65 | h.append(v<=v_max) 66 | h.append(gamma>=self.gamma_min) 67 | h.append(gamma<=self.gamma_max) 68 | h.append(rz>=0) 69 | return h 70 | 71 | def forward_buffer(self,x,u,bf): 72 | # state & input 73 | rx = x[0] 74 | ry = x[1] 75 | rz = x[2] 76 | v = x[3] # speed 77 | gamma = x[4] # path angle 78 | psi = x[5] # velocity heading 79 | 80 | CL = u[0] # lift coefficient 81 | phi = u[1] # bank angle 82 | thrust = u[2] # thrust 83 | 84 | h = [] 85 | h.append(CL>=bf[0] + self.CL_min) 86 | h.append(CL+bf[1]<=self.CL_max) 87 | h.append(phi>=bf[2]+self.phi_min) 88 | h.append(phi+bf[3]<=self.phi_max) 89 | h.append(thrust>=bf[4]+self.T_min) 90 | h.append(thrust+bf[5]<=self.T_max) 91 | h.append(v>=bf[6]+self.v_min) 92 | h.append(v+bf[7]<=self.v_max) 93 | h.append(gamma>=bf[8]+self.gamma_min) 94 | h.append(gamma+bf[9]<=self.gamma_max) 95 | h.append(rz >= bf[10]) 96 | return h 97 | 98 | def bc_final(self,x_cvx,xf): 99 | h = [] 100 | h.append(x_cvx == xf) 101 | 102 | return h 103 | 104 | class Aircraft3dofStateTriggered(OptimalcontrolConstraints): 105 | def __init__(self,name,ix,iu): 106 | super().__init__(name,ix,iu) 107 | self.idx_bc_f = slice(0, ix) 108 | self.CL_min = -0.31 109 | self.CL_max = 1.52 110 | self.phi_min = -np.deg2rad(15) 111 | self.phi_max = np.deg2rad(15) 112 | self.T_min = 0 113 | self.T_max = 1#1126.3 * 1e3 114 | self.v_min = 95 115 | self.v_max = 270 116 | self.gamma_min = -np.deg2rad(30) 117 | self.gamma_max = np.deg2rad(30) * 0 118 | self.scl_v = 1 119 | self.scl_f = 1 120 | self.ih = 11 121 | 122 | self.z_trigger = 2000 123 | self.y_min_trigger = -65000 124 | self.y_max_trigger = -55000 125 | 126 | def set_scale(self,scl_v,scl_f) : 127 | self.scl_v = scl_v 128 | self.scl_f = scl_f 129 | 130 | def forward(self,x,u,xbar=None,ubar=None,final=False): 131 | # state & input 132 | rx = x[0] 133 | ry = x[1] 134 | rz = x[2] 135 | v = x[3] # speed 136 | gamma = x[4] # path angle 137 | psi = x[5] # velocity heading 138 | 139 | CL = u[0] # lift coefficient 140 | phi = u[1] # bank angle 141 | thrust = u[2] # thrust 142 | 143 | # # scale 144 | T_min = self.T_min / self.scl_f 145 | T_max = self.T_max / self.scl_f 146 | v_max = self.v_max / self.scl_v 147 | v_min = self.v_min / self.scl_v 148 | 149 | h = [] 150 | h.append(CL>=self.CL_min) 151 | h.append(CL<=self.CL_max) 152 | h.append(phi>=self.phi_min) 153 | h.append(phi<=self.phi_max) 154 | h.append(thrust>=T_min) 155 | h.append(thrust<=T_max) 156 | h.append(v>=v_min) 157 | h.append(v<=v_max) 158 | h.append(gamma>=self.gamma_min) 159 | h.append(gamma<=self.gamma_max) 160 | h.append(rz>=0) 161 | 162 | # state-triggered 163 | rybar = xbar[1] 164 | rzbar = xbar[2] 165 | if rzbar < self.z_trigger : 166 | h1bar = -(rzbar-self.z_trigger) * (rybar-self.y_max_trigger) 167 | rh1_rz = -(rybar-self.y_max_trigger) * (rz-rzbar) 168 | rh1_ry = -(rzbar-self.z_trigger) * (ry-rybar) 169 | h.append(h1bar+rh1_rz+rh1_ry <= 0) 170 | 171 | h2bar = -(rzbar-self.z_trigger) * (-rybar+self.y_min_trigger) 172 | rh2_rz = (rybar-self.y_min_trigger) * (rz-rzbar) 173 | rh2_ry = (rzbar-self.z_trigger) * (ry-rybar) 174 | h.append(h2bar+rh2_rz+rh2_ry <= 0) 175 | return h 176 | 177 | 178 | def bc_final(self,x_cvx,xf): 179 | h = [] 180 | h.append(x_cvx == xf) 181 | 182 | return h 183 | 184 | 185 | 186 | class Aircraft3dofApprox(OptimalcontrolConstraints): 187 | def __init__(self,name,ix,iu): 188 | super().__init__(name,ix,iu) 189 | self.idx_bc_f = slice(0, ix) 190 | self.CL_min = -0.31 191 | self.CL_max = 1.52 192 | self.phi_min = -np.deg2rad(15) 193 | self.phi_max = np.deg2rad(15) 194 | self.T_min = 0 195 | self.T_max = 1#1126.3 * 1e3 196 | self.v_min = 95 197 | self.v_max = 270 198 | self.gamma_min = -np.deg2rad(20) 199 | self.gamma_max = np.deg2rad(20)*0 200 | self.scl_v = 1 201 | self.scl_f = 1 202 | self.ih = 11 203 | 204 | def set_scale(self,scl_v,scl_f) : 205 | self.scl_v = scl_v 206 | self.scl_f = scl_f 207 | 208 | def forward(self,x,u,xbar=None,ubar=None,final=False): 209 | # state & input 210 | rx = x[0] 211 | ry = x[1] 212 | rz = x[2] 213 | v = x[3] # speed 214 | gamma = x[4] # path angle 215 | psi = x[5] # velocity heading 216 | 217 | alpha = u[0] # lift coefficient 218 | phi = u[1] # bank angle 219 | thrust = u[2] # thrust 220 | 221 | CLalpha = 4.2 222 | CL0 = 0.4225 223 | CL = CL0 + CLalpha * alpha 224 | 225 | # # scale 226 | T_min = self.T_min / self.scl_f 227 | T_max = self.T_max / self.scl_f 228 | v_max = self.v_max / self.scl_v 229 | v_min = self.v_min / self.scl_v 230 | 231 | h = [] 232 | h.append(CL>=self.CL_min) 233 | h.append(CL<=self.CL_max) 234 | h.append(phi>=self.phi_min) 235 | h.append(phi<=self.phi_max) 236 | h.append(thrust>=T_min) 237 | h.append(thrust<=T_max) 238 | h.append(v>=v_min) 239 | h.append(v<=v_max) 240 | h.append(gamma>=self.gamma_min) 241 | h.append(gamma<=self.gamma_max) 242 | h.append(rz>=0) 243 | return h 244 | 245 | def bc_final(self,x_cvx,xf): 246 | h = [] 247 | h.append(x_cvx == xf) 248 | 249 | return h 250 | 251 | 252 | 253 | -------------------------------------------------------------------------------- /constraints/AircraftKinematicsConstraints.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | import scipy as sp 4 | import scipy.linalg 5 | import time 6 | import random 7 | def print_np(x): 8 | print ("Type is %s" % (type(x))) 9 | print ("Shape is %s" % (x.shape,)) 10 | print ("Values are: \n%s" % (x)) 11 | 12 | 13 | from constraints import OptimalcontrolConstraints 14 | 15 | class AircraftKinematics(OptimalcontrolConstraints): 16 | def __init__(self,name,ix,iu): 17 | super().__init__(name,ix,iu) 18 | self.idx_bc_f = slice(0, ix) 19 | self.CL_min = -0.27 20 | self.CL_max = 1.73 21 | self.phi_min = -np.deg2rad(30) 22 | self.phi_max = np.deg2rad(30) 23 | self.T_min = -1126.3*1e3 24 | self.T_max = 1126.3 * 1e3 25 | self.v_min = 0 26 | self.v_max = 270 27 | self.gamma_min = -np.deg2rad(20) 28 | self.gamma_max = 0 29 | self.gamma_dot_min = -np.deg2rad(5) 30 | self.gamma_dot_max = np.deg2rad(5) 31 | self.psi_dot_min = -np.deg2rad(5) 32 | self.psi_dot_max = np.deg2rad(5) 33 | 34 | def forward(self,x,u,xbar=None,ubar=None,final=False): 35 | # state & input 36 | rx = x[0] 37 | ry = x[1] 38 | rz = x[2] 39 | v = x[3] # speed 40 | gamma = x[4] # path angle 41 | psi = x[5] # velocity heading 42 | 43 | gamma_dot = u[0] 44 | psi_dot = u[1] 45 | thrust = u[2] # thrust 46 | 47 | h = [] 48 | h.append(thrust>=self.T_min) 49 | h.append(thrust<=self.T_max) 50 | h.append(v>=self.v_min) 51 | h.append(v<=self.v_max) 52 | h.append(gamma>=self.gamma_min) 53 | h.append(gamma<=self.gamma_max) 54 | h.append(gamma_dot>=self.gamma_dot_min) 55 | h.append(gamma_dot<=self.gamma_dot_max) 56 | h.append(psi_dot>=self.psi_dot_min) 57 | h.append(psi_dot<=self.psi_dot_max) 58 | return h 59 | 60 | def bc_final(self,x_cvx,xf): 61 | h = [] 62 | h.append(x_cvx == xf) 63 | 64 | return h 65 | 66 | 67 | 68 | -------------------------------------------------------------------------------- /constraints/Landing2DConstraints.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | import scipy as sp 4 | import scipy.linalg 5 | import time 6 | import random 7 | def print_np(x): 8 | print ("Type is %s" % (type(x))) 9 | print ("Shape is %s" % (x.shape,)) 10 | print ("Values are: \n%s" % (x)) 11 | 12 | 13 | from constraints import OptimalcontrolConstraints 14 | 15 | class Landing2D(OptimalcontrolConstraints): 16 | def __init__(self,name,ix,iu,ih): 17 | super().__init__(name,ix,iu,ih) 18 | self.idx_bc_f = slice(0, ix) 19 | 20 | def forward(self,x,u,xbar=None,ubar=None): 21 | # state & input 22 | rx = x[0] 23 | ry = x[1] 24 | vx = x[2] 25 | vy = x[3] 26 | t = x[4] 27 | w = x[5] 28 | 29 | gimbal = u[0] 30 | thrust = u[1] 31 | 32 | h = [] 33 | h.append(t-np.deg2rad(90) <= 0) 34 | h.append(-t-np.deg2rad(90) <= 0) 35 | # h.append(w-np.deg2rad(60) <= 0) 36 | # h.append(-w-np.deg2rad(60) <= 0) 37 | h.append(thrust-4 <= 0) 38 | h.append(-thrust+0 <= 0) 39 | h.append(gimbal-np.deg2rad(90) <= 0) 40 | h.append(-gimbal-np.deg2rad(90) <= 0) 41 | h.append(-ry <= 0) 42 | return h 43 | 44 | def bc_final(self,x_cvx,xf): 45 | h = [] 46 | h.append(x_cvx == xf) 47 | 48 | return h 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /constraints/Landing3DConstraints.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | import scipy as sp 4 | import scipy.linalg 5 | import time 6 | import random 7 | import cvxpy as cvx 8 | def print_np(x): 9 | print ("Type is %s" % (type(x))) 10 | print ("Shape is %s" % (x.shape,)) 11 | print ("Values are: \n%s" % (x)) 12 | 13 | from constraints import OptimalcontrolConstraints 14 | import IPython 15 | def quaternion_to_euler(w, x, y, z): 16 | 17 | t0 = +2.0 * (w * x + y * z) 18 | t1 = +1.0 - 2.0 * (x * x + y * y) 19 | roll_x = np.arctan2(t0, t1) 20 | 21 | t2 = +2.0 * (w * y - z * x) 22 | t2 = +1.0 if t2 > +1.0 else t2 23 | t2 = -1.0 if t2 < -1.0 else t2 24 | pitch_y = np.arcsin(t2) 25 | 26 | t3 = +2.0 * (w * z + x * y) 27 | t4 = +1.0 - 2.0 * (y * y + z * z) 28 | yaw_z = np.arctan2(t3, t4) 29 | 30 | return roll_x, pitch_y, yaw_z 31 | class Landing3D(OptimalcontrolConstraints): 32 | def __init__(self,name,ix,iu): 33 | super().__init__(name,ix,iu) 34 | self.m_dry = 1.0 35 | self.T_min = 1.5 36 | self.T_max = 6.5 37 | self.delta_max = np.deg2rad(30) # gimbal angle 38 | self.theta_max = np.deg2rad(90) # tilt angle 39 | self.gamma_gs = np.deg2rad(20) 40 | self.w_max = np.deg2rad(60) 41 | self.idx_bc_f = slice(1, 14) 42 | 43 | def forward(self,x,u,xbar,ubar,final=False): 44 | # state & input 45 | m = x[0] 46 | rx = x[1] 47 | ry = x[2] 48 | rz = x[3] 49 | vx = x[4] 50 | vy = x[5] 51 | vz = x[6] 52 | q = x[7:11] 53 | w = x[11:14] 54 | 55 | ux = u[0] 56 | uy = u[1] 57 | uz = u[2] 58 | 59 | h = [] 60 | # state constraints 61 | h.append(m >= self.m_dry) 62 | # if final == False : 63 | h.append(cvx.norm(x[1:3]) <= x[3] / np.tan(self.gamma_gs)) 64 | h.append(cvx.norm(x[8:10]) <= np.sqrt((1-np.cos(self.theta_max))/2)) 65 | h.append(cvx.norm(x[11:14]) <= self.w_max) 66 | 67 | # input constraints 68 | h.append(cvx.norm(u) <= self.T_max) 69 | h.append(self.T_min - ubar.T@u / cvx.norm(ubar) <= 0) 70 | h.append(np.cos(self.delta_max) * cvx.norm(u) <= u[2]) 71 | return h 72 | 73 | def bc_final(self,x_cvx,xf): 74 | h = [] 75 | h.append(x_cvx[self.idx_bc_f] == xf[self.idx_bc_f]) 76 | 77 | return h 78 | 79 | def bc_violation(self,xnn,xf) : 80 | position = np.linalg.norm(xnn[1:4]-xf[1:4]) 81 | velocity = np.linalg.norm(xnn[4:7]-xf[4:7]) 82 | roll,pitch,yaw = quaternion_to_euler(xnn[7],xnn[8],xnn[9],xnn[10]) 83 | roll_f,pitch_f,yaw_f = quaternion_to_euler(xf[7],xf[8],xf[9],xf[10]) 84 | attitude = np.rad2deg(np.linalg.norm(np.array([roll,pitch,yaw])-np.array([roll_f,pitch_f,yaw_f]))) 85 | angular_rate =np.rad2deg(np.linalg.norm(xnn[11:]-xf[11:]) ) 86 | return position,velocity,attitude,angular_rate 87 | 88 | def forward_full(self,x,u): 89 | xdim = np.ndim(x) 90 | if xdim == 1: # 1 step state & input 91 | N = 1 92 | x = np.expand_dims(x,axis=0) 93 | else : 94 | N = np.size(x,axis = 0) 95 | udim = np.ndim(u) 96 | if udim == 1 : 97 | u = np.expand_dims(u,axis=0) 98 | 99 | # state & input 100 | m = x[:,0] 101 | rx = x[:,1] 102 | ry = x[:,2] 103 | rz = x[:,3] 104 | vx = x[:,4] 105 | vy = x[:,5] 106 | vz = x[:,6] 107 | q = x[:,7:11] 108 | w = x[:,11:14] 109 | 110 | q0 = x[:,7] 111 | q1 = x[:,8] 112 | q2 = x[:,9] 113 | q3 = x[:,10] 114 | 115 | w = x[:,11:14] 116 | wx = x[:,11] 117 | wy = x[:,12] 118 | wz = x[:,13] 119 | 120 | ux = u[:,0] 121 | uy = u[:,1] 122 | uz = u[:,2] 123 | 124 | h = np.zeros((N,7)) 125 | h[:,0] = (self.m_dry - m) 126 | h[:,1] = np.rad2deg(np.linalg.norm(w,axis=1) - self.w_max) 127 | 128 | # h[:,2] = np.tan(self.gamma_gs) * np.linalg.norm(x[:,1:3],axis=1) - x[:,3] 129 | h[:,2] = np.rad2deg(self.gamma_gs - np.arctan(x[:,3] / np.linalg.norm(x[:,1:3],axis=1))) 130 | 131 | # h[:,3] = np.cos(self.theta_max) - 1 + 2*(q1**2 + q2**2) 132 | h[:,3] = np.rad2deg(np.arccos( 1 - 2*(q1**2 + q2**2)) - self.theta_max) 133 | 134 | h[:,4] = (np.linalg.norm(u,axis=1) - self.T_max) 135 | 136 | h[:,5] = (self.T_min - np.linalg.norm(u,axis=1)) 137 | 138 | # h[:,6] = np.cos(self.delta_max) * np.linalg.norm(u,axis=1) - uz 139 | h[:,6] = np.rad2deg(np.arccos(uz / np.linalg.norm(u,axis=1)) - self.delta_max) 140 | return h 141 | 142 | def normalized_constraint_violation(self,x,u): 143 | xdim = np.ndim(x) 144 | if xdim == 1: # 1 step state & input 145 | N = 1 146 | x = np.expand_dims(x,axis=0) 147 | else : 148 | N = np.size(x,axis = 0) 149 | udim = np.ndim(u) 150 | if udim == 1 : 151 | u = np.expand_dims(u,axis=0) 152 | 153 | # state & input 154 | m = x[:,0] 155 | rx = x[:,1] 156 | ry = x[:,2] 157 | rz = x[:,3] 158 | vx = x[:,4] 159 | vy = x[:,5] 160 | vz = x[:,6] 161 | q = x[:,7:11] 162 | w = x[:,11:14] 163 | 164 | q0 = x[:,7] 165 | q1 = x[:,8] 166 | q2 = x[:,9] 167 | q3 = x[:,10] 168 | 169 | w = x[:,11:14] 170 | wx = x[:,11] 171 | wy = x[:,12] 172 | wz = x[:,13] 173 | 174 | ux = u[:,0] 175 | uy = u[:,1] 176 | uz = u[:,2] 177 | 178 | h = np.zeros((N,7)) 179 | h[:,0] = (self.m_dry - m)/self.m_dry 180 | h[:,1] = (np.linalg.norm(w,axis=1) - self.w_max)/self.w_max 181 | 182 | # h[:,2] = np.tan(self.gamma_gs) * np.linalg.norm(x[:,1:3],axis=1) - x[:,3] 183 | h[:,2] = (self.gamma_gs - np.arctan(x[:,3] / np.linalg.norm(x[:,1:3],axis=1)))/self.gamma_gs 184 | 185 | # h[:,3] = np.cos(self.theta_max) - 1 + 2*(q1**2 + q2**2) 186 | h[:,3] = (np.arccos( 1 - 2*(q1**2 + q2**2)) - self.theta_max) / self.theta_max 187 | 188 | h[:,4] = (np.linalg.norm(u,axis=1) - self.T_max) / self.T_max 189 | 190 | h[:,5] = (self.T_min - np.linalg.norm(u,axis=1)) / self.T_min 191 | 192 | # h[:,6] = np.cos(self.delta_max) * np.linalg.norm(u,axis=1) - uz 193 | h[:,6] = (np.arccos(uz / np.linalg.norm(u,axis=1)) - self.delta_max) / self.delta_max 194 | return h 195 | 196 | 197 | -------------------------------------------------------------------------------- /constraints/LinearConstraints.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | import scipy as sp 4 | import scipy.linalg 5 | import time 6 | import random 7 | def print_np(x): 8 | print ("Type is %s" % (type(x))) 9 | print ("Shape is %s" % (x.shape,)) 10 | print ("Values are: \n%s" % (x)) 11 | 12 | 13 | from constraints import OptimalcontrolConstraints 14 | 15 | class Linear(OptimalcontrolConstraints): 16 | def __init__(self,name,ix,iu,ih): 17 | super().__init__(name,ix,iu,ih) 18 | 19 | def forward(self,x,u): 20 | 21 | v = u[0] 22 | w = u[1] 23 | 24 | h = [] 25 | # h.append(v-0.2 <= 0) 26 | # h.append(-w+0 <= 0) 27 | 28 | return h 29 | 30 | -------------------------------------------------------------------------------- /constraints/QuadRotorPointMassConstraints.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | import time 4 | import random 5 | import cvxpy as cvx 6 | 7 | def print_np(x): 8 | print ("Type is %s" % (type(x))) 9 | print ("Shape is %s" % (x.shape,)) 10 | print ("Values are: \n%s" % (x)) 11 | 12 | from constraints import OptimalcontrolConstraints 13 | import IPython 14 | 15 | class quadrotorpm(OptimalcontrolConstraints): 16 | def __init__(self,name,ix,iu,c=None,H=None): 17 | super().__init__(name,ix,iu) 18 | # self.theta_max = np.deg2rad(30) # tilt angle 19 | self.idx_bc_f = slice(0, 6) 20 | self.T_min = 5 21 | self.T_max = 30 22 | self.c = c 23 | self.H = H 24 | self.delta_max = np.deg2rad(30) 25 | 26 | def set_obstacle(self,c,H) : 27 | self.c = c 28 | self.H = H 29 | 30 | def forward(self,x,u,xbar,ubar,idx=None): 31 | 32 | h = [] 33 | # state constraints 34 | # obstacle avoidance 35 | def get_obs_const(c1,H1) : 36 | return 1 - np.linalg.norm(H1@(xbar[0:3]-c1)) - (H1.T@H1@(xbar[0:3]-c1)/np.linalg.norm(H1@(xbar[0:3]-c1))).T@(x[0:3]-xbar[0:3]) <=0 37 | if self.H is not None : 38 | for c1,H1 in zip(self.c,self.H) : 39 | h.append(get_obs_const(c1,H1)) 40 | 41 | # input constraints 42 | h.append(cvx.norm(u) <= self.T_max) 43 | h.append(self.T_min - np.transpose(np.expand_dims(ubar,1))@u / np.linalg.norm(ubar) <= 0) 44 | h.append(np.cos(self.delta_max) * cvx.norm(u) <= u[2]) 45 | return h 46 | 47 | def bc_final(self,x_cvx,xf): 48 | h = [] 49 | h.append(x_cvx[self.idx_bc_f] == xf[self.idx_bc_f]) 50 | return h 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /constraints/QuadRotorSmallAngleConstraints.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | import time 4 | import random 5 | import cvxpy as cvx 6 | 7 | def print_np(x): 8 | print ("Type is %s" % (type(x))) 9 | print ("Shape is %s" % (x.shape,)) 10 | print ("Values are: \n%s" % (x)) 11 | 12 | from constraints import OptimalcontrolConstraints 13 | import IPython 14 | 15 | class quadrotorsa(OptimalcontrolConstraints): 16 | def __init__(self,name,ix,iu,ih,c=None,H=None): 17 | super().__init__(name,ix,iu,ih) 18 | self.idx_bc_f = slice(0, 12) 19 | self.T_min = 5 20 | self.T_max = 20 21 | self.c = c 22 | self.H = H 23 | self.roll_max = np.deg2rad(20) 24 | self.rolldot_max = np.deg2rad(40) 25 | self.pitch_max = np.deg2rad(20) 26 | self.pitchdot_max = np.deg2rad(40) 27 | def set_obstacle(self,c,H) : 28 | self.c = c 29 | self.H = H 30 | 31 | def forward(self,x,u,xbar,ubar): 32 | 33 | h = [] 34 | # state constraints 35 | h.append(x[3]<=self.roll_max) 36 | h.append(-self.roll_max <= x[3]) 37 | h.append(x[4]<=self.pitch_max) 38 | h.append(-self.pitch_max <= x[4]) 39 | 40 | h.append(x[9]<=self.rolldot_max) 41 | h.append(-self.rolldot_max <= x[9]) 42 | h.append(x[10]<=self.pitchdot_max) 43 | h.append(-self.pitchdot_max <= x[10]) 44 | 45 | # obstacle avoidance 46 | def get_obs_const(c1,H1) : 47 | return 1 - np.linalg.norm(H1@(xbar[0:3]-c1)) - (H1.T@H1@(xbar[0:3]-c1)/np.linalg.norm(H1@(xbar[0:3]-c1))).T@(x[0:3]-xbar[0:3]) <=0 48 | if self.H is not None : 49 | for c1,H1 in zip(self.c,self.H) : 50 | h.append(get_obs_const(c1,H1)) 51 | 52 | # input constraints 53 | h.append(u[0] <= self.T_max) 54 | h.append(self.T_min <= u[0]) 55 | return h 56 | 57 | def bc_final(self,x_cvx,xf): 58 | h = [] 59 | h.append(x_cvx[self.idx_bc_f] == xf[self.idx_bc_f]) 60 | return h 61 | 62 | 63 | 64 | -------------------------------------------------------------------------------- /constraints/UnicycleConstraints.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | import scipy as sp 4 | import scipy.linalg 5 | import time 6 | import random 7 | def print_np(x): 8 | print ("Type is %s" % (type(x))) 9 | print ("Shape is %s" % (x.shape,)) 10 | print ("Values are: \n%s" % (x)) 11 | 12 | 13 | from constraints import OptimalcontrolConstraints 14 | 15 | class UnicycleConstraints(OptimalcontrolConstraints): 16 | def __init__(self,name,ix,iu): 17 | super().__init__(name,ix,iu) 18 | self.idx_bc_f = slice(0, ix) 19 | self.ih = 4 20 | 21 | def forward(self,x,u,xbar=None,ybar=None,idx=None): 22 | 23 | v = u[0] 24 | w = u[1] 25 | 26 | h = [] 27 | h.append(v-2.0 <= 0) 28 | h.append(v >= -2.0) 29 | h.append(w<=2.0) 30 | h.append(w>=-2.0) 31 | 32 | return h 33 | 34 | def bc_final(self,x_cvx,xf): 35 | h = [] 36 | h.append(x_cvx == xf) 37 | 38 | return h 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /constraints/__pycache__/Aircraft3dofConstraints.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/constraints/__pycache__/Aircraft3dofConstraints.cpython-310.pyc -------------------------------------------------------------------------------- /constraints/__pycache__/Aircraft3dofConstraints.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/constraints/__pycache__/Aircraft3dofConstraints.cpython-38.pyc -------------------------------------------------------------------------------- /constraints/__pycache__/AircraftKinematicsConstraints.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/constraints/__pycache__/AircraftKinematicsConstraints.cpython-38.pyc -------------------------------------------------------------------------------- /constraints/__pycache__/Landing2DConstraints.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/constraints/__pycache__/Landing2DConstraints.cpython-38.pyc -------------------------------------------------------------------------------- /constraints/__pycache__/Landing3DConstraints.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/constraints/__pycache__/Landing3DConstraints.cpython-38.pyc -------------------------------------------------------------------------------- /constraints/__pycache__/LinearConstraints.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/constraints/__pycache__/LinearConstraints.cpython-38.pyc -------------------------------------------------------------------------------- /constraints/__pycache__/QuadRotorPointMassConstraints.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/constraints/__pycache__/QuadRotorPointMassConstraints.cpython-38.pyc -------------------------------------------------------------------------------- /constraints/__pycache__/QuadRotorSmallAngleConstraints.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/constraints/__pycache__/QuadRotorSmallAngleConstraints.cpython-38.pyc -------------------------------------------------------------------------------- /constraints/__pycache__/UnicycleConstraints.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/constraints/__pycache__/UnicycleConstraints.cpython-310.pyc -------------------------------------------------------------------------------- /constraints/__pycache__/UnicycleConstraints.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/constraints/__pycache__/UnicycleConstraints.cpython-38.pyc -------------------------------------------------------------------------------- /constraints/__pycache__/UnicycleConstraints.cpython-39.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/constraints/__pycache__/UnicycleConstraints.cpython-39.pyc -------------------------------------------------------------------------------- /constraints/__pycache__/constraints.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/constraints/__pycache__/constraints.cpython-310.pyc -------------------------------------------------------------------------------- /constraints/__pycache__/constraints.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/constraints/__pycache__/constraints.cpython-38.pyc -------------------------------------------------------------------------------- /constraints/__pycache__/constraints.cpython-39.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/constraints/__pycache__/constraints.cpython-39.pyc -------------------------------------------------------------------------------- /constraints/constraints.py: -------------------------------------------------------------------------------- 1 | 2 | # coding: utf-8 3 | 4 | # In[ ]: 5 | 6 | from __future__ import division 7 | import matplotlib.pyplot as plt 8 | import numpy as np 9 | import scipy as sp 10 | import scipy.linalg 11 | import time 12 | import random 13 | def print_np(x): 14 | print ("Type is %s" % (type(x))) 15 | print ("Shape is %s" % (x.shape,)) 16 | print ("Values are: \n%s" % (x)) 17 | 18 | 19 | 20 | 21 | class OptimalcontrolConstraints(object) : 22 | def __init__(self,name,ix,iu) : 23 | self.name = name 24 | self.ix = ix 25 | self.iu = iu 26 | 27 | def forward(self) : 28 | print("this is in parent class") 29 | pass 30 | 31 | def diff(self) : 32 | print("this is in parent class") 33 | pass 34 | 35 | def diff_numeric(self,x,u) : 36 | # state & input size 37 | ix = self.ix 38 | iu = self.iu 39 | ih = self.ih 40 | 41 | ndim = np.ndim(x) 42 | if ndim == 1: # 1 step state & input 43 | N = 1 44 | # x = np.expand_dims(x,axis=0) 45 | # u = np.expand_dims(u,axis=0) 46 | else : 47 | N = np.size(x,axis = 0) 48 | 49 | # numerical difference 50 | h = pow(2,-17) 51 | eps_x = np.identity(ix) 52 | eps_u = np.identity(iu) 53 | 54 | # expand to tensor 55 | x_mat = np.expand_dims(x,axis=2) 56 | u_mat = np.expand_dims(u,axis=2) 57 | 58 | # diag 59 | x_diag = np.tile(x_mat,(1,1,ix)) 60 | u_diag = np.tile(u_mat,(1,1,iu)) 61 | 62 | # augmented = [x_aug x], [u, u_aug] 63 | x_aug = x_diag + eps_x * h 64 | x_aug = np.dstack((x_aug,np.tile(x_mat,(1,1,iu)))) 65 | x_aug = np.reshape( np.transpose(x_aug,(0,2,1)), (N*(iu+ix),ix)) 66 | 67 | u_aug = u_diag + eps_u * h 68 | u_aug = np.dstack((np.tile(u_mat,(1,1,ix)),u_aug)) 69 | u_aug = np.reshape( np.transpose(u_aug,(0,2,1)), (N*(iu+ix),iu)) 70 | 71 | # numerical difference 72 | f_nominal = self.forward(x,u,) 73 | f_change = self.forward(x_aug,u_aug) 74 | # print_np(f_change) 75 | f_change = np.reshape(f_change,(N,ix+iu,ih)) 76 | # print_np(f_nominal) 77 | # print_np(f_change) 78 | f_diff = ( f_change - np.reshape(f_nominal,(N,1,ih)) ) / h 79 | # print_np(f_diff) 80 | f_diff = np.transpose(f_diff,[0,2,1]) 81 | fx = f_diff[:,:,0:ix] 82 | fu = f_diff[:,:,ix:ix+iu] 83 | 84 | return np.squeeze(fx), np.squeeze(fu) 85 | 86 | -------------------------------------------------------------------------------- /cost/Aircraft3dofCost.py: -------------------------------------------------------------------------------- 1 | from __future__ import division 2 | import matplotlib.pyplot as plt 3 | import numpy as np 4 | import time 5 | import random 6 | import cvxpy as cvx 7 | 8 | def print_np(x): 9 | print ("Type is %s" % (type(x))) 10 | print ("Shape is %s" % (x.shape,)) 11 | # print ("Values are: \n%s" % (x)) 12 | 13 | from cost import OptimalcontrolCost 14 | 15 | class Aircraft3dof(OptimalcontrolCost): 16 | def __init__(self,name,ix,iu,N): 17 | super().__init__(name,ix,iu,N) 18 | self.R = np.eye(iu) 19 | self.R[0,0] = 0 20 | self.R[1,1] = 0 21 | 22 | # def estimate_cost(self,x,u): 23 | # # dimension 24 | # ndim = np.ndim(x) 25 | # if ndim == 1: # 1 step state & input 26 | # N = 1 27 | # x = np.expand_dims(x,axis=0) 28 | # u = np.expand_dims(u,axis=0) 29 | # else : 30 | # N = np.size(x,axis = 0) 31 | # cost_total = u[:,2] 32 | # return cost_total 33 | 34 | # def estimate_final_cost(self,x,u) : 35 | # return self.estimate_cost(x,u) 36 | 37 | def estimate_cost_cvx(self,x,u,idx=0): 38 | cost_total = cvx.quad_form(u,self.R) 39 | # cost_total = u[2] 40 | return cost_total -------------------------------------------------------------------------------- /cost/AircraftKinematicsCost.py: -------------------------------------------------------------------------------- 1 | from __future__ import division 2 | import matplotlib.pyplot as plt 3 | import numpy as np 4 | import time 5 | import random 6 | import cvxpy as cp 7 | 8 | def print_np(x): 9 | print ("Type is %s" % (type(x))) 10 | print ("Shape is %s" % (x.shape,)) 11 | # print ("Values are: \n%s" % (x)) 12 | 13 | from cost import OptimalcontrolCost 14 | 15 | class AircraftKinematics(OptimalcontrolCost): 16 | def __init__(self,name,ix,iu,N): 17 | super().__init__(name,ix,iu,N) 18 | 19 | self.Q = 0.0*np.identity(self.ix) 20 | 21 | self.R = np.identity(self.iu) 22 | self.R[0,0] = 1e3 23 | self.R[1,1] = 1e3 24 | self.R[2,2] = 1e-8 25 | # self.R[2,2] = 1.0 26 | 27 | def estimate_cost(self,x,u): 28 | # dimension 29 | ndim = np.ndim(x) 30 | if ndim == 1: # 1 step state & input 31 | N = 1 32 | x = np.expand_dims(x,axis=0) 33 | u = np.expand_dims(u,axis=0) 34 | else : 35 | N = np.size(x,axis = 0) 36 | 37 | x_mat = np.expand_dims(x,2) 38 | Q_mat = np.tile(self.Q,(N,1,1)) 39 | lx = np.squeeze(np.matmul(np.matmul(np.transpose(x_mat,(0,2,1)),Q_mat),x_mat)) 40 | 41 | # cost for input 42 | u_mat = np.expand_dims(u,axis=2) 43 | R_mat = np.tile(self.R,(N,1,1)) 44 | lu = np.squeeze( np.matmul(np.matmul(np.transpose(u_mat,(0,2,1)),R_mat),u_mat) ) 45 | 46 | cost_total = 0.5*(lx + lu) 47 | 48 | return cost_total 49 | 50 | def estimate_final_cost(self,x,u) : 51 | return self.estimate_cost(x,u) 52 | 53 | def estimate_cost_cvx(self,x,u,idx=0): 54 | # dimension 55 | cost_total = 0.5*(cp.quad_form(x, self.Q) + cp.quad_form(u,self.R)) 56 | 57 | return cost_total -------------------------------------------------------------------------------- /cost/FinaltimeFreeCost.py: -------------------------------------------------------------------------------- 1 | from __future__ import division 2 | import matplotlib.pyplot as plt 3 | import numpy as np 4 | import time 5 | import random 6 | import cvxpy as cp 7 | 8 | def print_np(x): 9 | print ("Type is %s" % (type(x))) 10 | print ("Shape is %s" % (x.shape,)) 11 | # print ("Values are: \n%s" % (x)) 12 | 13 | from cost import OptimalcontrolCost 14 | 15 | class Finaltime(OptimalcontrolCost): 16 | def __init__(self,name,ix,iu,N): 17 | super().__init__(name,ix,iu,N) 18 | 19 | def estimate_cost_cvx(self,sigma): 20 | return sigma -------------------------------------------------------------------------------- /cost/Landing2DCost.py: -------------------------------------------------------------------------------- 1 | from __future__ import division 2 | import matplotlib.pyplot as plt 3 | import numpy as np 4 | import time 5 | import random 6 | import cvxpy as cp 7 | 8 | def print_np(x): 9 | print ("Type is %s" % (type(x))) 10 | print ("Shape is %s" % (x.shape,)) 11 | # print ("Values are: \n%s" % (x)) 12 | 13 | from cost import OptimalcontrolCost 14 | 15 | class Landing2D(OptimalcontrolCost): 16 | def __init__(self,name,ix,iu,N): 17 | super().__init__(name,ix,iu,N) 18 | 19 | self.Q = 0.0*np.identity(self.ix) 20 | 21 | self.R = 1e-6 * np.identity(self.iu) 22 | self.R[0,0] = 0.0 * self.R[0,0] 23 | 24 | 25 | def estimate_cost(self,x,u): 26 | # dimension 27 | ndim = np.ndim(x) 28 | if ndim == 1: # 1 step state & input 29 | N = 1 30 | x = np.expand_dims(x,axis=0) 31 | u = np.expand_dims(u,axis=0) 32 | else : 33 | N = np.size(x,axis = 0) 34 | 35 | x_mat = np.expand_dims(x,2) 36 | Q_mat = np.tile(self.Q,(N,1,1)) 37 | lx = np.squeeze(np.matmul(np.matmul(np.transpose(x_mat,(0,2,1)),Q_mat),x_mat)) 38 | 39 | # cost for input 40 | u_mat = np.expand_dims(u,axis=2) 41 | R_mat = np.tile(self.R,(N,1,1)) 42 | lu = np.squeeze( np.matmul(np.matmul(np.transpose(u_mat,(0,2,1)),R_mat),u_mat) ) 43 | 44 | cost_total = 0.5*(lx + lu) 45 | 46 | return cost_total 47 | 48 | def estimate_final_cost(self,x,u) : 49 | return self.estimate_cost(x,u) 50 | 51 | def estimate_cost_cvx(self,x,u,idx=0): 52 | # dimension 53 | cost_total = 0.5*(cp.quad_form(x, self.Q) + cp.quad_form(u,self.R)) 54 | 55 | return cost_total -------------------------------------------------------------------------------- /cost/Landing3DCost.py: -------------------------------------------------------------------------------- 1 | from __future__ import division 2 | import matplotlib.pyplot as plt 3 | import numpy as np 4 | import time 5 | import random 6 | import cvxpy as cp 7 | 8 | def print_np(x): 9 | print ("Type is %s" % (type(x))) 10 | print ("Shape is %s" % (x.shape,)) 11 | # print ("Values are: \n%s" % (x)) 12 | 13 | from cost import OptimalcontrolCost 14 | 15 | class Landing3D(OptimalcontrolCost): 16 | def __init__(self,name,ix,iu,N): 17 | super().__init__(name,ix,iu,N) 18 | self.N = N 19 | 20 | self.Q = 0*np.identity(self.ix) 21 | self.R = 0 * np.identity(self.iu) 22 | self.R[0,0] = 0 23 | 24 | 25 | def estimate_cost(self,x,u): 26 | # dimension 27 | ndim = np.ndim(x) 28 | if ndim == 1: # 1 step state & input 29 | N = 1 30 | x = np.expand_dims(x,axis=0) 31 | u = np.expand_dims(u,axis=0) 32 | else : 33 | N = np.size(x,axis = 0) 34 | 35 | x_mat = np.expand_dims(x,2) 36 | Q_mat = np.tile(self.Q,(N,1,1)) 37 | lx = np.squeeze(np.matmul(np.matmul(np.transpose(x_mat,(0,2,1)),Q_mat),x_mat)) 38 | 39 | # cost for input 40 | u_mat = np.expand_dims(u,axis=2) 41 | R_mat = np.tile(self.R,(N,1,1)) 42 | lu = np.squeeze( np.matmul(np.matmul(np.transpose(u_mat,(0,2,1)),R_mat),u_mat) ) 43 | 44 | cost_total = 0.5*(lx + lu) 45 | 46 | return cost_total 47 | 48 | def estimate_final_cost(self,x,u) : 49 | ndim = np.ndim(x) 50 | assert ndim == 1 51 | return -x[0] 52 | 53 | def estimate_cost_cvx(self,x,u,idx): 54 | if idx == self.N : 55 | cost_total = -x[0] 56 | # cost_total = -0*x[0] 57 | # dimension 58 | else : 59 | cost_total = 0.5*(cp.quad_form(x, self.Q) + cp.quad_form(u,self.R)) 60 | 61 | return cost_total -------------------------------------------------------------------------------- /cost/LinearCost.py: -------------------------------------------------------------------------------- 1 | from __future__ import division 2 | import matplotlib.pyplot as plt 3 | import numpy as np 4 | import time 5 | import random 6 | import cvxpy as cp 7 | 8 | def print_np(x): 9 | print ("Type is %s" % (type(x))) 10 | print ("Shape is %s" % (x.shape,)) 11 | # print ("Values are: \n%s" % (x)) 12 | 13 | from cost import OptimalcontrolCost 14 | 15 | class Linear(OptimalcontrolCost): 16 | def __init__(self,name,ix,iu,N): 17 | super().__init__(name,ix,iu,N) 18 | 19 | self.Q = 0*np.identity(ix) 20 | 21 | self.R = 1 * np.identity(iu) 22 | 23 | self.ix = ix 24 | self.iu = iu 25 | self.N = N 26 | 27 | def estimate_cost(self,x,u): 28 | # dimension 29 | ndim = np.ndim(x) 30 | if ndim == 1: # 1 step state & input 31 | N = 1 32 | x = np.expand_dims(x,axis=0) 33 | u = np.expand_dims(u,axis=0) 34 | else : 35 | N = np.size(x,axis = 0) 36 | 37 | x_mat = np.expand_dims(x,2) 38 | Q_mat = np.tile(self.Q,(N,1,1)) 39 | lx = np.squeeze(np.matmul(np.matmul(np.transpose(x_mat,(0,2,1)),Q_mat),x_mat)) 40 | 41 | # cost for input 42 | u_mat = np.expand_dims(u,axis=2) 43 | R_mat = np.tile(self.R,(N,1,1)) 44 | lu = np.squeeze( np.matmul(np.matmul(np.transpose(u_mat,(0,2,1)),R_mat),u_mat) ) 45 | 46 | cost_total = 0.5*(lx + lu) 47 | 48 | return cost_total 49 | 50 | def estimate_cost_cvx(self,x,u): 51 | # dimension 52 | cost_total = 0.5*(cp.quad_form(x, self.Q) + cp.quad_form(u,self.R)) 53 | 54 | return cost_total -------------------------------------------------------------------------------- /cost/QuadRotorPointMassCost.py: -------------------------------------------------------------------------------- 1 | from __future__ import division 2 | import matplotlib.pyplot as plt 3 | import numpy as np 4 | import time 5 | import random 6 | import cvxpy as cp 7 | 8 | def print_np(x): 9 | print ("Type is %s" % (type(x))) 10 | print ("Shape is %s" % (x.shape,)) 11 | # print ("Values are: \n%s" % (x)) 12 | 13 | from cost import OptimalcontrolCost 14 | 15 | class quadrotorpm(OptimalcontrolCost): 16 | def __init__(self,name,ix,iu,N): 17 | super().__init__(name,ix,iu,N) 18 | self.N = N 19 | self.Q = 0*np.identity(self.ix) 20 | self.R = 1*np.identity(self.iu) 21 | 22 | def estimate_cost(self,x,u): 23 | # dimension 24 | ndim = np.ndim(x) 25 | if ndim == 1: # 1 step state & input 26 | N = 1 27 | x = np.expand_dims(x,axis=0) 28 | u = np.expand_dims(u,axis=0) 29 | else : 30 | N = np.size(x,axis = 0) 31 | 32 | x_mat = np.expand_dims(x,2) 33 | Q_mat = np.tile(self.Q,(N,1,1)) 34 | lx = np.squeeze(np.matmul(np.matmul(np.transpose(x_mat,(0,2,1)),Q_mat),x_mat)) 35 | 36 | # cost for input 37 | u_mat = np.expand_dims(u,axis=2) 38 | R_mat = np.tile(self.R,(N,1,1)) 39 | lu = np.squeeze( np.matmul(np.matmul(np.transpose(u_mat,(0,2,1)),R_mat),u_mat) ) 40 | 41 | cost_total = 0.5*(lx + lu) 42 | 43 | return cost_total 44 | 45 | def estimate_final_cost(self,x,u) : 46 | return self.estimate_cost(x,u) 47 | # ndim = np.ndim(x) 48 | # assert ndim == 1 49 | # return 0 50 | 51 | def estimate_cost_cvx(self,x,u,idx): 52 | # if idx == self.N : 53 | # cost_total = 0 54 | # else : 55 | cost_total = 0.5*(cp.quad_form(x, self.Q) + cp.quad_form(u,self.R)) 56 | 57 | return cost_total -------------------------------------------------------------------------------- /cost/QuadRotorSmallAngleCost.py: -------------------------------------------------------------------------------- 1 | from __future__ import division 2 | import matplotlib.pyplot as plt 3 | import numpy as np 4 | import time 5 | import random 6 | import cvxpy as cp 7 | 8 | def print_np(x): 9 | print ("Type is %s" % (type(x))) 10 | print ("Shape is %s" % (x.shape,)) 11 | # print ("Values are: \n%s" % (x)) 12 | 13 | from cost import OptimalcontrolCost 14 | 15 | class quadrotorsa(OptimalcontrolCost): 16 | def __init__(self,name,ix,iu,N): 17 | super().__init__(name,ix,iu,N) 18 | self.N = N 19 | self.Q = 0*np.identity(self.ix) 20 | self.R = np.identity(self.iu) 21 | self.R[0,0] = 1 22 | self.R[1,1] = 2 23 | self.R[2,2] = 2 24 | self.R[3,3] = 2 25 | self.R *= 1 26 | self.g = 9.81 27 | 28 | def estimate_cost(self,x,u): 29 | # dimension 30 | ndim = np.ndim(x) 31 | if ndim == 1: # 1 step state & input 32 | N = 1 33 | x = np.expand_dims(x,axis=0) 34 | u = np.expand_dims(u,axis=0) 35 | else : 36 | N = np.size(x,axis = 0) 37 | 38 | x_mat = np.expand_dims(x,2) 39 | Q_mat = np.tile(self.Q,(N,1,1)) 40 | lx = np.squeeze(np.matmul(np.matmul(np.transpose(x_mat,(0,2,1)),Q_mat),x_mat)) 41 | 42 | # cost for input 43 | u_diff = np.zeros_like(u) 44 | u_diff[:,0] = u[:,0] - self.g*0 45 | u_diff[:,1] = u[:,1] 46 | u_diff[:,2] = u[:,2] 47 | u_diff[:,3] = u[:,3] 48 | u_mat = np.expand_dims(u_diff,axis=2) 49 | R_mat = np.tile(self.R,(N,1,1)) 50 | lu = np.squeeze( np.matmul(np.matmul(np.transpose(u_mat,(0,2,1)),R_mat),u_mat) ) 51 | 52 | cost_total = 0.5*(lx + lu) 53 | 54 | return cost_total 55 | 56 | def estimate_final_cost(self,x,u) : 57 | return self.estimate_cost(x,u) 58 | # ndim = np.ndim(x) 59 | # assert ndim == 1 60 | # return 0 61 | 62 | def estimate_cost_cvx(self,x,u,idx): 63 | # if idx == self.N : 64 | # cost_total = 0 65 | # else : 66 | # cost_total = 0.5*(cp.quad_form(x, self.Q) + cp.quad_form(u,self.R)) 67 | cost_total = 0.5*(cp.quad_form(x, self.Q) + cp.quad_form(u-np.array([0,0,0,0]),self.R)) 68 | 69 | return cost_total -------------------------------------------------------------------------------- /cost/UnicycleCost.py: -------------------------------------------------------------------------------- 1 | from __future__ import division 2 | import matplotlib.pyplot as plt 3 | import numpy as np 4 | import time 5 | import random 6 | import cvxpy as cvx 7 | 8 | def print_np(x): 9 | print ("Type is %s" % (type(x))) 10 | print ("Shape is %s" % (x.shape,)) 11 | # print ("Values are: \n%s" % (x)) 12 | 13 | from cost import OptimalcontrolCost 14 | 15 | class unicycle(OptimalcontrolCost): 16 | def __init__(self,name,ix,iu,N): 17 | super().__init__(name,ix,iu,N) 18 | self.ix = 3 19 | self.iu = 2 20 | self.N = N 21 | 22 | self.S = 0*np.identity(ix) 23 | self.R = 1 * np.identity(iu) 24 | 25 | def bc_final(self,x_cvx,xf): 26 | h = [] 27 | h.append(x_cvx == xf) 28 | return h 29 | 30 | def estimate_cost_cvx(self,x,u,idx=None): 31 | # dimension 32 | cost_total = cvx.quad_form(x, self.S) + cvx.quad_form(u,self.R) 33 | 34 | return cost_total -------------------------------------------------------------------------------- /cost/__pycache__/Aircraft3dofCost.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/cost/__pycache__/Aircraft3dofCost.cpython-38.pyc -------------------------------------------------------------------------------- /cost/__pycache__/AircraftKinematicsCost.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/cost/__pycache__/AircraftKinematicsCost.cpython-38.pyc -------------------------------------------------------------------------------- /cost/__pycache__/FinaltimeFreeCost.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/cost/__pycache__/FinaltimeFreeCost.cpython-310.pyc -------------------------------------------------------------------------------- /cost/__pycache__/FinaltimeFreeCost.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/cost/__pycache__/FinaltimeFreeCost.cpython-38.pyc -------------------------------------------------------------------------------- /cost/__pycache__/Landing2DCost.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/cost/__pycache__/Landing2DCost.cpython-38.pyc -------------------------------------------------------------------------------- /cost/__pycache__/Landing3DCost.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/cost/__pycache__/Landing3DCost.cpython-38.pyc -------------------------------------------------------------------------------- /cost/__pycache__/LinearCost.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/cost/__pycache__/LinearCost.cpython-38.pyc -------------------------------------------------------------------------------- /cost/__pycache__/QuadRotorPointMassCost.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/cost/__pycache__/QuadRotorPointMassCost.cpython-38.pyc -------------------------------------------------------------------------------- /cost/__pycache__/QuadRotorSmallAngleCost.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/cost/__pycache__/QuadRotorSmallAngleCost.cpython-38.pyc -------------------------------------------------------------------------------- /cost/__pycache__/UnicycleCost.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/cost/__pycache__/UnicycleCost.cpython-310.pyc -------------------------------------------------------------------------------- /cost/__pycache__/UnicycleCost.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/cost/__pycache__/UnicycleCost.cpython-38.pyc -------------------------------------------------------------------------------- /cost/__pycache__/UnicycleCost.cpython-39.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/cost/__pycache__/UnicycleCost.cpython-39.pyc -------------------------------------------------------------------------------- /cost/__pycache__/cost.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/cost/__pycache__/cost.cpython-310.pyc -------------------------------------------------------------------------------- /cost/__pycache__/cost.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/cost/__pycache__/cost.cpython-38.pyc -------------------------------------------------------------------------------- /cost/__pycache__/cost.cpython-39.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/cost/__pycache__/cost.cpython-39.pyc -------------------------------------------------------------------------------- /cost/cost.py: -------------------------------------------------------------------------------- 1 | 2 | # coding: utf-8 3 | 4 | # In[1]: 5 | 6 | from __future__ import division 7 | import matplotlib.pyplot as plt 8 | import numpy as np 9 | import time 10 | import random 11 | import cvxpy as cp 12 | 13 | def print_np(x): 14 | print ("Type is %s" % (type(x))) 15 | print ("Shape is %s" % (x.shape,)) 16 | # print ("Values are: \n%s" % (x)) 17 | class OptimalcontrolCost(object) : 18 | def __init__(self,name,ix,iu,N) : 19 | self.name = name 20 | self.ix = ix 21 | self.iu = iu 22 | self.N = N 23 | 24 | def estimate_cost(self) : 25 | print("this is in parent class") 26 | pass 27 | 28 | def estimate_final_cost(self,x,u) : 29 | return self.estimate_cost(x,u) 30 | 31 | # def diff_cost(self,x,u): 32 | 33 | # # state & input size 34 | # ix = self.ix 35 | # iu = self.iu 36 | 37 | # ndim = np.ndim(x) 38 | # if ndim == 1: # 1 step state & input 39 | # N = 1 40 | 41 | # else : 42 | # N = np.size(x,axis = 0) 43 | 44 | # # numerical difference 45 | # h = pow(2,-17) 46 | # eps_x = np.identity(ix) 47 | # eps_u = np.identity(iu) 48 | 49 | # # expand to tensor 50 | # # print_np(x) 51 | # x_mat = np.expand_dims(x,axis=2) 52 | # u_mat = np.expand_dims(u,axis=2) 53 | 54 | # # diag 55 | # x_diag = np.tile(x_mat,(1,1,ix)) 56 | # u_diag = np.tile(u_mat,(1,1,iu)) 57 | 58 | # # augmented = [x_diag x], [u, u_diag] 59 | # x_aug = x_diag + eps_x * h 60 | # x_aug = np.dstack((x_aug,np.tile(x_mat,(1,1,iu)))) 61 | # x_aug = np.reshape( np.transpose(x_aug,(0,2,1)), (N*(iu+ix),ix)) 62 | 63 | # u_aug = u_diag + eps_u * h 64 | # u_aug = np.dstack((np.tile(u_mat,(1,1,ix)),u_aug)) 65 | # u_aug = np.reshape( np.transpose(u_aug,(0,2,1)), (N*(iu+ix),iu)) 66 | 67 | # # numerical difference 68 | # c_nominal = self.estimate_cost(x,u) 69 | # c_change = self.estimate_cost(x_aug,u_aug) 70 | # c_change = np.reshape(c_change,(N,1,iu+ix)) 71 | 72 | # c_diff = ( c_change - np.reshape(c_nominal,(N,1,1)) ) / h 73 | # c_diff = np.reshape(c_diff,(N,iu+ix)) 74 | 75 | # return np.squeeze(c_diff) 76 | 77 | # def hess_cost(self,x,u): 78 | 79 | # # state & input size 80 | # ix = self.ix 81 | # iu = self.iu 82 | 83 | # ndim = np.ndim(x) 84 | # if ndim == 1: # 1 step state & input 85 | # N = 1 86 | 87 | # else : 88 | # N = np.size(x,axis = 0) 89 | 90 | # # numerical difference 91 | # h = pow(2,-17) 92 | # eps_x = np.identity(ix) 93 | # eps_u = np.identity(iu) 94 | 95 | # # expand to tensor 96 | # x_mat = np.expand_dims(x,axis=2) 97 | # u_mat = np.expand_dims(u,axis=2) 98 | 99 | # # diag 100 | # x_diag = np.tile(x_mat,(1,1,ix)) 101 | # u_diag = np.tile(u_mat,(1,1,iu)) 102 | 103 | # # augmented = [x_diag x], [u, u_diag] 104 | # x_aug = x_diag + eps_x * h 105 | # x_aug = np.dstack((x_aug,np.tile(x_mat,(1,1,iu)))) 106 | # x_aug = np.reshape( np.transpose(x_aug,(0,2,1)), (N*(iu+ix),ix)) 107 | 108 | # u_aug = u_diag + eps_u * h 109 | # u_aug = np.dstack((np.tile(u_mat,(1,1,ix)),u_aug)) 110 | # u_aug = np.reshape( np.transpose(u_aug,(0,2,1)), (N*(iu+ix),iu)) 111 | 112 | 113 | # # numerical difference 114 | # c_nominal = self.diff_cost(x,u) 115 | # c_change = self.diff_cost(x_aug,u_aug) 116 | # c_change = np.reshape(c_change,(N,iu+ix,iu+ix)) 117 | # c_hess = ( c_change - np.reshape(c_nominal,(N,1,ix+iu)) ) / h 118 | # c_hess = np.reshape(c_hess,(N,iu+ix,iu+ix)) 119 | 120 | # return np.squeeze(c_hess) 121 | 122 | 123 | 124 | 125 | 126 | -------------------------------------------------------------------------------- /forward.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | from scipy.integrate import odeint 3 | from scipy.integrate import solve_ivp 4 | import numpy as np 5 | import time 6 | import random 7 | def print_np(x): 8 | print ("Type is %s" % (type(x))) 9 | print ("Shape is %s" % (x.shape,)) 10 | # print ("Values are: \n%s" % (x)) 11 | import IPython 12 | 13 | def forward_full(model,x0,u,N,type_discretization="zoh") : 14 | ix = model.ix 15 | iu = model.iu 16 | 17 | def dfdt(t,x,um,up) : 18 | if type_discretization == "zoh" : 19 | u = um 20 | elif type_discretization == "foh" : 21 | alpha = (model.delT - t) / model.delT 22 | beta = t / model.delT 23 | u = alpha * um + beta * up 24 | return np.squeeze(model.forward(x,u,discrete=False)) 25 | 26 | xnew = np.zeros((N+1,ix)) 27 | xnew[0] = x0 28 | cnew = np.zeros(N+1) 29 | 30 | for i in range(N) : 31 | sol = solve_ivp(dfdt,(0,model.delT),xnew[i],args=(u[i],u[i+1]),method='RK45',rtol=1e-6,atol=1e-10) 32 | xnew[i+1] = sol.y[:,-1] 33 | # cnew[i] = self.cost.estimate_cost(xnew[i],u[i]) 34 | # cnew[N] = self.cost.estimate_cost(xnew[N],np.zeros(self.model.iu)) 35 | 36 | return xnew,u 37 | 38 | 39 | def forward_one_step(model,x,u) : 40 | ix = model.ix 41 | iu = model.iu 42 | 43 | N = np.size(x,axis = 0) 44 | 45 | def dfdt(t,x,u) : 46 | x_ = np.reshape(x,(N,ix)) 47 | u_ = np.reshape(u,(N,iu)) 48 | x_dot = np.squeeze(model.forward(x_,u_,discrete=False)) 49 | x_dot = np.reshape(x_dot,(ix*N)) 50 | return x_dot 51 | 52 | x = np.reshape(x,(ix*N)) 53 | u = np.reshape(u,(iu*N)) 54 | sol = solve_ivp(dfdt,(0,model.delT),x,args=(u,),method='RK45',rtol=1e-6,atol=1e-10) 55 | x_next = sol.y[:,-1] 56 | x_next = np.reshape(x_next,(N,ix)) 57 | return x_next -------------------------------------------------------------------------------- /images/Landing2D.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/images/Landing2D.gif -------------------------------------------------------------------------------- /images/Landing3D.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/images/Landing3D.gif -------------------------------------------------------------------------------- /images/airplane_landing_0222_01.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/images/airplane_landing_0222_01.gif -------------------------------------------------------------------------------- /images/airplane_landing_0222_02.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/images/airplane_landing_0222_02.gif -------------------------------------------------------------------------------- /images/airplane_landing_0222_03.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/images/airplane_landing_0222_03.gif -------------------------------------------------------------------------------- /images/quadrotor.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/images/quadrotor.gif -------------------------------------------------------------------------------- /images/unicycle.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/images/unicycle.gif -------------------------------------------------------------------------------- /model/Aircraft3dofApproxModel.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | import time 4 | import random 5 | def print_np(x): 6 | print ("Type is %s" % (type(x))) 7 | print ("Shape is %s" % (x.shape,)) 8 | print ("Values are: \n%s" % (x)) 9 | 10 | from model import OptimalcontrolModel 11 | 12 | 13 | class Aircraft3dofApproxModel(OptimalcontrolModel): 14 | def __init__(self,name,ix,iu,linearization="numeric_central"): 15 | super().__init__(name,ix,iu,linearization) 16 | self.m = 49940 17 | self.g = 9.8 18 | self.Sw = 112 19 | self.CD0 = 0.0197 20 | self.K = 0.0459 21 | self.T_max = 137.81 * 1e3 22 | self.CLalpha = 4.2 23 | self.CL0 = 0.4225 24 | 25 | def forward(self,x,u,idx=None): 26 | xdim = np.ndim(x) 27 | if xdim == 1: # 1 step state & input 28 | N = 1 29 | x = np.expand_dims(x,axis=0) 30 | else : 31 | N = np.size(x,axis = 0) 32 | udim = np.ndim(u) 33 | if udim == 1 : 34 | u = np.expand_dims(u,axis=0) 35 | 36 | # state & input 37 | rx = x[:,0] 38 | ry = x[:,1] 39 | rz = x[:,2] 40 | v = x[:,3] # speed 41 | gamma = x[:,4] # path angle 42 | psi = x[:,5] # velocity heading 43 | 44 | alpha = u[:,0] # angle of attack 45 | phi = u[:,1] # bank angle 46 | thrust = self.T_max * u[:,2] # thrust 47 | 48 | rho = 1.225 49 | 50 | # Lift & drag force 51 | CL = self.CL0 + self.CLalpha * alpha 52 | L = 0.5 * rho * v * v * self.Sw * CL 53 | D = 0.5 * rho * v * v * self.Sw * (self.CD0 + self.K * CL * CL) 54 | 55 | 56 | # output 57 | f = np.zeros_like(x) 58 | f[:,0] = v * np.cos(gamma) * np.cos(psi) 59 | f[:,1] = v * np.cos(gamma) * np.sin(psi) 60 | f[:,2] = v * np.sin(gamma) 61 | f[:,3] = 1 / self.m * (thrust * np.cos(alpha) - D - self.m * self.g * np.sin(gamma)) 62 | f[:,4] = 1 /(self.m * v) * (thrust * np.sin(alpha) + L * np.cos(phi) - self.m * self.g * np.cos(gamma) ) 63 | f[:,5] = - L * np.sin(phi) / (self.m * v * np.cos(gamma)) 64 | # # output 65 | # f = np.zeros_like(x) 66 | # f[:,0] = v * np.cos(psi) 67 | # f[:,1] = v * np.sin(psi) 68 | # f[:,2] = v * gamma 69 | # f[:,3] = 1 / self.m * (thrust - D - self.m * self.g * gamma) 70 | # f[:,4] = 1 /(self.m * v_) * (L - self.m * self.g ) 71 | # f[:,5] = - L * phi / (self.m * v_) 72 | 73 | return f 74 | 75 | 76 | -------------------------------------------------------------------------------- /model/Aircraft3dofModelNondimension.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | import time 4 | import random 5 | def print_np(x): 6 | print ("Type is %s" % (type(x))) 7 | print ("Shape is %s" % (x.shape,)) 8 | print ("Values are: \n%s" % (x)) 9 | 10 | from model import OptimalcontrolModel 11 | 12 | 13 | class Aircraft3dofNondimension(OptimalcontrolModel): 14 | def __init__(self,name,ix,iu,linearization="numeric_central",beta=600): 15 | super().__init__(name,ix,iu,linearization) 16 | self.m = 288938 17 | self.g = 9.81 18 | self.Sw = 510.97 19 | self.CD0 = 0.022 20 | self.K = 0.045 21 | self.rho = 1.225 22 | 23 | # normalized parameter 24 | self.beta = beta 25 | self.rho_bar = (self.rho * (self.g**2)) / (2*self.m*self.g*(self.beta**2)/self.Sw) 26 | 27 | def forward(self,x,u,idx=None,discrete=True): 28 | xdim = np.ndim(x) 29 | if xdim == 1: # 1 step state & input 30 | N = 1 31 | x = np.expand_dims(x,axis=0) 32 | else : 33 | N = np.size(x,axis = 0) 34 | udim = np.ndim(u) 35 | if udim == 1 : 36 | u = np.expand_dims(u,axis=0) 37 | 38 | # state & input 39 | rx = x[:,0] 40 | ry = x[:,1] 41 | rz = x[:,2] 42 | v = x[:,3] # speed 43 | gamma = x[:,4] # path angle 44 | psi = x[:,5] # velocity heading 45 | 46 | CL = u[:,0] # lift coefficient 47 | phi = u[:,1] # bank angle 48 | thrust = u[:,2] # thrust 49 | 50 | # output 51 | f = np.zeros(x.shape) 52 | f[:,0] = v * np.cos(gamma) * np.cos(psi) 53 | f[:,1] = v * np.cos(gamma) * np.sin(psi) 54 | f[:,2] = v * np.sin(gamma) 55 | f[:,3] = thrust - self.rho_bar * v * v * (self.CD0 + self.K * CL * CL) - np.sin(gamma) 56 | f[:,4] = self.rho_bar * v * CL * np.cos(phi) - np.cos(gamma) / v 57 | f[:,5] = - self.rho_bar * v * CL * np.sin(phi) / np.cos(gamma) 58 | 59 | return f 60 | 61 | # def diff(self,x,u,discrete=True) : 62 | # assert discrete == False 63 | # # state & input size 64 | # ix = self.ix 65 | # iu = self.iu 66 | 67 | # ndim = np.ndim(x) 68 | # if ndim == 1: # 1 step state & input 69 | # N = 1 70 | # x = np.expand_dims(x,axis=0) 71 | # u = np.expand_dims(u,axis=0) 72 | # else : 73 | # N = np.size(x,axis = 0) 74 | # # state & input 75 | # rx = x[:,0] 76 | # ry = x[:,1] 77 | # rz = x[:,2] 78 | # v = x[:,3] # speed 79 | # gamma = x[:,4] # path angle 80 | # psi = x[:,5] # velocity heading 81 | 82 | # CL = u[:,0] # lift coefficient 83 | # phi = u[:,1] # bank angle 84 | # thrust = u[:,2] # thrust 85 | 86 | # m,g,Sw,CD0,K = self.m,self.g,self.Sw,self.CD0,self.K 87 | 88 | # fx = np.zeros((N,ix,ix)) 89 | # fx[:,0,3] = np.cos(gamma)*np.cos(psi) 90 | # fx[:,0,4] = -v*np.sin(gamma)*np.cos(psi) 91 | # fx[:,0,5] = -v*np.sin(psi)*np.cos(gamma) 92 | # fx[:,1,3] = np.sin(psi)*np.cos(gamma) 93 | # fx[:,1,4] = -v*np.sin(gamma)*np.sin(psi) 94 | # fx[:,1,5] = v*np.cos(gamma)*np.cos(psi) 95 | # fx[:,2,3] = np.sin(gamma) 96 | # fx[:,2,4] = v*np.cos(gamma) 97 | # fx[:,3,2] = (0.00600217215675481*Sw*v**2*(1 - 2.25237731658222e-5*rz)**4.256*(CD0 + CL**2*K)/(82.667366 - 0.001861981*rz) - 1.38139853548573e-5*Sw*v**2*(1 - 2.25237731658222e-5*rz)**5.256*(CD0 + CL**2*K)/(1 - 2.25237731658222e-5*rz)**2)/m 98 | # fx[:,3,3] = -101.400930904549*Sw*v*(1 - 2.25237731658222e-5*rz)**5.256*(CD0 + CL**2*K)/(m*(82.667366 - 0.001861981*rz)) 99 | # fx[:,3,4] = -g*np.cos(gamma) 100 | # fx[:,4,2] = (-0.00600217215675481*CL*Sw*v**2*(1 - 2.25237731658222e-5*rz)**4.256*np.cos(phi)/(82.667366 - 0.001861981*rz) + 1.38139853548573e-5*CL*Sw*v**2*(1 - 2.25237731658222e-5*rz)**5.256*np.cos(phi)/(1 - 2.25237731658222e-5*rz)**2)/(m*v) 101 | # fx[:,4,3] = 101.400930904549*CL*Sw*(1 - 2.25237731658222e-5*rz)**5.256*np.cos(phi)/(m*(82.667366 - 0.001861981*rz)) - (50.7004654522744*CL*Sw*v**2*(1 - 2.25237731658222e-5*rz)**5.256*np.cos(phi)/(82.667366 - 0.001861981*rz) - g*m*np.cos(gamma))/(m*v**2) 102 | # fx[:,4,4] = g*np.sin(gamma)/v 103 | # fx[:,5,2] = 0.00600217215675481*CL*Sw*v*(1 - 2.25237731658222e-5*rz)**4.256*np.sin(phi)/(m*(82.667366 - 0.001861981*rz)*np.cos(gamma)) - 1.38139853548573e-5*CL*Sw*v*(1 - 2.25237731658222e-5*rz)**5.256*np.sin(phi)/(m*(1 - 2.25237731658222e-5*rz)**2*np.cos(gamma)) 104 | # fx[:,5,3] = -50.7004654522744*CL*Sw*(1 - 2.25237731658222e-5*rz)**5.256*np.sin(phi)/(m*(82.667366 - 0.001861981*rz)*np.cos(gamma)) 105 | # fx[:,5,4] = -50.7004654522744*CL*Sw*v*(1 - 2.25237731658222e-5*rz)**5.256*np.sin(gamma)*np.sin(phi)/(m*(82.667366 - 0.001861981*rz)*np.cos(gamma)**2) 106 | 107 | 108 | # fu = np.zeros((N,ix,iu)) 109 | 110 | # fu[:,3,0] = -101.400930904549*CL*K*Sw*v**2*(1 - 2.25237731658222e-5*rz)**5.256/(m*(82.667366 - 0.001861981*rz)) 111 | # fu[:,3,2] = 1/m 112 | # fu[:,4,0] = 50.7004654522744*Sw*v*(1 - 2.25237731658222e-5*rz)**5.256*np.cos(phi)/(m*(82.667366 - 0.001861981*rz)) 113 | # fu[:,4,1] = -50.7004654522744*CL*Sw*v*(1 - 2.25237731658222e-5*rz)**5.256*np.sin(phi)/(m*(82.667366 - 0.001861981*rz)) 114 | # fu[:,5,0] = -50.7004654522744*Sw*v*(1 - 2.25237731658222e-5*rz)**5.256*np.sin(phi)/(m*(82.667366 - 0.001861981*rz)*np.cos(gamma)) 115 | # fu[:,5,1] = -50.7004654522744*CL*Sw*v*(1 - 2.25237731658222e-5*rz)**5.256*np.cos(phi)/(m*(82.667366 - 0.001861981*rz)*np.cos(gamma)) 116 | 117 | # return fx.squeeze(),fu.squeeze() -------------------------------------------------------------------------------- /model/Aircraft3dofModel_tmp.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | import time 4 | import random 5 | def print_np(x): 6 | print ("Type is %s" % (type(x))) 7 | print ("Shape is %s" % (x.shape,)) 8 | print ("Values are: \n%s" % (x)) 9 | 10 | from model import OptimalcontrolModel 11 | 12 | 13 | class Aircraft3dof_tmp(OptimalcontrolModel): 14 | def __init__(self,name,ix,iu,linearization="numeric_central"): 15 | super().__init__(name,ix,iu,linearization) 16 | self.m = 288938 17 | self.g = 9.81 18 | self.Sw = 510.97 19 | self.CD0 = 0.022 20 | self.K = 0.045 21 | self.rho = 1.225 22 | 23 | self.scl_x = 1 24 | self.scl_kg = 1 25 | self.scl_rho = 1 26 | self.scl_Sw = 1 27 | self.scl_g = 1 28 | 29 | def set_scale(self,scl_x,scl_kg,scl_rho,scl_Sw,scl_g) : 30 | self.scl_x = scl_x 31 | self.scl_kg = scl_kg 32 | self.scl_rho = scl_rho 33 | self.scl_Sw = scl_Sw 34 | self.scl_g = scl_g 35 | 36 | def forward(self,x,u,idx=None,discrete=True): 37 | xdim = np.ndim(x) 38 | if xdim == 1: # 1 step state & input 39 | N = 1 40 | x = np.expand_dims(x,axis=0) 41 | else : 42 | N = np.size(x,axis = 0) 43 | udim = np.ndim(u) 44 | if udim == 1 : 45 | u = np.expand_dims(u,axis=0) 46 | 47 | # state & input 48 | rx = x[:,0] 49 | ry = x[:,1] 50 | rz = x[:,2] 51 | v = x[:,3] # speed 52 | gamma = x[:,4] # path angle 53 | psi = x[:,5] # velocity heading 54 | 55 | CL = u[:,0] # lift coefficient 56 | phi = u[:,1] # bank angle 57 | thrust = u[:,2] # thrust 58 | 59 | # density 60 | def get_density(rz) : 61 | # flag_1 = rz < 11000 62 | T1 = 15.04 - 0.00649 * rz * self.scl_x # celsius 63 | p1 = 101.29 * np.power((T1+273.1)/288.08,5.256) 64 | rho1 = p1 / (0.2869 * (T1 + 273.1)) 65 | 66 | # flag_2 = np.logical_and(rz >= 11000, rz<25000) 67 | # T2 = -56.46 # not used 68 | # p2 = 22.65 * np.exp(1.73-0.000157 * rz) 69 | # rho2 = p2 / (0.2869 * (T2 + 273.1)) 70 | 71 | # flag_3 = rz >= 25000 72 | # T3 = -131.21 + 0.00299 * rz 73 | # p3 = 2.488 * np.power((T1+273.1)/216.6,-11.388) 74 | # rho3 = p3 / (0.2869 * (T3 + 273.1)) 75 | # return rho1*flag_1 + rho2*flag_2 + rho3*flag_3 76 | return rho1 / self.scl_rho 77 | # rho = get_density(rz) 78 | rho = self.rho / self.scl_rho 79 | 80 | m = self.m / self.scl_kg 81 | g = self.g / self.scl_g 82 | Sw = self.Sw / self.scl_Sw 83 | 84 | # Lift & drag force 85 | L = 0.5 * rho * v * v * Sw * CL 86 | D = 0.5 * rho * v * v * Sw * (self.CD0 + self.K * CL * CL) 87 | 88 | # output 89 | # f = np.zeros_like(x) 90 | f = np.zeros(x.shape) 91 | f[:,0] = v * np.cos(gamma) * np.cos(psi) 92 | f[:,1] = v * np.cos(gamma) * np.sin(psi) 93 | f[:,2] = v * np.sin(gamma) 94 | f[:,3] = 1 / m * (thrust - D - m * g * np.sin(gamma)) 95 | f[:,4] = 1 /(m * v) * (L * np.cos(phi) - m * g * np.cos(gamma)) 96 | f[:,5] = - L * np.sin(phi) / (m * v * np.cos(gamma)) 97 | 98 | # f[:,0] = v*np.cos(gamma)*np.cos(psi) 99 | # f[:,1] = v*np.sin(psi)*np.cos(gamma) 100 | # f[:,2] = v*np.sin(gamma) 101 | # f[:,3] = (-50.7004654522744*self.Sw*v**2*(1 - 2.25237731658222e-5*rz)**5.256*(self.CD0 + CL**2*self.K)/(82.667366 - 0.001861981*rz) - self.g*self.m*np.sin(gamma) + thrust)/self.m 102 | # f[:,4] = (50.7004654522744*CL*self.Sw*v**2*(1 - 2.25237731658222e-5*rz)**5.256*np.cos(phi)/(82.667366 - 0.001861981*rz) - self.g*self.m*np.cos(gamma))/(self.m*v) 103 | # f[:,5] = -50.7004654522744*CL*self.Sw*v*(1 - 2.25237731658222e-5*rz)**5.256*np.sin(phi)/(self.m*(82.667366 - 0.001861981*rz)*np.cos(gamma)) 104 | 105 | if discrete is True : 106 | return np.squeeze(x + f * self.delT) 107 | else : 108 | return f 109 | 110 | # def diff(self,x,u,discrete=True) : 111 | # assert discrete == False 112 | # # state & input size 113 | # ix = self.ix 114 | # iu = self.iu 115 | 116 | # ndim = np.ndim(x) 117 | # if ndim == 1: # 1 step state & input 118 | # N = 1 119 | # x = np.expand_dims(x,axis=0) 120 | # u = np.expand_dims(u,axis=0) 121 | # else : 122 | # N = np.size(x,axis = 0) 123 | # # state & input 124 | # rx = x[:,0] 125 | # ry = x[:,1] 126 | # rz = x[:,2] 127 | # v = x[:,3] # speed 128 | # gamma = x[:,4] # path angle 129 | # psi = x[:,5] # velocity heading 130 | 131 | # CL = u[:,0] # lift coefficient 132 | # phi = u[:,1] # bank angle 133 | # thrust = u[:,2] # thrust 134 | 135 | # m,g,Sw,CD0,K = self.m,self.g,self.Sw,self.CD0,self.K 136 | 137 | # fx = np.zeros((N,ix,ix)) 138 | # fx[:,0,3] = np.cos(gamma)*np.cos(psi) 139 | # fx[:,0,4] = -v*np.sin(gamma)*np.cos(psi) 140 | # fx[:,0,5] = -v*np.sin(psi)*np.cos(gamma) 141 | # fx[:,1,3] = np.sin(psi)*np.cos(gamma) 142 | # fx[:,1,4] = -v*np.sin(gamma)*np.sin(psi) 143 | # fx[:,1,5] = v*np.cos(gamma)*np.cos(psi) 144 | # fx[:,2,3] = np.sin(gamma) 145 | # fx[:,2,4] = v*np.cos(gamma) 146 | # fx[:,3,2] = (0.00600217215675481*Sw*v**2*(1 - 2.25237731658222e-5*rz)**4.256*(CD0 + CL**2*K)/(82.667366 - 0.001861981*rz) - 1.38139853548573e-5*Sw*v**2*(1 - 2.25237731658222e-5*rz)**5.256*(CD0 + CL**2*K)/(1 - 2.25237731658222e-5*rz)**2)/m 147 | # fx[:,3,3] = -101.400930904549*Sw*v*(1 - 2.25237731658222e-5*rz)**5.256*(CD0 + CL**2*K)/(m*(82.667366 - 0.001861981*rz)) 148 | # fx[:,3,4] = -g*np.cos(gamma) 149 | # fx[:,4,2] = (-0.00600217215675481*CL*Sw*v**2*(1 - 2.25237731658222e-5*rz)**4.256*np.cos(phi)/(82.667366 - 0.001861981*rz) + 1.38139853548573e-5*CL*Sw*v**2*(1 - 2.25237731658222e-5*rz)**5.256*np.cos(phi)/(1 - 2.25237731658222e-5*rz)**2)/(m*v) 150 | # fx[:,4,3] = 101.400930904549*CL*Sw*(1 - 2.25237731658222e-5*rz)**5.256*np.cos(phi)/(m*(82.667366 - 0.001861981*rz)) - (50.7004654522744*CL*Sw*v**2*(1 - 2.25237731658222e-5*rz)**5.256*np.cos(phi)/(82.667366 - 0.001861981*rz) - g*m*np.cos(gamma))/(m*v**2) 151 | # fx[:,4,4] = g*np.sin(gamma)/v 152 | # fx[:,5,2] = 0.00600217215675481*CL*Sw*v*(1 - 2.25237731658222e-5*rz)**4.256*np.sin(phi)/(m*(82.667366 - 0.001861981*rz)*np.cos(gamma)) - 1.38139853548573e-5*CL*Sw*v*(1 - 2.25237731658222e-5*rz)**5.256*np.sin(phi)/(m*(1 - 2.25237731658222e-5*rz)**2*np.cos(gamma)) 153 | # fx[:,5,3] = -50.7004654522744*CL*Sw*(1 - 2.25237731658222e-5*rz)**5.256*np.sin(phi)/(m*(82.667366 - 0.001861981*rz)*np.cos(gamma)) 154 | # fx[:,5,4] = -50.7004654522744*CL*Sw*v*(1 - 2.25237731658222e-5*rz)**5.256*np.sin(gamma)*np.sin(phi)/(m*(82.667366 - 0.001861981*rz)*np.cos(gamma)**2) 155 | 156 | 157 | # fu = np.zeros((N,ix,iu)) 158 | 159 | # fu[:,3,0] = -101.400930904549*CL*K*Sw*v**2*(1 - 2.25237731658222e-5*rz)**5.256/(m*(82.667366 - 0.001861981*rz)) 160 | # fu[:,3,2] = 1/m 161 | # fu[:,4,0] = 50.7004654522744*Sw*v*(1 - 2.25237731658222e-5*rz)**5.256*np.cos(phi)/(m*(82.667366 - 0.001861981*rz)) 162 | # fu[:,4,1] = -50.7004654522744*CL*Sw*v*(1 - 2.25237731658222e-5*rz)**5.256*np.sin(phi)/(m*(82.667366 - 0.001861981*rz)) 163 | # fu[:,5,0] = -50.7004654522744*Sw*v*(1 - 2.25237731658222e-5*rz)**5.256*np.sin(phi)/(m*(82.667366 - 0.001861981*rz)*np.cos(gamma)) 164 | # fu[:,5,1] = -50.7004654522744*CL*Sw*v*(1 - 2.25237731658222e-5*rz)**5.256*np.cos(phi)/(m*(82.667366 - 0.001861981*rz)*np.cos(gamma)) 165 | 166 | # return fx.squeeze(),fu.squeeze() -------------------------------------------------------------------------------- /model/AircraftKinematicsModel.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | import time 4 | import random 5 | def print_np(x): 6 | print ("Type is %s" % (type(x))) 7 | print ("Shape is %s" % (x.shape,)) 8 | print ("Values are: \n%s" % (x)) 9 | 10 | from model import OptimalcontrolModel 11 | 12 | 13 | class AircraftKinematics(OptimalcontrolModel): 14 | def __init__(self,name,ix,iu,delT,linearization="numeric_central"): 15 | super().__init__(name,ix,iu,linearization) 16 | self.m = 288938 17 | # self.m = 1 18 | 19 | def forward(self,x,u,idx=None): 20 | xdim = np.ndim(x) 21 | if xdim == 1: # 1 step state & input 22 | N = 1 23 | x = np.expand_dims(x,axis=0) 24 | else : 25 | N = np.size(x,axis = 0) 26 | udim = np.ndim(u) 27 | if udim == 1 : 28 | u = np.expand_dims(u,axis=0) 29 | 30 | # state & input 31 | rx = x[:,0] 32 | ry = x[:,1] 33 | rz = x[:,2] 34 | v = x[:,3] # speed 35 | gamma = x[:,4] # path angle 36 | psi = x[:,5] # velocity heading 37 | 38 | # gamma = u[:,0] 39 | gamma_dot = u[:,0] 40 | psi_dot = u[:,1] 41 | thrust = u[:,2] # thrust 42 | 43 | # output 44 | f = np.zeros_like(x) 45 | f[:,0] = v * np.cos(gamma) * np.cos(psi) 46 | f[:,1] = v * np.cos(gamma) * np.sin(psi) 47 | f[:,2] = v * np.sin(gamma) 48 | f[:,3] = 1 / self.m * thrust 49 | f[:,4] = gamma_dot 50 | f[:,5] = psi_dot 51 | 52 | return f 53 | # def diff(self,x,u): 54 | 55 | # # dimension 56 | # ndim = np.ndim(x) 57 | # if ndim == 1: # 1 step state & input 58 | # N = 1 59 | # x = np.expand_dims(x,axis=0) 60 | # u = np.expand_dims(u,axis=0) 61 | # else : 62 | # N = np.size(x,axis = 0) 63 | 64 | # # state & input 65 | # x1 = x[:,0] 66 | # x2 = x[:,1] 67 | # x3 = x[:,2] 68 | 69 | # v = u[:,0] 70 | # w = u[:,1] 71 | 72 | # fx = np.zeros((N,self.ix,self.ix)) 73 | # fx[:,0,0] = 1.0 74 | # fx[:,0,1] = 0.0 75 | # fx[:,0,2] = - self.delT * v * np.sin(x3) 76 | # fx[:,1,0] = 0.0 77 | # fx[:,1,1] = 1.0 78 | # fx[:,1,2] = self.delT * v * np.cos(x3) 79 | # fx[:,2,0] = 0.0 80 | # fx[:,2,1] = 0.0 81 | # fx[:,2,2] = 1.0 82 | 83 | # fu = np.zeros((N,self.ix,self.iu)) 84 | # fu[:,0,0] = self.delT * np.cos(x3) 85 | # fu[:,0,1] = 0.0 86 | # fu[:,1,0] = self.delT * np.sin(x3) 87 | # fu[:,1,1] = 0.0 88 | # fu[:,2,0] = 0.0 89 | # fu[:,2,1] = self.delT 90 | 91 | # return np.squeeze(fx) , np.squeeze(fu) -------------------------------------------------------------------------------- /model/HypersonicEntry3DofPolar.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | import time 4 | import random 5 | def print_np(x): 6 | print ("Type is %s" % (type(x))) 7 | print ("Shape is %s" % (x.shape,)) 8 | # print ("Values are: \n%s" % (x)) 9 | 10 | from model import OptimalcontrolModel 11 | 12 | 13 | 14 | class Entry3dofSpherical(OptimalcontrolModel): 15 | def __init__(self,name,ix,iu,linearization="numeric_central"): 16 | super().__init__(name,ix,iu,linearization) 17 | self.ge = 9.81 18 | self.re = 6371*1e3 19 | self.rhoe = 1.3 20 | self.H = 7e3 21 | self.beta = 1/self.H 22 | 23 | self.nt = np.sqrt(self.re/self.ge) 24 | self.nd = self.re 25 | self.nv = np.sqrt(self.ge*self.re) 26 | 27 | self.Kq = 1.2035 * 1e-5 28 | self.mass = 104305 29 | self.Sref = 391.22 30 | self.alpha_param = np.deg2rad(40) 31 | self.alpha_deg = np.rad2deg(self.alpha_param) 32 | self.B = self.re * self.Sref / (2*self.mass) 33 | 34 | def forward(self,x,u,idx=None): 35 | xdim = np.ndim(x) 36 | if xdim == 1: # 1 step state & input 37 | N = 1 38 | x = np.expand_dims(x,axis=0) 39 | else : 40 | N = np.size(x,axis = 0) 41 | udim = np.ndim(u) 42 | if udim == 0 : 43 | u = np.array([u]) 44 | udim = np.ndim(u) 45 | if udim == 1 : 46 | u = np.expand_dims(u,axis=0) 47 | 48 | # state & input 49 | r = x[:,0] 50 | theta = x[:,1] 51 | phi = x[:,2] 52 | v = x[:,3] # speed 53 | gamma = x[:,4] # path angle 54 | psi = x[:,5] # velocity heading 55 | 56 | sigma = u[:,0] # lift coefficient 57 | 58 | nv,rhoe,beta,re,B = self.nv,self.rhoe,self.beta,self.re,self.B 59 | 60 | # extrfxt sines and cosines of various values 61 | cp = np.cos(phi) 62 | sp = np.sin(phi) 63 | cg = np.cos(gamma) 64 | sg = np.sin(gamma) 65 | cps = np.cos(psi) 66 | sps = np.sin(psi) 67 | cs = np.cos(sigma) 68 | ss = np.sin(sigma) 69 | 70 | # Determine lift and drag coefficients from velocity 71 | flag_tmp = nv*v > 4750 72 | alpha = 40-0.20705*((v*nv-4570)**2)/(340**2) 73 | alpha[flag_tmp] = 40 74 | 75 | Cl = -0.041065+0.016292*alpha+0.0002602*alpha**2 76 | Cd = 0.080505-0.03026*Cl+0.86495*Cl**2 77 | 78 | # determine atm dens, lift, drag quantities 79 | rho = rhoe*np.exp(-beta*re*(r - 1 )) 80 | L = B*rho*Cl*v**2 81 | D = B*rho*Cd*v**2 82 | 83 | # output 84 | f = np.zeros_like(x) 85 | 86 | f[:,0] = v*sg 87 | f[:,1] = v*cg*sps/(r*cp) 88 | f[:,2] = v*cg*cps/r 89 | f[:,3] = -D-sg/(r**2) 90 | f[:,4] = (1/v)*(L*cs+(v**2-1/r)*cg/r) 91 | f[:,5] = (1/v)*(L*ss/cg+(v**2)*cg*sps*np.tan(phi)/r) 92 | 93 | return f 94 | 95 | def diff(self,x,u) : 96 | # state & input size 97 | ix = self.ix 98 | iu = self.iu 99 | 100 | ndim = np.ndim(x) 101 | if ndim == 1: # 1 step state & input 102 | N = 1 103 | x = np.expand_dims(x,axis=0) 104 | else : 105 | N = np.size(x,axis = 0) 106 | udim = np.ndim(u) 107 | if udim == 0 : 108 | u = np.array([u]) 109 | udim = np.ndim(u) 110 | if udim == 1 : 111 | u = np.expand_dims(u,axis=0) 112 | 113 | # state & input 114 | r = x[:,0] 115 | theta = x[:,1] 116 | phi = x[:,2] 117 | v = x[:,3] # speed 118 | gamma = x[:,4] # path angle 119 | psi = x[:,5] # velocity heading 120 | 121 | sigma = u[:,0] # lift coefficient 122 | 123 | nv,rhoe,beta,re,B = self.nv,self.rhoe,self.beta,self.re,self.B 124 | 125 | # extrfxt sines and cosines of various values 126 | cp = np.cos(phi) 127 | sp = np.sin(phi) 128 | cg = np.cos(gamma) 129 | sg = np.sin(gamma) 130 | cps = np.cos(psi) 131 | sps = np.sin(psi) 132 | cs = np.cos(sigma) 133 | ss = np.sin(sigma) 134 | 135 | # Determine lift and drag coefficients from velocity 136 | flag_tmp = nv*v > 4750 137 | alpha = 40-0.20705*((v*nv-4570)**2)/(340**2) 138 | alpha[flag_tmp] = 40 139 | 140 | Cl = -0.041065+0.016292*alpha+0.0002602*alpha**2 141 | Cd = 0.080505-0.03026*Cl+0.86495*Cl**2 142 | 143 | # determine atm dens, lift, drag quantities 144 | rho = rhoe*np.exp(-beta*re*(r - 1 )) 145 | L = B*rho*Cl*v**2 146 | D = B*rho*Cd*v**2 147 | # determine derivatives of above quanitites 148 | drho = -beta*re*rho 149 | Lr = B*Cl*(v**2)*drho 150 | Lv = 2*B*Cl*rho*v 151 | Dr = B*Cd*(v**2)*drho 152 | Dv = 2*B*Cd*rho*v 153 | 154 | 155 | fx = np.zeros((N,ix,ix)) 156 | fx[:,0,3] = sg 157 | fx[:,0,4] = v*cg 158 | fx[:,1,0] = -v*cg*sps/((r**2)*cp) 159 | fx[:,1,2] = v*cg*sps*sp/(r*(cp**2)) 160 | fx[:,1,3] = cg*sps/(r*cp) 161 | fx[:,1,4] = -v*sg*sps/(r*cp) 162 | fx[:,1,5] = v*cg*cps/(r*cp) 163 | fx[:,2,0] = -v*cg*cps/(r**2) 164 | fx[:,2,3] = cg*cps/r 165 | fx[:,2,4] = -v*sg*cps/r 166 | fx[:,2,5] = -v*cg*sps/r 167 | fx[:,3,0] = -Dr + 2*sg/(r**3) 168 | fx[:,3,3] = -Dv 169 | fx[:,3,4] = -cg/(r**2) 170 | fx[:,4,0] = (cs/v)*Lr-v*cg/(r**2)+2*cg/(v*(r**3)) 171 | fx[:,4,3] = (cs/v)*(Lv-L/v)+cg/r+cg/((v**2)*(r**2)) 172 | fx[:,4,4] = (1/(v*r) -v)*sg/r 173 | fx[:,5,0] = ss*Lr/(v*cg)-v*cg*sps*np.tan(phi)/(r**2) 174 | fx[:,5,2] = v*cg*sps/(r*(cp**2)) 175 | fx[:,5,3] = (1/v)*(ss*Lv/cg)+ cg*sps*np.tan(phi)/r - L*ss/(cg*(v**2)) 176 | fx[:,5,4] = L*ss*sg/((cg**2)*v) -(v/r)*sg*sps*np.tan(phi) 177 | fx[:,5,5] = (v/r)*cg*cps*np.tan(phi) 178 | 179 | fu = np.zeros((N,ix,iu)) 180 | 181 | fu[:,4,0] = -L*ss/v; 182 | fu[:,5,0] = L*cs/(v*cg); 183 | 184 | return fx,fu -------------------------------------------------------------------------------- /model/Landing2DModel.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | import time 4 | import random 5 | def print_np(x): 6 | print ("Type is %s" % (type(x))) 7 | print ("Shape is %s" % (x.shape,)) 8 | print ("Values are: \n%s" % (x)) 9 | 10 | from model import OptimalcontrolModel 11 | 12 | 13 | class Landing2D(OptimalcontrolModel): 14 | def __init__(self,name,ix,iu,delT,linearization="numeric_central"): 15 | super().__init__(name,ix,iu,delT,linearization) 16 | self.m = 2 17 | self.I = 1e-2 18 | self.r_t = 1e-2 19 | self.g = 1 20 | 21 | def forward(self,x,u,idx=None,discrete=True): 22 | xdim = np.ndim(x) 23 | if xdim == 1: # 1 step state & input 24 | N = 1 25 | x = np.expand_dims(x,axis=0) 26 | else : 27 | N = np.size(x,axis = 0) 28 | udim = np.ndim(u) 29 | if udim == 1 : 30 | u = np.expand_dims(u,axis=0) 31 | 32 | # state & input 33 | rx = x[:,0] 34 | ry = x[:,1] 35 | vx = x[:,2] 36 | vy = x[:,3] 37 | t = x[:,4] 38 | w = x[:,5] 39 | 40 | gimbal = u[:,0] 41 | thrust = u[:,1] 42 | 43 | # output 44 | f = np.zeros_like(x) 45 | f[:,0] = vx 46 | f[:,1] = vy 47 | f[:,2] = 1 / self.m * (-np.sin(t+gimbal)) * thrust 48 | f[:,3] = 1 / self.m * (np.cos(t+gimbal)) * thrust - self.g 49 | f[:,4] = w 50 | f[:,5] = 1 / self.I * (-np.sin(gimbal)*thrust*self.r_t) 51 | 52 | if discrete is True : 53 | return np.squeeze(x + f * self.delT) 54 | else : 55 | return f 56 | 57 | # def diff(self,x,u): 58 | 59 | # # dimension 60 | # ndim = np.ndim(x) 61 | # if ndim == 1: # 1 step state & input 62 | # N = 1 63 | # x = np.expand_dims(x,axis=0) 64 | # u = np.expand_dims(u,axis=0) 65 | # else : 66 | # N = np.size(x,axis = 0) 67 | 68 | # # state & input 69 | # x1 = x[:,0] 70 | # x2 = x[:,1] 71 | # x3 = x[:,2] 72 | 73 | # v = u[:,0] 74 | # w = u[:,1] 75 | 76 | # fx = np.zeros((N,self.ix,self.ix)) 77 | # fx[:,0,0] = 1.0 78 | # fx[:,0,1] = 0.0 79 | # fx[:,0,2] = - self.delT * v * np.sin(x3) 80 | # fx[:,1,0] = 0.0 81 | # fx[:,1,1] = 1.0 82 | # fx[:,1,2] = self.delT * v * np.cos(x3) 83 | # fx[:,2,0] = 0.0 84 | # fx[:,2,1] = 0.0 85 | # fx[:,2,2] = 1.0 86 | 87 | # fu = np.zeros((N,self.ix,self.iu)) 88 | # fu[:,0,0] = self.delT * np.cos(x3) 89 | # fu[:,0,1] = 0.0 90 | # fu[:,1,0] = self.delT * np.sin(x3) 91 | # fu[:,1,1] = 0.0 92 | # fu[:,2,0] = 0.0 93 | # fu[:,2,1] = self.delT 94 | 95 | # return np.squeeze(fx) , np.squeeze(fu) -------------------------------------------------------------------------------- /model/Landing3DModel.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | import time 4 | import random 5 | import IPython 6 | def print_np(x): 7 | print ("Type is %s" % (type(x))) 8 | print ("Shape is %s" % (x.shape,)) 9 | # print ("Values are: \n%s" % (x)) 10 | 11 | from model import OptimalcontrolModel 12 | 13 | 14 | class Landing3D(OptimalcontrolModel): 15 | def __init__(self,name,ix,iu,delT,linearization="numeric_central"): 16 | super().__init__(name,ix,iu,delT,linearization) 17 | 18 | # self.m_wet = 2 19 | # self.m_dry = 0.75 20 | # self.J = 1e-2 21 | # self.J_mat = self.J * np.array([1,1,1]) 22 | # self.r_t = 1e-2 23 | # self.r_t_mat = self.r_t * np.array([0,0,-1]) 24 | # self.g = 1 25 | # self.g_mat = self.g * np.array([0,0,-1]) 26 | # self.alpha_m = 0.05 27 | 28 | # param from Successive Convexification for Real-Time Six-Degree-of-Freedom Powered Descent Guidance with State-Triggered Constraints 29 | self.J_x = 0.168 * 1 30 | self.J_y = 0.168 * 1 31 | self.J_z = 0.168 * 1e-2 32 | self.J_mat = np.array([self.J_x,self.J_y,self.J_z]) 33 | self.r_t = 0.25 34 | self.r_t_mat = self.r_t * np.array([0,0,-1]) 35 | self.g = 1 36 | self.g_mat = self.g * np.array([0,0,-1]) 37 | self.alpha_m = 0.01 38 | 39 | 40 | 41 | def get_dcm(self,q) : 42 | N = q.shape[0] 43 | 44 | C_B_I = np.zeros((N,3,3)) 45 | C_B_I[:,0,0] = 1 - 2 * (q[:,2] ** 2 + q[:,3] ** 2) 46 | C_B_I[:,0,1] = 2 * (q[:,1] * q[:,2] + q[:,0] * q[:,3]) 47 | C_B_I[:,0,2] = 2 * (q[:,1] * q[:,3] - q[:,0] * q[:,2]) 48 | C_B_I[:,1,0] = 2 * (q[:,1] * q[:,2] - q[:,0] * q[:,3]) 49 | C_B_I[:,1,1] = 1 - 2 * (q[:,1] ** 2 + q[:,3] ** 2) 50 | C_B_I[:,1,2] = 2 * (q[:,2] * q[:,3] + q[:,0] * q[:,1]) 51 | C_B_I[:,2,0] = 2 * (q[:,1] * q[:,3] + q[:,0] * q[:,2]) 52 | C_B_I[:,2,1] = 2 * (q[:,2] * q[:,3] - q[:,0] * q[:,1]) 53 | C_B_I[:,2,2] = 1 - 2 * (q[:,1] ** 2 + q[:,2] ** 2) 54 | 55 | return C_B_I 56 | 57 | def get_omega(self,xi) : 58 | N = xi.shape[0] 59 | 60 | xi_x = xi[:,0] 61 | xi_y = xi[:,1] 62 | xi_z = xi[:,2] 63 | 64 | omega = np.zeros((N,4,4)) 65 | 66 | omega[:,0,0] = 0 67 | omega[:,0,1] = -xi_x 68 | omega[:,0,2] = -xi_y 69 | omega[:,0,3] = -xi_z 70 | 71 | omega[:,1,0] = xi_x 72 | omega[:,1,1] = 0 73 | omega[:,1,2] = xi_z 74 | omega[:,1,3] = -xi_y 75 | 76 | omega[:,2,0] = xi_y 77 | omega[:,2,1] = -xi_z 78 | omega[:,2,2] = 0 79 | omega[:,2,3] = xi_x 80 | 81 | omega[:,3,0] = xi_z 82 | omega[:,3,1] = xi_y 83 | omega[:,3,2] = -xi_x 84 | omega[:,3,3] = 0 85 | 86 | return omega 87 | 88 | def get_skew(self,xi) : 89 | N = xi.shape[0] 90 | 91 | xi_x = xi[:,0] 92 | xi_y = xi[:,1] 93 | xi_z = xi[:,2] 94 | 95 | skew = np.zeros((N,3,3)) 96 | 97 | skew[:,0,0] = 0 98 | skew[:,0,1] = -xi_z 99 | skew[:,0,2] = xi_y 100 | 101 | skew[:,1,0] = xi_z 102 | skew[:,1,1] = 0 103 | skew[:,1,2] = -xi_x 104 | 105 | skew[:,2,0] = -xi_y 106 | skew[:,2,1] = xi_x 107 | skew[:,2,2] = 0 108 | 109 | return skew 110 | 111 | def forward(self,x,u,idx=None,discrete=True): 112 | xdim = np.ndim(x) 113 | if xdim == 1: # 1 step state & input 114 | N = 1 115 | x = np.expand_dims(x,axis=0) 116 | else : 117 | N = np.size(x,axis = 0) 118 | udim = np.ndim(u) 119 | if udim == 1 : 120 | u = np.expand_dims(u,axis=0) 121 | 122 | # input = [ux uy uz] 123 | 124 | # state & input 125 | m = x[:,0] 126 | rx = x[:,1] 127 | ry = x[:,2] 128 | rz = x[:,3] 129 | vx = x[:,4] 130 | vy = x[:,5] 131 | vz = x[:,6] 132 | q = x[:,7:11] 133 | w = x[:,11:14] 134 | 135 | q0 = x[:,7] 136 | q1 = x[:,8] 137 | q2 = x[:,9] 138 | q3 = x[:,10] 139 | 140 | wx = x[:,11] 141 | wy = x[:,12] 142 | wz = x[:,13] 143 | 144 | ux = u[:,0] 145 | uy = u[:,1] 146 | uz = u[:,2] 147 | 148 | # direction cosine matrix 149 | C_B_I = self.get_dcm(q) 150 | C_I_B = np.transpose(C_B_I,(0, 2, 1)) 151 | 152 | omega_w = self.get_omega(w) 153 | skew_w = self.get_skew(w) 154 | 155 | r_t_repeat = np.repeat(np.expand_dims(self.r_t_mat,0),N,axis=0) 156 | skew_r_t = self.get_skew(r_t_repeat) 157 | 158 | J_x,J_y,J_z = self.J_x,self.J_y,self.J_z 159 | r_t = self.r_t 160 | J_inv = np.repeat(1/np.expand_dims(self.J_mat,0),N,axis=0) 161 | # output 162 | f = np.zeros_like(x) 163 | f[:,0] = -self.alpha_m*np.sqrt(ux**2 + uy**2 + uz**2) 164 | f[:,1] = vx 165 | f[:,2] = vy 166 | f[:,3] = vz 167 | f[:,4] = ux*(-2*q2**2 - 2*q3**2 + 1)/m + uy*(-2*q0*q3 + 2*q1*q2)/m + uz*(2*q0*q2 + 2*q1*q3)/m 168 | f[:,5] = ux*(2*q0*q3 + 2*q1*q2)/m + uy*(-2*q1**2 - 2*q3**2 + 1)/m + uz*(-2*q0*q1 + 2*q2*q3)/m 169 | f[:,6] = -self.g + ux*(-2*q0*q2 + 2*q1*q3)/m + uy*(2*q0*q1 + 2*q2*q3)/m + uz*(-2*q1**2 - 2*q2**2 + 1)/m 170 | f[:,7] = -0.5*q1*wx - 0.5*q2*wy - 0.5*q3*wz 171 | f[:,8] = 0.5*q0*wx + 0.5*q2*wz - 0.5*q3*wy 172 | f[:,9] = 0.5*q0*wy - 0.5*q1*wz + 0.5*q3*wx 173 | f[:,10] = 0.5*q0*wz + 0.5*q1*wy - 0.5*q2*wx 174 | f[:,11] = (J_y*wy*wz - J_z*wy*wz + self.r_t*uy)/J_x 175 | f[:,12] = (-J_x*wx*wz + J_z*wx*wz - self.r_t*ux)/J_y 176 | f[:,13] = (J_x*wx*wy - J_y*wx*wy)/J_z 177 | 178 | if discrete is True : 179 | return np.squeeze(x + f * self.delT) 180 | else : 181 | return f 182 | 183 | def diff(self,x,u,discrete=True) : 184 | assert discrete == False 185 | # state & input size 186 | ix = self.ix 187 | iu = self.iu 188 | 189 | ndim = np.ndim(x) 190 | if ndim == 1: # 1 step state & input 191 | N = 1 192 | x = np.expand_dims(x,axis=0) 193 | u = np.expand_dims(u,axis=0) 194 | else : 195 | N = np.size(x,axis = 0) 196 | m = x[:,0] 197 | rx = x[:,1] 198 | ry = x[:,2] 199 | rz = x[:,3] 200 | vx = x[:,4] 201 | vy = x[:,5] 202 | vz = x[:,6] 203 | 204 | q0 = x[:,7] 205 | q1 = x[:,8] 206 | q2 = x[:,9] 207 | q3 = x[:,10] 208 | 209 | wx = x[:,11] 210 | wy = x[:,12] 211 | wz = x[:,13] 212 | 213 | ux = u[:,0] 214 | uy = u[:,1] 215 | uz = u[:,2] 216 | 217 | J_x,J_y,J_z = self.J_x,self.J_y,self.J_z 218 | 219 | fx = np.zeros((N,ix,ix)) 220 | fx[:,1,4] = 1 221 | fx[:,2,5] = 1 222 | fx[:,3,6] = 1 223 | fx[:,4,0] = -ux*(-2*q2**2 - 2*q3**2 + 1)/m**2 - uy*(-2*q0*q3 + 2*q1*q2)/m**2 - uz*(2*q0*q2 + 2*q1*q3)/m**2 224 | fx[:,4,7] = 2*q2*uz/m - 2*q3*uy/m 225 | fx[:,4,8] = 2*q2*uy/m + 2*q3*uz/m 226 | fx[:,4,9] = 2*q0*uz/m + 2*q1*uy/m - 4*q2*ux/m 227 | fx[:,4,10] = -2*q0*uy/m + 2*q1*uz/m - 4*q3*ux/m 228 | fx[:,5,0] = -ux*(2*q0*q3 + 2*q1*q2)/m**2 - uy*(-2*q1**2 - 2*q3**2 + 1)/m**2 - uz*(-2*q0*q1 + 2*q2*q3)/m**2 229 | fx[:,5,7] = -2*q1*uz/m + 2*q3*ux/m 230 | fx[:,5,8] = -2*q0*uz/m - 4*q1*uy/m + 2*q2*ux/m 231 | fx[:,5,9] = 2*q1*ux/m + 2*q3*uz/m 232 | fx[:,5,10] = 2*q0*ux/m + 2*q2*uz/m - 4*q3*uy/m 233 | fx[:,6,0] = -ux*(-2*q0*q2 + 2*q1*q3)/m**2 - uy*(2*q0*q1 + 2*q2*q3)/m**2 - uz*(-2*q1**2 - 2*q2**2 + 1)/m**2 234 | fx[:,6,7] = 2*q1*uy/m - 2*q2*ux/m 235 | fx[:,6,8] = 2*q0*uy/m - 4*q1*uz/m + 2*q3*ux/m 236 | fx[:,6,9] = -2*q0*ux/m - 4*q2*uz/m + 2*q3*uy/m 237 | fx[:,6,10] = 2*q1*ux/m + 2*q2*uy/m 238 | fx[:,7,8] = -0.5*wx 239 | fx[:,7,9] = -0.5*wy 240 | fx[:,7,10] = -0.5*wz 241 | fx[:,7,11] = -0.5*q1 242 | fx[:,7,12] = -0.5*q2 243 | fx[:,7,13] = -0.5*q3 244 | fx[:,8,7] = 0.5*wx 245 | fx[:,8,9] = 0.5*wz 246 | fx[:,8,10] = -0.5*wy 247 | fx[:,8,11] = 0.5*q0 248 | fx[:,8,12] = -0.5*q3 249 | fx[:,8,13] = 0.5*q2 250 | fx[:,9,7] = 0.5*wy 251 | fx[:,9,8] = -0.5*wz 252 | fx[:,9,10] = 0.5*wx 253 | fx[:,9,11] = 0.5*q3 254 | fx[:,9,12] = 0.5*q0 255 | fx[:,9,13] = -0.5*q1 256 | fx[:,10,7] = 0.5*wz 257 | fx[:,10,8] = 0.5*wy 258 | fx[:,10,9] = -0.5*wx 259 | fx[:,10,11] = -0.5*q2 260 | fx[:,10,12] = 0.5*q1 261 | fx[:,10,13] = 0.5*q0 262 | fx[:,11,12] = (J_y*wz - J_z*wz)/J_x 263 | fx[:,11,13] = (J_y*wy - J_z*wy)/J_x 264 | fx[:,12,11] = (-J_x*wz + J_z*wz)/J_y 265 | fx[:,12,13] = (-J_x*wx + J_z*wx)/J_y 266 | fx[:,13,11] = (J_x*wy - J_y*wy)/J_z 267 | fx[:,13,12] = (J_x*wx - J_y*wx)/J_z 268 | 269 | alpha_m = self.alpha_m 270 | # J = self.J 271 | r_t = self.r_t 272 | 273 | fu = np.zeros((N,ix,iu)) 274 | 275 | fu[:,0,0] = -alpha_m*ux/np.sqrt(ux**2 + uy**2 + uz**2) 276 | fu[:,0,1] = -alpha_m*uy/np.sqrt(ux**2 + uy**2 + uz**2) 277 | fu[:,0,2] = -alpha_m*uz/np.sqrt(ux**2 + uy**2 + uz**2) 278 | fu[:,4,0] = (-2*q2**2 - 2*q3**2 + 1)/m 279 | fu[:,4,1] = (-2*q0*q3 + 2*q1*q2)/m 280 | fu[:,4,2] = (2*q0*q2 + 2*q1*q3)/m 281 | fu[:,5,0] = (2*q0*q3 + 2*q1*q2)/m 282 | fu[:,5,1] = (-2*q1**2 - 2*q3**2 + 1)/m 283 | fu[:,5,2] = (-2*q0*q1 + 2*q2*q3)/m 284 | fu[:,6,0] = (-2*q0*q2 + 2*q1*q3)/m 285 | fu[:,6,1] = (2*q0*q1 + 2*q2*q3)/m 286 | fu[:,6,2] = (-2*q1**2 - 2*q2**2 + 1)/m 287 | fu[:,11,1] = r_t/J_x 288 | fu[:,12,0] = -r_t/J_y 289 | 290 | return fx.squeeze(),fu.squeeze() 291 | 292 | -------------------------------------------------------------------------------- /model/Landing3DModel_UEN.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | import time 4 | import random 5 | import IPython 6 | def print_np(x): 7 | print ("Type is %s" % (type(x))) 8 | print ("Shape is %s" % (x.shape,)) 9 | # print ("Values are: \n%s" % (x)) 10 | 11 | from model import OptimalcontrolModel 12 | 13 | 14 | class Landing3D(OptimalcontrolModel): 15 | def __init__(self,name,ix,iu,delT,linearization="numeric_central"): 16 | super().__init__(name,ix,iu,delT,linearization) 17 | # self.m_wet = 2 18 | # self.m_dry = 0.75 19 | self.J = 1e-2 20 | self.J_mat = self.J * np.array([1,1,1]) 21 | self.r_t = 1e-2 22 | self.r_t_mat = self.r_t * np.array([-1,0,0]) 23 | self.g = 1 24 | self.g_mat = self.g * np.array([-1,0,0]) 25 | self.alpha_m = 0.01 26 | 27 | def get_dcm(self,q) : 28 | N = q.shape[0] 29 | 30 | C_B_I = np.zeros((N,3,3)) 31 | C_B_I[:,0,0] = 1 - 2 * (q[:,2] ** 2 + q[:,3] ** 2) 32 | C_B_I[:,0,1] = 2 * (q[:,1] * q[:,2] + q[:,0] * q[:,3]) 33 | C_B_I[:,0,2] = 2 * (q[:,1] * q[:,3] - q[:,0] * q[:,2]) 34 | C_B_I[:,1,0] = 2 * (q[:,1] * q[:,2] - q[:,0] * q[:,3]) 35 | C_B_I[:,1,1] = 1 - 2 * (q[:,1] ** 2 + q[:,3] ** 2) 36 | C_B_I[:,1,2] = 2 * (q[:,2] * q[:,3] + q[:,0] * q[:,1]) 37 | C_B_I[:,2,0] = 2 * (q[:,1] * q[:,3] + q[:,0] * q[:,2]) 38 | C_B_I[:,2,1] = 2 * (q[:,2] * q[:,3] - q[:,0] * q[:,1]) 39 | C_B_I[:,2,2] = 1 - 2 * (q[:,1] ** 2 + q[:,2] ** 2) 40 | 41 | return C_B_I 42 | 43 | def get_omega(self,xi) : 44 | N = xi.shape[0] 45 | 46 | xi_x = xi[:,0] 47 | xi_y = xi[:,1] 48 | xi_z = xi[:,2] 49 | 50 | omega = np.zeros((N,4,4)) 51 | 52 | omega[:,0,0] = 0 53 | omega[:,0,1] = -xi_x 54 | omega[:,0,2] = -xi_y 55 | omega[:,0,3] = -xi_z 56 | 57 | omega[:,1,0] = xi_x 58 | omega[:,1,1] = 0 59 | omega[:,1,2] = xi_z 60 | omega[:,1,3] = -xi_y 61 | 62 | omega[:,2,0] = xi_y 63 | omega[:,2,1] = -xi_z 64 | omega[:,2,2] = 0 65 | omega[:,2,3] = xi_x 66 | 67 | omega[:,3,0] = xi_z 68 | omega[:,3,1] = xi_y 69 | omega[:,3,2] = -xi_x 70 | omega[:,3,3] = 0 71 | 72 | return omega 73 | 74 | def get_skew(self,xi) : 75 | N = xi.shape[0] 76 | 77 | xi_x = xi[:,0] 78 | xi_y = xi[:,1] 79 | xi_z = xi[:,2] 80 | 81 | skew = np.zeros((N,3,3)) 82 | 83 | skew[:,0,0] = 0 84 | skew[:,0,1] = -xi_z 85 | skew[:,0,2] = xi_y 86 | 87 | skew[:,1,0] = xi_z 88 | skew[:,1,1] = 0 89 | skew[:,1,2] = -xi_x 90 | 91 | skew[:,2,0] = -xi_y 92 | skew[:,2,1] = xi_x 93 | skew[:,2,2] = 0 94 | 95 | return skew 96 | 97 | def forward(self,x,u,idx=None,discrete=True): 98 | xdim = np.ndim(x) 99 | if xdim == 1: # 1 step state & input 100 | N = 1 101 | x = np.expand_dims(x,axis=0) 102 | else : 103 | N = np.size(x,axis = 0) 104 | udim = np.ndim(u) 105 | if udim == 1 : 106 | u = np.expand_dims(u,axis=0) 107 | 108 | # input = [ux uy uz] 109 | 110 | # state & input 111 | m = x[:,0] 112 | rx = x[:,1] 113 | ry = x[:,2] 114 | rz = x[:,3] 115 | vx = x[:,4] 116 | vy = x[:,5] 117 | vz = x[:,6] 118 | q = x[:,7:11] 119 | w = x[:,11:14] 120 | 121 | q0 = x[:,7] 122 | q1 = x[:,8] 123 | q2 = x[:,9] 124 | q3 = x[:,10] 125 | 126 | wx = x[:,11] 127 | wy = x[:,12] 128 | wz = x[:,13] 129 | 130 | ux = u[:,0] 131 | uy = u[:,1] 132 | uz = u[:,2] 133 | 134 | # direction cosine matrix 135 | C_B_I = self.get_dcm(q) 136 | C_I_B = np.transpose(C_B_I,(0, 2, 1)) 137 | 138 | omega_w = self.get_omega(w) 139 | skew_w = self.get_skew(w) 140 | 141 | r_t_repeat = np.repeat(np.expand_dims(self.r_t_mat,0),N,axis=0) 142 | skew_r_t = self.get_skew(r_t_repeat) 143 | 144 | J_inv = np.repeat(1/np.expand_dims(self.J_mat,0),N,axis=0) 145 | # output 146 | f = np.zeros_like(x) 147 | f[:,0] = -self.alpha_m*np.sqrt(ux**2 + uy**2 + uz**2) 148 | f[:,1] = vx 149 | f[:,2] = vy 150 | f[:,3] = vz 151 | f[:,4] = -self.g + ux*(-2*q2**2 - 2*q3**2 + 1)/m + uy*(-2*q0*q3 + 2*q1*q2)/m + uz*(2*q0*q2 + 2*q1*q3)/m 152 | f[:,5] = ux*(2*q0*q3 + 2*q1*q2)/m + uy*(-2*q1**2 - 2*q3**2 + 1)/m + uz*(-2*q0*q1 + 2*q2*q3)/m 153 | f[:,6] = ux*(-2*q0*q2 + 2*q1*q3)/m + uy*(2*q0*q1 + 2*q2*q3)/m + uz*(-2*q1**2 - 2*q2**2 + 1)/m 154 | f[:,7] = -0.5*q1*wx - 0.5*q2*wy - 0.5*q3*wz 155 | f[:,8] = 0.5*q0*wx + 0.5*q2*wz - 0.5*q3*wy 156 | f[:,9] = 0.5*q0*wy - 0.5*q1*wz + 0.5*q3*wx 157 | f[:,10] = 0.5*q0*wz + 0.5*q1*wy - 0.5*q2*wx 158 | f[:,12] = 1.0*self.r_t*uz/self.J 159 | f[:,13] = -1.0*self.r_t*uy/self.J 160 | 161 | if discrete is True : 162 | return np.squeeze(x + f * self.delT) 163 | else : 164 | return f 165 | 166 | def diff(self,x,u,discrete=True) : 167 | assert discrete == False 168 | # state & input size 169 | ix = self.ix 170 | iu = self.iu 171 | 172 | ndim = np.ndim(x) 173 | if ndim == 1: # 1 step state & input 174 | N = 1 175 | x = np.expand_dims(x,axis=0) 176 | u = np.expand_dims(u,axis=0) 177 | else : 178 | N = np.size(x,axis = 0) 179 | m = x[:,0] 180 | rx = x[:,1] 181 | ry = x[:,2] 182 | rz = x[:,3] 183 | vx = x[:,4] 184 | vy = x[:,5] 185 | vz = x[:,6] 186 | 187 | q0 = x[:,7] 188 | q1 = x[:,8] 189 | q2 = x[:,9] 190 | q3 = x[:,10] 191 | 192 | wx = x[:,11] 193 | wy = x[:,12] 194 | wz = x[:,13] 195 | 196 | ux = u[:,0] 197 | uy = u[:,1] 198 | uz = u[:,2] 199 | 200 | fx = np.zeros((N,ix,ix)) 201 | fx[:,1,4] = 1 202 | fx[:,2,5] = 1 203 | fx[:,3,6] = 1 204 | fx[:,4,0] = -ux*(-2*q2**2 - 2*q3**2 + 1)/m**2 - uy*(-2*q0*q3 + 2*q1*q2)/m**2 - uz*(2*q0*q2 + 2*q1*q3)/m**2 205 | fx[:,4,7] = 2*q2*uz/m - 2*q3*uy/m 206 | fx[:,4,8] = 2*q2*uy/m + 2*q3*uz/m 207 | fx[:,4,9] = 2*q0*uz/m + 2*q1*uy/m - 4*q2*ux/m 208 | fx[:,4,10] = -2*q0*uy/m + 2*q1*uz/m - 4*q3*ux/m 209 | fx[:,5,0] = -ux*(2*q0*q3 + 2*q1*q2)/m**2 - uy*(-2*q1**2 - 2*q3**2 + 1)/m**2 - uz*(-2*q0*q1 + 2*q2*q3)/m**2 210 | fx[:,5,7] = -2*q1*uz/m + 2*q3*ux/m 211 | fx[:,5,8] = -2*q0*uz/m - 4*q1*uy/m + 2*q2*ux/m 212 | fx[:,5,9] = 2*q1*ux/m + 2*q3*uz/m 213 | fx[:,5,10] = 2*q0*ux/m + 2*q2*uz/m - 4*q3*uy/m 214 | fx[:,6,0] = -ux*(-2*q0*q2 + 2*q1*q3)/m**2 - uy*(2*q0*q1 + 2*q2*q3)/m**2 - uz*(-2*q1**2 - 2*q2**2 + 1)/m**2 215 | fx[:,6,7] = 2*q1*uy/m - 2*q2*ux/m 216 | fx[:,6,8] = 2*q0*uy/m - 4*q1*uz/m + 2*q3*ux/m 217 | fx[:,6,9] = -2*q0*ux/m - 4*q2*uz/m + 2*q3*uy/m 218 | fx[:,6,10] = 2*q1*ux/m + 2*q2*uy/m 219 | fx[:,7,8] = -0.5*wx 220 | fx[:,7,9] = -0.5*wy 221 | fx[:,7,10] = -0.5*wz 222 | fx[:,7,11] = -0.5*q1 223 | fx[:,7,12] = -0.5*q2 224 | fx[:,7,13] = -0.5*q3 225 | fx[:,8,7] = 0.5*wx 226 | fx[:,8,9] = 0.5*wz 227 | fx[:,8,10] = -0.5*wy 228 | fx[:,8,11] = 0.5*q0 229 | fx[:,8,12] = -0.5*q3 230 | fx[:,8,13] = 0.5*q2 231 | fx[:,9,7] = 0.5*wy 232 | fx[:,9,8] = -0.5*wz 233 | fx[:,9,10] = 0.5*wx 234 | fx[:,9,11] = 0.5*q3 235 | fx[:,9,12] = 0.5*q0 236 | fx[:,9,13] = -0.5*q1 237 | fx[:,10,7] = 0.5*wz 238 | fx[:,10,8] = 0.5*wy 239 | fx[:,10,9] = -0.5*wx 240 | fx[:,10,11] = -0.5*q2 241 | fx[:,10,12] = 0.5*q1 242 | fx[:,10,13] = 0.5*q0 243 | 244 | alpha_m = self.alpha_m 245 | J = self.J 246 | r_t = self.r_t 247 | 248 | fu = np.zeros((N,ix,iu)) 249 | 250 | fu[:,0,0] = -alpha_m*ux/np.sqrt(ux**2 + uy**2 + uz**2) 251 | fu[:,0,1] = -alpha_m*uy/np.sqrt(ux**2 + uy**2 + uz**2) 252 | fu[:,0,2] = -alpha_m*uz/np.sqrt(ux**2 + uy**2 + uz**2) 253 | fu[:,4,0] = (-2*q2**2 - 2*q3**2 + 1)/m 254 | fu[:,4,1] = (-2*q0*q3 + 2*q1*q2)/m 255 | fu[:,4,2] = (2*q0*q2 + 2*q1*q3)/m 256 | fu[:,5,0] = (2*q0*q3 + 2*q1*q2)/m 257 | fu[:,5,1] = (-2*q1**2 - 2*q3**2 + 1)/m 258 | fu[:,5,2] = (-2*q0*q1 + 2*q2*q3)/m 259 | fu[:,6,0] = (-2*q0*q2 + 2*q1*q3)/m 260 | fu[:,6,1] = (2*q0*q1 + 2*q2*q3)/m 261 | fu[:,6,2] = (-2*q1**2 - 2*q2**2 + 1)/m 262 | fu[:,12,2] = 1.0*r_t/J 263 | fu[:,13,1] = -1.0*r_t/J 264 | 265 | return fx.squeeze(),fu.squeeze() 266 | 267 | -------------------------------------------------------------------------------- /model/LinearModel.py: -------------------------------------------------------------------------------- 1 | 2 | # coding: utf-8 3 | 4 | # In[ ]: 5 | 6 | from __future__ import division 7 | import matplotlib.pyplot as plt 8 | import numpy as np 9 | import time 10 | import random 11 | def print_np(x): 12 | print ("Type is %s" % (type(x))) 13 | print ("Shape is %s" % (x.shape,)) 14 | print ("Values are: \n%s" % (x)) 15 | 16 | from model import OptimalcontrolModel 17 | 18 | class Linear(OptimalcontrolModel): 19 | def __init__(self,name,ix,iu,delT): 20 | super().__init__(name,ix,iu,delT) 21 | 22 | def forward(self,x,u,idx=None,discrete=True): 23 | 24 | xdim = np.ndim(x) 25 | if xdim == 1: # 1 step state & input 26 | N = 1 27 | x = np.expand_dims(x,axis=0) 28 | else : 29 | N = np.size(x,axis = 0) 30 | udim = np.ndim(u) 31 | if udim == 1 : 32 | u = np.expand_dims(u,axis=0) 33 | 34 | # state & input 35 | px = x[:,0] 36 | vx = x[:,1] 37 | py = x[:,2] 38 | vy = x[:,3] 39 | 40 | fx = u[:,0] 41 | fy = u[:,1] 42 | 43 | # output 44 | f = np.zeros_like(x) 45 | f[:,0] = vx 46 | f[:,1] = fx 47 | f[:,2] = vy 48 | f[:,3] = fy - 1 49 | 50 | if discrete is True : 51 | return np.squeeze(x + f * self.delT) 52 | else : 53 | # print("hello") 54 | return f 55 | -------------------------------------------------------------------------------- /model/QuadRotorPointMassModel.py: -------------------------------------------------------------------------------- 1 | 2 | # coding: utf-8 3 | 4 | # In[ ]: 5 | 6 | from __future__ import division 7 | import matplotlib.pyplot as plt 8 | import numpy as np 9 | import time 10 | import random 11 | def print_np(x): 12 | print ("Type is %s" % (type(x))) 13 | print ("Shape is %s" % (x.shape,)) 14 | print ("Values are: \n%s" % (x)) 15 | 16 | from model import OptimalcontrolModel 17 | 18 | class quadrotorpm(OptimalcontrolModel): 19 | def __init__(self,name,ix,iu,delT,linearization="analytic"): 20 | super().__init__(name,ix,iu,delT,linearization) 21 | self.g = 9.81 22 | 23 | def forward(self,x,u,idx=None,discrete=True): 24 | 25 | xdim = np.ndim(x) 26 | if xdim == 1: # 1 step state & input 27 | N = 1 28 | x = np.expand_dims(x,axis=0) 29 | else : 30 | N = np.size(x,axis = 0) 31 | udim = np.ndim(u) 32 | if udim == 1 : 33 | u = np.expand_dims(u,axis=0) 34 | 35 | # state & input 36 | rx = x[:,0] 37 | ry = x[:,1] 38 | rz = x[:,2] 39 | vx = x[:,3] 40 | vy = x[:,4] 41 | vz = x[:,5] 42 | 43 | ax = u[:,0] 44 | ay = u[:,1] 45 | az = u[:,2] 46 | 47 | # output 48 | f = np.zeros_like(x) 49 | f[:,0] = vx 50 | f[:,1] = vy 51 | f[:,2] = vz 52 | f[:,3] = ax 53 | f[:,4] = ay 54 | f[:,5] = az-self.g 55 | 56 | if discrete is True : 57 | return np.squeeze(x + f * self.delT) 58 | else : 59 | return f 60 | 61 | def diff(self,x,u,idx=None,discrete=False) : 62 | # state & input size 63 | ix = self.ix 64 | iu = self.iu 65 | 66 | ndim = np.ndim(x) 67 | if ndim == 1: # 1 step state & input 68 | N = 1 69 | x = np.expand_dims(x,axis=0) 70 | u = np.expand_dims(u,axis=0) 71 | else : 72 | N = np.size(x,axis = 0) 73 | 74 | fx = np.zeros((N,ix,ix)) 75 | fu = np.zeros((N,ix,iu)) 76 | 77 | fx[:,0,3] = 1 78 | fx[:,1,4] = 1 79 | fx[:,2,5] = 1 80 | 81 | fu[:,3,0] = 1 82 | fu[:,4,1] = 1 83 | fu[:,5,2] = 1 84 | 85 | if discrete == False : 86 | return fx.squeeze(), fu.squeeze() 87 | 88 | else : 89 | return np.repeat(np.expand_dims(np.eye(ix),0),N,0)+fu*self.delT,fu*self.delT -------------------------------------------------------------------------------- /model/QuadRotorSmallAngleModel.py: -------------------------------------------------------------------------------- 1 | 2 | # coding: utf-8 3 | 4 | # In[ ]: 5 | 6 | from __future__ import division 7 | import matplotlib.pyplot as plt 8 | import numpy as np 9 | import time 10 | import random 11 | def print_np(x): 12 | print ("Type is %s" % (type(x))) 13 | print ("Shape is %s" % (x.shape,)) 14 | print ("Values are: \n%s" % (x)) 15 | 16 | from model import OptimalcontrolModel 17 | 18 | class quadrotorsa(OptimalcontrolModel): 19 | def __init__(self,name,ix,iu,delT,linearization="numeric_central"): 20 | super().__init__(name,ix,iu,delT,linearization) 21 | self.g = 9.81 22 | 23 | def forward(self,x,u,idx=None,discrete=True): 24 | 25 | xdim = np.ndim(x) 26 | if xdim == 1: # 1 step state & input 27 | N = 1 28 | x = np.expand_dims(x,axis=0) 29 | else : 30 | N = np.size(x,axis = 0) 31 | udim = np.ndim(u) 32 | if udim == 1 : 33 | u = np.expand_dims(u,axis=0) 34 | 35 | # state & input 36 | rx = x[:,0] 37 | ry = x[:,1] 38 | rz = x[:,2] 39 | roll = x[:,3] 40 | pitch = x[:,4] 41 | yaw = x[:,5] 42 | xdot = x[:,6] 43 | ydot = x[:,7] 44 | zdot = x[:,8] 45 | rolldot = x[:,9] 46 | pitchdot = x[:,10] 47 | yawdot = x[:,11] 48 | 49 | thrust = u[:,0] 50 | Mx = u[:,1] 51 | My = u[:,2] 52 | Mz = u[:,3] 53 | 54 | # output 55 | f = np.zeros_like(x) 56 | f[:,0] = xdot 57 | f[:,1] = ydot 58 | f[:,2] = zdot 59 | f[:,3] = rolldot 60 | f[:,4] = pitchdot 61 | f[:,5] = yawdot 62 | f[:,6] = thrust*(np.cos(roll)*np.sin(pitch)*np.cos(yaw)+np.sin(roll)*np.sin(pitch)) 63 | f[:,7] = thrust*(np.cos(roll)*np.sin(pitch)*np.sin(yaw)-np.sin(roll)*np.cos(pitch)) 64 | f[:,8] = thrust*np.cos(roll)*np.cos(pitch)-self.g 65 | f[:,9] = Mx 66 | f[:,10] = My 67 | f[:,11] = Mz 68 | 69 | if discrete is True : 70 | return np.squeeze(x + f * self.delT) 71 | else : 72 | return f 73 | -------------------------------------------------------------------------------- /model/Reentry.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | import time 4 | import random 5 | def print_np(x): 6 | print ("Type is %s" % (type(x))) 7 | print ("Shape is %s" % (x.shape,)) 8 | print ("Values are: \n%s" % (x)) 9 | 10 | from model import OptimalcontrolModel 11 | 12 | 13 | class Reentry(OptimalcontrolModel): 14 | def __init__(self,name,ix,iu,linearization="numeric_central"): 15 | super().__init__(name,ix,iu,linearization) 16 | self.m = 907.186 17 | self.ge = 9.806 18 | self.Sref = 0.484 19 | self.rhoe = 1.225 20 | self.Re = 6371*1000 21 | self.bet = 0.14*1e-3 22 | self.CLstr = 0.45 23 | self.Estr = 3.24 24 | self.Bcnst = 0.5*self.rhoe*self.Re*self.Sref*self.CLstr/self.m 25 | self.scl_t = np.sqrt(self.Re/self.ge) 26 | self.scl_d = self.Re 27 | self.scl_v = np.sqrt(self.ge*self.Re) 28 | 29 | def forward(self,x,u,idx=None,discrete=True): 30 | xdim = np.ndim(x) 31 | if xdim == 1: # 1 step state & input 32 | N = 1 33 | x = np.expand_dims(x,axis=0) 34 | else : 35 | N = np.size(x,axis = 0) 36 | udim = np.ndim(u) 37 | if udim == 1 : 38 | u = np.expand_dims(u,axis=0) 39 | 40 | # state & input 41 | rx = x[:,0] 42 | ry = x[:,1] 43 | rh = x[:,2] 44 | v = x[:,3] 45 | gamma = x[:,4] 46 | theta = x[:,5] 47 | 48 | lam = u[:,0] 49 | sig = u[:,1] 50 | 51 | # output 52 | f = np.zeros_like(x) 53 | f[:,0] = v * np.cos(theta) 54 | f[:,1] = v * np.sin(theta) 55 | f[:,2] = v * gamma 56 | f[:,3] = -0.5 * (self.Bcnst * v * v * np.exp(-self.bet * self.Re * rh)* (1+lam * lam))/self.Estr 57 | f[:,4] = self.Bcnst * v * np.exp(-self.bet * self.Re * rh) * lam * np.cos(sig) - 1 / v + v 58 | f[:,5] = self.Bcnst * v * np.exp(-self.bet * self.Re * rh) * lam * np.sin(sig) 59 | 60 | if discrete is True : 61 | return np.squeeze(x + f * self.delT) 62 | else : 63 | return f 64 | 65 | # def diff(self,x,u): 66 | 67 | # # dimension 68 | # ndim = np.ndim(x) 69 | # if ndim == 1: # 1 step state & input 70 | # N = 1 71 | # x = np.expand_dims(x,axis=0) 72 | # u = np.expand_dims(u,axis=0) 73 | # else : 74 | # N = np.size(x,axis = 0) 75 | 76 | # # state & input 77 | # x1 = x[:,0] 78 | # x2 = x[:,1] 79 | # x3 = x[:,2] 80 | 81 | # v = u[:,0] 82 | # w = u[:,1] 83 | 84 | # fx = np.zeros((N,self.ix,self.ix)) 85 | # fx[:,0,0] = 1.0 86 | # fx[:,0,1] = 0.0 87 | # fx[:,0,2] = - self.delT * v * np.sin(x3) 88 | # fx[:,1,0] = 0.0 89 | # fx[:,1,1] = 1.0 90 | # fx[:,1,2] = self.delT * v * np.cos(x3) 91 | # fx[:,2,0] = 0.0 92 | # fx[:,2,1] = 0.0 93 | # fx[:,2,2] = 1.0 94 | 95 | # fu = np.zeros((N,self.ix,self.iu)) 96 | # fu[:,0,0] = self.delT * np.cos(x3) 97 | # fu[:,0,1] = 0.0 98 | # fu[:,1,0] = self.delT * np.sin(x3) 99 | # fu[:,1,1] = 0.0 100 | # fu[:,2,0] = 0.0 101 | # fu[:,2,1] = self.delT 102 | 103 | # return np.squeeze(fx) , np.squeeze(fu) -------------------------------------------------------------------------------- /model/UnicycleModel.py: -------------------------------------------------------------------------------- 1 | 2 | # coding: utf-8 3 | 4 | # In[ ]: 5 | 6 | from __future__ import division 7 | import matplotlib.pyplot as plt 8 | import numpy as np 9 | import time 10 | import random 11 | def print_np(x): 12 | print ("Type is %s" % (type(x))) 13 | print ("Shape is %s" % (x.shape,)) 14 | # print ("Values are: \n%s" % (x)) 15 | 16 | from model import OptimalcontrolModel 17 | 18 | class unicycle(OptimalcontrolModel): 19 | def __init__(self,name,ix,iu,linearzation): 20 | super().__init__(name,ix,iu,linearzation) 21 | 22 | def forward(self,x,u,idx=None): 23 | xdim = np.ndim(x) 24 | if xdim == 1: # 1 step state & input 25 | N = 1 26 | x = np.expand_dims(x,axis=0) 27 | else : 28 | N = np.size(x,axis = 0) 29 | udim = np.ndim(u) 30 | if udim == 1 : 31 | u = np.expand_dims(u,axis=0) 32 | 33 | # state & input 34 | x1 = x[:,0] 35 | x2 = x[:,1] 36 | x3 = x[:,2] 37 | 38 | v = u[:,0] 39 | w = u[:,1] 40 | 41 | # output 42 | f = np.zeros_like(x) 43 | f[:,0] = v * np.cos(x3) 44 | f[:,1] = v * np.sin(x3) 45 | f[:,2] = w 46 | 47 | return f 48 | 49 | # def diff(self,x,u): 50 | 51 | # # dimension 52 | # ndim = np.ndim(x) 53 | # if ndim == 1: # 1 step state & input 54 | # N = 1 55 | # x = np.expand_dims(x,axis=0) 56 | # u = np.expand_dims(u,axis=0) 57 | # else : 58 | # N = np.size(x,axis = 0) 59 | 60 | # # state & input 61 | # x1 = x[:,0] 62 | # x2 = x[:,1] 63 | # x3 = x[:,2] 64 | 65 | # v = u[:,0] 66 | # w = u[:,1] 67 | 68 | # fx = np.zeros((N,self.ix,self.ix)) 69 | # fx[:,0,0] = 1.0 70 | # fx[:,0,1] = 0.0 71 | # fx[:,0,2] = - self.delT * v * np.sin(x3) 72 | # fx[:,1,0] = 0.0 73 | # fx[:,1,1] = 1.0 74 | # fx[:,1,2] = self.delT * v * np.cos(x3) 75 | # fx[:,2,0] = 0.0 76 | # fx[:,2,1] = 0.0 77 | # fx[:,2,2] = 1.0 78 | 79 | # fu = np.zeros((N,self.ix,self.iu)) 80 | # fu[:,0,0] = self.delT * np.cos(x3) 81 | # fu[:,0,1] = 0.0 82 | # fu[:,1,0] = self.delT * np.sin(x3) 83 | # fu[:,1,1] = 0.0 84 | # fu[:,2,0] = 0.0 85 | # fu[:,2,1] = self.delT 86 | 87 | # return np.squeeze(fx) , np.squeeze(fu) 88 | -------------------------------------------------------------------------------- /model/__pycache__/Aircraft3dofApproxModel.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/model/__pycache__/Aircraft3dofApproxModel.cpython-38.pyc -------------------------------------------------------------------------------- /model/__pycache__/Aircraft3dofModel.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/model/__pycache__/Aircraft3dofModel.cpython-310.pyc -------------------------------------------------------------------------------- /model/__pycache__/Aircraft3dofModel.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/model/__pycache__/Aircraft3dofModel.cpython-38.pyc -------------------------------------------------------------------------------- /model/__pycache__/Aircraft3dofModel_tmp.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/model/__pycache__/Aircraft3dofModel_tmp.cpython-310.pyc -------------------------------------------------------------------------------- /model/__pycache__/AircraftKinematicsModel.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/model/__pycache__/AircraftKinematicsModel.cpython-38.pyc -------------------------------------------------------------------------------- /model/__pycache__/HypersonicEntry3DofPolar.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/model/__pycache__/HypersonicEntry3DofPolar.cpython-38.pyc -------------------------------------------------------------------------------- /model/__pycache__/Landing2DModel.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/model/__pycache__/Landing2DModel.cpython-38.pyc -------------------------------------------------------------------------------- /model/__pycache__/Landing3DModel.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/model/__pycache__/Landing3DModel.cpython-38.pyc -------------------------------------------------------------------------------- /model/__pycache__/Landing3DModel_UEN.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/model/__pycache__/Landing3DModel_UEN.cpython-38.pyc -------------------------------------------------------------------------------- /model/__pycache__/LinearModel.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/model/__pycache__/LinearModel.cpython-38.pyc -------------------------------------------------------------------------------- /model/__pycache__/QuadRotorPointMassModel.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/model/__pycache__/QuadRotorPointMassModel.cpython-38.pyc -------------------------------------------------------------------------------- /model/__pycache__/QuadRotorSmallAngleModel.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/model/__pycache__/QuadRotorSmallAngleModel.cpython-38.pyc -------------------------------------------------------------------------------- /model/__pycache__/Reentry.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/model/__pycache__/Reentry.cpython-310.pyc -------------------------------------------------------------------------------- /model/__pycache__/Reentry.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/model/__pycache__/Reentry.cpython-38.pyc -------------------------------------------------------------------------------- /model/__pycache__/UnicycleModel.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/model/__pycache__/UnicycleModel.cpython-310.pyc -------------------------------------------------------------------------------- /model/__pycache__/UnicycleModel.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/model/__pycache__/UnicycleModel.cpython-38.pyc -------------------------------------------------------------------------------- /model/__pycache__/UnicycleModel.cpython-39.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/model/__pycache__/UnicycleModel.cpython-39.pyc -------------------------------------------------------------------------------- /model/__pycache__/model.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/model/__pycache__/model.cpython-310.pyc -------------------------------------------------------------------------------- /model/__pycache__/model.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/model/__pycache__/model.cpython-38.pyc -------------------------------------------------------------------------------- /model/__pycache__/model.cpython-39.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/model/__pycache__/model.cpython-39.pyc -------------------------------------------------------------------------------- /notebooks/(need_to_update)test_RocketLanding3D.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": [ 9 | "import matplotlib.pyplot as plt\n", 10 | "%matplotlib inline\n", 11 | "# %matplotlib qt\n", 12 | "%load_ext autoreload\n", 13 | "%autoreload 2\n", 14 | "import numpy as np\n", 15 | "import time\n", 16 | "import random\n", 17 | "def print_np(x):\n", 18 | " print (\"Type is %s\" % (type(x)))\n", 19 | " print (\"Shape is %s\" % (x.shape,))\n", 20 | "# print (\"Values are: \\n%s\" % (x))" 21 | ] 22 | }, 23 | { 24 | "cell_type": "code", 25 | "execution_count": null, 26 | "metadata": {}, 27 | "outputs": [], 28 | "source": [ 29 | "import sys\n", 30 | "# sys.path.append('../')\n", 31 | "sys.path.append('../')\n", 32 | "sys.path.append('../model')\n", 33 | "sys.path.append('../cost')\n", 34 | "sys.path.append('../constraints')\n", 35 | "sys.path.append('../utils')\n", 36 | "import Landing3DModel\n", 37 | "import Landing3DCost\n", 38 | "import Landing3DConstraints\n", 39 | "from scipy.integrate import solve_ivp\n", 40 | "from Scvx import Scvx" 41 | ] 42 | }, 43 | { 44 | "cell_type": "code", 45 | "execution_count": null, 46 | "metadata": {}, 47 | "outputs": [], 48 | "source": [ 49 | "def euler_to_quaternion(attitude):\n", 50 | " roll, pitch, yaw = attitude[0],attitude[1],attitude[2]\n", 51 | " qx = np.sin(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) - np.cos(roll/2) * np.sin(pitch/2) * np.sin(yaw/2)\n", 52 | " qy = np.cos(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.cos(pitch/2) * np.sin(yaw/2)\n", 53 | " qz = np.cos(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) - np.sin(roll/2) * np.sin(pitch/2) * np.cos(yaw/2)\n", 54 | " qw = np.cos(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.sin(pitch/2) * np.sin(yaw/2)\n", 55 | " return [qw, qx, qy, qz]\n", 56 | "\n", 57 | "def quaternion_to_euler(w, x, y, z):\n", 58 | "\n", 59 | " t0 = +2.0 * (w * x + y * z)\n", 60 | " t1 = +1.0 - 2.0 * (x * x + y * y)\n", 61 | " roll_x = np.arctan2(t0, t1)\n", 62 | "\n", 63 | " t2 = +2.0 * (w * y - z * x)\n", 64 | " t2 = +1.0 if t2 > +1.0 else t2\n", 65 | " t2 = -1.0 if t2 < -1.0 else t2\n", 66 | " pitch_y = np.arcsin(t2)\n", 67 | "\n", 68 | " t3 = +2.0 * (w * z + x * y)\n", 69 | " t4 = +1.0 - 2.0 * (y * y + z * z)\n", 70 | " yaw_z = np.arctan2(t3, t4)\n", 71 | "\n", 72 | " return roll_x, pitch_y, yaw_z" 73 | ] 74 | }, 75 | { 76 | "cell_type": "code", 77 | "execution_count": null, 78 | "metadata": {}, 79 | "outputs": [], 80 | "source": [ 81 | "ix = 14\n", 82 | "iu = 3\n", 83 | "ih = 7\n", 84 | "tf = 5\n", 85 | "N = 30\n", 86 | "delT = tf/N\n", 87 | "max_iter = 15" 88 | ] 89 | }, 90 | { 91 | "cell_type": "code", 92 | "execution_count": null, 93 | "metadata": {}, 94 | "outputs": [], 95 | "source": [ 96 | "ei = np.array([np.deg2rad(15),np.deg2rad(0),0])\n", 97 | "qi = euler_to_quaternion(ei)\n", 98 | "xi = np.array([2, 3,0,3, 0.0,0.0,-1, qi[0],qi[1],qi[2],qi[3], 0,0,0])\n", 99 | "xf = np.array([1, 0,0,0, 0.0,0.0,-0.1, 1,0,0,0, 0,0,0])\n", 100 | "\n", 101 | "myModel = Landing3DModel.Landing3D('Hello',ix,iu,delT,linearization=\"analytic\")\n", 102 | "myCost = Landing3DCost.Landing3D('Hello',ix,iu,N)\n", 103 | "myConst = Landing3DConstraints.Landing3D('Hello',ix,iu)\n", 104 | "\n", 105 | "x0 = np.zeros((N+1,ix))\n", 106 | "for i in range(N+1) :\n", 107 | " x0[i,0:7] = (N-i)/N * xi[0:7] + i/N * xf[0:7]\n", 108 | " x0[i,7:11] = euler_to_quaternion( (N-i)/N * ei + i/N * np.zeros(3))\n", 109 | " x0[i,11:] = (N-i)/N * xi[11:] + i/N * xf[11:]\n", 110 | "\n", 111 | "u0 = np.zeros((N+1,iu))\n", 112 | "u0[:,2] = x0[:,0] * 1" 113 | ] 114 | }, 115 | { 116 | "cell_type": "code", 117 | "execution_count": null, 118 | "metadata": { 119 | "scrolled": false 120 | }, 121 | "outputs": [], 122 | "source": [ 123 | "i1 = Scvx('unicycle',N,max_iter,myModel,myCost,myConst,\n", 124 | " type_discretization='zoh',w_c=1,w_vc=1e4,w_tr=1e1,tol_vc=1e-4,flag_policyopt=False)\n", 125 | "x,u,xbar,ubar,total_num_iter,flag_boundary,l,l_vc,l_tr = i1.run(x0,u0,xi,xf)" 126 | ] 127 | }, 128 | { 129 | "cell_type": "code", 130 | "execution_count": null, 131 | "metadata": {}, 132 | "outputs": [], 133 | "source": [ 134 | "# xbar,ubar = x,u" 135 | ] 136 | }, 137 | { 138 | "cell_type": "code", 139 | "execution_count": null, 140 | "metadata": {}, 141 | "outputs": [], 142 | "source": [ 143 | "list_time = delT * np.array([i for i in range(N+1)])\n", 144 | "u_norm = np.linalg.norm(ubar,axis=1)\n", 145 | "x_norm_12 = np.linalg.norm(xbar[:,1:3],axis=1)\n", 146 | "glide_slope_angle = np.arctan(xbar[:,3]/x_norm_12)\n", 147 | "tilt_angle = np.arccos(1 - 2 * (xbar[:,8]**2+xbar[:,9]**2))\n", 148 | "gimbal_angle = np.arccos(ubar[:,2] / u_norm)" 149 | ] 150 | }, 151 | { 152 | "cell_type": "code", 153 | "execution_count": null, 154 | "metadata": {}, 155 | "outputs": [], 156 | "source": [ 157 | "from utils_plot import plot_rocket3d\n", 158 | "%matplotlib qt\n", 159 | "# %matplotlib inline\n", 160 | "fig = plt.figure(1,figsize=(15,15))\n", 161 | "plot_rocket3d(fig,xbar,ubar,x)" 162 | ] 163 | }, 164 | { 165 | "cell_type": "code", 166 | "execution_count": null, 167 | "metadata": { 168 | "scrolled": false 169 | }, 170 | "outputs": [], 171 | "source": [ 172 | "%matplotlib inline\n", 173 | "from utils_plot import make_rocket3d_trajectory_fig\n", 174 | "fig = plt.figure(1,figsize=(15,15))\n", 175 | "plot_rocket3d(fig,xbar,ubar,x)\n", 176 | "\n", 177 | "fS = 15\n", 178 | "plt.figure(figsize=(15,6))\n", 179 | "plt.subplot(311)\n", 180 | "plt.plot(list_time[:N],ubar[:N,0])\n", 181 | "plt.xlabel('time (s)',fontsize=fS)\n", 182 | "plt.ylim([-6,6])\n", 183 | "plt.title('T1',fontsize=fS)\n", 184 | "plt.subplot(312)\n", 185 | "plt.plot(list_time[:N],ubar[:N,1])\n", 186 | "plt.xlabel('time (s)',fontsize=fS)\n", 187 | "plt.ylim([-6,6])\n", 188 | "plt.title('T2',fontsize=fS)\n", 189 | "plt.subplot(313)\n", 190 | "plt.plot(list_time[:N],ubar[:N,2])\n", 191 | "plt.xlabel('time (s)',fontsize=fS)\n", 192 | "plt.ylim([-6,6])\n", 193 | "plt.title('T3',fontsize=fS)\n", 194 | "\n", 195 | "plt.figure(figsize=(15,3))\n", 196 | "plt.plot(list_time[:N],u_norm[:N])\n", 197 | "plt.plot(list_time[:N],list_time[:N]*0+myConst.T_max,'--',color='tab:orange')\n", 198 | "plt.plot(list_time[:N],list_time[:N]*0+myConst.T_min,'--',color='tab:orange')\n", 199 | "plt.xlabel('time (s)',fontsize=fS)\n", 200 | "plt.ylim([-1+myConst.T_min,myConst.T_max+1])\n", 201 | "plt.title('u_norm',fontsize=fS)\n", 202 | "\n", 203 | "plt.figure(figsize=(15,3))\n", 204 | "plt.plot(list_time[:N],np.rad2deg(gimbal_angle[:N]))\n", 205 | "plt.plot(list_time[:N],list_time[:N]*0+np.rad2deg(myConst.delta_max),'--',color='tab:orange')\n", 206 | "plt.xlabel('time (s)',fontsize=fS)\n", 207 | "plt.title('gimbal angle',fontsize=fS)\n", 208 | "plt.ylim([0,np.rad2deg(myConst.delta_max)+10])\n", 209 | "\n", 210 | "plt.figure(figsize=(15,3))\n", 211 | "plt.plot(list_time,xbar[:,0])\n", 212 | "plt.plot(list_time,list_time*0+2,'--',color='tab:orange')\n", 213 | "plt.plot(list_time,list_time*0+myConst.m_dry,'--',color='tab:orange')\n", 214 | "plt.xlabel('time (s)',fontsize=fS)\n", 215 | "plt.title('mass',fontsize=fS)\n", 216 | "plt.ylim([-0.5+myConst.m_dry,2+0.5])\n", 217 | "\n", 218 | "plt.figure(figsize=(15,3))\n", 219 | "plt.plot(list_time,np.rad2deg(tilt_angle))\n", 220 | "plt.plot(list_time,list_time*0+np.rad2deg(myConst.theta_max),'--',color='tab:orange')\n", 221 | "plt.xlabel('time (s)',fontsize=fS)\n", 222 | "plt.title('tilt angle',fontsize=fS)\n", 223 | "plt.ylim([0,np.rad2deg(myConst.theta_max)+10])\n", 224 | "\n", 225 | "plt.figure(figsize=(15,3))\n", 226 | "plt.plot(list_time,np.rad2deg(glide_slope_angle))\n", 227 | "plt.plot(list_time,list_time*0+np.rad2deg(myConst.gamma_gs),'--',color='tab:orange')\n", 228 | "plt.xlabel('time (s)',fontsize=fS)\n", 229 | "plt.title('glide slope angle',fontsize=fS)\n", 230 | "plt.ylim([0,100])\n" 231 | ] 232 | }, 233 | { 234 | "cell_type": "code", 235 | "execution_count": null, 236 | "metadata": {}, 237 | "outputs": [], 238 | "source": [ 239 | "# make_rocket3d_trajectory_fig(x,u,'Landing3D')" 240 | ] 241 | }, 242 | { 243 | "cell_type": "code", 244 | "execution_count": null, 245 | "metadata": {}, 246 | "outputs": [], 247 | "source": [] 248 | } 249 | ], 250 | "metadata": { 251 | "kernelspec": { 252 | "display_name": "Python 3", 253 | "language": "python", 254 | "name": "python3" 255 | }, 256 | "language_info": { 257 | "codemirror_mode": { 258 | "name": "ipython", 259 | "version": 3 260 | }, 261 | "file_extension": ".py", 262 | "mimetype": "text/x-python", 263 | "name": "python", 264 | "nbconvert_exporter": "python", 265 | "pygments_lexer": "ipython3", 266 | "version": "3.8.13" 267 | } 268 | }, 269 | "nbformat": 4, 270 | "nbformat_minor": 5 271 | } 272 | -------------------------------------------------------------------------------- /notebooks/.ipynb_checkpoints/test_unicycle-checkpoint.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "id": "b956317e", 7 | "metadata": {}, 8 | "outputs": [], 9 | "source": [ 10 | "import matplotlib.pyplot as plt\n", 11 | "%matplotlib inline \n", 12 | "%load_ext autoreload\n", 13 | "%autoreload 2\n", 14 | "import numpy as np\n", 15 | "import time\n", 16 | "import random\n", 17 | "def print_np(x):\n", 18 | " print (\"Type is %s\" % (type(x)))\n", 19 | " print (\"Shape is %s\" % (x.shape,))\n", 20 | "# print (\"Values are: \\n%s\" % (x))" 21 | ] 22 | }, 23 | { 24 | "cell_type": "code", 25 | "execution_count": null, 26 | "id": "6e840481", 27 | "metadata": {}, 28 | "outputs": [], 29 | "source": [ 30 | "import sys\n", 31 | "# sys.path.append('../')\n", 32 | "sys.path.append('../')\n", 33 | "sys.path.append('../model')\n", 34 | "sys.path.append('../cost')\n", 35 | "sys.path.append('../constraints')\n", 36 | "import UnicycleModel\n", 37 | "import UnicycleCost\n", 38 | "import UnicycleConstraints\n", 39 | "from scipy.integrate import solve_ivp\n", 40 | "from PTR import PTR" 41 | ] 42 | }, 43 | { 44 | "cell_type": "code", 45 | "execution_count": null, 46 | "id": "d2d75361", 47 | "metadata": {}, 48 | "outputs": [], 49 | "source": [ 50 | "ix = 3\n", 51 | "iu = 2\n", 52 | "ih = 2\n", 53 | "N = 30\n", 54 | "tf = 3\n", 55 | "delT = tf/N\n", 56 | "max_iter = 20" 57 | ] 58 | }, 59 | { 60 | "cell_type": "code", 61 | "execution_count": null, 62 | "id": "6405177b", 63 | "metadata": {}, 64 | "outputs": [], 65 | "source": [ 66 | "xi = np.zeros(3)\n", 67 | "xi[0] = -2.0\n", 68 | "xi[1] = -2.0 \n", 69 | "xi[2] = 0\n", 70 | "\n", 71 | "xf = np.zeros(3)\n", 72 | "xf[0] = 2.0\n", 73 | "xf[1] = 2.0\n", 74 | "xf[2] = 0\n", 75 | "\n", 76 | "myModel = UnicycleModel.unicycle('Hello',ix,iu,'numeric_central')\n", 77 | "myCost = UnicycleCost.unicycle('Hello',ix,iu,N)\n", 78 | "myConst = UnicycleConstraints.UnicycleConstraints('Hello',ix,iu)\n", 79 | "\n", 80 | "x0 = np.zeros((N+1,ix))\n", 81 | "for i in range(N+1) :\n", 82 | " x0[i] = (N-i)/N * xi + i/N * xf\n", 83 | "# u0 = np.random.rand(N,iu)\n", 84 | "u0 = np.zeros((N+1,iu))" 85 | ] 86 | }, 87 | { 88 | "cell_type": "code", 89 | "execution_count": null, 90 | "id": "e3f88e36", 91 | "metadata": { 92 | "scrolled": false 93 | }, 94 | "outputs": [], 95 | "source": [ 96 | "i1 = PTR('unicycle',N,tf,max_iter,myModel,myCost,myConst,type_discretization=\"zoh\",\n", 97 | " w_c=1,w_vc=1e3,w_tr=1e-1,w_rate=0,\n", 98 | " tol_vc=1e-6,tol_tr=1e-3)\n", 99 | "x,u,xbar,ubar,total_num_iter,flag_boundary,l,l_vc,l_tr,x_traj,u_traj,T_traj = i1.run(x0,u0,xi,xf)\n" 100 | ] 101 | }, 102 | { 103 | "cell_type": "code", 104 | "execution_count": null, 105 | "id": "241c2711", 106 | "metadata": {}, 107 | "outputs": [], 108 | "source": [ 109 | "t_index = np.array(range(N+1))*delT\n", 110 | "\n", 111 | "plt.figure(figsize=(10,10))\n", 112 | "fS = 18\n", 113 | "plt.subplot(221)\n", 114 | "plt.plot(x[:,0], x[:,1],'--', linewidth=2.0)\n", 115 | "plt.plot(xbar[:,0], xbar[:,1],'-', linewidth=2.0)\n", 116 | "plt.plot(xf[0],xf[1],\"o\",label='goal')\n", 117 | "plt.gca().set_aspect('equal', adjustable='box')\n", 118 | "plt.axis([-3, 3, -3, 3])\n", 119 | "plt.xlabel('X (m)', fontsize = fS)\n", 120 | "plt.ylabel('Y (m)', fontsize = fS)\n", 121 | "plt.subplot(222)\n", 122 | "plt.plot(t_index, xbar[:,0], linewidth=2.0,label='naive')\n", 123 | "plt.xlabel('time (s)', fontsize = fS)\n", 124 | "plt.ylabel('x1 (m)', fontsize = fS)\n", 125 | "plt.subplot(223)\n", 126 | "plt.plot(t_index, xbar[:,1], linewidth=2.0,label='naive')\n", 127 | "plt.xlabel('time (s)', fontsize = fS)\n", 128 | "plt.ylabel('x2 (m)', fontsize = fS)\n", 129 | "plt.subplot(224)\n", 130 | "plt.plot(t_index, xbar[:,2], linewidth=2.0,label='naive')\n", 131 | "plt.xlabel('time (s)', fontsize = fS)\n", 132 | "plt.ylabel('x3 (rad)', fontsize = fS)\n", 133 | "plt.legend(fontsize=fS)\n", 134 | "plt.show()\n", 135 | "\n", 136 | "plt.figure()\n", 137 | "plt.subplot(121)\n", 138 | "if i1.type_discretization == \"zoh\" :\n", 139 | " plt.step(t_index, [*ubar[:N,0],ubar[N-1,0]],alpha=1.0,where='post',linewidth=2.0)\n", 140 | "elif i1.type_discretization == \"foh\" :\n", 141 | " plt.plot(t_index, ubar[:,0], linewidth=2.0)\n", 142 | "plt.xlabel('time (s)', fontsize = fS)\n", 143 | "plt.ylabel('v (m/s)', fontsize = fS)\n", 144 | "plt.subplot(122)\n", 145 | "if i1.type_discretization == \"zoh\" :\n", 146 | " plt.step(t_index, [*ubar[:N,1],ubar[N-1,1]],alpha=1.0,where='post',linewidth=2.0)\n", 147 | "elif i1.type_discretization == \"foh\" :\n", 148 | " plt.plot(t_index, ubar[:,1], linewidth=2.0)\n", 149 | "plt.xlabel('time (s)', fontsize = fS)\n", 150 | "plt.ylabel('w (rad/s)', fontsize = fS)\n", 151 | "plt.show()" 152 | ] 153 | }, 154 | { 155 | "cell_type": "code", 156 | "execution_count": null, 157 | "id": "1f54c20b", 158 | "metadata": {}, 159 | "outputs": [], 160 | "source": [ 161 | "# from matplotlib.patches import Rectangle\n", 162 | "# import imageio\n", 163 | "# import os" 164 | ] 165 | }, 166 | { 167 | "cell_type": "code", 168 | "execution_count": null, 169 | "id": "3bf64bec", 170 | "metadata": {}, 171 | "outputs": [], 172 | "source": [ 173 | "# filenames = []\n", 174 | "# for i in range(N+1) :\n", 175 | "# fS = 18\n", 176 | "# fig = plt.figure(figsize=(10,10))\n", 177 | "# ax = fig.add_subplot(111)\n", 178 | "# plt.gca().set_aspect('equal', adjustable='box')\n", 179 | "# plt.plot(x[:i+1,0], x[:i+1,1], linewidth=2.0) \n", 180 | "# plt.plot(xf[0], xf[1],'*', linewidth=2.0)\n", 181 | "# plt.plot(x[i,0], x[i,1],'*', linewidth=2.0) \n", 182 | "# plt.plot(x[i,0], x[i,1], marker=(3, 0, x[i,2]*180/np.pi-90), markersize=20, linestyle='None')\n", 183 | "# # ax.add_patch(rec)\n", 184 | "# plt.axis([-3, 3, -3, 3])\n", 185 | "# plt.xlabel('X (m)', fontsize = fS)\n", 186 | "# plt.ylabel('Y (m)', fontsize = fS)\n", 187 | "\n", 188 | "# filename = '../images/{:d}.png'.format(i)\n", 189 | "# plt.savefig(filename)\n", 190 | "# filenames.append(filename)\n", 191 | "# plt.close()" 192 | ] 193 | }, 194 | { 195 | "cell_type": "code", 196 | "execution_count": null, 197 | "id": "d2eb995f", 198 | "metadata": {}, 199 | "outputs": [], 200 | "source": [ 201 | "# with imageio.get_writer('../images/unicycle.gif', mode='I') as writer:\n", 202 | "# for filename in filenames:\n", 203 | "# image = imageio.imread(filename)\n", 204 | "# writer.append_data(image)\n", 205 | "# for filename in set(filenames):\n", 206 | "# os.remove(filename)" 207 | ] 208 | }, 209 | { 210 | "cell_type": "code", 211 | "execution_count": null, 212 | "id": "0c67b533", 213 | "metadata": {}, 214 | "outputs": [], 215 | "source": [] 216 | } 217 | ], 218 | "metadata": { 219 | "kernelspec": { 220 | "display_name": "Python 3 (ipykernel)", 221 | "language": "python", 222 | "name": "python3" 223 | }, 224 | "language_info": { 225 | "codemirror_mode": { 226 | "name": "ipython", 227 | "version": 3 228 | }, 229 | "file_extension": ".py", 230 | "mimetype": "text/x-python", 231 | "name": "python", 232 | "nbconvert_exporter": "python", 233 | "pygments_lexer": "ipython3", 234 | "version": "3.9.13" 235 | } 236 | }, 237 | "nbformat": 4, 238 | "nbformat_minor": 5 239 | } 240 | -------------------------------------------------------------------------------- /notebooks/dynamics_sympy/.ipynb_checkpoints/air_density_model-checkpoint.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": 1, 6 | "id": "58ba6d67", 7 | "metadata": {}, 8 | "outputs": [], 9 | "source": [ 10 | "import matplotlib.pyplot as plt\n", 11 | "%matplotlib inline\n", 12 | "# %matplotlib qt\n", 13 | "%load_ext autoreload\n", 14 | "%autoreload 2\n", 15 | "import numpy as np\n", 16 | "import time\n", 17 | "import random\n", 18 | "def print_np(x):\n", 19 | " print (\"Type is %s\" % (type(x)))\n", 20 | " print (\"Shape is %s\" % (x.shape,))\n", 21 | "# print (\"Values are: \\n%s\" % (x))" 22 | ] 23 | }, 24 | { 25 | "cell_type": "code", 26 | "execution_count": null, 27 | "id": "43637dfe", 28 | "metadata": {}, 29 | "outputs": [], 30 | "source": [ 31 | "# flag_1 = rz < 11000 \n", 32 | "T1 = 15.04 - 0.00649 * rz # celsius\n", 33 | "p1 = 101.29 * np.power((T1+273.1)/288.08,5.256)\n", 34 | "rho1 = p1 / (0.2869 * (T1 + 273.1))" 35 | ] 36 | }, 37 | { 38 | "cell_type": "code", 39 | "execution_count": null, 40 | "id": "5eb66072", 41 | "metadata": {}, 42 | "outputs": [], 43 | "source": [] 44 | } 45 | ], 46 | "metadata": { 47 | "kernelspec": { 48 | "display_name": "Python 3 (ipykernel)", 49 | "language": "python", 50 | "name": "python3" 51 | }, 52 | "language_info": { 53 | "codemirror_mode": { 54 | "name": "ipython", 55 | "version": 3 56 | }, 57 | "file_extension": ".py", 58 | "mimetype": "text/x-python", 59 | "name": "python", 60 | "nbconvert_exporter": "python", 61 | "pygments_lexer": "ipython3", 62 | "version": "3.8.12" 63 | } 64 | }, 65 | "nbformat": 4, 66 | "nbformat_minor": 5 67 | } 68 | -------------------------------------------------------------------------------- /notebooks/test_DLTV/.ipynb_checkpoints/test_entry_linearization-checkpoint.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": 1, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": [ 9 | "import matplotlib.pyplot as plt\n", 10 | "%matplotlib inline\n", 11 | "# %matplotlib qt\n", 12 | "%load_ext autoreload\n", 13 | "%autoreload 2\n", 14 | "import numpy as np\n", 15 | "import time\n", 16 | "import random\n", 17 | "def print_np(x):\n", 18 | " print (\"Type is %s\" % (type(x)))\n", 19 | " print (\"Shape is %s\" % (x.shape,))\n", 20 | "# print (\"Values are: \\n%s\" % (x))\n", 21 | "from scipy.interpolate import interp1d" 22 | ] 23 | }, 24 | { 25 | "cell_type": "code", 26 | "execution_count": 2, 27 | "metadata": {}, 28 | "outputs": [], 29 | "source": [ 30 | "import sys\n", 31 | "# sys.path.append('../')\n", 32 | "sys.path.append('../')\n", 33 | "sys.path.append('../model')\n", 34 | "sys.path.append('../cost')\n", 35 | "sys.path.append('../constraints')\n", 36 | "sys.path.append('../utils')\n", 37 | "from scipy.integrate import solve_ivp\n", 38 | "from HypersonicEntry3DofPolar import Entry3dofSpherical" 39 | ] 40 | }, 41 | { 42 | "cell_type": "markdown", 43 | "metadata": {}, 44 | "source": [ 45 | "## Skye" 46 | ] 47 | }, 48 | { 49 | "cell_type": "code", 50 | "execution_count": 3, 51 | "metadata": {}, 52 | "outputs": [], 53 | "source": [ 54 | "tf = 1.04*1600\n", 55 | "N = 30\n", 56 | "delT = tf/N\n", 57 | "sigma_init = np.linspace(0,np.deg2rad(-10),N+1)\n", 58 | "ix = 6\n", 59 | "iu = 1" 60 | ] 61 | }, 62 | { 63 | "cell_type": "code", 64 | "execution_count": 4, 65 | "metadata": {}, 66 | "outputs": [], 67 | "source": [ 68 | "my_model = Entry3dofSpherical(\"hi\",ix,iu,linearization=\"analytic\")\n" 69 | ] 70 | }, 71 | { 72 | "cell_type": "code", 73 | "execution_count": 5, 74 | "metadata": {}, 75 | "outputs": [], 76 | "source": [ 77 | "Ts_init = tf / my_model.nt\n", 78 | "dts_init = delT/my_model.nt\n", 79 | "ts_init = np.linspace(0,Ts_init,N+1)" 80 | ] 81 | }, 82 | { 83 | "cell_type": "code", 84 | "execution_count": 6, 85 | "metadata": {}, 86 | "outputs": [], 87 | "source": [ 88 | "u_t = interp1d(ts_init,sigma_init)" 89 | ] 90 | }, 91 | { 92 | "cell_type": "code", 93 | "execution_count": 7, 94 | "metadata": {}, 95 | "outputs": [ 96 | { 97 | "name": "stdout", 98 | "output_type": "stream", 99 | "text": [ 100 | "[1.0156961230576047, 0, 0, 0.9423624367981697, -0.008726646259971648, 1.5707963267948966]\n" 101 | ] 102 | } 103 | ], 104 | "source": [ 105 | "h0 = 100*1e3\n", 106 | "r0_s = (h0+my_model.re)/my_model.nd; \n", 107 | "theta0_s = 0\n", 108 | "phi0_s = 0\n", 109 | "v0_s = 7450/my_model.nv\n", 110 | "gamma0 = np.deg2rad(-0.5)\n", 111 | "psi0 = np.deg2rad(90)\n", 112 | "\n", 113 | "x0_s = [r0_s,theta0_s,phi0_s,v0_s,gamma0,psi0]\n", 114 | "print(x0_s)" 115 | ] 116 | }, 117 | { 118 | "cell_type": "code", 119 | "execution_count": 8, 120 | "metadata": {}, 121 | "outputs": [], 122 | "source": [ 123 | "def dfdt(t,x,t0,u0) :\n", 124 | " u_t = interp1d(t0,u0)\n", 125 | " u = u_t(t)\n", 126 | " return np.squeeze(my_model.forward(x,u))" 127 | ] 128 | }, 129 | { 130 | "cell_type": "code", 131 | "execution_count": 9, 132 | "metadata": {}, 133 | "outputs": [], 134 | "source": [ 135 | "sol = solve_ivp(dfdt,(ts_init[0],ts_init[-1]),x0_s,args=(ts_init,sigma_init),t_eval=ts_init,rtol=1e-12,atol=1e-12)\n" 136 | ] 137 | }, 138 | { 139 | "cell_type": "code", 140 | "execution_count": 10, 141 | "metadata": {}, 142 | "outputs": [ 143 | { 144 | "data": { 145 | "text/plain": [ 146 | "array([ 1.00476425, 1.28343912, 0.03778173, 0.12249751, -0.08225446,\n", 147 | " 1.15150802])" 148 | ] 149 | }, 150 | "execution_count": 10, 151 | "metadata": {}, 152 | "output_type": "execute_result" 153 | } 154 | ], 155 | "source": [ 156 | "sol.y[:,-1]" 157 | ] 158 | }, 159 | { 160 | "cell_type": "code", 161 | "execution_count": 11, 162 | "metadata": {}, 163 | "outputs": [ 164 | { 165 | "data": { 166 | "text/plain": [ 167 | "array([ 0. , -0.00581776, -0.01163553, -0.01745329, -0.02327106,\n", 168 | " -0.02908882, -0.03490659, -0.04072435, -0.04654211, -0.05235988,\n", 169 | " -0.05817764, -0.06399541, -0.06981317, -0.07563093, -0.0814487 ,\n", 170 | " -0.08726646, -0.09308423, -0.09890199, -0.10471976, -0.11053752,\n", 171 | " -0.11635528, -0.12217305, -0.12799081, -0.13380858, -0.13962634,\n", 172 | " -0.1454441 , -0.15126187, -0.15707963, -0.1628974 , -0.16871516,\n", 173 | " -0.17453293])" 174 | ] 175 | }, 176 | "execution_count": 11, 177 | "metadata": {}, 178 | "output_type": "execute_result" 179 | } 180 | ], 181 | "source": [ 182 | "sigma_init" 183 | ] 184 | }, 185 | { 186 | "cell_type": "code", 187 | "execution_count": 12, 188 | "metadata": {}, 189 | "outputs": [ 190 | { 191 | "name": "stdout", 192 | "output_type": "stream", 193 | "text": [ 194 | "Type is \n", 195 | "Shape is (31, 6)\n", 196 | "Type is \n", 197 | "Shape is (31, 1)\n" 198 | ] 199 | } 200 | ], 201 | "source": [ 202 | "x = sol.y.T\n", 203 | "u = np.expand_dims(sigma_init,1)\n", 204 | "print_np(x)\n", 205 | "print_np(u)" 206 | ] 207 | }, 208 | { 209 | "cell_type": "code", 210 | "execution_count": 15, 211 | "metadata": {}, 212 | "outputs": [ 213 | { 214 | "data": { 215 | "text/plain": [ 216 | "array([ 1.00476425, 1.28343912, 0.03778173, 0.12249751, -0.08225446,\n", 217 | " 1.15150802])" 218 | ] 219 | }, 220 | "execution_count": 15, 221 | "metadata": {}, 222 | "output_type": "execute_result" 223 | } 224 | ], 225 | "source": [ 226 | "x[-1,:]" 227 | ] 228 | }, 229 | { 230 | "cell_type": "code", 231 | "execution_count": 14, 232 | "metadata": {}, 233 | "outputs": [], 234 | "source": [ 235 | "A,Bm,Bp,s,z,x_prop_n = my_model.diff_discrete_foh(x[0:N,:],u,dts_init,Ts_init)" 236 | ] 237 | }, 238 | { 239 | "cell_type": "code", 240 | "execution_count": 16, 241 | "metadata": {}, 242 | "outputs": [ 243 | { 244 | "name": "stdout", 245 | "output_type": "stream", 246 | "text": [ 247 | "Type is \n", 248 | "Shape is (30, 6, 6)\n" 249 | ] 250 | } 251 | ], 252 | "source": [ 253 | "print_np(A)" 254 | ] 255 | }, 256 | { 257 | "cell_type": "code", 258 | "execution_count": null, 259 | "metadata": {}, 260 | "outputs": [], 261 | "source": [] 262 | }, 263 | { 264 | "cell_type": "code", 265 | "execution_count": null, 266 | "metadata": {}, 267 | "outputs": [], 268 | "source": [] 269 | }, 270 | { 271 | "cell_type": "markdown", 272 | "metadata": {}, 273 | "source": [ 274 | "## Behcet" 275 | ] 276 | }, 277 | { 278 | "cell_type": "code", 279 | "execution_count": null, 280 | "metadata": {}, 281 | "outputs": [], 282 | "source": [ 283 | "h0 = 100*1e3\n", 284 | "r0_s = (h0+my_model.re)/my_model.nd; \n", 285 | "theta0_s = 0\n", 286 | "phi0_s = 0\n", 287 | "v0_s = 7450/my_model.nv\n", 288 | "gamma0 = np.deg2rad(-0.5)\n", 289 | "psi0 = 0\n", 290 | "\n", 291 | "x0_s = [r0_s,theta0_s,phi0_s,v0_s,gamma0,psi0]\n", 292 | "print(x0_s)" 293 | ] 294 | }, 295 | { 296 | "cell_type": "code", 297 | "execution_count": null, 298 | "metadata": {}, 299 | "outputs": [], 300 | "source": [ 301 | "def dfdt(t,x,t0,u0) :\n", 302 | " u_t = interp1d(t0,u0)\n", 303 | " u = u_t(t)\n", 304 | " return np.squeeze(my_model.forward(x,u))" 305 | ] 306 | }, 307 | { 308 | "cell_type": "code", 309 | "execution_count": null, 310 | "metadata": {}, 311 | "outputs": [], 312 | "source": [ 313 | "sol = solve_ivp(dfdt,(ts_init[0],ts_init[-1]),x0_s,\n", 314 | " args=([ts_init[0],ts_init[-1]],[sigma_init[0],sigma_init[1]]),\n", 315 | " t_eval=ts_init,rtol=1e-12,atol=1e-12)\n" 316 | ] 317 | }, 318 | { 319 | "cell_type": "code", 320 | "execution_count": null, 321 | "metadata": {}, 322 | "outputs": [], 323 | "source": [ 324 | "x = sol.y.T\n", 325 | "u = np.expand_dims(sigma_init,1)" 326 | ] 327 | }, 328 | { 329 | "cell_type": "code", 330 | "execution_count": null, 331 | "metadata": {}, 332 | "outputs": [], 333 | "source": [ 334 | "ts_init" 335 | ] 336 | }, 337 | { 338 | "cell_type": "code", 339 | "execution_count": null, 340 | "metadata": {}, 341 | "outputs": [], 342 | "source": [ 343 | "Ts_init" 344 | ] 345 | }, 346 | { 347 | "cell_type": "code", 348 | "execution_count": null, 349 | "metadata": {}, 350 | "outputs": [], 351 | "source": [ 352 | "dts_init * np.ones(N)" 353 | ] 354 | }, 355 | { 356 | "cell_type": "code", 357 | "execution_count": null, 358 | "metadata": {}, 359 | "outputs": [], 360 | "source": [ 361 | "A,Bm,Bp,s,z,x_prop_n = my_model.diff_discrete_foh(x[0:N,:],u,dts_init,Ts_init)\n", 362 | "# A,Bm,Bp,s,z,x_prop_n = my_model.diff_discrete_foh_var_vectorized(x[0:N,:],u,dts_init * np.ones(N))" 363 | ] 364 | }, 365 | { 366 | "cell_type": "code", 367 | "execution_count": null, 368 | "metadata": {}, 369 | "outputs": [], 370 | "source": [ 371 | "A[-1,:,:]" 372 | ] 373 | }, 374 | { 375 | "cell_type": "code", 376 | "execution_count": null, 377 | "metadata": {}, 378 | "outputs": [], 379 | "source": [ 380 | "x_prop_n[-1]" 381 | ] 382 | }, 383 | { 384 | "cell_type": "code", 385 | "execution_count": null, 386 | "metadata": {}, 387 | "outputs": [], 388 | "source": [ 389 | "x[1]" 390 | ] 391 | }, 392 | { 393 | "cell_type": "code", 394 | "execution_count": null, 395 | "metadata": {}, 396 | "outputs": [], 397 | "source": [ 398 | "u[1]" 399 | ] 400 | }, 401 | { 402 | "cell_type": "code", 403 | "execution_count": null, 404 | "metadata": {}, 405 | "outputs": [], 406 | "source": [ 407 | "A,B = my_model.diff_numeric_central(x[1],u[1])" 408 | ] 409 | }, 410 | { 411 | "cell_type": "code", 412 | "execution_count": null, 413 | "metadata": {}, 414 | "outputs": [], 415 | "source": [ 416 | "A[:,:,3]" 417 | ] 418 | }, 419 | { 420 | "cell_type": "code", 421 | "execution_count": null, 422 | "metadata": {}, 423 | "outputs": [], 424 | "source": [ 425 | "x[24]" 426 | ] 427 | }, 428 | { 429 | "cell_type": "code", 430 | "execution_count": null, 431 | "metadata": {}, 432 | "outputs": [], 433 | "source": [ 434 | "u[24]" 435 | ] 436 | }, 437 | { 438 | "cell_type": "code", 439 | "execution_count": null, 440 | "metadata": {}, 441 | "outputs": [], 442 | "source": [ 443 | "A,B = my_model.diff(x[24],u[24])" 444 | ] 445 | }, 446 | { 447 | "cell_type": "code", 448 | "execution_count": null, 449 | "metadata": {}, 450 | "outputs": [], 451 | "source": [ 452 | "A" 453 | ] 454 | }, 455 | { 456 | "cell_type": "code", 457 | "execution_count": null, 458 | "metadata": {}, 459 | "outputs": [], 460 | "source": [] 461 | } 462 | ], 463 | "metadata": { 464 | "kernelspec": { 465 | "display_name": "Python 3", 466 | "language": "python", 467 | "name": "python3" 468 | }, 469 | "language_info": { 470 | "codemirror_mode": { 471 | "name": "ipython", 472 | "version": 3 473 | }, 474 | "file_extension": ".py", 475 | "mimetype": "text/x-python", 476 | "name": "python", 477 | "nbconvert_exporter": "python", 478 | "pygments_lexer": "ipython3", 479 | "version": "3.8.13" 480 | } 481 | }, 482 | "nbformat": 4, 483 | "nbformat_minor": 5 484 | } 485 | -------------------------------------------------------------------------------- /notebooks/test_DLTV/test_entry_linearization.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": 1, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": [ 9 | "import matplotlib.pyplot as plt\n", 10 | "%matplotlib inline\n", 11 | "# %matplotlib qt\n", 12 | "%load_ext autoreload\n", 13 | "%autoreload 2\n", 14 | "import numpy as np\n", 15 | "import time\n", 16 | "import random\n", 17 | "def print_np(x):\n", 18 | " print (\"Type is %s\" % (type(x)))\n", 19 | " print (\"Shape is %s\" % (x.shape,))\n", 20 | "# print (\"Values are: \\n%s\" % (x))\n", 21 | "from scipy.interpolate import interp1d" 22 | ] 23 | }, 24 | { 25 | "cell_type": "code", 26 | "execution_count": 2, 27 | "metadata": {}, 28 | "outputs": [], 29 | "source": [ 30 | "import sys\n", 31 | "# sys.path.append('../')\n", 32 | "sys.path.append('../')\n", 33 | "sys.path.append('../model')\n", 34 | "sys.path.append('../cost')\n", 35 | "sys.path.append('../constraints')\n", 36 | "sys.path.append('../utils')\n", 37 | "from scipy.integrate import solve_ivp\n", 38 | "from HypersonicEntry3DofPolar import Entry3dofSpherical" 39 | ] 40 | }, 41 | { 42 | "cell_type": "markdown", 43 | "metadata": {}, 44 | "source": [ 45 | "## Skye" 46 | ] 47 | }, 48 | { 49 | "cell_type": "code", 50 | "execution_count": 3, 51 | "metadata": {}, 52 | "outputs": [], 53 | "source": [ 54 | "tf = 1.04*1600\n", 55 | "N = 30\n", 56 | "delT = tf/N\n", 57 | "sigma_init = np.linspace(0,np.deg2rad(-10),N+1)\n", 58 | "ix = 6\n", 59 | "iu = 1" 60 | ] 61 | }, 62 | { 63 | "cell_type": "code", 64 | "execution_count": 4, 65 | "metadata": {}, 66 | "outputs": [], 67 | "source": [ 68 | "my_model = Entry3dofSpherical(\"hi\",ix,iu,linearization=\"analytic\")\n" 69 | ] 70 | }, 71 | { 72 | "cell_type": "code", 73 | "execution_count": 5, 74 | "metadata": {}, 75 | "outputs": [], 76 | "source": [ 77 | "Ts_init = tf / my_model.nt\n", 78 | "dts_init = delT/my_model.nt\n", 79 | "ts_init = np.linspace(0,Ts_init,N+1)" 80 | ] 81 | }, 82 | { 83 | "cell_type": "code", 84 | "execution_count": 6, 85 | "metadata": {}, 86 | "outputs": [], 87 | "source": [ 88 | "u_t = interp1d(ts_init,sigma_init)" 89 | ] 90 | }, 91 | { 92 | "cell_type": "code", 93 | "execution_count": 7, 94 | "metadata": {}, 95 | "outputs": [ 96 | { 97 | "name": "stdout", 98 | "output_type": "stream", 99 | "text": [ 100 | "[1.0156961230576047, 0, 0, 0.9423624367981697, -0.008726646259971648, 1.5707963267948966]\n" 101 | ] 102 | } 103 | ], 104 | "source": [ 105 | "h0 = 100*1e3\n", 106 | "r0_s = (h0+my_model.re)/my_model.nd; \n", 107 | "theta0_s = 0\n", 108 | "phi0_s = 0\n", 109 | "v0_s = 7450/my_model.nv\n", 110 | "gamma0 = np.deg2rad(-0.5)\n", 111 | "psi0 = np.deg2rad(90)\n", 112 | "\n", 113 | "x0_s = [r0_s,theta0_s,phi0_s,v0_s,gamma0,psi0]\n", 114 | "print(x0_s)" 115 | ] 116 | }, 117 | { 118 | "cell_type": "code", 119 | "execution_count": 8, 120 | "metadata": {}, 121 | "outputs": [], 122 | "source": [ 123 | "def dfdt(t,x,t0,u0) :\n", 124 | " u_t = interp1d(t0,u0)\n", 125 | " u = u_t(t)\n", 126 | " return np.squeeze(my_model.forward(x,u))" 127 | ] 128 | }, 129 | { 130 | "cell_type": "code", 131 | "execution_count": 9, 132 | "metadata": {}, 133 | "outputs": [], 134 | "source": [ 135 | "sol = solve_ivp(dfdt,(ts_init[0],ts_init[-1]),x0_s,args=(ts_init,sigma_init),t_eval=ts_init,rtol=1e-12,atol=1e-12)\n" 136 | ] 137 | }, 138 | { 139 | "cell_type": "code", 140 | "execution_count": 10, 141 | "metadata": {}, 142 | "outputs": [ 143 | { 144 | "data": { 145 | "text/plain": [ 146 | "array([ 1.00476425, 1.28343912, 0.03778173, 0.12249751, -0.08225446,\n", 147 | " 1.15150802])" 148 | ] 149 | }, 150 | "execution_count": 10, 151 | "metadata": {}, 152 | "output_type": "execute_result" 153 | } 154 | ], 155 | "source": [ 156 | "sol.y[:,-1]" 157 | ] 158 | }, 159 | { 160 | "cell_type": "code", 161 | "execution_count": 11, 162 | "metadata": {}, 163 | "outputs": [ 164 | { 165 | "data": { 166 | "text/plain": [ 167 | "array([ 0. , -0.00581776, -0.01163553, -0.01745329, -0.02327106,\n", 168 | " -0.02908882, -0.03490659, -0.04072435, -0.04654211, -0.05235988,\n", 169 | " -0.05817764, -0.06399541, -0.06981317, -0.07563093, -0.0814487 ,\n", 170 | " -0.08726646, -0.09308423, -0.09890199, -0.10471976, -0.11053752,\n", 171 | " -0.11635528, -0.12217305, -0.12799081, -0.13380858, -0.13962634,\n", 172 | " -0.1454441 , -0.15126187, -0.15707963, -0.1628974 , -0.16871516,\n", 173 | " -0.17453293])" 174 | ] 175 | }, 176 | "execution_count": 11, 177 | "metadata": {}, 178 | "output_type": "execute_result" 179 | } 180 | ], 181 | "source": [ 182 | "sigma_init" 183 | ] 184 | }, 185 | { 186 | "cell_type": "code", 187 | "execution_count": 12, 188 | "metadata": {}, 189 | "outputs": [ 190 | { 191 | "name": "stdout", 192 | "output_type": "stream", 193 | "text": [ 194 | "Type is \n", 195 | "Shape is (31, 6)\n", 196 | "Type is \n", 197 | "Shape is (31, 1)\n" 198 | ] 199 | } 200 | ], 201 | "source": [ 202 | "x = sol.y.T\n", 203 | "u = np.expand_dims(sigma_init,1)\n", 204 | "print_np(x)\n", 205 | "print_np(u)" 206 | ] 207 | }, 208 | { 209 | "cell_type": "code", 210 | "execution_count": 15, 211 | "metadata": {}, 212 | "outputs": [ 213 | { 214 | "data": { 215 | "text/plain": [ 216 | "array([ 1.00476425, 1.28343912, 0.03778173, 0.12249751, -0.08225446,\n", 217 | " 1.15150802])" 218 | ] 219 | }, 220 | "execution_count": 15, 221 | "metadata": {}, 222 | "output_type": "execute_result" 223 | } 224 | ], 225 | "source": [ 226 | "x[-1,:]" 227 | ] 228 | }, 229 | { 230 | "cell_type": "code", 231 | "execution_count": 14, 232 | "metadata": {}, 233 | "outputs": [], 234 | "source": [ 235 | "A,Bm,Bp,s,z,x_prop_n = my_model.diff_discrete_foh(x[0:N,:],u,dts_init,Ts_init)" 236 | ] 237 | }, 238 | { 239 | "cell_type": "code", 240 | "execution_count": 16, 241 | "metadata": {}, 242 | "outputs": [ 243 | { 244 | "name": "stdout", 245 | "output_type": "stream", 246 | "text": [ 247 | "Type is \n", 248 | "Shape is (30, 6, 6)\n" 249 | ] 250 | } 251 | ], 252 | "source": [ 253 | "print_np(A)" 254 | ] 255 | }, 256 | { 257 | "cell_type": "code", 258 | "execution_count": null, 259 | "metadata": {}, 260 | "outputs": [], 261 | "source": [] 262 | }, 263 | { 264 | "cell_type": "code", 265 | "execution_count": null, 266 | "metadata": {}, 267 | "outputs": [], 268 | "source": [] 269 | }, 270 | { 271 | "cell_type": "markdown", 272 | "metadata": {}, 273 | "source": [ 274 | "## Behcet" 275 | ] 276 | }, 277 | { 278 | "cell_type": "code", 279 | "execution_count": null, 280 | "metadata": {}, 281 | "outputs": [], 282 | "source": [ 283 | "h0 = 100*1e3\n", 284 | "r0_s = (h0+my_model.re)/my_model.nd; \n", 285 | "theta0_s = 0\n", 286 | "phi0_s = 0\n", 287 | "v0_s = 7450/my_model.nv\n", 288 | "gamma0 = np.deg2rad(-0.5)\n", 289 | "psi0 = 0\n", 290 | "\n", 291 | "x0_s = [r0_s,theta0_s,phi0_s,v0_s,gamma0,psi0]\n", 292 | "print(x0_s)" 293 | ] 294 | }, 295 | { 296 | "cell_type": "code", 297 | "execution_count": null, 298 | "metadata": {}, 299 | "outputs": [], 300 | "source": [ 301 | "def dfdt(t,x,t0,u0) :\n", 302 | " u_t = interp1d(t0,u0)\n", 303 | " u = u_t(t)\n", 304 | " return np.squeeze(my_model.forward(x,u))" 305 | ] 306 | }, 307 | { 308 | "cell_type": "code", 309 | "execution_count": null, 310 | "metadata": {}, 311 | "outputs": [], 312 | "source": [ 313 | "sol = solve_ivp(dfdt,(ts_init[0],ts_init[-1]),x0_s,\n", 314 | " args=([ts_init[0],ts_init[-1]],[sigma_init[0],sigma_init[1]]),\n", 315 | " t_eval=ts_init,rtol=1e-12,atol=1e-12)\n" 316 | ] 317 | }, 318 | { 319 | "cell_type": "code", 320 | "execution_count": null, 321 | "metadata": {}, 322 | "outputs": [], 323 | "source": [ 324 | "x = sol.y.T\n", 325 | "u = np.expand_dims(sigma_init,1)" 326 | ] 327 | }, 328 | { 329 | "cell_type": "code", 330 | "execution_count": null, 331 | "metadata": {}, 332 | "outputs": [], 333 | "source": [ 334 | "ts_init" 335 | ] 336 | }, 337 | { 338 | "cell_type": "code", 339 | "execution_count": null, 340 | "metadata": {}, 341 | "outputs": [], 342 | "source": [ 343 | "Ts_init" 344 | ] 345 | }, 346 | { 347 | "cell_type": "code", 348 | "execution_count": null, 349 | "metadata": {}, 350 | "outputs": [], 351 | "source": [ 352 | "dts_init * np.ones(N)" 353 | ] 354 | }, 355 | { 356 | "cell_type": "code", 357 | "execution_count": null, 358 | "metadata": {}, 359 | "outputs": [], 360 | "source": [ 361 | "A,Bm,Bp,s,z,x_prop_n = my_model.diff_discrete_foh(x[0:N,:],u,dts_init,Ts_init)\n", 362 | "# A,Bm,Bp,s,z,x_prop_n = my_model.diff_discrete_foh_var_vectorized(x[0:N,:],u,dts_init * np.ones(N))" 363 | ] 364 | }, 365 | { 366 | "cell_type": "code", 367 | "execution_count": null, 368 | "metadata": {}, 369 | "outputs": [], 370 | "source": [ 371 | "A[-1,:,:]" 372 | ] 373 | }, 374 | { 375 | "cell_type": "code", 376 | "execution_count": null, 377 | "metadata": {}, 378 | "outputs": [], 379 | "source": [ 380 | "x_prop_n[-1]" 381 | ] 382 | }, 383 | { 384 | "cell_type": "code", 385 | "execution_count": null, 386 | "metadata": {}, 387 | "outputs": [], 388 | "source": [ 389 | "x[1]" 390 | ] 391 | }, 392 | { 393 | "cell_type": "code", 394 | "execution_count": null, 395 | "metadata": {}, 396 | "outputs": [], 397 | "source": [ 398 | "u[1]" 399 | ] 400 | }, 401 | { 402 | "cell_type": "code", 403 | "execution_count": null, 404 | "metadata": {}, 405 | "outputs": [], 406 | "source": [ 407 | "A,B = my_model.diff_numeric_central(x[1],u[1])" 408 | ] 409 | }, 410 | { 411 | "cell_type": "code", 412 | "execution_count": null, 413 | "metadata": {}, 414 | "outputs": [], 415 | "source": [ 416 | "A[:,:,3]" 417 | ] 418 | }, 419 | { 420 | "cell_type": "code", 421 | "execution_count": null, 422 | "metadata": {}, 423 | "outputs": [], 424 | "source": [ 425 | "x[24]" 426 | ] 427 | }, 428 | { 429 | "cell_type": "code", 430 | "execution_count": null, 431 | "metadata": {}, 432 | "outputs": [], 433 | "source": [ 434 | "u[24]" 435 | ] 436 | }, 437 | { 438 | "cell_type": "code", 439 | "execution_count": null, 440 | "metadata": {}, 441 | "outputs": [], 442 | "source": [ 443 | "A,B = my_model.diff(x[24],u[24])" 444 | ] 445 | }, 446 | { 447 | "cell_type": "code", 448 | "execution_count": null, 449 | "metadata": {}, 450 | "outputs": [], 451 | "source": [ 452 | "A" 453 | ] 454 | }, 455 | { 456 | "cell_type": "code", 457 | "execution_count": null, 458 | "metadata": {}, 459 | "outputs": [], 460 | "source": [] 461 | } 462 | ], 463 | "metadata": { 464 | "kernelspec": { 465 | "display_name": "Python 3", 466 | "language": "python", 467 | "name": "python3" 468 | }, 469 | "language_info": { 470 | "codemirror_mode": { 471 | "name": "ipython", 472 | "version": 3 473 | }, 474 | "file_extension": ".py", 475 | "mimetype": "text/x-python", 476 | "name": "python", 477 | "nbconvert_exporter": "python", 478 | "pygments_lexer": "ipython3", 479 | "version": "3.8.13" 480 | } 481 | }, 482 | "nbformat": 4, 483 | "nbformat_minor": 5 484 | } 485 | -------------------------------------------------------------------------------- /sensor/QuadRotorSensor.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | import time 4 | import random 5 | def print_np(x): 6 | print ("Type is %s" % (type(x))) 7 | print ("Shape is %s" % (x.shape,)) 8 | # print ("Values are: \n%s" % (x)) 9 | from sensor import Sensor 10 | import time 11 | 12 | 13 | class vicon(Sensor) : 14 | def __init__(self,name,ix,iu) : 15 | super().__init__(name,ix,iu) 16 | self.max_num_obstacle = 8 17 | 18 | def state2obs(self,x,c,H) : 19 | xdim = np.ndim(x) 20 | if xdim == 1: # 1 step state & input 21 | N = 1 22 | x = np.expand_dims(x,axis=0) 23 | else : 24 | N = np.size(x,axis = 0) 25 | 26 | length_list = np.zeros((N,self.max_num_obstacle*3)) 27 | 28 | num_obstacle = len(c) 29 | vicon_data = 100*np.ones((self.max_num_obstacle,3)) 30 | for i in range(num_obstacle) : 31 | rx,ry = c[i][0],c[i][1] 32 | r = 1/H[i][0,0] 33 | vicon_data[i,:] = np.array([rx,ry,r]) 34 | ascending_index = np.argsort(vicon_data[:,1]) # ascending order in ry 35 | vicon_data = vicon_data[ascending_index] 36 | num_not_obstacle = self.max_num_obstacle - num_obstacle 37 | 38 | for i in range(N) : 39 | each_vicon_data = np.copy(vicon_data) 40 | obstacle_vicon_data = each_vicon_data[:-num_not_obstacle] 41 | obstacle_vicon_data[:,0] -= x[i][0] 42 | obstacle_vicon_data[:,1] -= x[i][1] 43 | theta = np.arctan2(obstacle_vicon_data[:,1],obstacle_vicon_data[:,0]) 44 | ascending_index = np.argsort(theta) 45 | 46 | each_vicon_data[:-num_not_obstacle] = obstacle_vicon_data[ascending_index] 47 | # np.random.shuffle(each_vicon_data) 48 | length_list[i] = each_vicon_data.reshape(-1) 49 | 50 | observ = {} 51 | observ['length'] = np.array(length_list).squeeze() 52 | observ['point'] = None 53 | return observ 54 | 55 | class lidar(Sensor) : 56 | def __init__(self,name,ix,iu) : 57 | super().__init__(name,ix,iu) 58 | self.dtheta = np.pi/64 59 | self.num_sensor = int(np.pi/self.dtheta+1) 60 | self.theta_sensor = [-i*self.dtheta for i in range(self.num_sensor)] 61 | self.N_theta = len(self.theta_sensor) 62 | self.N_lidar = 200 63 | self.length_lidar = 3 64 | self.d_lidar = self.length_lidar / self.N_lidar 65 | 66 | def check_obstacle(self,xt,yt,c,H) : 67 | x = np.array([xt,yt,0]) 68 | for c1,H1 in zip(c,H) : 69 | if 1-np.linalg.norm(H1@(x-c1)) >= 0 : 70 | return True 71 | return False 72 | 73 | def check_obstacle_new(self,p_mat,c,H) : 74 | N_data,_ = np.shape(p_mat) 75 | p_mat = np.expand_dims(p_mat,2) 76 | flag_obstacle = np.zeros(N_data) # if obstacle, True 77 | for c1,H1 in zip(c,H) : 78 | H_mat = np.repeat(np.expand_dims(H1,0),N_data,0) 79 | c_mat = np.expand_dims(np.repeat(np.expand_dims(c1,0),N_data,0),2) 80 | flag_obstacle_e = 1 - np.linalg.norm(np.squeeze(H_mat@(p_mat-c_mat)),axis=1) >=0 81 | flag_obstacle = np.logical_or(flag_obstacle,flag_obstacle_e) 82 | return flag_obstacle 83 | 84 | def state2obs(self,x,c,H,method=2) : 85 | 86 | xdim = np.ndim(x) 87 | if xdim == 1: # 1 step state & input 88 | N = 1 89 | x = np.expand_dims(x,axis=0) 90 | else : 91 | N = np.size(x,axis = 0) 92 | 93 | observ = {} 94 | length_list = [] 95 | point_list = [] 96 | 97 | # start = time.time() 98 | if method == 3 : 99 | point_mat = np.zeros((N,self.N_lidar,self.N_theta,3)) 100 | for idx,rx in enumerate(x) : 101 | for idx_point in range(1,self.N_lidar+1) : 102 | r = round(idx_point * self.d_lidar,4) 103 | point_mat[idx,idx_point-1,:,0] = rx[0] + r * np.cos(self.theta_sensor) 104 | point_mat[idx,idx_point-1,:,1] = rx[1] + r * np.sin(self.theta_sensor) 105 | # for idx_point in range(1,self.N_lidar+1) : 106 | # r = round(idx_point * self.d_lidar,4) 107 | # point_mat[:,idx_point-1,:,0] = x[:,0] + r * np.cos(self.theta_sensor) 108 | # point_mat[:,idx_point-1,:,1] = x[:,1] + r * np.sin(self.theta_sensor) 109 | 110 | v,h,d,r = np.shape(point_mat) 111 | point_mat = np.reshape(point_mat,(v*h*d,r)) 112 | flag_obstacle = self.check_obstacle_new(point_mat,c,H) 113 | point_mat = np.reshape(point_mat,(v,h,d,r)) 114 | flag_obstacle = np.reshape(flag_obstacle,(v,h,d,1)) 115 | 116 | # obs = self.d_lidar * np.ones((N,len(self.theta_sensor))) 117 | # flag_not_meet_obstacle = np.ones((N,len(self.theta_sensor))) 118 | # for idx_point in range(1,self.N_lidar+1) : 119 | # # r = round(idx_point * self.d_lidar,4) 120 | # flag_not_obs = np.logical_not(flag_obstacle[:,idx_point-1,:,0]) 121 | # flag_not_meet_obstacle = np.logical_and(flag_not_meet_obstacle,flag_not_obs) 122 | # obs += flag_not_meet_obstacle * self.d_lidar 123 | # obs[obs > self.length_lidar] = self.length_lidar 124 | # point = None 125 | # length_list.append(obs) 126 | # point_list.append(point) 127 | 128 | for idx,rx in enumerate(x) : 129 | obs = [] 130 | point = [] 131 | for idx_sensor,theta in enumerate(self.theta_sensor) : 132 | for idx_point in range(1,self.N_lidar+1) : 133 | r = round(idx_point * self.d_lidar,4) 134 | xt = point_mat[idx,idx_point-1,idx_sensor,0] 135 | yt = point_mat[idx,idx_point-1,idx_sensor,1] 136 | if c is None : 137 | flag_obs = False 138 | else : 139 | flag_obs = flag_obstacle[idx,idx_point-1,idx_sensor,0] 140 | if flag_obs == True : 141 | break 142 | obs.append(r) 143 | point.append([xt,yt]) 144 | length_list.append(obs) 145 | point_list.append(point) 146 | 147 | if method == 1 or method == 2 : 148 | for rx in x : 149 | obs = [] 150 | point = [] 151 | # method 2 152 | if method == 2 : 153 | point_mat = np.zeros((self.N_lidar,self.N_theta,3)) 154 | for idx_point in range(1,self.N_lidar+1) : 155 | r = round(idx_point * self.d_lidar,4) 156 | point_mat[idx_point-1,:,0] = rx[0] + r * np.cos(self.theta_sensor) 157 | point_mat[idx_point-1,:,1] = rx[1] + r * np.sin(self.theta_sensor) 158 | 159 | v,h,d = np.shape(point_mat) 160 | point_mat = np.reshape(point_mat,(v*h,d)) 161 | flag_obstacle = self.check_obstacle_new(point_mat,c,H) 162 | point_mat = np.reshape(point_mat,(v,h,d)) 163 | flag_obstacle = np.reshape(flag_obstacle,(v,h,1)) 164 | 165 | obs = self.d_lidar * np.ones(len(self.theta_sensor)) 166 | point = np.zeros((len(self.theta_sensor),2)) 167 | flag_not_meet_obstacle = np.ones(len(self.theta_sensor)) 168 | for idx_point in range(1,self.N_lidar+1) : 169 | # r = round(idx_point * self.d_lidar,4) 170 | point[:,0][flag_not_meet_obstacle==True] = point_mat[idx_point-1,:,0][flag_not_meet_obstacle==True] 171 | point[:,1][flag_not_meet_obstacle==True] = point_mat[idx_point-1,:,1][flag_not_meet_obstacle==True] 172 | 173 | flag_not_obs = np.logical_not(flag_obstacle[idx_point-1,:,0]) 174 | flag_not_meet_obstacle = np.logical_and(flag_not_meet_obstacle,flag_not_obs) 175 | obs += flag_not_meet_obstacle * self.d_lidar 176 | 177 | obs[obs > self.length_lidar] = self.length_lidar 178 | # point = None 179 | 180 | # for idx_sensor,theta in enumerate(self.theta_sensor) : 181 | # for idx_point in range(1,self.N_lidar+1) : 182 | # # r = round(idx_point * self.d_lidar,4) 183 | # r = idx_point * self.d_lidar 184 | # xt = point_mat[idx_point-1,idx_sensor,0] 185 | # yt = point_mat[idx_point-1,idx_sensor,1] 186 | # if c is None : 187 | # flag_obs = False 188 | # else : 189 | # flag_obs = flag_obstacle[idx_point-1,idx_sensor,0] 190 | # if flag_obs == True : 191 | # break 192 | # obs.append(r) 193 | # point.append([xt,yt]) 194 | 195 | length_list.append(obs) 196 | point_list.append(point) 197 | 198 | # method 1 199 | if method == 1 : 200 | for idx_sensor,theta in enumerate(self.theta_sensor) : 201 | for idx_point in range(1,self.N_lidar+1) : 202 | r = round(idx_point * self.d_lidar,4) 203 | xt = rx[0] + r * np.cos(theta) 204 | yt = rx[1] + r * np.sin(theta) 205 | if c is None : 206 | flag_obs = False 207 | else : 208 | flag_obs = self.check_obstacle(xt,yt,c,H) 209 | if flag_obs == True : 210 | break 211 | obs.append(r) 212 | point.append([xt,yt]) 213 | 214 | length_list.append(obs) 215 | point_list.append(point) 216 | 217 | observ['length'] = np.array(length_list).squeeze() 218 | observ['point'] = np.array(point_list).squeeze() 219 | # end = time.time() 220 | # print(end - start) 221 | 222 | return observ 223 | 224 | 225 | 226 | 227 | 228 | -------------------------------------------------------------------------------- /sensor/__pycache__/QuadRotorPointMassObs.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/sensor/__pycache__/QuadRotorPointMassObs.cpython-38.pyc -------------------------------------------------------------------------------- /sensor/__pycache__/QuadRotorSensor.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/sensor/__pycache__/QuadRotorSensor.cpython-38.pyc -------------------------------------------------------------------------------- /sensor/__pycache__/observation.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/sensor/__pycache__/observation.cpython-38.pyc -------------------------------------------------------------------------------- /sensor/__pycache__/sensor.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/sensor/__pycache__/sensor.cpython-38.pyc -------------------------------------------------------------------------------- /sensor/__pycache__/utils_sensor.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/sensor/__pycache__/utils_sensor.cpython-38.pyc -------------------------------------------------------------------------------- /sensor/sensor.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | import time 4 | import random 5 | def print_np(x): 6 | print ("Type is %s" % (type(x))) 7 | print ("Shape is %s" % (x.shape,)) 8 | # print ("Values are: \n%s" % (x)) 9 | 10 | class Sensor(object) : 11 | def __init__(self,name,ix,iu) : 12 | self.name = name 13 | self.ix = ix 14 | self.iu = iu 15 | 16 | def forward(self,x,u,idx=None,discrete=True): 17 | print("this is in parent class") 18 | pass 19 | 20 | def diff(self) : 21 | print("this is in parent class") 22 | pass -------------------------------------------------------------------------------- /utils/__pycache__/utils_obs.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/utils/__pycache__/utils_obs.cpython-38.pyc -------------------------------------------------------------------------------- /utils/__pycache__/utils_plot.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taewankim1/sequential_convex_programming/77bc7078e1f2afb4740e460e1f08a25a5256d9bf/utils/__pycache__/utils_plot.cpython-38.pyc -------------------------------------------------------------------------------- /utils/utils_obs.py: -------------------------------------------------------------------------------- 1 | import imageio 2 | import os 3 | import matplotlib.pyplot as plt 4 | import numpy as np 5 | import time 6 | import random 7 | import math 8 | def print_np(x): 9 | print ("Type is %s" % (type(x))) 10 | print ("Shape is %s" % (x.shape,)) 11 | # print ("Values are: \n%s" % (x)) 12 | def get_H_obs(r) : 13 | return np.diag([1/r,1/r,0]) 14 | def get_H_safe(r,r_safe) : 15 | return np.diag([1/(r+r_safe),1/(r+r_safe),0]) 16 | def generate_obstacle(point_range,dice=-1,r_safe=0.3) : 17 | # dice = np.random.randint(low=1, high=4) 18 | c_list = [] 19 | H_obs_list = [] 20 | H_safe_list = [] 21 | r_list = [] 22 | if dice != 1 : 23 | c = np.array([1+np.random.uniform(-point_range,point_range),4+np.random.uniform(0,1),0]) 24 | r = np.random.uniform(0.2,0.5) 25 | c_list.append(c) 26 | H_obs_list.append(get_H_obs(r)) 27 | H_safe_list.append(get_H_safe(r,r_safe)) 28 | r_list.append(r) 29 | if dice != 2 : 30 | c = np.array([1+np.random.uniform(-point_range,point_range),2+np.random.uniform(-1,0),0]) 31 | r = np.random.uniform(0.2,0.5) 32 | c_list.append(c) 33 | H_obs_list.append(get_H_obs(r)) 34 | H_safe_list.append(get_H_safe(r,r_safe)) 35 | r_list.append(r) 36 | if dice != 3 : 37 | c = np.array([-1+np.random.uniform(-point_range,point_range),4+np.random.uniform(0,1),0]) 38 | r = np.random.uniform(0.2,0.5) 39 | c_list.append(c) 40 | H_obs_list.append(get_H_obs(r)) 41 | H_safe_list.append(get_H_safe(r,r_safe)) 42 | r_list.append(r) 43 | if dice != 4 : 44 | c = np.array([-1+np.random.uniform(-point_range,point_range),2+np.random.uniform(-1,0),0]) 45 | r = np.random.uniform(0.2,0.5) 46 | c_list.append(c) 47 | H_obs_list.append(get_H_obs(r)) 48 | H_safe_list.append(get_H_safe(r,r_safe)) 49 | r_list.append(r) 50 | assert len(c_list) == len(H_obs_list) 51 | assert len(c_list) == len(H_safe_list) 52 | num_obstacle = len(c) 53 | return c_list,H_obs_list,H_safe_list,r_list 54 | 55 | def generate_obstacle_circle(r_safe=0.3) : 56 | # dice = np.random.randint(low=1, high=4) 57 | c_list = [] 58 | H_obs_list = [] 59 | H_safe_list = [] 60 | r_list = [] 61 | min_theta = np.arctan(0.8) 62 | angle = np.random.uniform(0,2*np.pi) 63 | angle += np.random.uniform(min_theta,2/3*np.pi-min_theta) 64 | length = np.random.uniform(1,2) 65 | c = np.array([length*np.cos(angle),length*np.sin(angle)+3,0]) 66 | r = np.random.uniform(0.2,0.5) 67 | c_list.append(c) 68 | H_obs_list.append(get_H_obs(r)) 69 | H_safe_list.append(get_H_safe(r,r_safe)) 70 | r_list.append(r) 71 | 72 | for i in range(2) : 73 | angle += min_theta 74 | angle += np.random.uniform(min_theta,2/3*np.pi-min_theta) 75 | length = np.random.uniform(1,2) 76 | c = np.array([length*np.cos(angle),length*np.sin(angle)+3,0]) 77 | r = np.random.uniform(0.2,0.5) 78 | c_list.append(c) 79 | H_obs_list.append(get_H_obs(r)) 80 | H_safe_list.append(get_H_safe(r,r_safe)) 81 | r_list.append(r) 82 | 83 | assert len(c_list) == len(H_obs_list) 84 | assert len(c_list) == len(H_safe_list) 85 | num_obstacle = len(c) 86 | return c_list,H_obs_list,H_safe_list,r_list 87 | 88 | def generate_obstacle_massive(r_safe=0.3) : 89 | c_list = [] 90 | H_obs_list = [] 91 | H_safe_list = [] 92 | r_list = [] 93 | 94 | y_start = 1.75 95 | for j in range(3) : 96 | x_start = np.random.uniform(-2,-2+0.5) 97 | for i in range(3) : 98 | r = np.random.uniform(0.2,0.3) 99 | c = np.array([x_start,y_start+np.random.uniform(-0.1,0.1),0]) 100 | c_list.append(c) 101 | H_obs_list.append(get_H_obs(r)) 102 | H_safe_list.append(get_H_safe(r,r_safe)) 103 | r_list.append(r) 104 | x_start += np.random.uniform(r+0.1+0.3+0.6,r+0.1+0.3+1.1) 105 | y_start += np.random.uniform(r+0.3+0.3+0.6,r+0.3+0.3+1.1) 106 | assert len(c_list) == len(H_obs_list) 107 | assert len(c_list) == len(H_safe_list) 108 | num_obstacle = len(c) 109 | return c_list,H_obs_list,H_safe_list,r_list 110 | def generate_obstacle_random(r_safe=0.3,num_obs=None) : 111 | def euclidean_distance(x1, y1, x2, y2): 112 | return math.hypot((x1 - x2), (y1 - y2)) 113 | run = True 114 | circle_list = [] 115 | max_iter = 5000 116 | if num_obs is None : 117 | num_obstacle = np.random.randint(4,8) 118 | # num_obstacle = np.random.randint(4,6) 119 | else : 120 | num_obstacle = num_obs 121 | for i in range(max_iter) : 122 | if i == max_iter -1 : 123 | print("reach to the max iter") 124 | if len(circle_list) == num_obstacle : 125 | break 126 | # r = np.random.uniform(0.5,0.6) 127 | # r = np.random.uniform(0.4,0.6) 128 | r = np.random.uniform(0.5,1.0) 129 | x = np.random.uniform(-1.0,1.0) 130 | y = np.random.uniform(0.7,5.3) 131 | if not any((x2, y2, r2) for x2, y2, r2 in circle_list if euclidean_distance(x, y, x2, y2) < r + r2): 132 | circle_list.append((x, y, r)) 133 | c_list = [] 134 | H_obs_list = [] 135 | H_safe_list = [] 136 | r_list = [] 137 | for circle in circle_list : 138 | x,y,r = circle[0],circle[1],circle[2] 139 | r -= r_safe 140 | c = np.array([x,y,0]) 141 | c_list.append(c) 142 | H_obs_list.append(get_H_obs(r)) 143 | H_safe_list.append(get_H_safe(r,r_safe)) 144 | r_list.append(r) 145 | assert len(c_list) == len(H_obs_list) 146 | assert len(c_list) == len(H_safe_list) 147 | num_obstacle = len(c) 148 | return c_list,H_obs_list,H_safe_list,r_list -------------------------------------------------------------------------------- /utils/utils_plot.py: -------------------------------------------------------------------------------- 1 | import imageio 2 | import os 3 | from mpl_toolkits.mplot3d import art3d 4 | 5 | import matplotlib.pyplot as plt 6 | import numpy as np 7 | import time 8 | import random 9 | def print_np(x): 10 | print ("Type is %s" % (type(x))) 11 | print ("Shape is %s" % (x.shape,)) 12 | # print ("Values are: \n%s" % (x)) 13 | 14 | # vector scaling 15 | thrust_scale = 0.1 16 | attitude_scale = 0.3 17 | 18 | def make_rocket3d_trajectory_fig(x,u,img_name='untitled') : 19 | filenames = [] 20 | N = np.shape(x)[0] 21 | for k in range(N): 22 | 23 | fS = 18 24 | fig = plt.figure(figsize=(10,10)) 25 | ax = fig.add_subplot(111, projection='3d') 26 | ax.set_xlabel('X, east') 27 | ax.set_ylabel('Y, north') 28 | ax.set_zlabel('Z, up') 29 | rx, ry, rz = x[k,1:4] 30 | qw, qx, qy, qz = x[k,7:11] 31 | 32 | CBI = np.array([ 33 | [1 - 2 * (qy ** 2 + qz ** 2), 2 * (qx * qy + qw * qz), 2 * (qx * qz - qw * qy)], 34 | [2 * (qx * qy - qw * qz), 1 - 2 * (qx ** 2 + qz ** 2), 2 * (qy * qz + qw * qx)], 35 | [2 * (qx * qz + qw * qy), 2 * (qy * qz - qw * qx), 1 - 2 * (qx ** 2 + qy ** 2)] 36 | ]) 37 | 38 | dx, dy, dz = np.dot(np.transpose(CBI), np.array([0., 0., 1.])) 39 | if k != N-1 : 40 | Fx, Fy, Fz = np.dot(np.transpose(CBI), u[k, :]) 41 | 42 | # attitude vector 43 | ax.quiver(rx, ry, rz, dx, dy, dz, length=attitude_scale, arrow_length_ratio=0.0, color='blue') 44 | 45 | # thrust vector 46 | ax.quiver(rx, ry, rz, -Fx, -Fy, -Fz, length=thrust_scale, arrow_length_ratio=0.0, color='red') 47 | scale = x[0, 3] 48 | ax.auto_scale_xyz([-scale / 2, scale / 2], [-scale / 2, scale / 2], [0, scale]) 49 | 50 | pad = plt.Circle((0, 0), 0.2, color='lightgrey') 51 | ax.add_patch(pad) 52 | art3d.pathpatch_2d_to_3d(pad) 53 | 54 | ax.plot(x[:, 1], x[:, 2], x[:, 3]) 55 | 56 | filename = '../images/{:d}.png'.format(k) 57 | plt.savefig(filename) 58 | filenames.append(filename) 59 | plt.close() 60 | 61 | with imageio.get_writer('../images/'+img_name+'.gif', mode='I') as writer: 62 | for filename in filenames: 63 | image = imageio.imread(filename) 64 | writer.append_data(image) 65 | for filename in set(filenames): 66 | os.remove(filename) 67 | 68 | def plot_rocket3d(fig, x, u, xppg=None): 69 | ax = fig.add_subplot(111, projection='3d') 70 | 71 | N = np.shape(x)[0] 72 | 73 | ax.set_xlabel('X, east') 74 | ax.set_ylabel('Y, north') 75 | ax.set_zlabel('Z, up') 76 | 77 | for k in range(N): 78 | rx, ry, rz = x[k,1:4] 79 | qw, qx, qy, qz = x[k,7:11] 80 | 81 | CBI = np.array([ 82 | [1 - 2 * (qy ** 2 + qz ** 2), 2 * (qx * qy + qw * qz), 2 * (qx * qz - qw * qy)], 83 | [2 * (qx * qy - qw * qz), 1 - 2 * (qx ** 2 + qz ** 2), 2 * (qy * qz + qw * qx)], 84 | [2 * (qx * qz + qw * qy), 2 * (qy * qz - qw * qx), 1 - 2 * (qx ** 2 + qy ** 2)] 85 | ]) 86 | 87 | dx, dy, dz = np.dot(np.transpose(CBI), np.array([0., 0., 1.])) 88 | if k != N-1 : 89 | Fx, Fy, Fz = np.dot(np.transpose(CBI), u[k, :]) 90 | 91 | # attitude vector 92 | ax.quiver(rx, ry, rz, dx, dy, dz, length=attitude_scale, arrow_length_ratio=0.0, color='blue') 93 | 94 | # thrust vector 95 | ax.quiver(rx, ry, rz, -Fx, -Fy, -Fz, length=thrust_scale, arrow_length_ratio=0.0, color='red') 96 | 97 | scale = x[0, 3] 98 | ax.auto_scale_xyz([-scale / 2, scale / 2], [-scale / 2, scale / 2], [0, scale]) 99 | 100 | # pad = plt.Circle((0, 0), 0.2, color='lightgrey') 101 | # ax.add_patch(pad) 102 | # art3d.pathpatch_2d_to_3d(pad) 103 | 104 | ax.plot(x[:, 1], x[:, 2], x[:, 3]) 105 | if xppg is not None : 106 | ax.plot(xppg[:, 1], xppg[:, 2], xppg[:, 3],'--') 107 | # ax.set_aspect('equal') 108 | 109 | def make_rocket2d_trajectory_fig(x,u,img_name) : 110 | N = np.shape(x)[0] -1 111 | Fx = +np.sin(x[:,4] + u[:,0]) * u[:,1] 112 | Fy = -np.cos(x[:,4] + u[:,0]) * u[:,1] 113 | filenames = [] 114 | for i in range(N+10) : 115 | fS = 18 116 | plt.figure(figsize=(10,10)) 117 | plt.gca().set_aspect('equal', adjustable='box') 118 | if i <= N : 119 | index = i 120 | else : 121 | index = N 122 | plt.plot(x[:i+1,0], x[:i+1,1], linewidth=2.0) 123 | plt.plot(0, 0,'*', linewidth=2.0) 124 | plt.quiver(x[index,0], x[index,1], -np.sin(x[index,4]), np.cos(x[index,4]), color='blue', width=0.003, scale=15, headwidth=1, headlength=0) 125 | if i < N : 126 | plt.quiver(x[index,0], x[index,1], Fx[index], Fy[index], color='red', width=0.003, scale=100, headwidth=1, headlength=0) 127 | plt.axis([-2, 5, -1, 5]) 128 | plt.xlabel('X ()', fontsize = fS) 129 | plt.ylabel('Y ()', fontsize = fS) 130 | filename = '../images/{:d}.png'.format(i) 131 | plt.savefig(filename) 132 | filenames.append(filename) 133 | plt.close() 134 | 135 | with imageio.get_writer('../images/'+img_name+'.gif', mode='I') as writer: 136 | for filename in filenames: 137 | image = imageio.imread(filename) 138 | writer.append_data(image) 139 | for filename in set(filenames): 140 | os.remove(filename) 141 | 142 | def plot_Landing2D_trajectory (x,u,xppg=None,delT=0.1) : 143 | N = np.shape(x)[0]-1 144 | fS = 18 145 | Fx = +np.sin(x[:,4] + u[:,0]) * u[:,1] 146 | Fy = -np.cos(x[:,4] + u[:,0]) * u[:,1] 147 | plt.figure(1,figsize=(10,10)) 148 | plt.plot(x[:,0], x[:,1], linewidth=2.0) 149 | if xppg is not None : 150 | plt.plot(xppg[:,0], xppg[:,1], '--',linewidth=2.0) 151 | plt.plot(0,0,'o') 152 | plt.gca().set_aspect('equal', adjustable='box') 153 | index = np.linspace(0,N-1,30) 154 | index = [int(i) for i in index] 155 | plt.quiver(x[index,0], x[index,1], -np.sin(x[index,4]), np.cos(x[index,4]), color='blue', width=0.003, scale=15, headwidth=1, headlength=0) 156 | plt.quiver(x[index,0], x[index,1], Fx[index], Fy[index], color='red', width=0.003, scale=100, headwidth=1, headlength=0) 157 | plt.quiver(x[N,0], x[N,1], -np.sin(x[N,4]), np.cos(x[N,4]), color='blue', width=0.003, scale=15, headwidth=1, headlength=0) 158 | plt.axis([-5, 5, -1, 7]) 159 | plt.xlabel('X ()', fontsize = fS) 160 | plt.ylabel('Y ()', fontsize = fS) 161 | 162 | plt.figure(2,figsize=(10,15)) 163 | plt.subplot(321) 164 | plt.plot(np.array(range(N+1))*delT, x[:,0], linewidth=2.0,label='naive') 165 | plt.xlabel('time (s)', fontsize = fS) 166 | plt.ylabel('rx ()', fontsize = fS) 167 | plt.subplot(322) 168 | plt.plot(np.array(range(N+1))*delT, x[:,1], linewidth=2.0,label='naive') 169 | plt.xlabel('time (s)', fontsize = fS) 170 | plt.ylabel('ry ()', fontsize = fS) 171 | plt.subplot(323) 172 | plt.plot(np.array(range(N+1))*delT, x[:,2], linewidth=2.0,label='naive') 173 | plt.xlabel('time (s)', fontsize = fS) 174 | plt.ylabel('vx ()', fontsize = fS) 175 | plt.subplot(324) 176 | plt.plot(np.array(range(N+1))*delT, x[:,3], linewidth=2.0,label='naive') 177 | plt.xlabel('time (s)', fontsize = fS) 178 | plt.ylabel('vy ()', fontsize = fS) 179 | plt.legend(fontsize=fS) 180 | plt.subplot(325) 181 | plt.plot(np.array(range(N+1))*delT, x[:,4]*180/np.pi, linewidth=2.0,label='naive') 182 | plt.xlabel('time (s)', fontsize = fS) 183 | plt.ylabel('theta (degree)', fontsize = fS) 184 | plt.subplot(326) 185 | plt.plot(np.array(range(N+1))*delT, x[:,5]*180/np.pi, linewidth=2.0,label='naive') 186 | plt.xlabel('time (s)', fontsize = fS) 187 | plt.ylabel('theta dot (rad/s)', fontsize = fS) 188 | plt.legend(fontsize=fS) 189 | plt.show() 190 | 191 | plt.figure(3) 192 | plt.subplot(121) 193 | plt.plot(np.array(range(N))*delT, u[:N,0]*180/np.pi, linewidth=2.0) 194 | plt.xlabel('time (s)', fontsize = fS) 195 | plt.ylabel('gimbal (degree)', fontsize = fS) 196 | plt.subplot(122) 197 | plt.plot(np.array(range(N))*delT, u[:N,1], linewidth=2.0) 198 | plt.xlabel('time (s)', fontsize = fS) 199 | plt.ylabel('thrust ()', fontsize = fS) 200 | plt.show() 201 | 202 | def make_quadrotor_trajectory_fig(x,obs,c,H,r,img_name='quadrotor') : 203 | filenames = [] 204 | N = np.shape(x)[0] 205 | for k in range(N): 206 | xp = x[k] 207 | lp = obs['point'][k] 208 | plt.figure(figsize=(5,8)) 209 | fS = 18 210 | ax=plt.gca() 211 | for ce,He,re in zip(c,H,r) : 212 | circle1 = plt.Circle((ce[0],ce[1]),re,color='tab:red',alpha=0.5,fill=True) 213 | ax.add_patch(circle1) 214 | for ro in lp : 215 | plt.plot([xp[0],ro[0]],[xp[1],ro[1]],color='tab:blue') 216 | plt.plot(ro[0],ro[1],'o',color='tab:blue') 217 | plt.plot(xp[0],xp[1],'o',color='black') 218 | plt.axis([-2.5, 2.5, -1, 7]) 219 | plt.gca().set_aspect('equal', adjustable='box') 220 | 221 | filename = '../images/{:d}.png'.format(k) 222 | plt.savefig(filename) 223 | filenames.append(filename) 224 | plt.close() 225 | 226 | with imageio.get_writer('../images/'+img_name+'.gif', mode='I') as writer: 227 | for filename in filenames: 228 | image = imageio.imread(filename) 229 | writer.append_data(image) 230 | for filename in set(filenames): 231 | os.remove(filename) --------------------------------------------------------------------------------