% Demonstration of algorithms on the inverted pendulum problem

%% Initialization

% common config
ccfg.problem = 'ipsetup_problem';
% this configures the problenm with angular velocity bound 15*pi rad/s, maximum voltage 3V, and
% sampling time 5ms
ccfg.model_params = {[], 'type=ip maxx=[pi;15*pi] maxu=3', 'Ts=0.005'};
ccfg.gamma = 0.98;  % discount factor
ccfg.eps = 1;       % convergence threshold
% some output and visualization config
ccfg.target = 'screen'; 
ccfg.datadir = pwd;
ccfg.viscfg = struct;
ccfg.viscfg.figsize = [800 500];
ccfg.viscfg.pause = -1;
 
%% Fuzzy Q-iteration
cfg = ccfg;
cfg.xgrids = symequidgrid([41 41], [pi, 15*pi]);    % state interpolation grid (# points, bounds)
cfg.ugrids = {symequidgrid(5, 3)};                  % action discretization grid (#points, bounds)
cfg.run = 1;                                        % run the algorithm
cfg.datafile = 'ipsetup_fuzzyqidemo';               % save to this data file
% some output and visualization config
cfg.savetheta = 1;
cfg.visualize = 1;
cfg.iterdisp = 5;
cfg.itervis = 10;
fuzzyqi(cfg);

% look at a controlled trajectory with this solution, from the pointing=-down state
fuzzyqi('replay datafile=ipsetup_fuzzyqidemo x0=[-pi,0] tend=3');


%% LSPI
cfg = ccfg;
% create an approximator with 11x15 RBFs over x and a 3-point discretization over u
% with the RBFs and the u discretization equally spaced
cfg.approx = 'enhanced type=rbfdisc N=[11,15] M=3 xspace=eq uspace=eq';
% generate 10 thousand samples randomly in the continuous x - discrete u space
cfg.samples = 'randdisc N=10000';
cfg.run = 1;
cfg.datafile = 'ipsetup_lspidemo';
%
cfg.visualize = 1;
cfg.iterdisp = 1;
cfg.itervis = 1;
cfg.viscfg.savefig = 'ipsetup_lspidemo';            
rand('twister', 1000);  % uncomment for repeatable results
lspi(cfg);

%% OPD -- Part III demos start here
cfg = ccfg;
% we change the reward function to so-called "normalized LQR" -- the same as before, except now
% it's scaled and translated into [0,1]
% we also increase the sampling time to allow the algorithm a larger effective horizon for a given
% number of steps
cfg.model_params = {'nlqr', 'type=ip maxx=[pi;15*pi] maxu=3', 'Ts=0.05'};
% planning algorithm parameters
cfg.planalg = 'opd';            % algorithm: optimistic planning for deterministic systems
cfg.planparams.n = 200;         % budget of expansions at each step
cfg.planparams.U = [-3 0 3];    % action space -- the same as for M=3 in the approximators above
cfg.reset = [-pi, 0]';          % always reset pointing down
cfg.trials = 1;                 % one trial is enough to see performance, as we're not learning
cfg.maxsteps = 80;              % 80 steps = 4 seconds should be enough for a swingup
% some output and visualization config
cfg.datadir = pwd;
cfg.datafile = 'ipsetup_opd';
cfg.visualize = 3;             % this means: only visualize system steps
cfg.itervis = 1;
cfg.viscfg.pause = 0.01;      

% run the algorithm
cfg.run = 1;
genmpc(cfg);              

% look at the controlled trajectory
genmpc('plottrial datafile=ipsetup_opd');

%% OPC (with SOPC variant)

cfg = ccfg;
cfg.model_params = {'nlqr', 'type=ip maxx=[pi;15*pi] maxu=3', 'Ts=0.05'};
% since OPC variants are driven by the discount factor, it helps to 
% make it smaller; this allows better refinement of early actions
cfg.gamma = 0.85;
% planning algorithm parameters
cfg.planalg = 'sooplp';             % algorithm: continuous-action planning
cfg.planparams.method = 'SOOP2';    % this leads to the simultaneous-OPC algorithm
cfg.planparams.dimselect = 'disc';  % dimension selection based on discounting
cfg.planparams.alpha = cfg.gamma;  % with the discount factor same as the MDP
cfg.planparams.n = 500;             % budget of simulations at each step
cfg.reset = [-pi, 0]';              % always reset pointing down
cfg.trials = 1;                     % one trial is enough to see performance, as we're not learning
cfg.maxsteps = 80;                  % 80 steps = 4 seconds should be enough for a swingup
% some output and visualization config
cfg.datadir = pwd;
cfg.datafile = 'ipsetup_sopc';
cfg.visualize = 3;
cfg.itervis = 1;
cfg.viscfg.pause = 0.01;      

% run the algorithm
cfg.run = 1;
genmpc(cfg);              

% look at the controlled trajectory
genmpc('plottrial datafile=ipsetup_sopc');

