def solve_path(vessel_profile, vessel_state, vessel_final_state): global solved_path, n_i print('----------vessel_profile(original)------------') for k in (vessel_profile.__dict__): print('-- %s: \n%s' % (k, vessel_profile.__dict__[k])) print('----------vessel_state(original)------------') for k in (vessel_state.__dict__): print('-- %s: \n%s' % (k, vessel_state.__dict__[k])) print('----------vessel_final_state(original)------------') for k in (vessel_final_state.__dict__): print('-- %s: \n%s' % (k, vessel_final_state.__dict__[k])) solver_options = SC_params.SolverOptions() solver_options.w_delta = lambda i:(1e-3 * (2 ** i)) #solver_options.w_nu = 1e5 print('---------solving----------') solved_path = solver.solve(vessel_profile, vessel_state, vessel_final_state, solver_options=solver_options, use_c=True, verbose=True) if solved_path != None: (x, u, tf) = solved_path qw, qx, qy, qz = x[7:11, :] x[7:11, :] = vec(qx, qy, qz, qw) # wxyz转xyzw n_i = -100 solved_path = (x, u, tf) # print('x slice') # print(x[:, 0:3]) # print('u slice') # print(u[:, 0:3]) print('---------solve done----------') if debug_lines: update_lines(x, u) else: print('---------solve error----------')
def get_vessel_profile(vessel): p = SC_params.VesselProfile() p.isp = vessel.specific_impulse p.g = vec(-g0, 0., 0.) # gravity p.m_dry = vessel.dry_mass p.gamma_gs = np.deg2rad(params['gamma_gs']) # glide slope p.theta_max = np.linspace(np.deg2rad(params['max_tilt']), np.deg2rad(10), SC_params.SuperParams().K) # tilt p.omega_max = np.deg2rad(params['max_omega']) # rotation vel p.delta_max = np.deg2rad(params['max_delta']) # gimbal p.T_min = vessel.available_thrust * throttle_limit[0] p.T_max = vessel.available_thrust * throttle_limit[1] p.r_T_B = vec(engine_y, 0., 0.) # thrust offset p.J_B_I = np.array(vessel.inertia_tensor).reshape((3, 3)) p.airfric_k = params['airfric_k'] p.time_guess = params['tf_guess'] return p
def get_final_state(vessel, final_height): optimal_acc = vessel.available_thrust / vessel.mass * params['final_throttle'] - g0 final_vel = math.sqrt(2 * optimal_acc * final_height) state = SC_params.VesselState() state.mass = vessel.mass state.pos = vec(final_height, 0, 0) state.vel = vec(-final_vel, 0, 0) state.rotation = vec(1, 0, 0, 0) state.omega = vec(0, 0, 0) return state
def predict_vessel_state(vessel, est_height): # ref_target flight vel = vec(vessel.velocity(ref_target)) pos = vec(vessel.position(ref_target)) est_t = (pos[0] - est_height) / (-vel[0]) #匀速 est_pos = pos + est_t * vel hdg_right = (flight.heading + 90) * deg2rad #右侧指向 rot_axis = v3(0, math.cos(hdg_right), math.sin(hdg_right)) rot_quat = quat(rot_axis, 90 * deg2rad) #qx, qy, qz, qw = vessel.rotation(ref_target) #xyzw转wxyz qx, qy, qz, qw = rot_quat #xyzw转wxyz state = SC_params.VesselState() state.mass = vessel.mass state.pos = est_pos state.vel = vel state.rotation = vec(qw, qx, qy, qz) #state.rotation = vec(1, 0, 0, 0) state.omega = vec(0, 0, 0) return state
def solve(params, params_super=None): #super params if (params_super == None): params_super = SC_params.SuperParams() # default K = params_super.K res = sc_subproblem_solver.cg_solve( A=params.A.reshape(K * 14, 14), B=params.B.reshape(K * 14, 3), C=params.C.reshape(K * 14, 3), S=params.S.reshape(K * 14, 1), z=params.z.reshape(K * 14, 1), x_last=params.x_last, u_last=params.u_last, u_last_dir=params.u_last_dir, s_last=wrap(params.s_last), w_nu=wrap(params.w_nu), w_delta=wrap(params.w_delta), w_delta_s=wrap(params.w_delta_s), x_initial=params.x_initial, x_final=params.x_final, #sparse m_dry=wrap(params.m_dry), tan_gamma_gs=wrap(params.tan_gamma_gs), cos_theta_max=(np.array([params.cos_theta_max] * K) if type(params.cos_theta_max) != np.ndarray else params.cos_theta_max).reshape(1, K), omega_max=wrap(params.omega_max), cos_delta_max=wrap(params.cos_delta_max), T_max=wrap(params.T_max), T_min=wrap(params.T_min)) return (res, np.array(res[0]['x']), np.array(res[0]['u']), res[0]['s'][0, 0], np.array(res[0]['nu']), np.array(res[0]['delta']), res[0]['delta_s']) #tuple(结果,状态,控制,时间scale)
# time init game_delta_time = 0.02 game_prev_time = space_center.ut start_time = time.time() # references print('creating target frame...') ref_local = vessel.reference_frame ref_surface = vessel.surface_reference_frame #地面参考系 原点为载具质心 ref_body = body.reference_frame #地固系 ref_target_temp = space_center.ReferenceFrame.create_relative(ref_body, position=target_body_pos) #地固系原点平移到目标位置 ref_target = space_center.ReferenceFrame.create_hybrid(ref_target_temp, rotation=ref_surface, velocity=ref_target_temp) #混合坐标系 原点在目标处,旋转同地面系(Up-North-East) prev_vel = vec(vessel.velocity(ref_surface)) K = SC_params.SuperParams().K solved_path = None n_i = -1 error = vec(vessel.position(ref_target)) #print('current error: %s' % error) debug_lines = params['debug_lines'] if debug_lines: print('debug lines...') lines = [conn.drawing.add_line((0,0,0),(0,0,0), ref_target) for i in range(K-1)] #轨迹线 directions = [conn.drawing.add_line((0,0,0), (1,0,0), ref_target) for i in range(K)] #机头指向 thrustvecs = [conn.drawing.add_line((0,0,0), (1,0,0), ref_target) for i in range(K)] #推力指向 target_line = conn.drawing.add_line((0,0,0),(1,0,0),ref_target) #轨迹采样标记 target_line.color = (0,0,1) target2_line = conn.drawing.add_line((0,0,0),(1,0,0),ref_target) #提前采样 target2_line.color = (0,0,1) head_line = conn.drawing.add_line((0,0,0),(1,0,0),ref_target) #目标姿态指向
import numpy as np import SC_solver, SC_params import GFOLD_solver, GFOLD_params import trajectory.plot as plot import pickle N=GFOLD_params.SuperParams().N K=SC_params.SuperParams().K vessel = SC_params.VesselProfile.get_default() state_initial = SC_params.VesselState.get_default_initial() state_final = SC_params.VesselState.get_default_final() #vessel.T_min = 1.0 #vessel.T_max = 5.0 #state_initial.pos = np.array([20., 5., 3.]) #state_initial.vel = np.array([-4., -0., -3.]) thrust = 24000 vessel.isp = 203.94 vessel.g = np.array((-3.71, 0., 0.)) # gravity vessel.m_dry = 2.0e3 vessel.gamma_gs = np.deg2rad(30) # glide slope vessel.theta_max = np.deg2rad(60) # tilt vessel.theta_max = np.linspace(vessel.theta_max, np.deg2rad(10), K) vessel.omega_max = np.deg2rad(60) # rotation vel
def solve(vessel_profile, vessel_initial, vessel_final, solver_options=None, params_super=None, use_c=False, verbose=False): # default params if (params_super == None): params_super = SC_params.SuperParams() if (solver_options == None): solver_options = SC_params.SolverOptions() #normalize Ut = vessel_profile.time_guess / 5. Ul = vessel_initial.pos[0] / 20. Um = vessel_initial.mass / 1.5 vessel_profile = SC_params.normalize(vessel_profile, Ut, Ul, Um) vessel_initial = SC_params.normalize(vessel_initial, Ut, Ul, Um) vessel_final = SC_params.normalize(vessel_final, Ut, Ul, Um) # solver if (use_c): import SC_subproblem_gen as solver else: import SC_subproblem as solver import GFOLD_solver, GFOLD_params # super params K = params_super.K dt = 1 / (K - 1) iterations = solver_options.iterations if (verbose): print('K=%s, iterations=%s' % (K, iterations)) # 参数的转换 将飞行器参数计算转化后填入求解器参数 params = SC_params.Params(K) #sparse params.m_dry = vessel_profile.m_dry params.tan_gamma_gs = math.tan(vessel_profile.gamma_gs) params.cos_theta_max = np.cos(vessel_profile.theta_max) params.omega_max = vessel_profile.omega_max params.cos_delta_max = math.cos(vessel_profile.delta_max) params.T_min = vessel_profile.T_min params.T_max = vessel_profile.T_max #initial params.x_initial[0:1, 0] = vessel_initial.mass params.x_initial[1:4, 0] = vessel_initial.pos params.x_initial[4:7, 0] = vessel_initial.vel params.x_initial[7:11, 0] = vessel_initial.rotation params.x_initial[11:14, 0] = vessel_initial.omega #final params.x_final[0:1, 0] = vessel_profile.m_dry params.x_final[1:4, 0] = vessel_final.pos params.x_final[4:7, 0] = vessel_final.vel params.x_final[7:11, 0] = vessel_final.rotation params.x_final[11:14, 0] = vessel_final.omega # time of flight guess params.s_last = vessel_profile.time_guess # solver options 凸优化目标函数里各部分权重 #params.w_delta = solver_options.w_delta params.w_nu = solver_options.w_nu params.w_delta_s = solver_options.w_delta_s integrator = Integrator(vessel_profile, params_super, use_c=False) #use_c is WIP don't use # initial guess with GFOLD if (verbose): print('solving GFOLD for initial guess') x_guess, u_guess, m_guess = None, None, None for i in range(10): # gfold until feasible x_guess, u_guess, m_guess = GFOLD_solver.solve(vessel_profile, vessel_initial, vessel_final, use_c=use_c, verbose=verbose) if type(x_guess) == type(None): vessel_profile.time_guess *= 1.2 if (verbose): print('GFOLD retry %s, tf=%s' % (i + 1, vessel_profile.time_guess)) else: break k_space = np.linspace( 0, K - 1, GFOLD_params.SuperParams().N) # for interpolation in case N != K params.s_last = vessel_profile.time_guess for k in range(K): # alpha1 = (K - k) / K # alpha2 = (k / K) #xk = params.x_initial * alpha1 + params.x_final * alpha2 xk = np.zeros((14, 1)) xk[7:11, 0] = np.array([1.0, 0.0, 0.0, 0.0]) xk[0, 0] = np.interp(k, k_space, m_guess[0, :]) #m_guess[0, k] for l_i in range(1, 7): xk[l_i, 0] = np.interp(k, k_space, x_guess[l_i - 1, :]) uk = np.zeros((3, 1)) for l_i in range(3): uk[l_i, 0] = np.linalg.norm(np.interp(k, k_space, u_guess[l_i, :])) params.x_last[:, k] = xk[:, 0] #params.u_last[:, k] = xk[0, 0] * -vessel_profile.g # hover #params.u_last_dir[:, k] = -vessel_profile.g / np.linalg.norm(vessel_profile.g) # thrust dir down params.u_last[:, k] = np.array([np.linalg.norm(uk), 0, 0]) params.u_last_dir[:, k] = params.u_last[:, k] / np.linalg.norm( params.u_last[:, k]) x_res, u_res, s_res = params.x_last, params.u_last, params.s_last #迭代求解 for iteration in range(iterations): if (verbose): print("Iteration", iteration + 1) #(1)积分 准备每个时间步的矩阵A B C S z 过程中需要计算积分,耗时较长 start_time = time() for k in range(0, K - 1): params.A[k][:, :], params.B[k][:, :], params.C[k][:, :], params.S[k][:, 0], params.z[k][:, 0] = \ integrator.solve(params.x_last[:, k], params.u_last[:, k], params.u_last[:, k+1], params.s_last) #积分耗时 if (verbose): print(time() - start_time, "sec to integrate") # 令w_delta随着迭代数改变 if isinstance(solver_options.w_delta, type(lambda: 0)): params.w_delta = solver_options.w_delta(iteration) else: params.w_delta = solver_options.w_delta # if solver_options.force_converge: # if iteration < solver_options.force_converge_start: # params.w_delta = solver_options.w_delta # else: # params.w_delta = solver_options.w_delta * solver_options.force_converge_amount # w_delta变大以促进delta收敛 # else: # params.w_delta = solver_options.w_delta #(2)求解凸优化子问题 start_time = time() res, x_res, u_res, s_res, nu_res, delta_res, delta_s_res = solver.solve( params, params_super) #凸优化耗时 if (verbose): print(time() - start_time, "sec to solve") # 这次迭代的结果作为下一次迭代的输入 params.x_last = x_res params.u_last = u_res params.s_last = s_res for k in range(K): #计算u的方向的单位向量 params.u_last_dir[:, k] = params.u_last[:, k] / np.linalg.norm( params.u_last[:, k]) # 收敛情况的评价指标 delta_norm = np.linalg.norm(delta_res) nu_norm = np.linalg.norm(nu_res, ord=1) if (verbose): print("Flight time:", s_res, end=' | ') print("Delta_norm:", delta_norm, end=' | ') print("Nu_norm:", nu_norm) #终止条件 if delta_norm < solver_options.delta_tol and nu_norm < solver_options.nu_tol: if (verbose): print("Converged after", iteration + 1, "iterations!") break #inv normalization x_res[0, :] *= Um x_res[1:4, :] *= Ul x_res[4:7, :] *= Ul / Ut x_res[11:14, 0] *= 1. / Ut u_res *= Um * Ul / Ut**2 s_res *= Ut #返回 状态,控制,tf return x_res, u_res, s_res
vessel = SC_params.VesselProfile.get_default() state_initial = SC_params.VesselState.get_default_initial() state_final = SC_params.VesselState.get_default_final() #vessel.T_min = 1.0 #vessel.T_max = 5.0 #state_initial.pos = np.array([20., 5., 3.]) #state_initial.vel = np.array([-4., -0., -3.]) vessel.isp = 299.5 vessel.g = np.array((-9.807, 0., 0.)) # gravity vessel.m_dry = 19770.810546875 vessel.gamma_gs = np.deg2rad(10) # glide slope vessel.theta_max = np.deg2rad(90) # tilt vessel.theta_max = np.linspace(np.deg2rad(90), np.deg2rad(10), SC_params.SuperParams().K) vessel.omega_max = np.deg2rad(60) # rotation vel vessel.delta_max = np.deg2rad(10) # gimbal vessel.T_min = 190162.0 vessel.T_max = 760648.0 vessel.r_T_B = np.array([-4.49584946, 0., 0.]) vessel.J_B_I = np.array([[3.43008688e+05, 9.74064052e-01, -2.31322622e+00], [9.73825634e-01, 3.87805234e+04, 8.40314293e+00], [-2.31325603e+00, 8.40314293e+00, 3.49099750e+05]]) vessel.airfric_k = 14 vessel.time_guess = 25 state_initial.mass = 25583.166015625 state_initial.pos = np.array([1400., 165.28223741 * 0.9, 247.82434027 * 0.9]) state_initial.vel = np.array([-98.40400201, -6.23042187, -10.52199281]) state_initial.rotation = np.array([0.70710678, 0., 0.58785953, -0.39296459])
# free sym of res_phi: # {u_t, J, t, sigma, u_t1, Phi, V, dt} # free sym of res_b: # {u_t, J, t, sigma, u_t1, alpha, Phi_A_xi, V, rTB, dt} # free sym of res_c: # {u_t, J, t, sigma, u_t1, alpha, Phi_A_xi, V, rTB, dt} # free sym of res_s: # {u_t, J, t, u_t1, alpha, g_I, Phi_A_xi, V, rTB, dt} # free sym of res_z: # {u_t, J, t, sigma, u_t1, alpha, Phi_A_xi, V, rTB, dt} if __name__ == '__main__': #testing #from dynamics_functions_sympy import Dynamics integrator = Integrator(SC_params.VesselProfile.get_default(), SC_params.SuperParams()) if (len(sys.argv) > 2 and sys.argv[1] == 'codegen'): codegen_path = sys.argv[2] codegen(codegen_path) else: print("invalid input") print(sys.argv) # #from sympy import * #from sympy.abc import x, y, z #from sympy.utilities.autowrap import autowrap #expr = ((x - y + z)**(13)).expand() #binary_func = autowrap(expr, language='C', backend='cython', tempdir='sc_integrator_codegen') #
def solve(params, params_super = None, codegen = False): #super params if (params_super == None): params_super = SC_params.SuperParams() # default K = params_super.K #优化变量 x = Variable(14, K, name='x') u = Variable(3, K, name='u') s = Variable(1, 1, name='s') nu = Variable(14, K-1, name='nu') delta = Variable(1, K, name='delta') delta_s = Variable(1, 1, name='delta_s') #参数 # A = [Parameter(14, 14, name='A_%s'%i) for i in range(K)] # B = [Parameter(14, 3, name='B_%s'%i) for i in range(K)] # C = [Parameter(14, 3, name='C_%s'%i) for i in range(K)] # S = [Parameter(14, 1, name='S_%s'%i) for i in range(K)] # z = [Parameter(14, 1, name='z_%s'%i) for i in range(K)] #暴力数组会超python255参数限制。。。 A = Parameter(14 * K, 14, name='A') B = Parameter(14 * K, 3, name='B') C = Parameter(14 * K, 3, name='C') S = Parameter(14 * K, 1, name='S') z = Parameter(14 * K, 1, name='z') x_last = Parameter(14, K, name='x_last') u_last = Parameter(3, K, name='u_last') u_last_dir = Parameter(3, K, name='u_last_dir') s_last = Parameter(2, 1, name='s_last') #2行1列,第二列占位用,因为1行1列在codegen会出bug w_nu = Parameter(2, 1, name='w_nu', sign="positive") #权重非负,保证dcp w_delta = Parameter(2, 1, name='w_delta', sign="positive") w_delta_s = Parameter(2, 1, name='w_delta_s', sign="positive") x_initial = Parameter(14, 1, name='x_initial') x_final = Parameter(14, 1, name='x_final') #sparse m_dry = Parameter(2, 1, name='m_dry') tan_gamma_gs = Parameter(2, 1, name='tan_gamma_gs', sign="positive") cos_theta_max = Parameter(1, K, name='cos_theta_max') omega_max = Parameter(2, 1, name='omega_max') cos_delta_max = Parameter(2, 1, name='cos_delta_max', sign="positive") T_max = Parameter(2, 1, name='T_max') T_min = Parameter(2, 1, name='T_min') if (not codegen): #填入实际参数 A.value = params.A.reshape(K * 14, 14) B.value = params.B.reshape(K * 14, 3) C.value = params.C.reshape(K * 14, 3) S.value = params.S.reshape(K * 14, 1) z.value = params.z.reshape(K * 14, 1) x_last.value = params.x_last u_last.value = params.u_last u_last_dir.value = params.u_last_dir s_last.value = [params.s_last, 0] w_nu.value = [params.w_nu, 0] w_delta.value = [params.w_delta, 0] w_delta_s.value = [params.w_delta_s, 0] x_initial.value = params.x_initial x_final.value = params.x_final #sparse m_dry.value = [params.m_dry, 0] tan_gamma_gs.value = [params.tan_gamma_gs, 0] cos_theta_max.value = np.array([params.cos_theta_max] * K).reshape(1, K) if type(params.cos_theta_max)!=np.ndarray else params.cos_theta_max omega_max.value = [params.omega_max, 0] cos_delta_max.value = [params.cos_delta_max, 0] T_max.value = [params.T_max, 0] T_min.value = [params.T_min, 0] #限制条件 cons = [] #(1)边界条件 #初始 cons += [ x[0, 0] == x_initial[0, 0], # mass x[1:4, 0] == x_initial[1:4, 0], # position x[4:7, 0] == x_initial[4:7, 0], # velocity x[7:11, 0] == x_initial[7:11, 0], # quanternion 更改:初态姿态固定 x[11:14, 0] == x_initial[11:14, 0], # angular vel ] #结束 cons += [ # x[0, K-1] == x_final[0, 0], # mass x[1:4, K-1] == x_final[1:4, 0], # position x[4:7, K-1] == x_final[4:7, 0], # velocity #x[7:11, K-1] == x_final[7:11, 0], # quanternion x[11:14, K-1] == x_final[11:14, 0], # angular vel ] #thrust最后朝下 cons += [ u[1, K-1] == 0, u[2, K-1] == 0, ] #(2)动力学方程 for k in range(K - 1): cons += [ x[:, k+1] == A[14*k:14*(k+1),:]*x[:, k] + B[14*k:14*(k+1),:]*u[:, k] + C[14*k:14*(k+1),:]*u[:, k+1] + S[14*k:14*(k+1),:]*s + z[14*k:14*(k+1),:] + nu[:, k] ] #(3)状态限制 for k in range(K): cons += [ x[0, k] >= m_dry[0,0], #燃料耗尽 norm(x[2:4, k]) * tan_gamma_gs[0,0] <= x[1, k], #在锥内部 cos_theta_max[0,k] <= 1 - 2 * sum_squares(x[9:11, k]), # 倾角 norm(x[11:14, k]) <= omega_max[0,0], #角速度 ] cons += [0 == x[9:11, K-1]] # 规定最终头朝上但是滚转轴随意 即四元数jk分量为0 #cons += [0 == x[8, 0]] cons += [s >= 0] #(4)输入量(推力)限制 for k in range(K): cons += [ #T_min[0,0] <= u_last_dir[:, k].T * u[:, k], #最小推力线性近似 T_min[0,0] <= u_last_dir[0, k]*u[0, k] + u_last_dir[1, k]*u[1, k] + u_last_dir[2, k]*u[2, k], #点乘展开写 norm(u[:, k]) <= T_max[0,0], #最大推力凸约束 norm(u[:, k]) * cos_delta_max[0,0] <= u[0, k], #gimbal限制 ] #(5)trust region for k in range(K): dx = x[:, k] - x_last[:, k] du = u[:, k] - u_last[:, k] cons += [ sum_squares(dx) + sum_squares(du) <= delta[:, k] ] cons += [norm(s - s_last[0,0], 1) <= delta_s] # Objective: objective = Minimize( -x[0,K-1] + w_nu[0,0] * norm(nu, 1) # virtual control(1范数) + w_delta[0,0] * norm(delta) # trust region on dynamics(2范数) + w_delta_s[0,0] * norm(delta_s, 1) # trust region on sigma(1范数) ) # Problem problem = Problem(objective, cons) #print('is DCP: %s' % problem.is_dcp()) #检查是否符合凸优化规则 # Solve or Codegen if (codegen): cpg.codegen(problem, codegen_path) else: obj_opt = problem.solve(solver=ECOS, verbose=False) return (obj_opt, np.array(x.value), np.array(u.value), s.value, np.array(nu.value), np.array(delta.value), delta_s.value) #tuple(结果,状态,控制,时间scale)