AXY:Resonance frequency model for Delta kinematics/Verification

The following 3D-graphs show the resonance frequency of TCP over the whole X/Y working space on a certain Z-level.

• The surface is the modeled frequency.
• The dots are measured frequencies.

DR1200G3-4S

• Resonance frequency for motions in main directions (Z-level 900mm)
• 

  X-direction

• 

  Y-direction

• 

  Z-direction

Script for aico.motionAnalyser

The graphs above are created using the following aico.motionAnalyser script.

cfDir      = [1; 0; 0];
fZ         = 0.900;
sRecPath   = 'D:\\PROJEKTE\\Delta\\ResFreqModelValidation\\DR1200-G3\\';

DeltaDR1200       = delta_newDR1200_4S();
DeltaDR1200.fM_P  = 0.9;
DeltaDR1200.fCr   = 50000;


[Fig, Ax] = figure_new('Resonance frequency on Z-level ' + string(fZ));

// plot calculated frequencies first
RecPDm   = record_new('PDm',  cfDir);
RecPDDm  = record_new('PDDm', [0; 0; 0]);

[mfXmesh, mfYmesh] = meshgrid(-0.600:0.05:0.600, -0.600:0.05:0.600);
mfZmesh            = zeros(mfXmesh);
  
for iI = 1 : size(mfXmesh, 1)
  for iJ = 1 : size(mfYmesh, 2)

    if (norm([mfXmesh(iI, iJ); mfYmesh(iI, iJ)]) > 0.600) then 
      mfZmesh(iI,iJ)  = %nan;
      continue; 
    end

    RecPm   = record_new('Pm', [mfXmesh(iI, iJ); mfYmesh(iI, iJ); fZ]);
    
    [RecQRad, RecQDRad, RecQDDRad]                        = delta_inverseKinematics(DeltaDR1200, RecPm, RecPDm, RecPDDm);
    [RecResFreq, RecStiffness, RecStiffnessJoints, RecStiffnessRods]  = delta_resonanceFrequency(DeltaDR1200, RecQRad, RecPm, RecPDm);
      
    mfZmesh(iI,iJ)  = record_getData(RecResFreq);
    
  end // iJ
end // iI

sca(Ax);
mesh(mfXmesh, mfYmesh, mfZmesh);

//  abort

// now plot the mesured pfequencies
function fResonance = findResonanceFrequency(RecData)
  
  // disgard signal before first zero crossing
  for iIndex = 2 : record_getLength(RecData)
    if (sign(record_getData(RecData, 1, iIndex-1)) <> sign(record_getData(RecData, 1, iIndex))) then
      RecData = record_crop(RecData, iIndex:$)
      break
    end
  end
  
  // compute fourier transform  
  rfData = record_getData(RecData, 1)
  rfFFT  = fft(rfData - mean(rfData))
  
  // find main frequency
  [rfDummy, riIndeces] = sort(abs(rfFFT(1:($/2))))
  
  // return resonance frequency
  fResonance1 = (riIndeces(1)-1) / (record_getSamplingPeriod(RecData) * record_getLength(RecData)) 
  fAbs1       = abs(rfFFT(riIndeces(1))) / (record_getLength(RecData)/2)
  fResonance2 = (riIndeces(2)-1) / (record_getSamplingPeriod(RecData) * record_getLength(RecData)) 
  fAbs2       = abs(rfFFT(riIndeces(2))) / (record_getLength(RecData)/2)
  fResonance  = (fResonance1*fAbs1 + fResonance2*fAbs2) / (fAbs1+fAbs2)
  
  fAmp       = abs(rfFFT(riIndeces(1))) / (record_getLength(RecData)/2)
  fPhase     = atan(imag(rfFFT(riIndeces(1))),real(rfFFT(riIndeces(1))))

  // plot verification
  rfTimes   = record_getSamplingPeriod(RecData) : record_getSamplingPeriod(RecData) : record_getSamplingPeriod(RecData) * record_getLength(RecData)
  rfCosData = fAmp * cos(2*%pi*fResonance * rfTimes + fPhase)
  RecCos    = record_new('Cosine', rfCosData, record_getSamplingPeriod(RecData), record_getStartTime(RecData)) 
  //axes_plot(RecData, RecCos)  
  //axes_plot2d(signal_fourierTransform(RecData)) 
  
  //rfFr  = (0:(record_getLength(RecData) / 2)) / (record_getSamplingPeriod(RecData) * record_getLength(RecData)); //associated frequency vector
  //clf
  //plot(rfFr, abs(rfFFT(1:length(rfFr))))
  //plot(fResonance, 10, 'r*')
endfunction

lRec = record_loadAmcs(sRecPath + 'STF_G1.REC');

RecPhase    = lRec(1);
RecSetPoint = record_join(lRec(2), lRec(3), lRec(4));
RecMoveDir  = record_join(lRec(5), lRec(6), lRec(7));
RecHere     = record_join(lRec(8), lRec(9), lRec(10));

mfPos  = [];
rfFreq = [];

for iPhase = 1 : max(record_getData(RecPhase))
  
  rbPhaseIndizes  = record_getData(RecPhase) == iPhase;
  
  // select data for phase
  RecPhSetPoint = record_crop(RecSetPoint, rbPhaseIndizes);
  RecPhMoveDir  = record_crop(RecMoveDir,  rbPhaseIndizes);
  RecPhHere     = record_crop(RecHere,     rbPhaseIndizes);
  
  // filter position and moving direction
  if (((abs(record_getData(RecPhMoveDir, :, 1))' * abs(cfDir)) < 1) | (abs(record_getData(record_scale(RecPhSetPoint, 0.001), 3, 1) - fZ) > 0.01)) then
    continue;
  end
 
  lRecScope = record_loadTPS2000(sRecPath+string(iPhase)+'.csv');
  
  rfFreq($+1)    = findResonanceFrequency(lRecScope(1)); 
  
  mfPos(:, $+1) = record_getData(record_scale(RecPhSetPoint, 0.001), :, 1);
  
end // iPhase

sca(Ax);
param3d(mfPos(1,2:$), mfPos(2,2:$), rfFreq');
set(axes_getPolylines(Ax), 'mark_mode', 'on');
set(axes_getPolylines(Ax), 'mark_size', 3);
set(axes_getPolylines(Ax), 'line_mode', 'off');