Robotic_collectives_simulation/vb_runThreePulseForce.m at main · ksw851216/Robotic_collectives_simulation · 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
%% Initialize workspace
clearvars;
close all;

%% Create a output folder
odr='./dataVibration/';
if ~exist(odr,'dir')
    mkdir(odr);
end

%% Parameter scan range
mtTrqAll=0.001:0.001:0.035;
extFrcAll=0.09:0.01:0.2;
trmx=numel(mtTrqAll);

mtTrq=0;
mtFlc=0;

%% Repeat cases
rmx=1;
sdDtAll=cell(rmx,1);

for rpc=1:rmx

    %% define model parameters
    % dskRd: disk radius, R0.
    % atCon: attraction strength
    % atDis: attraction range
    % mtTrq: mean motor torque
    % mtFlc: motor torque fluctuation magnitude
    % mtFlcWth: motor torque fluctuation width
    % mtFlcCut: motor torque fluctuation cut off
    % plPrd: pulse period (up+down)
    % plFlc: pulse fluctuation (0-1, 0:no fluctuation, 1:max fluctuation)
    % afCon: angular friction between disk and floor

    [dskRd,when]=deal(1,'20230915');
    [atCon,atDis]=deal(0.05,3);
    [mtFlcWth,mtFlcCut]=deal(0.00025,0.001);
    [plPrd,plFlc]=deal(30,0);
    extFrc=0.1557;
    afCon=10;

    %% Create a structure variable that contains all model parameters
    gmp=struct;
    [gmp.dskRd,gmp.afCon]=deal(dskRd,afCon);
    [gmp.atCon,gmp.atDis]=deal(atCon,atDis);
    [gmp.mtTrq,gmp.mtFlc,gmp.mtFlcWth,gmp.mtFlcCut]=...
        deal(mtTrq,mtFlc,mtFlcWth,mtFlcCut);
    [gmp.plPrd,gmp.plFlc]=deal(plPrd,plFlc);
    gmp.extFrc=extFrc;
    gmp.dt=0.05;
    gmp.plCnt=floor(gmp.plPrd/gmp.dt);

    rng('shuffle');

    %% Generate initial configuration of four disks
    dis=(2/gmp.atCon-gmp.atDis)/(1/gmp.atCon-1)*gmp.dskRd;
    height=sqrt(3)*dis/2;
    sd=[0,0;dis,0;dis/2,height];
    gmp.angTol=0.05;
    gmp.nFa=size(sd,1);
    sdOrn=rand(gmp.nFa,1)*2*pi;
    extFrcMat=zeros(gmp.nFa,2);
    extFrcMat(3,2)=-extFrc;

    %% Set initial motor torque values
    sdMt=ones(gmp.nFa,1)*gmp.mtTrq;
    mtCnt=zeros(gmp.nFa,1);
    for fac=1:gmp.nFa
        mtCnt(fac)=floor((0.8*rand()+0.2)*gmp.plCnt);
        mtCnt(fac)=mtCnt(fac)-mod(mtCnt(fac),2);
    end
    hfCnt=floor(mtCnt/2);

    itMx=gmp.plCnt*30;
    tmSt=gmp.plCnt/10;
    dmx=itMx/tmSt+1;

    sdDt=cell(dmx,1);
    sdDt{1}=[sd,sdOrn,sdMt];
    dtc=2;

    for tmc=1:itMx
        [sd,sdOrn]=vb_iterationForce(sd,sdOrn,sdMt,gmp,extFrcMat);
        sd(sd(:,2)<0,2)=0;
        mtCnt=mtCnt-1;

        if any(mtCnt==0) && gmp.mtFlc~=0
            fId=find(mtCnt==0);
            for fac=1:numel(fId)
                mtCnt(fId(fac))=floor((0.4*rand()+0.8)*gmp.plCnt);
                mtCnt(fId(fac))=mtCnt(fId(fac))-mod(mtCnt(fId(fac)),2);
                hfCnt(fId(fac))=floor(mtCnt(fId(fac))/2);
                chk=0;
                while chk==0
                    mtFlcMag=normrnd(gmp.mtFlc,gmp.mtFlcWth);
                    if mtFlcMag>mtFlc-mtFlcCut && mtFlcMag<mtFlc+mtFlcCut
                        chk=1;
                    end
                end
                sdMt(fId(fac))=gmp.mtTrq+mtFlcMag;
            end
        end

        if any(mtCnt==hfCnt) && gmp.mtFlc~=0
            fId=find(mtCnt==hfCnt);
            for fac=1:numel(fId)
                sdMt(fId(fac))=gmp.mtTrq+(gmp.mtTrq-sdMt(fId(fac)));
            end
        end

        if mod(tmc,tmSt)==0
            sdDt{dtc}=[sd,sdOrn,sdMt];
            dtc=dtc+1;
        end
    end

    sdDtAll{rpc}=sdDt;
end