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)