inKeep = sysCtrl.InputName + sysPlantDist.InputName[2:] outKeep = sysCtrl.OutputName + sysPlantDist.OutputName sysLoopOut = ConnectName([sysCtrl, sysPlantDist], connectNames, inKeep, outKeep) # Lo = GK # Closed-Loop connectNames = sysLoopOut.OutputName[2:] inKeep = sysLoopOut.InputName[:2] + sysLoopOut.InputName[4:] outKeep = sysLoopOut.OutputName sysCL = ConnectName([sysLoopOut], connectNames, inKeep, outKeep) # Loop Lo - (e -> z) sysLo = sysLoopOut[2:4, 0:2] sysLo.InputName = sysLoopOut.InputName[0:2] sysLo.OutputName = sysLoopOut.OutputName[2:4] LoLinNom = FreqTrans.FreqResp(sysLo, freqLin_rps) # Loop To - (r -> z) sysTo = sysCL[2:4, 0:2] sysTo.InputName = sysCL.InputName[0:2] sysTo.OutputName = sysCL.OutputName[2:4] ToLinNom = FreqTrans.FreqResp(sysTo, freqLin_rps) # Loop So sysSo = np.eye(2) - sysTo pSigma = [0.0, 0.0] mSigma = [1.0, 1.0] pIn = 0.5 * control.ss([],[],[],np.diag(pSigma))
freqLin_hz = np.linspace(1e-1, 1e1, 400)
freqLin_rps = freqLin_hz * hz2rps
plantK11 = 1.0 ; plantWn11 = 3 * hz2rps; plantD11 = 0.2;
plantK12 = 0.5 ; plantWn12 = 5 * hz2rps; plantD12 = 0.3;
plantK21 = 0.25; plantWn21 = 4 * hz2rps; plantD21 = 0.1;
plantK22 = 1.0 ; plantWn22 = 6 * hz2rps; plantD22 = 0.4;
sysPlant = control.tf([[[0, 0, plantK11 * plantWn11**2], [0, 0, plantK21 * plantWn21**2]],
[[0, 0, plantK12 * plantWn12**2], [0, 0, plantK22 * plantWn22**2]]],
[[[1, 2.0*plantD11*plantWn11, plantWn11**2], [1, 2.0*plantD21*plantWn21, plantWn21**2]],
[[1, 2.0*plantD12*plantWn12, plantWn12**2], [1, 2.0*plantD22*plantWn22, plantWn22**2]]])
# Plant Response
TLinNom = FreqTrans.FreqResp(sysPlant, freqLin_rps)
#%% Excitation
numExc = 2
numCycles = 3
ampInit = 1
ampFinal = 1
freqMinDes_rps = 0.1 * hz2rps * np.ones(numExc)
freqMaxDes_rps = 10.1 * hz2rps * np.ones(numExc)
freqStepDes_rps = (10/freqRate_hz) * hz2rps
methodSW = 'zip' # "zippered" component distribution
# Generate MultiSine Frequencies
freqExc_rps, sigIndx, time_s = GenExcite.MultiSineComponents(freqMinDes_rps, freqMaxDes_rps, freqRate_hz, numCycles, freqStepDes_rps, methodSW)
freqGap_rps = freqExc_rps[0:-1] + 0.5 * np.diff(freqExc_rps)
sysLoopIn = ConnectName([sysPlantDist, sysCtrl], connectNames, inKeep, outKeep)
# Closed-Loop (connect [uCntrl])
connectNames = sysLoopIn.OutputName[0:2]
inKeep = sysLoopIn.InputName[:2] + sysLoopIn.InputName[4:]
outKeep = sysLoopIn.OutputName
sysCL = ConnectName([sysLoopIn], connectNames, inKeep, outKeep)
# Loop Li - (uExc -> uCtrl)
sysLi = -sysLoopIn[
0:2, 4:
6] # Li = KG, u_e to u # FIXIT - YES Negative : sysLi.dcgain() = array([[0.5, 0.25], [0.125, 0.5]])
sysLi.InputName = sysLoopIn.InputName[4:6]
sysLi.OutputName = sysLoopIn.OutputName[0:2]
LiLinNom = FreqTrans.FreqResp(sysLi, freqLin_rps)
# Loop Ti - (uExc -> uCtrl)
sysTi = -sysCL[
0:2, 2:
4] # FIXIT - YES Negative : sysTi.dcgain() = array([[0.32394366, 0.11267606], [0.05633803, 0.32394366]])
sysTi.InputName = sysCL.InputName[2:4]
sysTi.OutputName = sysCL.OutputName[0:2]
TiLinNom = FreqTrans.FreqResp(sysTi, freqLin_rps)
sysSi = (np.eye(2) - sysTi)
pStd = [0.0, 0.0]
mStd = [1.0, 1.0]
CuzMean = np.mean(Cuz, axis=-1) CuzStd = np.std(Cuz, axis=-1) CuzMergeMeanErgotic = (np.sqrt(1/numSeg) * (1 - CuzMergeMean)) + CuzMergeMean CuzMergeStdErgotic = np.sqrt(1/numSeg) * CuzMergeStd coher = CuzMergeMean N_s = CuzFinalMean.shape[-1] coherError = np.sqrt(2 / (N_s * coher)) * (1 - coher) # = std(coher) / coher; coher is the estimated mean square coherence, Bendat2010 Table 9.6 coherError * CuzMergeMean #%% Linear System sysLin = control.tf([objServo.freqNat_rps**2], [1, 2*objServo.damp*objServo.freqNat_rps, objServo.freqNat_rps**2]) TLinRefine = FreqTrans.FreqResp(sysLin, freqExc_rps) TLinFinal = FreqTrans.FreqResp(sysLin, freqExcFinal_rps) # Error TErrRefine = (TuzRefine - TLinRefine).squeeze() TErrSqdRefine = np.abs(TErrRefine**2) TErrFinal = (TuzFinal - TLinFinal).squeeze() TErrSqdFinal = np.abs(TErrFinal**2) TErrMergeMean = np.mean(np.mean(TErrFinal, axis = -1), axis=-1) TErrMergeStd = np.std(np.mean(TErrFinal, axis = -1), axis=-1) + np.mean(np.std(TErrFinal, axis = -1), axis=-1) TErr = (Tuz - TLinFinal).squeeze() TErrSqd = np.abs(TErr**2)
# Combine into a single DF response
nSat_temp = np.copy(nSat)
nSat_temp[np.isnan(nSat_temp)] = 1.0
nBack_temp = np.copy(nBack)
# nBack_temp[np.isnan(nBack_temp)] = 0.0
nRL_temp = np.copy(nRL)
nRL_temp[np.isnan(nRL_temp)] = 1.0
nDF = nSat_temp * nDelay * nBack_temp * nRL_temp
# Linear System
sysLin = control.tf([objServo.freqNat_rps**2], [1, 2*objServo.damp*objServo.freqNat_rps, objServo.freqNat_rps**2])
nLin = FreqTrans.FreqResp(sysLin, freqExc_rps)
#% Plot
fig = None
fig = FreqTrans.PlotGainType(cmd, np.abs(nSat), np.angle(nSat, deg=True), fig=fig, dB = False, label = 'Saturation Limit')
fig = FreqTrans.PlotGainType(cmd, np.abs(nDelay), np.angle(nDelay, deg=True), fig=fig, dB = False, label = 'Time Delay')
# fig = FreqTrans.PlotGainType(cmd, np.abs(nLim), np.angle(nLim, deg=True), fig=fig, dB = False, label = 'Hard Limit with Freeplay')
# fig = FreqTrans.PlotGainType(cmd, np.abs(nCmdLim), np.angle(nCmdLim, deg=True), fig=fig, dB = False, label = 'Command Limit')
fig = FreqTrans.PlotGainType(cmd, np.abs(nBack), np.angle(nBack, deg=True), fig=fig, dB = False, label = 'Backlash Limit')
fig = FreqTrans.PlotGainType(cmd, np.abs(nRL), np.angle(nRL, deg=True), fig=fig, dB = False, label = 'Rate Limit')
fig = FreqTrans.PlotGainType(cmd, np.abs(nDF), np.angle(nDF, deg=True), fig=fig, dB = False, label = 'Describing Function')
ax = fig.get_axes()
ax[0].set_xscale("linear")
# ax[0].set_ylabel("|NL/L|")
