-
Notifications
You must be signed in to change notification settings - Fork 1
/
old_dynamics.jl
156 lines (134 loc) · 5.37 KB
/
old_dynamics.jl
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
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
module OldDynamics
using DifferentialEquations
using ForwardDiff
using DiffResults
using StaticArrays
using ..ProbInfo, ..LinPoint
const mass_idx = 1
const r_idx = SVector(2,3,4)
const v_idx = SVector(5,6,7)
const qbi_idx = SVector(8,9,10,11)
const omb_idx = SVector(12,13,14)
const state_dim = 14
const control_dim = 3
struct IntegratorParameters{T}
dt :: Float64
uk :: SArray{Tuple{3}, T, 1, 3}
up :: SArray{Tuple{3}, T, 1, 3}
end
struct LinRes
Ak::Array{Float64,2}
Bk::Array{Float64,2}
Ck::Array{Float64,2}
Sigk::Array{Float64,1}
Zk::Array{Float64,1}
iter_end::Array{Float64, 1} #dunno
end
function DCM(quat::SArray{Tuple{4}, T, 1, 4} where T)
q0 = quat[1]
q1 = quat[2]
q2 = quat[3]
q3 = quat[4]
return transpose(@SMatrix [(1-2*(q2^2 + q3^2)) (2*(q1 * q2 + q0 * q3)) (2*(q1*q3 - q0 * q2));
(2*(q1 * q2 - q0 * q3)) (1-2*(q1^2 + q3^2)) (2*(q2*q3 + q0 * q1));
(2*(q1 * q3 + q0 * q2)) (2*(q2*q3 - q0 * q1)) (1-2*(q1^2 + q2^2)) ])
end
function Omega{T}(omegab::SArray{Tuple{3}, T, 1, 3})
z = zero(T)
return @SMatrix [z -omegab[1] -omegab[2] -omegab[3];
omegab[1] z omegab[3] -omegab[2];
omegab[2] -omegab[3] z omegab[1];
omegab[3] omegab[2] -omegab[1] z ]
end
function dx(state::SArray{Tuple{14}, T, 1, 14} where T, u::MArray{Tuple{3}, T, 1, 3} where T, info::ProbInfo)
qbi = state[qbi_idx]
omb = state[omb_idx]
thr_acc = DCM(qbi) * u/state[mass_idx]
grv_acc = @SVector [-info.g0,0,0]
acc = thr_acc + grv_acc
rot_vel = 0.5*Omega(omb)*qbi
rot_acc = info.jBi*(cross(info.rTB,u) - cross(omb,info.jB*omb))
return @SVector [-info.a*norm(u),
state[5],state[6],state[7],
acc[1], acc[2], acc[3],
rot_vel[1], rot_vel[2], rot_vel[3], rot_vel[4],
rot_acc[1], rot_acc[2], rot_acc[3]]
end
function compute_A(state::SArray{Tuple{14}, T, 1, 14} where T, u::MArray{Tuple{3}, T, 1, 3} where T, target::MArray{Tuple{14,14}, T, 2, 196} where T, info::ProbInfo)
ForwardDiff.jacobian!(target, istate -> dx(istate, u, info), state)
end
function compute_B(state::SArray{Tuple{14}, T, 1, 14} where T, u::MArray{Tuple{3}, T, 1, 3} where T, target::MArray{Tuple{14,3}, T, 2, 42} where T, info::ProbInfo)
ForwardDiff.jacobian!(target, iu -> dx(state, iu, info), u)
end
function linearize_dynamics(states::Array{LinPoint,1}, sigma_lin::Float64, dt::Float64, info::ProbInfo)
#preallocate memory
const Aval = @MArray zeros(state_dim, state_dim)
const Bval = @MArray zeros(state_dim, control_dim)
const clin = @MVector zeros(control_dim)
const cneg = @MVector zeros(control_dim)
const cpls = @MVector zeros(control_dim)
#compute indices into the combined accumulator matrix
const state_idx = 1
const A_start = state_idx + 1
const A_end = A_start + state_dim - 1
const B_start = A_end + 1
const B_end = B_start + control_dim - 1
const C_start = B_end + 1
const C_end = C_start + control_dim - 1
const Sig_idx = C_end + 1
const Z_idx = Sig_idx + 1
#integrand worker
#the accumulator is laid out as
# state:1 | Ak:state_dim | Bkm:control_dim | Bkp:control_dim | sigK:1 | zk:1
# all are state_dim high. Dims are thus state_dim+control_dim*2+3 by state_dim.
function integrand(du,
u,#::MArray{Tuple{acc_height,acc_width}, Float64, 2, acc_height*acc_width},
p::IntegratorParameters,
t::Float64)
lkm = (p.dt-t)/p.dt
lkp = t/p.dt
broadcast!(*, cneg, lkm, p.uk)
broadcast!(*, cpls, lkp, p.up)
broadcast!(+, clin, cneg, cpls)
sv = SVector{14}(u[:,state_idx])
du[:,1] = dx(sv, clin, info)
compute_A(sv, clin, Aval, info)
compute_B(sv, clin, Bval, info)
broadcast!(*, Aval, sigma_lin, Aval)
broadcast!(*, Bval, sigma_lin, Bval)
phi = u[:,A_start:A_end]
iPhi = inv(phi)
du[:,A_start:A_end] = Aval*phi
du[:,B_start:B_end] = lkm*(iPhi * Bval)
du[:,C_start:C_end] = lkp*(iPhi * Bval)
du[:,Sig_idx] = iPhi*(du[:,1])
du[:,Z_idx] = iPhi*(-Aval*u[:,1] - Bval * clin)
end
results = Array{LinRes,1}(length(states)-1)
for i=1:length(states)-1
uk = states[i].control
up = states[i+1].control
int_iv = hcat(states[i].state, eye(Float64, state_dim), zeros(Float64, state_dim, control_dim*2 + 2))
prob = ODEProblem(integrand, int_iv, (0.0,dt), IntegratorParameters(dt, uk, up))
sol = DifferentialEquations.solve(prob, DP5(); dtmin=0.001, force_dtmin=true)
result = sol.u[end]
Ak::Array{Float64,2} = result[:,A_start:A_end]
Bkm::Array{Float64,2} = Ak*result[:,B_start:B_end]
Bkp::Array{Float64,2} = Ak*result[:,C_start:C_end]
Sigk::Array{Float64,1} = Ak*result[:,Sig_idx]
zk::Array{Float64,1} = Ak*result[:,Z_idx]
results[i] = LinRes(Ak,
Bkm,
Bkp,
Sigk,
zk,
result[:,1])
end
return results
end
function next_step(dynam, ab, abn, state, control_k, control_kp, sigma, sigHat, relax)
dk = dynam
return dk.Ak * state + dk.Bk * control_k + dk.Ck * control_kp + dk.Sigk*sigma + dk.Zk + relax
end
export linearize_dynamics, LinRes
end