43 lines
2.4 KiB
Matlab
43 lines
2.4 KiB
Matlab
function [frictionForce] = computeFrictionModel(robotName, Qp, frictionParameters, Z_m1, dt)
|
|
|
|
% Authors: Quentin Leboutet, Julien Roux, Alexandre Janot and Gordon Cheng
|
|
%
|
|
% Compute robot friction model in symbolic from. Returns a [nbDOFx(nbDOF+1)]
|
|
% matrix, whose columns are the friction torques generated with each model.
|
|
|
|
frictionForce = sym(zeros(numel(Qp), 8));
|
|
|
|
% No friction:
|
|
frictionForce(:,1) = 0*Qp + frictionParameters.Tau_off;
|
|
% Viscous friction:
|
|
frictionForce(:,2) = diag(frictionParameters.Fv)*Qp + frictionParameters.Tau_off;
|
|
% Coulomb friction:
|
|
frictionForce(:,3) = diag(frictionParameters.Fc)*tanh(100*Qp) + frictionParameters.Tau_off;
|
|
% Intagrated Viscous and Coulomb friction:
|
|
frictionForce(:,4) = diag(frictionParameters.Fv)*Qp + diag(frictionParameters.Fc)*tanh(100*Qp);
|
|
if strcmp(robotName, 'TX40') || strcmp(robotName, 'RX90')
|
|
frictionForce(5,4) = frictionForce(5,4) + Qp(6)*frictionParameters.Fvm(6) + tanh(100*Qp(6))*frictionParameters.Fcm(6);
|
|
frictionForce(6,4) = frictionForce(6,4) + Qp(5)*frictionParameters.Fvm(6) + tanh(100*Qp(5))*frictionParameters.Fcm(6);
|
|
end
|
|
% Intagrated Viscous and Coulomb friction with offset:
|
|
frictionForce(:,5) = diag(frictionParameters.Fv)*Qp + diag(frictionParameters.Fc)*tanh(100*Qp) + frictionParameters.Tau_off;
|
|
if strcmp(robotName, 'TX40') || strcmp(robotName, 'RX90')
|
|
frictionForce(5,5) = frictionForce(5,4) + Qp(6)*frictionParameters.Fvm(6) + tanh(100*Qp(6))*frictionParameters.Fcm(6);
|
|
frictionForce(6,5) = frictionForce(6,4) + Qp(5)*frictionParameters.Fvm(6) + tanh(100*Qp(5))*frictionParameters.Fcm(6);
|
|
end
|
|
% Stribeck friction:
|
|
frictionForce(:,6) = diag(frictionParameters.Fv)*Qp + diag(frictionParameters.Fc + diag(frictionParameters.Fs - frictionParameters.Fc)*exp(-(abs(Qp./frictionParameters.Vs).^frictionParameters.Es)))*tanh(100*Qp) + frictionParameters.Tau_off;
|
|
% LuGre friction:
|
|
[F, Z] = LuGre(Z_m1, Qp, frictionParameters.Fc, frictionParameters.Fs, frictionParameters.Vs, frictionParameters.Sigma_0, frictionParameters.Sigma_1, frictionParameters.Sigma_2, frictionParameters.Es, dt);
|
|
frictionForce(:,7) = F + frictionParameters.Tau_off;
|
|
% LuGre state:
|
|
frictionForce(:,8) = Z;
|
|
end
|
|
|
|
function [F, Z] = LuGre(Z, Qp, Fc, Fs, Vs, Sigma_0, Sigma_1, Sigma_2, Es, dt)
|
|
phi = (Fc + diag(Fs - Fc)*exp(-(abs(Qp./Vs).^Es))) ./ Sigma_0;
|
|
Zp = Qp - abs(Qp).*Z ./ phi;
|
|
Z = Z + Zp*dt;
|
|
F = diag(Sigma_0)*Z + diag(Sigma_1)*Zp + diag(Sigma_2)*Qp;
|
|
end
|