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