def simAA(error, maxAA, engine, base_thrust, wheels_ratio, error_rate=0, error_time=0): at_pid = PID2(1, 0.0, 1, 0, np.pi * 10) av_pid = PID3(1, 0.0, 1, -1, 1, 3 * dt) if wheels_ratio < 1: MoI = engine.maxThrust * lever * 2 / maxAA / ( 1 - wheels_ratio) * base_thrust wheels_torque = maxAA * wheels_ratio * MoI else: base_thrust = 0 wheels_torque = engine.maxThrust * lever * 2 MoI = wheels_torque / maxAA atc = ATC(engine, lever, MoI, at_pid, av_pid, base_thrust, tune_steering, wheels_torque) if error_rate > 0: if error_time > 0: return atc.simulate_random_attitude(error, error_rate, error_time, clampL(error * 2, 60)) return atc.simulate_linear_attitude(error, error_rate, clampL(error * 2, 60)) return atc.simulate_static_attitude(error, clampL(error * 2, 60))
def simTurnTime(error, mass, engine, turn_time, error_rate=0, error_time=0): pid = PID3(0.05, 0.0, 0.2, 0, 1, 0.1) hsc = HSC(engine, pid, mass, turn_time, tune_pid) if error_rate > 0: if error_time > 0: return hsc.simulate_random_speed(error, error_rate, error_time, clampL(error * 2, 60)) return hsc.simulate_linear_speed(error, error_rate, clampL(error * 2, 60)) return hsc.simulate_constant_speed(error, clampL(error * 2, 60))
def simAA(error, maxAA, error_rate=0, error_time=0): at_pid = PID2(1, 0, 0, 0, np.pi * 10) av_pid = PID3(1, 0.1, 0, -1, 1, 3 * dt) wheels_torque = maxAA MoI = 1 atc = BRC(wheels_torque, MoI, at_pid, av_pid, tune_steering) if error_rate > 0: if error_time > 0: return atc.simulate_random_attitude(error, error_rate, error_time, clampL(error * 2, 60)) return atc.simulate_linear_attitude(error, error_rate, clampL(error * 2, 60)) return atc.simulate_static_attitude(error, clampL(error * 2, 60))
def simAA(error, maxAA, error_rate=0, error_time=0): at_pid = PID2(1, 0, 0, 0, np.pi * 10) av_pid = PID3(1, 0.1, 0, -1, 1, 3 * dt) wheels_torque = maxAA MoI = 1 atc = BRC(wheels_torque, MoI, at_pid, av_pid, tune_steering) if error_rate > 0: if error_time > 0: return atc.simulate_random_attitude(error, error_rate, error_time, clampL(error * 2, 60)) return atc.simulate_linear_attitude(error, error_rate, clampL(error * 2, 60)) return atc.simulate_static_attitude(error, clampL(error * 2, 60))
def simTurnTime(error, mass, engine, turn_time, error_rate=0, error_time=0): pid = PID3(0.05, 0.0, 0.2, 0, 1, 0.1) hsc = HSC(engine, pid, mass, turn_time, tune_pid) if error_rate > 0: if error_time > 0: return hsc.simulate_random_speed(error, error_rate, error_time, clampL(error * 2, 60)) return hsc.simulate_linear_speed(error, error_rate, clampL(error * 2, 60)) return hsc.simulate_constant_speed(error, clampL(error * 2, 60))
def tune_steering_mixed(atc, iErrf, imaxAA, AM): if atc.InstantRatio > 0.1: atc.atPID.P = 1 + 50 * iErrf ** 2.2 * atc.InstantRatio**0.8 atc.atPID.D = 20 * imaxAA ** 0.5 * iErrf ** 0.3 atc.atPID.I = 2 * imaxAA ** 0.3 * atc.InstantRatio else: if atc.MaxAA >= 1: atc.atPID.P = (1 + 1*atc.MaxAA + 9 * iErrf ** 2)*(10*atc.InstantRatio+0.3) atc.atPID.D = 20+clampL((11-atc.MaxAA**2)*3, 0) atc.atPID.I = 0.1 + 0.7*atc.MaxAA**0.5 else: atc.atPID.P = (1 + 3 * atc.MaxAA + 7 * iErrf ** 2) * (10 * atc.InstantRatio + 0.3) atc.atPID.D = 20 + clampL((11 - atc.MaxAA ** 0.5) * 3, 0) atc.atPID.I = 0.1 + 0.7 * atc.MaxAA ** 0.5 update_pids(MixedConfig, atc, iErrf)
def tune_steering_slow(atc, iErrf, imaxAA, AM): atc.atPID.P = 1 atc.atPID.I = 0 atc.atPID.D = 0 slowF = (1 + SlowConfig.SlowTorqueF / max(atc.engineF.acceleration, atc.engineF.deceleration)) noise_scale = clamp( abs(50 * (abs(atc.avPID.perror) + abs(atc.error / np.pi)))**0.5, 0.01, 1) if atc.MaxAA >= 1: atc.avPID.P = SlowConfig.avP_HighAA_Scale / slowF * noise_scale atc.avPID.D = clampL( SlowConfig.avD_HighAA_Intersect - SlowConfig.avD_HighAA_Inclination * atc.MaxAA, SlowConfig.avD_HighAA_Max) * noise_scale else: atc.avPID.P = SlowConfig.avP_LowAA_Scale / slowF * noise_scale atc.avPID.D = ( SlowConfig.avD_LowAA_Intersect - SlowConfig.avD_LowAA_Inclination * atc.MaxAA) * noise_scale atc.avPID.I = SlowConfig.avI_Scale * clampH(atc.MaxAA, 1) * noise_scale # avFilter.ratio = clamp(abs(atc.AV)+abs(atc.error/np.pi)*500, 0.01, 1) # AV = avFilter.EWA(atc.AV) AV = atc.AV update_pids(atc, AV)
def simAA(error, maxAA, engine, base_thrust, wheels_ratio, error_rate=0): at_pid = PID2(1, 0.0, 1, -1, 1) if wheels_ratio < 1: MoI = engine.maxThrust*lever*2/maxAA/(1-wheels_ratio)*base_thrust wheels_torque = maxAA * wheels_ratio * MoI else: base_thrust = 0 wheels_torque = engine.maxThrust*lever*2 MoI = wheels_torque / maxAA atc = ATC(engine, lever, MoI, at_pid, base_thrust, tune_steering, wheels_torque) if error_rate > 0: return atc.simulate_linear_attitude(error, error_rate, clampL(error * 2, 60)) return atc.simulate_static_attitude(error, clampL(error * 2, 60))
def tune_steering_mixed(atc, iErrf, imaxAA, AM): if atc.InstantRatio > 0.1: atc.atPID.P = 1 + 50 * iErrf**2.2 * atc.InstantRatio**0.8 atc.atPID.D = 20 * imaxAA**0.5 * iErrf**0.3 atc.atPID.I = 2 * imaxAA**0.3 * atc.InstantRatio else: if atc.MaxAA >= 1: atc.atPID.P = (1 + 1 * atc.MaxAA + 9 * iErrf**2) * (10 * atc.InstantRatio + 0.3) atc.atPID.D = 20 + clampL((11 - atc.MaxAA**2) * 3, 0) atc.atPID.I = 0.1 + 0.7 * atc.MaxAA**0.5 else: atc.atPID.P = (1 + 3 * atc.MaxAA + 7 * iErrf**2) * (10 * atc.InstantRatio + 0.3) atc.atPID.D = 20 + clampL((11 - atc.MaxAA**0.5) * 3, 0) atc.atPID.I = 0.1 + 0.7 * atc.MaxAA**0.5 update_pids(MixedConfig, atc, iErrf)
def tune_steering_fast(cfg, atc, iErrf, imaxAA, AM): """ :param cfg: Configuration object :type cfg: FastConfig :param atc: Attitude Control model :type atc: ATC :param iErrf: inverse error factor :type iErrf: float :param imaxAA: inverse maxAA :type imaxAA: float :param AM: angular momentum :type AM: float """ # compute coefficients of attitude PID atP_iErrf = clampL(iErrf - cfg.atP_ErrThreshold, 0)**cfg.atP_ErrCurve if atc.MaxAA >= 1: atc.atPID.P = clampH( 1 + cfg.atP_HighAA_Scale * atc.MaxAA**cfg.atP_HighAA_Curve + atP_iErrf, cfg.atP_HighAA_Max) atc.atPID.D = cfg.atD_HighAA_Scale * imaxAA**cfg.atD_HighAA_Curve * clampH( iErrf + abs(AM), 1.2) else: atc.atPID.P = ( 1 + cfg.atP_LowAA_Scale * atc.MaxAA**cfg.atP_LowAA_Curve + atP_iErrf) atc.atPID.D = cfg.atD_LowAA_Scale * imaxAA**cfg.atD_LowAA_Curve * clampH( iErrf + abs(AM), 1.2) atI_iErrf = clampL(iErrf - cfg.atI_ErrThreshold, 0) overshot = atc.AV * atc.error < 0 if atI_iErrf <= 0 or overshot: atc.atPID.I = 0 atc.atPID.ierror = 0 else: atI_iErrf = atI_iErrf**cfg.atI_ErrCurve atc.atPID.I = (cfg.atI_Scale * atc.MaxAA * atI_iErrf / (1 + clampL(atc.AV * np.sign(atc.error), 0) * cfg.atI_AV_Scale * atI_iErrf)) # compute coefficients of angular velocity PID atc.avPID.P = clampL( cfg.avP_MaxAA_Intersect - cfg.avP_MaxAA_Inclination * atc.MaxAA**cfg.avP_MaxAA_Curve, cfg.avP_Min) atc.avPID.I = cfg.avI_Scale * atc.avPID.P atc.avPID.D = 0 AV = atc.AV update_pids(atc, AV)
def tune_steering_fast(cfg, atc, iErrf, imaxAA, AM): """ :param cfg: Configuration object :type cfg: FastConfig :param atc: Attitude Control model :type atc: BRC :param iErrf: inverse error factor :type iErrf: float :param imaxAA: inverse maxAA :type imaxAA: float :param AM: angular momentum :type AM: float """ # compute coefficients of attitude PID atP_iErrf = clampL(iErrf - cfg.atP_ErrThreshold, 0) ** cfg.atP_ErrCurve if atc.MaxAA >= 1: atc.atPID.P = clampH(1 + cfg.atP_HighAA_Scale * atc.MaxAA ** cfg.atP_HighAA_Curve + atP_iErrf, cfg.atP_HighAA_Max) atc.atPID.D = cfg.atD_HighAA_Scale * imaxAA ** cfg.atD_HighAA_Curve * clampH(iErrf + abs(AM), 1.2) else: atc.atPID.P = (1 + cfg.atP_LowAA_Scale * atc.MaxAA ** cfg.atP_LowAA_Curve + atP_iErrf) atc.atPID.D = cfg.atD_LowAA_Scale * imaxAA ** cfg.atD_LowAA_Curve * clampH(iErrf + abs(AM), 1.2) atI_iErrf = clampL(iErrf - cfg.atI_ErrThreshold, 0) overshot = atc.AV * atc.error < 0 if atI_iErrf <= 0 or overshot: atc.atPID.I = 0 atc.atPID.ierror = 0 else: atI_iErrf = atI_iErrf ** cfg.atI_ErrCurve atc.atPID.I = (cfg.atI_Scale * atc.MaxAA * atI_iErrf / (1 + clampL(atc.AV * np.sign(atc.error), 0) * cfg.atI_AV_Scale * atI_iErrf)) # compute coefficients of angular velocity PID atc.avPID.P = clampL(cfg.avP_MaxAA_Intersect - cfg.avP_MaxAA_Inclination * atc.MaxAA ** cfg.avP_MaxAA_Curve, cfg.avP_Min) atc.avPID.I = cfg.avI_Scale * atc.avPID.P atc.avPID.D = 0 AV = atc.AV update_pids(atc, AV)
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 tune_steering_slow(atc, iErrf, imaxAA, AM): slowF = (1 + SlowConfig.SlowTorqueF / max(atc.engineF.acceleration, atc.engineF.deceleration)) if atc.MaxAA >= 1: atc.atPID.P = 8 atc.atPID.D = 50 atc.atPID.I = 0#.1 + 0.7*atc.MaxAA**0.5 else: atc.atPID.P = (1 + 3 * atc.MaxAA + 7 * iErrf ** 2) atc.atPID.D = 20 + clampL((11 - atc.MaxAA ** 0.5) * 3, 0) atc.atPID.I = 0.1 + 0.7 * atc.MaxAA ** 0.5 update_pids(MixedConfig, atc, iErrf)
def tune_steering_slow(atc, iErrf, imaxAA, AM): slowF = (1 + SlowConfig.SlowTorqueF / max(atc.engineF.acceleration, atc.engineF.deceleration)) if atc.MaxAA >= 1: atc.atPID.P = 8 atc.atPID.D = 50 atc.atPID.I = 0 #.1 + 0.7*atc.MaxAA**0.5 else: atc.atPID.P = (1 + 3 * atc.MaxAA + 7 * iErrf**2) atc.atPID.D = 20 + clampL((11 - atc.MaxAA**0.5) * 3, 0) atc.atPID.I = 0.1 + 0.7 * atc.MaxAA**0.5 update_pids(MixedConfig, atc, iErrf)
def sim_PointNav(): P = 2 D = 0.1 AAk = 0.5 pid = PID(P, 0, D, 0, 10) accel = np.arange(0.1, 1.1, 0.1) distance = 500 distanceF = 0.1 cols = color_grad(len(accel)) for i, a in enumerate(accel): d = [distance] v = [0] act = [0] t = 0 A = [0] while d[-1] > 0: pid.kp = P * a / clampL(v[-1], 0.01) act.append(pid.update(d[-1] * distanceF)) aa = a * AAk if v[-1] < pid.action: if A[-1] < a: A.append(A[-1] + a * AAk * dt) else: A.append(a) elif v[-1] > pid.action: if A[-1] > -a: A.append(A[-1] - a * AAk * dt) else: A.append(-a) elif A[-1] > 0: A.append(A[-1] - a * AAk * dt) elif A[-1] < 0: A.append(A[-1] + a * AAk * dt) else: A.append(0) # print 'd=% 8.4f, v=% 8.4f, act=% 8.4f, A=% 8.4f' % (d[-1], v[-1], act[-1], A[-1]) v.append(v[-1] + A[-1] * dt) d.append(d[-1] - v[-1] * dt) t += dt print 'accel=%.2f, time: %.1fs' % (a, t) print len(d), len(A) plt.subplot(3, 1, 1) plt.plot(d, v, color=cols[i], label="accel=%.2f" % a) plt.ylabel("V") plt.xlabel("distance") plt.subplot(3, 1, 2) plt.plot(d, act, color=cols[i], label="accel=%.2f" % a) plt.ylabel("act") plt.xlabel("distance") plt.subplot(3, 1, 3) plt.plot(d, A, color=cols[i], label="accel=%.2f" % a) plt.ylabel("real accel"); plt.xlabel("distance") plt.legend() plt_show_maxed()
def tune_steering_mixed(atc, iErrf, imaxAA, AM): if atc.InstantRatio > 0.3: tune_steering_fast(MixedConfig3plus, atc, iErrf, imaxAA, AM) else: # if atc.MaxAA >= 1: # atc.atPID.P = 20 # atc.atPID.I = 0 # atc.atPID.D = 0 # atc.avPID.P = 1/clampL(abs(AM)/atc.MaxAA**0.2/12, 1) # atc.avPID.I = 0.002 # atc.avPID.D = 1.4/atc.MaxAA**0.3 # else: # atc.atPID.P = 40/clampL(abs(AM)/2, 1) # atc.atPID.I = 0 # atc.atPID.D = 0 # atc.avPID.P = 1/clampL(abs(AM)/12, 1) # atc.avPID.I = 0.001 # atc.avPID.D = 1.4/atc.MaxAA**0.3 # clampH(abs(40*(abs(atc.avPID.perror)+abs(atc.error/np.pi)))**6, 1) noise_scale = clamp((50 * (abs(atc.AV) + atc.errorF))**0.6, 0.001, 1) atc.atPID.P = 1 #clamp(0.5*atc.MaxAA**0.5, 0.0001, 1) atc.atPID.I = 0 atc.atPID.D = 0 atc.avPID.P = ( (MixedConfig.avP_A / (atc.InstantRatio**MixedConfig.avP_D + MixedConfig.avP_B) + MixedConfig.avP_C)) / clampL(abs(AM), 1) / atc.MaxAA * noise_scale atc.avPID.D = ( (MixedConfig.avD_A / (atc.InstantRatio**MixedConfig.avD_D + MixedConfig.avD_B) + MixedConfig.avD_C)) / atc.MaxAA * noise_scale atc.avPID.I = MixedConfig.avI_Scale * clampH(atc.MaxAA, 1) * noise_scale # atc.avPID.P = (1 + 8 / atc.MaxAA) * noise_scale # atc.avPID.D = clampH((clampL(abs(AM) / atc.MaxAA, 1) ** 0.1 - 1), 2) * noise_scale # avFilter.ratio = clamp((abs(atc.avPID.perror)+abs(atc.error/np.pi))/atc.InstantRatio*50, 0.1, 1) # AV = avFilter.EWA(atc.AV) # print avFilter.ratio AV = atc.AV update_pids(atc, AV)
def tune_steering_slow(atc, iErrf, imaxAA, AM): atc.atPID.P = 1 atc.atPID.I = 0 atc.atPID.D = 0 slowF = (1 + SlowConfig.SlowTorqueF / max(atc.engineF.acceleration, atc.engineF.deceleration)) noise_scale = clamp(abs(50 * (abs(atc.avPID.perror) + abs(atc.error / np.pi))) ** 0.5, 0.01, 1) if atc.MaxAA >= 1: atc.avPID.P = SlowConfig.avP_HighAA_Scale/slowF * noise_scale atc.avPID.D = clampL(SlowConfig.avD_HighAA_Intersect - SlowConfig.avD_HighAA_Inclination * atc.MaxAA, SlowConfig.avD_HighAA_Max) * noise_scale else: atc.avPID.P = SlowConfig.avP_LowAA_Scale/slowF * noise_scale atc.avPID.D = (SlowConfig.avD_LowAA_Intersect - SlowConfig.avD_LowAA_Inclination * atc.MaxAA) * noise_scale atc.avPID.I = SlowConfig.avI_Scale*clampH(atc.MaxAA, 1) * noise_scale # avFilter.ratio = clamp(abs(atc.AV)+abs(atc.error/np.pi)*500, 0.01, 1) # AV = avFilter.EWA(atc.AV) AV = atc.AV update_pids(atc, AV)
def tune_steering_mixed(atc, iErrf, imaxAA, AM): if atc.InstantRatio > 0.3: tune_steering_fast(MixedConfig3plus, atc, iErrf, imaxAA, AM) else: # if atc.MaxAA >= 1: # atc.atPID.P = 20 # atc.atPID.I = 0 # atc.atPID.D = 0 # atc.avPID.P = 1/clampL(abs(AM)/atc.MaxAA**0.2/12, 1) # atc.avPID.I = 0.002 # atc.avPID.D = 1.4/atc.MaxAA**0.3 # else: # atc.atPID.P = 40/clampL(abs(AM)/2, 1) # atc.atPID.I = 0 # atc.atPID.D = 0 # atc.avPID.P = 1/clampL(abs(AM)/12, 1) # atc.avPID.I = 0.001 # atc.avPID.D = 1.4/atc.MaxAA**0.3 # clampH(abs(40*(abs(atc.avPID.perror)+abs(atc.error/np.pi)))**6, 1) noise_scale = clamp((50 * (abs(atc.AV) + atc.errorF)) ** 0.6, 0.001, 1) atc.atPID.P = 1#clamp(0.5*atc.MaxAA**0.5, 0.0001, 1) atc.atPID.I = 0 atc.atPID.D = 0 atc.avPID.P = ((MixedConfig.avP_A / (atc.InstantRatio ** MixedConfig.avP_D + MixedConfig.avP_B) + MixedConfig.avP_C)) / clampL(abs(AM), 1) / atc.MaxAA * noise_scale atc.avPID.D = ((MixedConfig.avD_A / (atc.InstantRatio ** MixedConfig.avD_D + MixedConfig.avD_B) + MixedConfig.avD_C)) / atc.MaxAA * noise_scale atc.avPID.I = MixedConfig.avI_Scale * clampH(atc.MaxAA, 1) * noise_scale # atc.avPID.P = (1 + 8 / atc.MaxAA) * noise_scale # atc.avPID.D = clampH((clampL(abs(AM) / atc.MaxAA, 1) ** 0.1 - 1), 2) * noise_scale # avFilter.ratio = clamp((abs(atc.avPID.perror)+abs(atc.error/np.pi))/atc.InstantRatio*50, 0.1, 1) # AV = avFilter.EWA(atc.AV) # print avFilter.ratio AV = atc.AV update_pids(atc, AV)
def on_frame(): alt_err = alt - self.dX if self.AS > 0 or self.DS > 0: if alt_err > 0: self.pid.kp = clamp(0.01 * abs(alt_err) / clampL(self.upV, 1), 0.0, d) elif alt_err < 0: self.pid.kp = clamp(self.twr ** 2 / clampL(-self.upV, 1), 0.0, clampH(d * self.twr / 2, d)) else: self.pid.kp = d self.pid.kd = d / clampL(self.dX, 1) if alt_err < 0: alt_err = alt_err / clampL(self.dX, 1) self.maxV = self.pid.update(alt_err) print self.pid, alt_err, clamp(0.01 * abs(alt_err) / clampL(self.upV, 1), 0.0, d), d if self.N > 0: dV = (self.upV - self.dV) / clampL(self.dX, 1) if alt_err < 0: # print '% f %+f = %f' % (dV/clampL(self.dX/50, 1), clampL(alt_err*self.dX/5000*self.twr, self.pid.min*10), dV/clampL(self.dX/50, 1) + clampL(alt_err*self.dX/5000*self.twr, self.pid.min*10)) dV = dV + clampL(alt_err * self.dX / 500 * self.twr, self.pid.min * 10) else: dV = clampL(dV, 0) # print dV self.dVsp.append(dV) self.maxV += dV self.Vsp.append(self.maxV)
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 Tk(T, K, L=0): return [t * clampL(k, L) for t, k in zip(T, K)] def opt(target, engines, vK, eps):