-
Notifications
You must be signed in to change notification settings - Fork 1
/
ex_problem2Rotated.m
78 lines (62 loc) · 1.89 KB
/
ex_problem2Rotated.m
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
%% Initialize
problemName='ex3Rotated';
loadSubDirectories();
close all
robot=struct();
robot.distLen=1;
robot.proxLen=1;
robot.bodyPos=[0,0,1];
robot.bodyRot=[0,0,0];
robot.bodyW=1;
robot.bodyL=1;
fResult=[4 -5 -4];
nResult=[0 0 0];
% init=[...
% randomPoint(regions(1).mesh);...
% randomPoint(regions(2).mesh);...
% randomPoint(regions(3).mesh);...
% randomPoint(regions(4).mesh)];
init =[...
1.6738 1.7531 0.0185;...
-1.5422 1.3406 0.0375;...
-1.1492 -1.6833 0.0504;...
1.8399 0.6888 0.1484];
initStab=angularStabilityMargin(robot.bodyPos,init,fResult,nResult);
exportInitPts(init,problemName);
exportRobot(robot,problemName);
%% generate mesh
minAngles=...
[-70 -70 -70;...
110 -70 -70;...
110 -70 -70;...
-70 -70 -70];
maxAngles=...
[70 70 70;...
250 70 70;...
250 70 70;...
70 70 70];
regions=buildAllRegions(minAngles,maxAngles,10,'wave',robot);
displayRegions(regions)
exportRegions(regions,problemName);
%% Global Search - order type 3
orders=stepOrders(init,initStab,robot.bodyPos,fResult,nResult,4,3);
[xstars,fstars,exitflags]=optimizeAllShuffles(init,orders,fResult,nResult,regions,robot,true);
[xstars2,fstars2,orders2,exitflags2]=findMaxResult(orders,xstars,-fstars,exitflags);
%% results
Nresults=length(exitflags2);
for i=1:Nresults
endStab(i)=showResult(xstars2{i},orders2{i},init,regions,robot,fResult,nResult,exitflags2(i));
end
%% pareto front
[paretoFront, indices]=stepPareto(xstars,orders,exitflags,init,fResult,nResult,regions,robot);
plotPareto(paretoFront);
%% before and after
n=indices(end);
[endStab,stability,interstability]=showResult(xstars{n},orders{n},...
init,regions,robot,fResult,nResult,exitflags(n));
plotInitial(init)
plotStability(interstability,stability)
%%
iexport=indices(end);
xyz=xyzOfPoints(orders{iexport},xstars{iexport},regions);
exportShuffle(orders{iexport},xyz,'ex3',problemName);