function [f, ud, fcone] = gcontact( K, D, mu, p, pd, u )
% gcontact calculate 2D/3D ground reaction forces due to compliant contact
% [f,ud,fcone] = gcontact(K,D,mu,p,pd,u) calculates the ground-reaction
% forces acting on a set of points due to contact with a compliant ground
% plane. In 3D, the x-y plane is the ground plane, and z is up. In 2D,
% the x axis is the ground plane and y is up. K, D and mu are scalars
% giving the stiffness, damping and friction coefficients of the ground
% plane. p and pd are (nD)x(np) matrices giving the positions and
% velocities of a set of np points in nD-dimensional space (nD=2 or 3).
% gcontact uses the size of p to determine whether it is working in 2D or
% 3D space. u is an (nD-1)x(np) matrix containing the tangential
% deformation state variables used to implement the friction model. f is
% an (nD)x(np) matrix of calculated ground-reaction forces. If f(nD,i)==0
% then point i is not in contact; and if f(nD,i)>0 then point i is in
% contact. ud is the time derivative of u; and fcone is a 1x(np) matrix
% indicating the stick/slip status of each point. If fcone(i)<=1 then
% point i is sticking. This can happen only if point i is in contact. To
% facilitate use with Simulink, if gcontact is called with only a single
% return value then it returns the concatenation of f, ud and fcone (i.e.,
% the return value is [f;ud;fcone]). To model a frictionless surface,
% gcontact can be called as follows: f=gcontact(K,D,0,p,pd), where p, pd
% and f are all 1x(np) matrices containing only the normal components of
% position, velocity and reaction force.
% Implementation note: This function implements the nonlinear
% spring-damper model described in Azad & Featherstone "Modelling the
% Contact Between a Rolling Sphere and a Compliant Ground Plane", ACRA
% 2010, Brisbane, Australia, Dec. 1-3. However, the implementation here is
% for points rather than spheres.
% Normal force calculation
z = p(end,:);
zd = pd(end,:);
zr = sqrt(max(0,-z));
fn = zr .* ((-K)*z - D*zd);
if mu == 0 % special case: modelling frictionless
f = max( 0, fn ); % contact, so return the normal force now
return
end
% Algorithm for full normal+tangent force calculation: (1) set all output
% variables to their correct no-contact values; (2) correct all the values
% that are wrong because their respective points are in contact. Step (2)
% is subdivided into (2a) calculate correct values for sticking; (2b)
% adjust the values for those points that are found to be slipping; (2c)
% make the corrections to the output variables.
% Step 1: set output variables to correct no-contact values
[nd,np] = size(p);
f = zeros(nd,np);
ud = (-K/D) * u;
fcone = repmat(2,1,np); % 2 is just an arbitrary value > 1
in_ctact = fn > 0;
% Step 2: correct all those values that are wrong because their respective
% points are in contact. Note: variables with names ending in c have one
% column for each point that is in contact.
if any(in_ctact)
% Step 2a: calculate correct values for sticking
fnc = fn(in_ctact);
udc = pd(1:end-1,in_ctact);
zrc = repmat(zr(in_ctact),nd-1,1);
Kc = K * zrc;
Dc = D * zrc;
fKc = Kc .* u(:,in_ctact);
ftc = -fKc - Dc .* udc; % correct tangent force for sticking
% now test for slipping
fslip = mu * fnc;
if nd == 3 % 3D contact
ftcmag = sqrt(sum(ftc.^2,1));
else % 2D contact
ftcmag = abs(ftc);
end
fconec = ftcmag ./ fslip;
slipping = fconec > 1;
% Step 2b: adjust values for those points that are found to be slipping
if any(slipping)
attenuator = repmat(fconec(slipping),nd-1,1);
fts = ftc(:,slipping) ./ attenuator;
ftc(:,slipping) = fts;
udc(:,slipping) = -(fts + fKc(:,slipping)) ./ Dc(:,slipping);
end
% Step 2c: apply the corrections to the output variables
f(:,in_ctact) = [ftc;fnc];
ud(:,in_ctact) = udc;
fcone(in_ctact) = min(2,fconec);
end
if nargout == 1
f = [f; ud; fcone];
end