def opt(target, engines, vK, eps):
tm = abs(target)
comp = vec()
man = vec()
for e in engines:
if not e.manual:
e.limit_tmp = -e.current_torque * target / tm / abs(e.current_torque) * e.torque_ratio
if e.limit_tmp > 0:
comp += e.nominal_current_torque(e.vsf(vK) * e.limit)
elif e.maneuver:
if e.limit == 0: e.limit = eps
man += e.nominal_current_torque(e.vsf(vK) * e.limit)
else: e.limit_tmp = 0
else: e.limit_tmp = 0
compm = abs(comp)
manm = abs(man)
if compm < eps and manm == 0: return False
limits_norm = clamp01(tm / compm)
man_norm = clamp01(tm / manm)
for e in engines:
if e.manual: continue
if e.limit_tmp < 0:
e.limit = clamp01(e.limit * (1.0 - e.limit_tmp * man_norm))
else:
e.limit = clamp01(e.limit * (1.0 - e.limit_tmp * limits_norm))
return True
def drawVectors():
x = (1, 0, 0);
y = (0, 1, 0);
z = (0, 0, 1)
NoseUp = [
(0.426938, 0.6627575, 0.6152045), # right
(-0.9027369, 0.3521126, 0.2471495), # up
(-0.05282113, -0.6608852, 0.7486259), # fwd
(-0.825050456225237, 0.00142223040578914, 0.565057273153087), # Up
vec(-40.7348866753984, 82.6265182495117, -60.7879408364129).norm.v, # vel
]
NoseHor = [
(0.9065102, 0.2023994, 0.3705047), # right
(-0.4193277, 0.3297398, 0.8458344), # up
(0.04902627, -0.9221203, 0.3837842), # fwd
vec(-124.8083, 99.28588, 254.6226).norm.v, # -T
# vec(8.85797889364285, 42.9853324890137, 16.2476060788045).norm.v, #vel
# vec(14.74958, -277.06, 115.2648).norm.v, #[r*-T]
]
LookAhead = [
(-0.853199320849177, 0.0880317238524617, 0.514102455253879), # Up
(0.4738491, -0.3380022, 0.8131552), # r
(0.7306709, -0.3828829, -0.5652617), # lookahead
]
GreatCircle = [
(-0.853199320849177, 0.0880317238524617, 0.514102455253879), # Up
(0.553682, -0.2360972, 0.7985577), # VDir0
(-0.4253684, -0.6641812, -0.6147562) # VDir0
]
GreatCircle = [
(0, 1, 0), # Up
(np.sin(-103.6567 / 180 * np.pi), 0, np.cos(-103.6567 / 180 * np.pi)) # VDir0
]
Radar = [
(0.4143046, -0.6872179, 0.5967271), # Dir
(0.615147, -0.6593181, 0.4323122), # d
(-0.821889802172869, -0.00085793802058045, 0.569645869840723), # Up
(0.3722304, 0.7262448, 0.5779386), # right
]
MunNoseHor = [
(1, 0, 0),
(0, 1, 0),
(0.00198698, 0.999992, 0.003474206),
(-0.01996278, -0.1021231, -0.9945714),
(-0.0001221597, 0.9947699, -0.1021409),
(0.3317407, 0.9377342, -0.1029695),
]
# draw_vectors(*NoseUp)
draw_vectors(*MunNoseHor)
def solver_sim():
def vecs2scatter(vecs):
return np.array([v.v[:2] for v in vecs], dtype=float).transpose()
r0 = vec(314495.948447142, 650730.150160414, 0)
v0 = vec(-1800.99971342087, 1379.68533771485, 0)
r1 = vec(314495.948447142, 650730.150160414, 0)
# r1 = vec(404539.613450263, 596305.96712629, 0)
r2 = vec(+454949.7854, +515180.6808, +0.0000) * 0.9
s = lambert_solver(r1, r2, 1000, mu=3531600000000.0)
t, orb, vel = s.simulate_generic(r0, v0, 2200, 0.1)
start = 3500
r1 = orb[start];
v0 = vel[start]
ori = vecs2scatter((r1, r2, vec(0, 0, 0), orb[0], orb[-1]))
orb = vecs2scatter(orb)
def plot_solver(s):
x = np.linspace(-0.99, 0.99, 1000)
y = s.y(x)
f = s.F(x, y)
# f1 = s.F1(x, y, f)
# f2 = s.F2(x, y, f1)
# plt.plot(x, y, label='y')
plt.plot(x, f, label='f')
# plt.plot(x, f1, label='f1')
# plt.plot(x, f2, label='f2')
plt.legend()
plt.show()
def analyze_solution(tt):
s = lambert_solver(r1, r2, tt, mu=3531600000000.0)
s.solve()
# plot_solver(s)
print
print 'dV: %s' % (s.V - v0)
t, r, v = s.simulate(tt / 10000.0)
path = vecs2scatter(r)
print s
print 'TT: %f; Err: %s, dR*V %f' % (tt, r[-1] - r2, (r2 - r1).norm * s.V)
plt.scatter(ori[0], ori[1], color=['b', 'r', 'g', 'y', 'c'])
plt.plot(orb[0], orb[1], 'grey')
plt.plot(path[0], path[1])
plt.show()
# analyze_solution(1106.96240719769)
for tt in xrange(700, 3600, 100): analyze_solution(tt)
def solve(self):
if abs(self.tau - self.tauME) < 1e-6:
v = np.sqrt(self.mu) * self.y(0) / np.sqrt(self.n)
self.V = (r1.norm + self.cv / self.c) * v
elif self.tau <= 2.0 / 3 * (1 - self.sigma3):
print 'Only parabolic transfer is possible'
self.V = vec()
self.transfer_time = 0
else:
n = 1
x1 = np.nan
while np.isnan(x1) and n <= 2 ** 10:
x0 = 0
if np.isnan(x1):
x1 = 0.5 if self.tau < self.tauME else -0.5
while abs(x1 - x0) > 1e-6:
x0 = x1
x1 = self.next_x(x1, n)
# if n == 1: break
n *= 2
# if n > 2**10 and np.isnan(x1): n = 1
vr = np.sqrt(self.mu) * (self.y(x1) / np.sqrt(self.n) - x1 / np.sqrt(self.m))
vc = np.sqrt(self.mu) * (self.y(x1) / np.sqrt(self.n) + x1 / np.sqrt(self.m))
self.V = r1.norm * vr + self.cv / self.c * vc
return self.V, self.transfer_time
def __init__(self, pos, direction, spec_torque, min_thrust=0.0, max_thrust=100.0, maneuver=False, manual=False):
self.pos = pos
self.dir = direction
self.torque = spec_torque
self.min_thrust = float(min_thrust)
self.max_thrust = float(max_thrust)
self.limit = 1.0
self.limit_tmp = 1.0
self.best_limit = 1.0
self.torque_ratio = 1.0
self.current_torque = vec()
self.maneuver = maneuver
self.manual = manual
def sim_PIDf():
MoI = vec(3.517439, 5.191057, 3.517369)
def A(_t): return vec(_t / MoI[0], _t / MoI[1], _t / MoI[2])
def Tf(_t): return vec(MoI[0] / _t, MoI[1] / _t, MoI[2] / _t)
ph = 0.4;
pl = 0.05;
dp = ph - pl
def hill(x, x0=1.0, k=1.0):
p1 = x ** 2
p2 = -(x * 2 - x0 * 3)
p3 = (x * 2 + x0 * 3)
return p1 * p2 * p3
T = np.arange(0, 2, 0.01)
P = []
I = []
As = []
S = []
S1 = []
S2 = []
for t in T:
As.append(abs(A(t)))
# f = asymp01(As[-1], 10)
# P.append(pl+dp*(1.0-f))
# I.append(P[-1]*(1.0-f)/1.5)
S.append(hill(t, 1, 1))
# plt.plot(T, P)
# plt.plot(T, I)
# plt.show()
plt.plot(T, S)
plt.show()
def sentence2vec(words_in_sentence, model): array_of_vectors = map(lambda (x): common.vec(x, model), words_in_sentence) filtered = np.array([x for x in array_of_vectors if len(x) != 0]) return filtered.transpose()
def optR(engines, D=vec(), vK=1.0, eps=0.1, maxI=500, output=True):
torque_clamp = vec6()
torque_imbalance = vec()
ti_min = vec()
for e in engines:
e.limit = 1.0 if not e.maneuver else 0
ti_min += e.nominal_current_torque(0)
if abs(ti_min) > 0:
print ti_min
anti_ti_min = vec()
for e in engines:
if e.torque * ti_min < 0:
anti_ti_min += e.nominal_current_torque(1)
if abs(anti_ti_min) > 0:
vK = clampL(vK, clamp01(abs(ti_min) / abs(anti_ti_min) * 1.2))
for e in engines:
e.torque_ratio = clamp01(1.0 - abs(e.pos.norm * e.dir.norm)) ** 0.1
e.current_torque = e.nominal_current_torque(e.vsf(vK))
torque_imbalance += e.nominal_current_torque(e.vsf(vK) * e.limit)
torque_clamp.add(e.current_torque)
_d = torque_clamp.clamp(D)
if output:
print 'vK: %f' % vK
print 'Torque clamp:\n', torque_clamp
print 'demand: ', D
print 'clamped demand:', _d
print ('initial %s, error %s, dir error %s' %
(torque_imbalance, abs(torque_imbalance - _d), torque_imbalance.angle(_d)))
s = [];
s1 = [];
i = 0;
best_error = -1;
best_angle = -1;
best_index = -1;
for i in xrange(maxI):
s.append(abs(torque_imbalance - _d))
s1.append(torque_imbalance.angle(_d) if abs(_d) > 0 else 0)
if (s1[-1] <= 0 and s[-1] < best_error or
s[-1] + s1[-1] < best_error + best_angle or best_angle < 0):
for e in engines: e.best_limit = e.limit
best_error = s[-1]
best_angle = s1[-1]
best_index = len(s) - 1
# if len(s1) > 1 and s1[-1] < 55 and s1[-1]-s1[-2] > eps: break
# if s1[-1] > 0:
# if s1[-1] < eps or len(s1) > 1 and abs(s1[-1]-s1[-2]) < eps*eps: break
# elif
if s[-1] < eps or len(s) > 1 and abs(s[-1] - s[-2]) < eps / 10.0: break
if len(filter(lambda e: e.manual, engines)) == 0:
mlim = max(e.limit for e in engines)
if mlim > 0:
for e in engines: e.limit = clamp01(e.limit / mlim)
if not opt(_d - torque_imbalance, engines, vK, eps): break
torque_imbalance = vec.sum(e.nominal_current_torque(e.vsf(vK) * e.limit) for e in engines)
for e in engines: e.limit = e.best_limit
if output:
##########
print 'iterations:', i + 1
# print 'dAngle: ', abs(s1[-1]-s1[-2])
print 'limits: ', list(e.limit for e in engines)
print 'result %s, error %s, dir error %s' % (torque_imbalance, s[best_index], s1[best_index])
print 'engines:\n' + '\n'.join(str(e.nominal_current_torque(e.vsf(vK) * e.limit)) for e in engines)
print
##########
x = np.arange(len(s))
plt.subplot(2, 1, 1)
plt.plot(x, s, '-')
plt.xlabel('iterations')
plt.ylabel('torque error (kNm)')
plt.subplot(2, 1, 2)
plt.plot(x, s1, '-')
plt.xlabel('iterations')
plt.ylabel('torque direction error (deg)')
##########
return s[best_index], s1[best_index]
def Tf(_t): return vec(MoI[0] / _t, MoI[1] / _t, MoI[2] / _t)
ph = 0.4;
def A(_t): return vec(_t / MoI[0], _t / MoI[1], _t / MoI[2])
def Tf(_t): return vec(MoI[0] / _t, MoI[1] / _t, MoI[2] / _t)
# engineF(vec(1.5, 2.0, 0.0), min_thrust=0.0, max_thrust=250.0),
# engineF(vec(1.3, 2.0, 1.6), min_thrust=0.0, max_thrust=20.0, maneuver=True),
# engineF(vec(3.1, -2.0, -3.7), min_thrust=0.0, max_thrust=20.0, maneuver=True),
# engineF(vec(-3.1, -2.0, 3.7), min_thrust=0.0, max_thrust=20.0, maneuver=True),
# engineF(vec(-2.4, 2.0, -2.9), min_thrust=0.0, max_thrust=20.0, maneuver=True),
# ]
#
# Hover_Test = [
# engineF(vec(-6.2, 6.4, 0.6), max_thrust=450),
# engineF(vec(-6.2, -6.4, -0.6), max_thrust=450),
# engineF(vec(3.9, 7.4, 0.6), max_thrust=450),
# engineF(vec(3.9, -6.4, -0.6), max_thrust=450)
# ]
Uneven_Test = [
engine(vec(-0.1, -2.1, 0.0), vec(0.0, -1.0, 0.0), vec(0.0, 0.0, 0.1), 0, 200),
engine(vec(-1.4, -1.4, 0.0), vec(0.0, -1.0, 0.0), vec(0.0, 0.0, 1.4), 0, 60),
engine(vec(1.1, -1.4, 0.0), vec(0.0, -1.0, 0.0), vec(0.0, 0.0, -1.1), 0, 200),
engine(vec(-0.1, 0.2, 0.8), vec(0.0, -1.0, 0.1), vec(0.8, 0.0, 0.1), 0, 16),
engine(vec(-0.1, 0.2, -0.8), vec(0.0, -1.0, -0.1), vec(-0.8, 0.0, 0.1), 0, 16)
]
Shuttle_Test = [
engine(vec(0.0, -5.9, -2.7), vec(0.0, -1.0, 0.0), vec(-2.7, 0.0, 0.0), 0, 1500),
engine(vec(0.0, -6.2, 1.2), vec(0.0, -1.0, 0.0), vec(1.2, 0.0, 0.0), 0, 4000),
]
VTOL_Test_Bad_Demand = [
vec(0.0, 0.0, 0.0),
vec(10.0, 0.0, 0.0),
vec(0.0, 10.0, 0.0),