TRO2021-Dynamic-Modelling/id_1dof_dyn.m at main · RViMLab/TRO2021-Dynamic-Modelling · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
clear; clc; sd_1dof;

%% Drive to identify
global drv;
drv = driveNo3;
mass = 0.096; % constant payload
[loadInertia, payload] = extLoad(mass, drv.geometry.type);

%% Experimental Data
driveDir = ['\experiments\driveNo' num2str(drv.geometry.type) '\start_stop'];
dir = [pwd driveDir];
file = '200.csv';

[recordedData, samplingPeriod, simTime, ~, velocityAvg, actrTorqueAvg, velocity, actrTorque] = ...
    getEposData(dir, file, drv.actuation.nominalTorque);

actrTorqueAvg = timeseries(medfilt1(actrTorqueAvg.data, 5, 'truncate'), actrTorqueAvg.time);
velocityAvg = timeseries(medfilt1(velocityAvg.data, 3, 'truncate'), velocityAvg.time);
[velocityInit, frictionInit, dwellTimeInit] = getInitValues(velocityAvg.data, drv.friction.kinFriction,...
    drv.friction.strbInf, drv.friction.strbVelocity);

%% Dynamic parameter identification
inSupBreak = range.drvType(drv.geometry.type).inSupBreak;
inSupVisc = range.drvType(drv.geometry.type).inSupVisc;
outSupBreak = range.drvType(drv.geometry.type).outSupBreak;
outSupVisc = range.drvType(drv.geometry.type).outSupVisc;
kinFriction = range.drvType(drv.geometry.type).kinFriction;
strbInf = range.drvType(drv.geometry.type).strbInf;
strbVelocity = range.drvType(drv.geometry.type).strbVelocity;
risingCst = range.drvType(drv.geometry.type).risingCst;
memoryCst = range.drvType(drv.geometry.type).memoryCst;

sigma0 = [inSupBreak(1); inSupVisc(1); outSupBreak(1); outSupVisc(1); kinFriction(1); strbInf(1);...
    strbVelocity(1); risingCst(1); memoryCst(1)];
lb = [inSupBreak(2); inSupVisc(2); outSupBreak(2); outSupVisc(2); kinFriction(2); strbInf(2);...
    strbVelocity(2); risingCst(2); memoryCst(2)];
ub = [inSupBreak(3); inSupVisc(3); outSupBreak(3); outSupVisc(3); kinFriction(3); strbInf(3);...
    strbVelocity(3); risingCst(3); memoryCst(3)];

options = optimset('Display', 'Iter', 'TolFun', 1e-55, 'TolX', 1e-55, 'MaxFunEvals', 100000, 'MaxIter', 5);

sigma = lsqnonlin(@(sigma) getTorqueCost(sigma, actrTorqueAvg.Data), sigma0, lb, ub, options);
sigmaOpt = display(sigma)

%% Simulation

% Optimal fit
drv.friction = sigmaOpt;
tic; sim('simINVD_1dof.slx'); toc
figure(1)
plot(actrTorqueAvg*1000); hold on;
xlabel('Time [s]'); ylabel('Torque [mNm]');title('')
plot(torqueSim*1000); hold off;

%% functions

function error = getTorqueCost(sigma, expData)

    global drv
    drv.friction.inSupBreak = sigma(1);
    drv.friction.inSupVisc = sigma(2);
    drv.friction.outSupBreak = sigma(3);
    drv.friction.outSupVisc = sigma(4);
    drv.friction.kinFriction = sigma(5);
    drv.friction.strbInf = sigma(6);
    drv.friction.strbVelocity = sigma(7);
    drv.friction.risingCst = sigma(8);
    drv.friction.memoryCst = sigma(9);

    sim('simINVD_1dof.slx')

    simData = squeeze(torqueSim.Data);
    Fx1 = griddedInterpolant(simData);
    Fx2 = griddedInterpolant(expData);
    simData = Fx1(linspace(1,numel(simData),numel(expData))');
    expData = Fx2(linspace(1,numel(expData),numel(expData))');

    error = simData - expData;

end

function optParam = display(sigma)


    inSupBreak = sigma(1);
    inSupVisc = sigma(2);
    outSupBreak = sigma(3);
    outSupVisc = sigma(4);
    kinFriction = sigma(5);
    strbInf = sigma(6);
    strbVelocity = sigma(7);
    risingCst = sigma(8);
    memoryCst = sigma(9);

    optParam = struct('inSupBreak', inSupBreak, 'inSupVisc', inSupVisc, 'outSupBreak', outSupBreak,...
        'outSupVisc', outSupVisc, 'kinFriction', kinFriction, 'strbInf', strbInf,...
        'strbVelocity', strbVelocity, 'risingCst', risingCst, 'memoryCst', memoryCst);

end

function [loadInertia, payload] = extLoad(mass, driveType)

    gravity = 9.812;

    if driveType ~= 3
        loadInertia = mass;
        payload = gravity*mass;
    else
        pulleyRad = 0.02;
        loadInertia = mass*pulleyRad^2;
        payload = gravity*mass*pulleyRad;
    end

end

function [velocityInit, frictionInit, dwellTimeInit] = getInitValues(expVelocity, kinFriction, strbInf,...
    strbVelocity)

    velocityInit = mean(expVelocity(1:5));
    frictionInit = Transmission.getFrictionCoeff(velocityInit, kinFriction, strbInf, strbVelocity);

    if velocityInit == 0
        dwellTimeInit = 10;
    else
        dwellTimeInit = 0;
    end

end