remove jerks in p3dx movement

edit

Hi,

when i use aria matlab interface to move my p3dx. it receives jerks during movement especially very visible delays and jerks while moving on turns. can any one help? here is the sample code

points = {{3000, 0}, {3000, 3000}, {0, 3000}, {0, 0}}
velMult = 0.5 distThresh = 350 % mm angleThresh = 0.5 % deg aria_init arrobot_connect() arrobot_resetpos() curPoint = 1 xpositions = []; ypositions = []; %fig = figure('NextPlot', 'add') %ph = newplot pointsMoved = 5;
disp 'Press control-c to end this script. Call arrobot_stop to stop the robot, call arrobot_disconnect to disconnect from the robot.' while (pointsMoved <= 4)

  % get current robot position from aria
  rx = arrobot_getx;
  ry = arrobot_gety;

  xpositions = [xpositions rx];
  ypositions = [ypositions ry];
 % plot(xpositions, ypositions)

  % Check if we've reached goal point
  p = points{curPoint};
  px = p{1};
  py = p{2};
  d = sqrt( (px - rx)^2 + (py - ry)^2 );
  disp(sprintf(' distance remaining: %f', d))
  if d <= 25 %distThresh
    disp 'reached point'
    arrobot_stop

    % choose next point in list
    curPoint = curPoint + 1
    pointsMoved  = pointsMoved  + 1        
    if curPoint > length(points)
      curPoint = 1;          
    end
    p = points{curPoint}; 
    px = p{1};
    py = p{2};

    % determine how far to rotate to face this next point
    rt = arrobot_getth;
    dt =  (atan2(py - ry, px - rx) * (180/3.14159)) - rt;
    dt = mod((dt + 180), 360) - 180; % restrict to (-180,180)

    % request robot to turn
    disp(sprintf('Turning by %d deg.', dt))
    arrobot_setdeltaheading(dt)

    % wait until heading change is achieved within specified threshold
    target = rt + dt
    target = mod((target + 180), 360) - 180 % restrict to (-180,180)
    while abs(arrobot_getth() - target) > (angleThresh)  % note, get newest robot theta from aria each time
      disp(sprintf(' angle remaining %f', abs(arrobot_getth() - target)))
      pause(0.05)
    end
    disp 'Heading done.'
  else
    % request robot velocity proportionally to distance remaining
    arrobot_setvel(d*velMult)
    % Could also set delta heading or rotational velocity here to continuously
    % correct angle as well.
  end
  pause(0.08)
end