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]