-
Notifications
You must be signed in to change notification settings - Fork 1
/
SC_subproblem_gen.py
51 lines (42 loc) · 1.51 KB
/
SC_subproblem_gen.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
# codegen的模块套一层壳
import sc_subproblem_solver
import numpy as np
import SC_params
def wrap(v):
return np.matrix([[v], [0]])
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)