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):