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 update(self):
if self.on_update is not None:
self.on_update(self)
else:
self.PID.update(abs(self.error))
self.angle = lerp(self.angle, np.arctan2(self.PID.action*np.sign(self.error), 1), self.turn_speed*dt)
self.engine.limit = clamp01(self.base_limit/abs(np.cos(self.angle)))
self.engine.update()
self.accel = self.engine.thrust*np.sin(self.angle)/self.mass + (np.random.rand()-0.5)*1e-3
self.error -= self.accel*dt
def vK2(self):
# print('Thrust %.2f, W %.2f, H %.2f, upV %.2f, E %.2f, upA %.2f, upAF %.3f, dVSP %.2f, K0 %.3f, K1 %.3f'
# % (self.cthrust, self.M*9.81, self.upX, self.upV, self.maxV-self.upV, self.upA, upAF, (2.0+upAF)/self.twr,
# clamp01(self.E/2.0/(clampL(self.upA/self.K1+1.0, self.L1)**2)),
# clamp01(self.E/2.0/(clampL(self.upA/self.K1+1.0, self.L1)**2)+upAF)))
# return clampL(clamp01(self.E/2.0/(clampL(self.upA/self.K1+1.0, self.L1)**2)+upAF), 0.05)
# if self.upV <= self.maxV:
K = clamp01(self.E * 0.5 + self.upAF)
# else:
# K = clamp01(self.E*self.upA*0.5+self.upAF)
return clampL(K, self.MinVSF)
def update(self):
if self.on_update is not None:
self.on_update(self)
else:
self.PID.update(abs(self.error))
self.angle = lerp(self.angle,
np.arctan2(self.PID.action * np.sign(self.error), 1),
self.turn_speed * dt)
self.engine.limit = clamp01(self.base_limit / abs(np.cos(self.angle)))
self.engine.update()
self.accel = self.engine.thrust * np.sin(
self.angle) / self.mass + (np.random.rand() - 0.5) * 1e-3
self.error -= self.accel * dt
def EWA2(old, cur, ratio=0.7):
if cur < old: ratio = clamp01(1 - ratio)
return (1 - ratio) * old + ratio * cur
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]