adaptiveReaching.m

function simout = adaptiveReaching(loads,gamma,tvia,buildup,simin)

%SIMOUT = ADAPTIVEREACHING(LOADS,GAMMA,PERTURBATION,TVIA,BUILDUP,SIMIN)
%
%   LOADS: 1x2 vector with load parameter. The force field is
%   LOAD[1]*forward velocity, the expected load parameter is LOAD[2].
%   LOAD[3] is the parameter used to increase or decrease the cost for the
%   simulations of Fig. 6c. 
%   GAMMA: Online learning rate for A and B matrices. Only A is update in
%   this paper. 
%   TVIA: Time at which the point mass must be at the via-point. When TVIA
%   is zero, the simulation time is set to 0.6s (default). When TVIA is
%   non-zero, simulation time is set to 1sec, and the coordiante of the
%   point-mass is constrained at TVIA.
%   BUILDUP: Exponent of polynomial function for cost buildup (25).
%   SIMIN: Input simulation data contains, either empty or herited from
%   previous simulation run
%
%   SIMOUT: Output simulation data, contains:
%       .state:     state vector over time
%       .FBgains:   time series of optimal control gains
%       .estimate:  time series of estimated state vectors
%       .control:   time series of control vectors
%       .AestCont:  time series of estimated model matrix
%       .BestCont:  time series of estimated model matrix, both in
%                   continuous time representation



% script_adaptiveControl
Gx = .1;
Gy = Gx;
m = 2.5;
tau = 0.1;
L = loads(1); % load variable
lambda = 0;
k = 0;


A = [0 0 1 0 0 0;0 0 0 1 0 0;-k/m 0 -Gx/m L/m m^-1 0;...
    0 -k/m 0 -Gy/m 0 m^-1;0 0 0 0 -tau^-1 lambda/tau;0 0 0 0 lambda/tau -tau^-1];

ANull = [0 0 1 0 0 0;0 0 0 1 0 0;-k/m 0 -Gx/m 0/m m^-1 0;...
    0 -k/m 0 -Gy/m 0 m^-1;0 0 0 0 -tau^-1 lambda/tau;0 0 0 0 lambda/tau -tau^-1]; % For via-point

B = [0 0;0 0;0 0;0 0;tau^-1 lambda/tau; lambda/tau tau^-1];
%A

simdata.A = A;
simdata.ANull = ANull;
% simdata.AClamp = AClamp;

simdata.B = B;
xp = mvnrnd(zeros(size(A)),eye(6));
AParam = -eye(6);

% load AStable
Aest = A;

if isempty(simin)
    Aest(3,4) = loads(2)/m;
else
    Aest(3:4,1:4) = simin.AestCont(3:4,1:4,end);
end

simdata.AestCont = Aest;
simdata.BestCont = B;
simdata.AParamCont = AParam;

% Simulation time corresponds to Exp 1 or 2
if tvia == 0
    simdata.time = .6;
    xfinal = [0 .15 0 0 0 0]'; % Experiment 1
else
    simdata.time = 1;
    xfinal = [0 .16 0 0 0 0]'; % VP

end

simdata.tvia = tvia;
simdata.stab = .01;
simdata.delta= .01;
simdata.m = m;

% Cost and Learning Rate
simdata.alpha = [1000 1000 20 20 0 0];
if ~isempty(buildup)
    simdata.buildup = buildup; % Use n-order polynomial builup during movement- 3 good
else
    simdata.buildup = 3;
end
    
simdata.r = 10^-5;
simdata.p = 1;
simdata.gamma = gamma;
simdata.loads = loads;

% Initial and via-point states
xinit = [0 0 0 0 0 0]';
xvia = [0 .10 0 0 0 0]';

simout = adaptiveLQG(xinit,xfinal,xvia,simdata);

% Plot Routine
% -------------------------------------------------------------------------
% figure
subplot(221)
plot(simout.state(1,:),simout.state(2,:));
axis square, axis([-.1 .1 -.02 .18]), hold on;
plot(xinit(1),xinit(2),'ro','LineWidth',2);
plot(xfinal(1),xfinal(2),'ro','MarkerSize',10,'LineWidth',2);
xlabel('x [m]','FontSize',14);
ylabel('y [m]','FontSize',14);

if simdata.tvia>0
    plot(xvia(1),xvia(2),'bo','MarkerSize',15,'LineWidth',2);
end

if norm(simdata.gamma) == 0
    s = ':';
else s = '';
end

subplot(222)
plot(simout.state(1:2,:)',s);
xlabel('Time [#sample]','FontSize',14) 
ylabel('x and y [m]','FontSize',14) 
hold on

%extracting the estimated load parameter
nStep = size(simout.AestCont,3);
LoadEst = zeros(1,nStep);
LoadTrue = zeros(1,nStep);

in = [3,4];
normError = [];

for i = 1:nStep
    
    if ~isnan(simout.AestCont(in(1),in(2),i))
        LoadEst(i) = simout.AestCont(in(1),in(2),i);
        normError = [normError,norm(simout.AestCont(:,:,i)-A)];
    end
    
    LoadTrue(i) = A(in(1),in(2));
    
end

subplot(223)
plot(LoadEst), hold on
plot(LoadTrue,'r');
axis([0 size(simout.AestCont,3) -abs(max(L/m,1))*1.1 abs(max(L/m,1))*1.1])
xlabel('Time [#sample]','FontSize',14) 
ylabel('FF Parameter [Nsm^{-1}]','FontSize',14) 

subplot(224)
plot(normError)
xlabel('Time [#sample]','FontSize',14) 
ylabel('Norm of Error','FontSize',14) 
hold on