57 lines
3.0 KiB
Mathematica
57 lines
3.0 KiB
Mathematica
|
function [Xhi, numParam, Xhi_aug] = getStandardParameterVector(InertiaDH, Moment, Mass, Ia, frictionModel, frictionParameters, varargin)
|
||
|
|
|
||
|
|
% Authors: Quentin Leboutet, Julien Roux, Alexandre Janot and Gordon Cheng
|
||
|
|
%
|
||
|
|
% This function generates the vector of standard parameters Xhi of a robot
|
||
|
|
% from its separated dynamic and friction parameters
|
||
|
|
|
||
|
|
|
||
|
|
% Check dimentionality
|
||
|
|
|
||
|
|
if (size(InertiaDH,3) == size(Moment,2)) && (size(InertiaDH,3) == length(Mass)) && (size(InertiaDH,3) == length(Ia))
|
||
|
|
nbDOF = size(InertiaDH,3);
|
||
|
|
numParam = zeros(nbDOF,1);
|
||
|
|
else
|
||
|
|
error('getStandardParameterVector(): Error in parameter dimentionality !');
|
||
|
|
end
|
||
|
|
|
||
|
|
for i=1:nbDOF
|
||
|
|
XXi = InertiaDH(1,1,i);
|
||
|
|
XYi = InertiaDH(1,2,i);
|
||
|
|
XZi = InertiaDH(1,3,i);
|
||
|
|
YYi = InertiaDH(2,2,i);
|
||
|
|
YZi = InertiaDH(2,3,i);
|
||
|
|
ZZi = InertiaDH(3,3,i);
|
||
|
|
Mi = Mass(i);
|
||
|
|
MXi = Moment(1,i);
|
||
|
|
MYi = Moment(2,i);
|
||
|
|
MZi = Moment(3,i);
|
||
|
|
switch frictionModel
|
||
|
|
% Only Coulomb and viscous frictions are linear and can be identified simultaneously with the other parameters.
|
||
|
|
% Nonlinear friction models require state dependant parameter identification
|
||
|
|
case 'no'
|
||
|
|
Xhi_block = [XXi; XYi; XZi; YYi; YZi; ZZi; MXi; MYi; MZi; Mi; Ia(i); frictionParameters.Tau_off(i)];
|
||
|
|
case 'Viscous'
|
||
|
|
Xhi_block = [XXi; XYi; XZi; YYi; YZi; ZZi; MXi; MYi; MZi; Mi; Ia(i); frictionParameters.Fv(i); frictionParameters.Tau_off(i)];
|
||
|
|
case 'Coulomb'
|
||
|
|
Xhi_block = [XXi; XYi; XZi; YYi; YZi; ZZi; MXi; MYi; MZi; Mi; Ia(i); frictionParameters.Fc(i); frictionParameters.Tau_off(i)];
|
||
|
|
case 'ViscousCoulomb'
|
||
|
|
Xhi_block = [XXi; XYi; XZi; YYi; YZi; ZZi; MXi; MYi; MZi; Mi; Ia(i); frictionParameters.Fv(i); frictionParameters.Fc(i)];
|
||
|
|
case 'ViscousCoulombOff'
|
||
|
|
Xhi_block = [XXi; XYi; XZi; YYi; YZi; ZZi; MXi; MYi; MZi; Mi; Ia(i); frictionParameters.Fv(i); frictionParameters.Fc(i); frictionParameters.Tau_off(i)];
|
||
|
|
case 'Stribeck'
|
||
|
|
Xhi_block = [XXi; XYi; XZi; YYi; YZi; ZZi; MXi; MYi; MZi; Mi; Ia(i); frictionParameters.Tau_off(i)];
|
||
|
|
case 'LuGre'
|
||
|
|
Xhi_block = [XXi; XYi; XZi; YYi; YZi; ZZi; MXi; MYi; MZi; Mi; Ia(i); frictionParameters.Tau_off(i)];
|
||
|
|
otherwise
|
||
|
|
Xhi_block = [XXi; XYi; XZi; YYi; YZi; ZZi; MXi; MYi; MZi; Mi; Ia(i); frictionParameters.Tau_off(i)];
|
||
|
|
end
|
||
|
|
numParam(i) = numel(Xhi_block);
|
||
|
|
Xhi((i-1)*numel(Xhi_block)+1:i*numel(Xhi_block),1)=Xhi_block;
|
||
|
|
% Total Dynamic parameters vector:
|
||
|
|
Xhi_tot_block = [XXi; XYi; XZi; YYi; YZi; ZZi; MXi; MYi; MZi; Mi; Ia(i); frictionParameters.Fv(i); frictionParameters.Fc(i); frictionParameters.Fs(i); frictionParameters.Vs(i); frictionParameters.Es(i); frictionParameters.Sigma_0(i); frictionParameters.Sigma_1(i); frictionParameters.Sigma_2(i); frictionParameters.Z0(i)]; % Only Coulomb and viscous frictions are linear and can be identified simultaneously with the other parameters...
|
||
|
|
Xhi_aug((i-1)*numel(Xhi_tot_block)+1:i*numel(Xhi_tot_block),1) = Xhi_tot_block;
|
||
|
|
end
|
||
|
|
end
|
||
|
|
|