Kinematics and compliance

%% Kinematics and compliance: what a K&C rig measures, from the springs and the links
%
% Companion to the documentation page "Kinematics and compliance". Runs unchanged in GNU
% Octave (7 or later) and MATLAB. Edit the car in example_suspension.m, or type your own
% from the Kinematics tool, and run:
%
%   >> kinematics_compliance
%
% A K&C rig clamps the body and moves the wheel pads. What it reads splits in two: the
% kinematics, what the links do as the wheel moves, and the compliance, what they do as
% they bend under the tyre's forces. This script takes three of its readings.
%
% 1. The wheel standing. Its load, less the wheel's own weight, is what the springs hold;
%    the tyre squashes under the whole load, to the radius it stands on.
%
% 2. The heave element as the rig takes it. Each side moves the element by h(t), half the
%    change in its length (pd_heave), and the element's force is P + k s with s the sum
%    of both sides. At the static ride the springs hold the car up, so
%        P = (both wheels' spring load) / (h'_L + h'_R)      the element's preload,
%        k = 2 k_wheel / (h'_L + h'_R)^2                     its rate, from the rate at the wheel,
%    and one wheel's rate with both moving together is
%        dF/dt = k_wheel + (spring load) * h'' / h',
%    the second term because a rising motion ratio stiffens a loaded spring. The tyre is
%    in series with that. Where P + k s reaches zero the spring leaves its seat, and
%    below that travel the wheel hangs.
%
% 3. Compliance, with every link a 20 x 2 mm steel tube ("From tubes" in the app): each
%    link stretches by its force over its rate (pd_members gives the forces), the corner
%    is solved again with the links that long, the lower wishbone held at its angle, and
%    that is repeated until the wheel stops moving. Taken about the standing load with a
%    kilonewton either way, it gives the rig's table: toe and camber per kilonewton of
%    side force, and so on.
%
% Agreement. The app takes h' and h'' off a table of the element's travel at 2 mm steps;
% the script takes them off the exact geometry, 0.5 mm either side. The two agree to a
% few parts in ten thousand. The compliance rows agree to about 1e-5: the app stops
% iterating when the wheel centre moves less than 1e-5 mm, and it lets the pushrod
% stretch too (a turn of the rocker this script leaves out).
%
% Part of the PaceDynamics documentation, https://pacedyn.dev/docs
% SPDX-License-Identifier: MIT

[car, app] = example_suspension();
g = 9.81;
names = {'front', 'rear'};
frac = [car.frontMassFrac, 1 - car.frontMassFrac];

for ax = 1:2
  c = car.(names{ax});
  a = pd_corner(c);
  K = app.kc.(names{ax});

  %% 1. The wheel standing
  r.staticLoad = car.massTotal * g * frac(ax) / 2;                 % N
  r.springLoad = r.staticLoad - car.unsprungPerCorner * g;          % N
  r.standingRadius = car.tyreFreeRadius - r.staticLoad / car.tyreRate;   % mm

  %% 2. The heave element
  L0 = pd_heave(a, 0);
  share = @(t) (L0 - pd_heave(a, t)) / 2;                           % one side's share, + = compressed
  d = 0.5;
  h1 = (share(d) - share(-d)) / (2 * d);                            % h'
  h2 = (share(d) - 2 * share(0) + share(-d)) / d^2;                 % h''
  ch = app.wishbones.heave.(names{ax}).chord;                       % the rate at the wheel, as the
  mr = (pd_heave(a, ch(1)) - pd_heave(a, ch(2))) / (ch(2) - ch(1)); % Rates readout takes it
  kWheel = c.heaveRate * mr^2 / 2;
  r.elementRate = 2 * kWheel / (2 * h1)^2;
  r.preload = 2 * r.springLoad / (2 * h1);
  r.free = -r.preload / r.elementRate;
  r.wheelRateAtLoad = kWheel + r.springLoad * h2 / h1;
  r.rideRate = 1 / (1 / r.wheelRateAtLoad + 1 / car.tyreRate);
  r.droop = fzero(@(t) 2 * share(t) - r.free, [-40, 0]);

  %% 3. Compliance, links "From tubes"
  rate = c.linkRates([3, 4, 1, 2, 5]);                              % lower front, lower rear, upper front, upper rear, tie
  probes = {'lateral', [0, 1000, 0], 0; 'longitudinal', [1000, 0, 0], 0; 'aligning', [0, 0, 0], 1e5};
  for i = 1:3
    for sgn = [-1, 1]
      F = [0, 0, r.staticLoad] + sgn * probes{i, 2};              % N, corner frame: y outboard
      M = [0, 0, sgn * probes{i, 3}];                               % N*mm about the corner's vertical
      stretch = zeros(1, 5);
      for it = 1:20
        p = pd_upright(a, 'beta', 0, 0, stretch);
        m = pd_metrics(p.at(a.ubj), p.at(a.lbj), p.at(a.C), p.n, p.at(a.tieOuter), c.surveyRadius);
        geo = struct('ucaFront', a.ucaFront, 'ucaRear', a.ucaRear, 'lcaFront', a.lcaFront, 'lcaRear', a.lcaRear, ...
                     'uprightUpper', p.at(a.ubj), 'uprightLower', p.at(a.lbj), 'tieInner', a.tieInner, ...
                     'tieOuter', p.at(a.tieOuter), 'wheelCenter', p.at(a.C), 'contactPatch', m.patch, ...
                     'pushrodOuter', a.pushrodOuter, 'rockerToPushrod', a.rockerToPushrod);
        f = pd_members(geo, F, M);
        next = [f.lcaFront, f.lcaRear, f.ucaFront, f.ucaRear, f.tieRod] ./ rate;   % mm
        if max(abs(next - stretch)) < 1e-12, break; end
        stretch = next;
      end
      got(i, (sgn + 3) / 2) = m;                                    % 1 = minus, 2 = plus
    end
  end
  half = @(i, field, k) (got(i, 2).(field)(k) - got(i, 1).(field)(k)) / 2;
  r.toePerLateral      = half(1, 'toe', 1);                         % deg/kN
  r.camberPerLateral   = half(1, 'camber', 1);
  r.toePerLongitudinal = half(2, 'toe', 1);
  r.toePerAligning     = half(3, 'toe', 1);                         % deg per 100 N*m
  r.lateralPatch       = half(1, 'patch', 2);                       % mm/kN
  r.longitudinalPatch  = half(2, 'patch', 1);
  out.(names{ax}) = r;
end

%% Against the app
W = {'Front', 'Rear'};
for ax = 1:2
  r = out.(names{ax});  K = app.kc.(names{ax});  T = app.kc.tubes.(names{ax});
  fprintf('\n  %s, the wheel standing (exact)\n', W{ax});
  pd_compare('Wheel load [N]',                     r.staticLoad,     K.staticLoad);
  pd_compare('On the springs [N]',                 r.springLoad,     K.springLoad);
  pd_compare('Standing radius [mm]',               r.standingRadius, K.standingRadius);
  fprintf('  %s, the heave element (to 1e-3)\n', W{ax});
  pd_compare('Element rate [N/mm]',                r.elementRate,    K.elementRate, 1e-3);
  pd_compare('Element preload [N]',                r.preload,        K.preload, 1e-3);
  pd_compare('To its free length [mm]',            r.free,           K.free, 1e-3);
  pd_compare('Wheel rate at load [N/mm]',          r.wheelRateAtLoad, K.wheelRateAtLoad, 1e-3);
  pd_compare('Ride rate, tyre in series [N/mm]',   r.rideRate,       K.rideRate, 1e-3);
  pd_compare('Wheel hangs below [mm]',             r.droop,          K.droop, 1e-3);
  fprintf('  %s, compliance "From tubes" (to 1e-4)\n', W{ax});
  pd_compare('Toe per side force [deg/kN]',        r.toePerLateral,      T.toePerLateral, 1e-4);
  pd_compare('Camber per side force [deg/kN]',     r.camberPerLateral,   T.camberPerLateral, 1e-4);
  pd_compare('Toe per fore-aft force [deg/kN]',    r.toePerLongitudinal, T.toePerLongitudinal, 1e-4);
  pd_compare('Toe per aligning torque [/100 N*m]', r.toePerAligning,     T.toePerAligning, 1e-4);
  pd_compare('Patch sideways [mm/kN]',             r.lateralPatch,       T.lateralPatch, 1e-4);
  pd_compare('Patch fore-aft [mm/kN]',             r.longitudinalPatch,  T.longitudinalPatch, 1e-4);
end
pd_compare();