-
Notifications
You must be signed in to change notification settings - Fork 0
/
OptimiseTailobatics.m
71 lines (50 loc) · 2.16 KB
/
OptimiseTailobatics.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
%% Optimisation of motion pattern for Tail flip in robot
%% RSiddall 2021, optimisation process adapted from: https://uk.mathworks.com/videos/matlab-and-simulink-robotics-arena-walking-robots-part-3-trajectory-optimization-1506440520726.html
clear
close all
run TailobaticVariables
mdlName = 'TailFlip2DOF';
load_system(mdlName)
% videoFWrite = VideoWriter('AnimatedOptimisation.mp4','MPEG-4');
% open(videoFWrite);
videoFWrite = [];
F1 = figure('Units','normalized','Position',[0 0 0.8 0.9]);
%% Initial guess inputs
FlipTime = 0.5;
PitchInputGuess = [0 90 90 90 0];
RollInputGuess = [0 90 0 -90 0];
numPoints = numel(PitchInputGuess);
WaypointTime = linspace(0,FlipTime,numPoints);
TrajectoryParams = [PitchInputGuess,RollInputGuess];
tmax = FlipTime*2;
%% Set optimization options
opts = optimoptions('ga');
opts.Display = 'iter';
opts.MaxGenerations = 20;
opts.PopulationSize = 20;
opts.InitialPopulationMatrix = repmat(TrajectoryParams,[10 1]); % Add copies of initial gait
opts.PlotFcn = @gaplotbestf; % Add progress plot of fitness function
%% Set bounds and constraints
% Upper and lower angle bounds
upperBnd = [0 100*ones(1,numPoints-2) 0, ... % Pitch limits
0 370*ones(1,numPoints-2) 0]; ... % Roll limits
lowerBnd = [0 -100*ones(1,numPoints-2) 0, ... % Pitch limits
0 -370*ones(1,numPoints-2) 0]; ... % Roll limits
%% Run optimization
% Count the number of simulation executions required
global counter
counter = 0;
ticOuter = tic;
costFcn = @(TrajectoryParams)simulateFlip(TrajectoryParams,WaypointTime,FlipTime,plotting,videoFWrite);
disp(['Running optimization. Population: ' num2str(opts.PopulationSize) ...
', Max Generations: ' num2str(opts.MaxGenerations)])
[pFinal,reward] = ga(costFcn,numPoints*2,[],[],[],[], ...
lowerBnd,upperBnd,[],1:numPoints*2,opts);
disp(['Final reward function value: ' num2str(reward)])
toc(ticOuter)
outFileName = ['optimizedData_' datestr(now,'ddmmmyy_HHMM')];
save(outFileName);
tmax = FlipTime*3;
plotting = true;
simulateFlip(pFinal,WaypointTime,FlipTime,plotting,videoFWrite);
close(videoFWrite);