From 0706d857d5b0f6f2654e0adabd4c56d0e599fcf4 Mon Sep 17 00:00:00 2001
From: Natasha Rouse <nar50@case.edu>
Date: Mon, 22 Jun 2020 16:15:41 -0400
Subject: [PATCH 1/7] Adding run8 & Eval

run8 calls Eval with choice of DMP or RMP system, and choice between three different trajectories.
---
 Eval.m | 257 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 run8.m | 236 ++++++++++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 493 insertions(+)
 create mode 100644 Eval.m
 create mode 100644 run8.m

diff --git a/Eval.m b/Eval.m
new file mode 100644
index 0000000..c464728
--- /dev/null
+++ b/Eval.m
@@ -0,0 +1,257 @@
+function d = Eval(w, Nt, y0dot, y0, x0, alphax, tau, dt, sigma_or_rho, c_or_alpha, alphay, betay,g, desiredtraj, ShowPlot, OneforDMPtwoforRMP, w1,w2,Nsaddles)
+% %--------------------
+goaltol = .005;
+%ShowPlot = 1;
+% % initialize or variable descriptions (basics that are the same for RMP/DMP)
+%  Nt = 1000; % number of timesteps
+%  y0 = .001 * [1,1]; %system initial position
+%  y0dot= 0 * [1,1]; %system initial velocity
+%   dt = .01; %timestep 
+% tau = 1;   %time constant base system, also reused in canonical system        
+% alphay = 4; %damping base system
+% betay = alphay/4; %stiffness base system
+% alphax = .5; %how fast cannonical system decays
+% g = [1,0]; %goal position of base system
+%  desiredtraj = [0:.05:.5, fliplr(.5*cos((-pi/2):-.1:-(pi))+1);
+%                 0:.05:.5, fliplr(.5*sin((-pi/2):-.1:-(pi))+.5)]'; 
+
+% %shared naming
+% x0 = 1; %cannonical system initial state (scalar for DMP, vector for RMP)
+% w  = weights that are learned  
+    %for DMP w will be size 2 by Npsi (assuming planar trajectory)
+% w  =   [0.8726   -4.8226   -5.2870   -5.6955   11.9854   -1.6538    0.2696   -0.6114    2.2657   -0.8382;
+%        4.3342    1.8618    0.2612   -1.5936   -5.3375    5.2797   -3.1289    0.0051    0.4761   -1.3480];
+% for RMP w will be size 
+%
+% 
+
+% Nsaddles = 20;
+if OneforDMPtwoforRMP == 1
+% %for DMP
+%   Npsi = 10; %number of basis functions (use this generate c and sigma)
+ % c = logspace(log10(1), log10(.01), Npsi); % these are the positions of DMP in X, and they do not change. 
+ % sigma = 2*logspace(log10(.3),log10(.002), Npsi);
+ sigma = sigma_or_rho;
+ c = c_or_alpha;
+ 
+ x = x0;
+ if ShowPlot
+xall = nan*ones(Nt,1);
+psiall =  nan*ones(Nt,length(sigma));
+ end
+ 
+ else
+% %for RMP
+% Nsaddles = 10;
+alpha = 10;
+beta = 1;
+nu = 1.2;
+rho = calculaterho_notacycle(alpha*ones(Nsaddles,1), beta*ones(Nsaddles,1), nu*ones(Nsaddles,1));
+ 
+ 
+ep = 10^-9*ones(1,Nsaddles);
+a0 = ep;
+a0(1) = 1;
+a0(2) = .01;
+a = a0;
+if ShowPlot
+aall = nan*ones(Nt, Nsaddles);
+end
+% %%rho = []; %coupling matrix
+% %%alpha = []; %growth factors
+% % for now P and zd are zero since not measuring contact
+% % and C is zero because there is no coupling and no measured z
+% rho = sigma_or_rho;
+% alpha = c_or_alpha;
+% 
+end
+
+distance = 0;
+
+ydot = y0dot;
+y = y0;
+fnum = 0;
+
+maxtime = (Nt+1)*dt;
+timetogoal = nan;
+
+if ShowPlot
+yall = nan*ones(Nt,2);
+ydotall = nan*ones(Nt,2);
+whichcolor = nan*ones(Nt,1);
+alldistanceerror = nan*ones(Nt,1);
+whichtraj = nan*ones(Nt,1);
+allverror = nan*ones(Nt,1);
+end
+
+desiredtrajdir = diff(desiredtraj);
+desiredtrajdir = .5*( [desiredtrajdir;desiredtrajdir(end,:)] + [desiredtrajdir(1,:);desiredtrajdir]);%centerdiffs
+desiredtrajdir = desiredtrajdir ./sqrt(sum(desiredtrajdir.^2,2)); %normalize
+
+trajpointdistancessq = Inf*ones(size(desiredtraj, 1), 1);
+
+for i = 1:Nt
+    % advance diff equation
+    %DMP
+    if OneforDMPtwoforRMP == 1        
+%         w = [w(1,:); zeros(1,length(w))];
+%         w = [zeros(1,length(w));w(2,:)];
+    x = x + ((-alphax)*x)/tau*dt;
+    psi = exp(-1./(2*sigma.^2).*(x-c).^2);
+%     fnum = fnum + ((psi*w')*x);
+    f = (psi*w') *x /sum(psi);
+%     f = [0,f(2)];
+%     f = [f(1),0];
+%     %new weights
+%         beta2 = ((alphay*(betay*(g-y)-ydot))*(dt^2)/tau) + ydot*dt + y;
+%         beta1 = (psi*w'*(dt^2))*x/tau;
+%         f = beta2; 
+    end
+    %RMP
+    if OneforDMPtwoforRMP == 2
+    dW = sqrt(dt)*randn(1,Nsaddles);
+    da = a .* (alpha - a*rho) *dt + ep.*dW; %%%DID I forget tau??
+    a = max(min(a + da, 1), .0005);
+    f = a*w'/sum(a);
+    end
+    ydotdot = (alphay*(betay*(g-y)-ydot)+f)/tau;
+    ydot = ydotdot*dt + ydot;
+    y = ydot*dt+y;    
+    
+    
+    if ShowPlot
+        % find out which part of solution has max contribution
+        if OneforDMPtwoforRMP == 1
+        [~, whichcolor(i)] = max( [sum( abs(alphay*(betay*(g-y)-ydot)).^2)*.1;
+                                   sum((abs((psi.*w) *x /sum(psi))').^2,2)]);
+        else 
+            [~, whichcolor(i)] = max( [sum( abs(alphay*(betay*(g-y)-ydot)).^2)*.1;
+                                       sum((abs((a.*w)/sum(a))').^2,2)]);
+                                  
+        end
+        
+    end
+    trajpointdistancessq = min ((y(1) - desiredtraj(:,1)).^2 + (y(2) - desiredtraj(:,2)).^2, ...
+                                trajpointdistancessq);
+       
+    [dsquared, whichtrajpoint] = min( (y(1) - desiredtraj(:,1)).^2 + ...
+        (y(2) - desiredtraj(:,2)).^2);
+    distance = distance + ...
+        sqrt( dsquared);
+    desv = desiredtrajdir(whichtrajpoint, :);
+    vnormalerror = (desv(1)*ydot(2) - desv(2)*ydot(1))/sqrt(ydot*ydot');
+    
+    % here if its at the goal, I record time
+    if isnan(timetogoal) && sum((y-g).^2)<goaltol^2
+        timetogoal = i*dt;
+    end
+    
+    if ShowPlot
+        alldistanceerror(i) = sqrt( dsquared);
+        whichtraj(i) = whichtrajpoint;
+        allverror(i) = vnormalerror;
+        yall(i,:) = y;
+        ydotall(i,:) = ydot;
+        if OneforDMPtwoforRMP == 1
+        xall(i) = x;
+        psiall(i,:) = psi;
+        end
+        if OneforDMPtwoforRMP == 2
+            if Nsaddles == 1
+                aall = a;
+            else
+                aall(i,:) = a;
+            end
+        end
+    end
+end
+
+% %evaluate trajectory
+% distance = nan*ones(Nt,1);
+% whichtraj = nan*ones(Nt,1);
+% for i = 1:Nt
+%     distance(i) = sqrt( min( (yall(i,1) - desiredtraj(:,1)).^2 + ... 
+%                 (yall(i,2) - desiredtraj(:,2)).^2));
+%     [~,whichtraj(i)] = min( (yall(i,1) - desiredtraj(:,1)).^2 + ... 
+%                 (yall(i,2) - desiredtraj(:,2)).^2);
+% end
+
+if isnan(timetogoal) 
+    timetogoal = maxtime-dt;
+end
+
+d = 1*distance+...
+    10*timetogoal/Nt+...
+    1*sum(sqrt(trajpointdistancessq));
+
+
+if ShowPlot
+%grayout colors that aren't used
+colorstouse = setdiff(unique(whichcolor(1:i)-1), 0);
+% colorstouse = setdiff(unique(whichcolor(1:(timetogoal/dt)))-1, 0);
+colorstouse = setdiff(unique(whichcolor)-1, 0);
+
+psicolors = ones(1, size(w,2))'*[0.7, 0.7, 0.7];
+psicolors(colorstouse,:) = cool(length(colorstouse));
+
+
+figure(3)
+clf(3)
+figure(3)
+hold on 
+title ('Basis functions times weights')
+if OneforDMPtwoforRMP == 1
+    for j = 1:size(psi,2)
+        plot((1:Nt)*dt, psiall(:,j)*w(1,j), '-c', 'Color', psicolors(j,:))
+        plot((1:Nt)*dt, psiall(:,j)*w(2,j), '-c.', 'Color', psicolors(j,:))
+    end
+else
+    for j = 1:size(a,2)
+        plot((1:Nt)*dt, aall(:,j)*w(1,j), '-c', 'Color', psicolors(j,:))
+        plot((1:Nt)*dt, aall(:,j)*w(2,j), '-c.', 'Color', psicolors(j,:))
+    end
+end
+
+figure(2)
+ clf(2)
+ figure(2)
+hold on
+plot(yall(:,1), yall(:,2), 'k-')
+plot(desiredtraj(:,1), desiredtraj(:,2), '-g.');
+plot(w2(1,:), w2(2,:), '-r.')
+plot(w(1,:), w(2,:), '-ro', 'Color', [.6, .6, .6])
+
+plot(yall(whichcolor==1,1), yall(whichcolor==1,2), 'k.')
+for i = 1:size(w, 2)
+    plot(yall(whichcolor==(1+i),1), yall(whichcolor==(1+i),2), 'k.', 'Color', psicolors(i,:))
+    plot(w(1,i), w(2,i), 'k*', 'Color', psicolors(i,:))
+end
+axis equal
+
+figure(5)
+ clf(5)
+ figure(5)
+hold on
+for i = 1:size(w, 2)
+    plot(yall(whichcolor==(1+i),1), yall(whichcolor==(1+i),2), 'k.', 'Color', psicolors(i,:))
+end
+
+plot(desiredtraj(:,1), desiredtraj(:,2), '-k.');
+axis equal
+
+% figure(6)
+% clf(6)
+% figure(6)
+% 
+% hold on 
+% for j = 1:size(psi,2)
+%     plot((1:Nt)*dt, psiall(:,j), '-c', 'Color', psicolors(j,:))
+%     plot((1:Nt)*dt, psiall(:,j), '-c.', 'Color', psicolors(j,:))
+% end
+% saddlecolors  = cool(Nsaddles);
+% for j = 1:Nsaddles
+%     plot((1:Nt)*dt, 1.1+aall(:,j), '-c', 'Color', saddlecolors(j,:))
+%     plot((1:Nt)*dt, 1.1+aall(:,j), '-c.', 'Color', saddlecolors(j,:))
+% end
+end
+
diff --git a/run8.m b/run8.m
new file mode 100644
index 0000000..a451289
--- /dev/null
+++ b/run8.m
@@ -0,0 +1,236 @@
+close all
+clear all
+clc
+tic
+rng(1)
+
+finalds = [];
+for Nsaddles = 5:20
+OneforDMPtwoforRMP = 1
+
+Usepresents = 1;
+dt = .01;
+Nt = 1000;
+g = [1,0];
+y0 = .001 * [1,1];
+y0dot= 0 * [1,1];
+x0 = 1;
+
+% %ramp up bubble out
+% desiredtraj = [0:.05:.5, .5*cos((pi/2):-.1:0)+.5;
+%                0:.05:.5, .5*sin((pi/2):-.1:0)]'; 
+           
+% % %ramp up bubble in ------ 
+%    desiredtraj = [0:.05:.5, fliplr(.5*cos((-pi/2):-.1:-(pi))+1);
+%                   0:.05:.5, fliplr(.5*sin((-pi/2):-.1:-(pi))+.5)]'; 
+%-------             
+% %ramp up wrap around
+   desiredtraj = [(0:.05:.5)*0, .5*cos(pi/2:.1:pi*2.6);
+                   0:.05:.5, .5*sin(pi/2:.1:pi*2.6)]';
+  g = [0,0];
+% % ----- 
+%% IGNORE THIS
+%  bestw =[  3.2763   -7.9183    2.9071   -7.7999  -19.5263   -4.2977    7.2047   -0.1165  -1.7832    2.3749
+%           6.9902   -0.5676   -2.2115    2.5635   11.9405   -6.1655   -0.0285    0.6614 -0.6912    0.6141];
+%  bestw = [   2.5279   -6.3904    0.4768   -0.9810   -4.9774    3.7569 -0.7490    0.0487   -0.1125    0.3058;
+%             7.3744   -2.2085   -0.0588   -0.1458    0.6147   -0.3531   -0.0641    0.0472   -0.0146   -0.0088];
+% % After running all night! with all three factors
+% % bestw =  [-2.9178   -4.4224    0.7990   -0.0852
+%            2.1801    0.4769   -0.2220   -0.0464];
+% two factors (no direction)      
+%%
+tau = 1;
+alphay = 4; % if I decrease these numbers weird stuff happens % damping and stiffness
+betay = alphay/4; %alphay/4; % if I decrease these numbers weird stuff happens % stiffnesses
+alphax = .5; %how fast cannonical system decays
+Npsi = Nsaddles;
+% Npsi = 10;
+c = logspace(log10(1), log10(.01), Npsi); % these are the positions of DMP in X, and they do not change. 
+sigma = logspace(log10(.3),log10(.002), Npsi); %if I make this narrower (smaller sigma) it looks like it can get smaller turn radius, but best solution has a kink
+%Cz = 0 no coupling
+
+%-------------------
+Ktrials = 1000;
+% w = 5-10*rand(2, Npsi); % these are the weights - they do change.
+%Bestw for ramp up wrap around
+%  w =[0.1682   -0.0909   -2.5292   -2.0515   -0.4892    1.4525    2.4341    1.8060   -0.0140   -2.3589
+%     1.8293    2.7509    0.6173   -1.1173   -2.4039   -1.9603   -0.0670    1.5410    2.4332    1.5410];
+% w =[    0.0990   -0.3065   -2.3560   -2.0671   -0.4662    1.4635    2.3638    1.8060   -0.0140   -1.7901;
+%     2.0122    2.6248    0.6573   -1.1035   -2.4039   -1.9603   -0.2377    1.6481    2.3701    1.7008];
+
+%What happens when w = desired traj (simple sampling)
+% w = desiredtraj(round(linspace(1,length(desiredtraj),Npsi)) ,:)'*(alphay+betay); %when alphay = 4, betay = 1nice to multiply by 5
+% w =   [  0.2060   -0.3065   -2.4163   -2.0671   -0.4646    1.4635    2.2877    1.8060   -0.1399   -2.0545;
+%     1.7634    2.6421    0.6093   -1.1801   -2.4039   -1.8916   -0.2974    1.6481    2.4332    1.5173];
+w = desiredtraj(round(linspace(1,length(desiredtraj),Npsi)) ,:)';
+w1 = w;
+w = desiredtraj(round(linspace(1,length(desiredtraj),Npsi)) ,:)'*(alphay+betay);
+w2 = w;
+
+%500 trials
+% wbest =   [  0.1682   -0.0909   -2.5292   -2.0515   -0.4892    1.4525    2.4341    1.8060   -0.0140   -2.3589;
+%     1.8293    2.7509    0.6173   -1.1173   -2.4039   -1.9603   -0.0670    1.5410    2.4332    1.5410];
+%1000 trials
+% wbest =  [   0.2060   -0.3065   -2.4163   -2.0671   -0.4646    1.4635    2.2877    1.8060   -0.1399   -2.0545;
+%     1.7634    2.6421    0.6093   -1.1801   -2.4039   -1.8916   -0.2974    1.6481    2.4332    1.5173];
+
+if Usepresents
+%%%%%%% $$$ Here I am inserting linear algebra approach to weight finding
+% where we know f = tau*ydotdot - (alphay*(betay*(g-y) - ydot);
+desiredt =  linspace(0,10,length(desiredtraj))';% [1:length(desiredtraj)]'; % assumes constant velocity linspace(0,10,length(desiredtraj))
+desiredydot = [0,0;diff(desiredtraj)./(diff(desiredt)*[1,1])]; % desired speeds
+desiredydotdot = [0,0;diff(desiredydot)./(diff(desiredt)*[1,1])];
+fdesired = tau * desiredydotdot - (alphay * (betay*(g-desiredtraj) - desiredydot));
+
+if OneforDMPtwoforRMP == 1
+    expected_x = x0*exp(-alphax/tau*(desiredt-desiredt(1)));
+    expectedPsi = exp(ones(length(desiredt),1)*(-1./(2*sigma.^2)) .*(expected_x - c).^2); %n traj points by n basis 
+    
+    % proveXexpworking % run this code to make a plot
+    betaw =  (expected_x .* expectedPsi) \ (sum(expectedPsi')'.* fdesired);
+    errors = (expected_x .* expectedPsi)*betaw - (sum(expectedPsi')'.* fdesired);
+else
+    
+    
+% Nsaddles = 10; %DUPLICATED IN EVAL
+alpha = 10; %DUPLICATED IN EVAL
+beta = 1;  %DUPLICATED IN EVAL
+nu = 1.2; %DUPLICATED IN EVAL
+rho = calculaterho_notacycle(alpha*ones(Nsaddles,1), beta*ones(Nsaddles,1), nu*ones(Nsaddles,1));
+ep = 10^-9*ones(1,Nsaddles);  %DUPLICATED IN EVAL
+a0 = ep;  %DUPLICATED IN EVAL
+a0(1) = 1; %DUPLICATED IN EVAL
+a0(2) = .01; %DUPLICATED IN EVAL
+a = a0;
+index = 1;
+expected_SHCas = a;
+
+for t = desiredt(1): dt: desiredt(end)    
+    dW = sqrt(dt)*randn(1,Nsaddles);
+    da = a .* (alpha - a*rho) *dt + ep.*dW; %wondering if I forgot tau
+    a = max(min(a + da, 1), .0005);
+    
+    if t>=desiredt(index)
+        index = index+1;
+    else
+        expected_SHCas(index, :) = a;
+    end
+end
+
+
+    size(expected_SHCas)
+    betaw =  ( expected_SHCas) \ (sum(expected_SHCas')'.* fdesired);
+end
+
+
+% meanerror = sum(sum(errors.^2))
+w = betaw';
+%%%%%%% $$$ end insertion
+end
+
+
+%TWO main functions here, one to evaluate weights
+d = Eval(w, Nt, y0dot, y0, x0, alphax, tau, dt, sigma, c, alphay, betay,g, desiredtraj, 1, OneforDMPtwoforRMP, w1,w2,Nsaddles);
+
+delta = 1;
+allds = [];
+for passes = 1:10
+bestw = w;
+bestd = d;
+for i = 1:Ktrials
+    if rand(1)>.5
+        % try random stuff
+       w = 1-.5*rand(2, Npsi);
+       d = Eval(w, Nt, y0dot, y0, x0, alphax, tau, dt, sigma, c, alphay, betay,g, desiredtraj, 0,OneforDMPtwoforRMP, w1,w2,Nsaddles);
+       if d<bestd
+           bestw = w;
+           bestd = d;
+           delta = 1;
+       end
+    else
+        %try different random stuff based on size delta 
+       w = bestw + 3*delta*randn(2, Npsi);
+       d = Eval(w, Nt, y0dot, y0, x0, alphax, tau, dt, sigma, c, alphay, betay,g, desiredtraj, 0,OneforDMPtwoforRMP, w1,w2,Nsaddles);
+       if d<bestd
+           bestw = w;
+           bestd = d;
+       end
+       
+    end
+ 
+end
+w = bestw;
+d = bestd;
+delta = delta*.99;
+Eval(w, Nt, y0dot, y0, x0, alphax, tau, dt, sigma, c, alphay, betay,g, desiredtraj, 1,OneforDMPtwoforRMP, w1,w2,Nsaddles);
+allds = [allds,bestd];
+
+
+% now for a gradient desecent on the best one. 
+
+pause(1)
+chosenew = Inf;
+chosedel = Inf;
+while chosenew+chosedel>=2
+chosenew = 0;
+chosedel = 0;
+ for i = 1:size(w,1)
+     for j = 1:size(w,2)
+         wtrial = bestw;
+         wnew = bestw;
+         delt = delta*(1-2*rand(1));
+         wtrial(i,j) = bestw(i,j) + delt;
+         dtrial =  Eval(wtrial, Nt, y0dot, y0, x0, alphax, tau, dt, sigma, c, alphay, betay,g, desiredtraj, 0,OneforDMPtwoforRMP,w1,w2,Nsaddles);
+         wnew(i,j) = bestw(i,j) + max(min(bestd*delt/(bestd - dtrial), delta*5), -delta*5);
+         dnew =  Eval(wnew, Nt, y0dot, y0, x0, alphax, tau, dt, sigma, c, alphay, betay,g, desiredtraj, 0,OneforDMPtwoforRMP,w1,w2,Nsaddles);
+         if dnew < bestd
+             bestw = wnew;
+             bestd = dnew;
+             chosenew = chosenew +1;
+         end
+         if dtrial < bestd
+             bestw = wtrial;
+             bestd = dtrial;
+             chosedel = chosedel +1;
+         end
+         
+     end
+ end
+%  chosenew
+%  chosedel
+end
+ 
+%at the end of one pass, save progress
+allds = [allds,bestd];
+w = bestw;
+d = bestd;
+%ShowTraj(w, Nt, y0dot, y0, x0, alphax, tau, dt, sigma, c, alphay, betay,g, desiredtraj);
+% if passes==1 || passes==5 || passes==10
+%     pause
+% end
+end
+Eval(w, Nt, y0dot, y0, x0, alphax, tau, dt, sigma, c, alphay, betay,g, desiredtraj, 1,OneforDMPtwoforRMP,w1,w2,Nsaddles);
+
+% figure(4)
+% plot(allds)
+
+%  
+% pause
+% ShowTraj(w, Nt, y0dot, y0, x0, alphax, 2*tau, dt, sigma, c, alphay, betay,g, desiredtraj);
+% w = weights
+% example 
+%     0.8726   -4.8226   -5.2870   -5.6955   11.9854   -1.6538    0.2696   -0.6114    2.2657   -0.8382
+%     4.3342    1.8618    0.2612   -1.5936   -5.3375    5.2797   -3.1289    0.0051    0.4761   -1.3480
+% Nt = number of timesteps
+% y0 initial conditions
+% x0 initial conditions
+% alphax % keep same as training
+% tau % changing this makes shape pretty different, sloppier if highter
+%dt  %seems like I can change this without changing shape
+
+finalds = [finalds,allds(end)]
+figure(6)
+plot(finalds)
+end
+
+toc
\ No newline at end of file

From c1834b3f33d65b4f06a65a9098295b644b8671e7 Mon Sep 17 00:00:00 2001
From: Natasha Rouse <nar50@case.edu>
Date: Fri, 3 Jul 2020 12:35:13 -0400
Subject: [PATCH 2/7] Add files via upload

---
 DMPandRMP1.m         | 416 +++++++++++++++++++++++++++++++++++
 Eval.m               |  38 ++--
 RunbothRMP_and_DMP.m |  28 +++
 run8.m               | 332 +++++++++++++++++++++++++---
 run9.m               | 506 +++++++++++++++++++++++++++++++++++++++++++
 wbestDMP1.mat        | Bin 0 -> 16023 bytes
 wbestRMP1.mat        | Bin 0 -> 18572 bytes
 7 files changed, 1273 insertions(+), 47 deletions(-)
 create mode 100644 DMPandRMP1.m
 create mode 100644 RunbothRMP_and_DMP.m
 create mode 100644 run9.m
 create mode 100644 wbestDMP1.mat
 create mode 100644 wbestRMP1.mat

diff --git a/DMPandRMP1.m b/DMPandRMP1.m
new file mode 100644
index 0000000..af2b47f
--- /dev/null
+++ b/DMPandRMP1.m
@@ -0,0 +1,416 @@
+%June 2020
+%DMP and RMP structures using simple trajectory following (no point mass)
+
+close all
+clear all
+clc
+tic
+
+rng(2) %seeding different random numbers to check validity of cost evaluation
+
+%TRAJECTORY CHOICES
+% %ramp up bubble out -----
+% desiredtraj = [0:.05:.5, .5*cos((pi/2):-.1:0)+.5;
+%                0:.05:.5, .5*sin((pi/2):-.1:0)]'; 
+% g = [1,0]; %goal
+% %ramp up bubble in ------ 
+% desiredtraj = [0:.05:.5, fliplr(.5*cos((-pi/2):-.1:-(pi))+1);
+%               0:.05:.5, fliplr(.5*sin((-pi/2):-.1:-(pi))+.5)]';
+% g = [1,0];
+%ramp up wrap around -----
+desiredtraj = [(0:.05:.5)*0, .5*cos(pi/2:.1:pi*2.6);
+               0:.05:.5, .5*sin(pi/2:.1:pi*2.6)]';
+g = [0,0];
+y0 = .001 * [1,1];
+y0dot= 0 * [1,1];
+x0 = 1;
+bfs = 10;
+
+%system init
+S = struct('desiredtraj',desiredtraj,'dt',0.01,'Nt',1000,'g',g);
+%trajectory start, velocity at trajectory start, canonical state vector,
+%size of timestep, number of timesteps, goal
+
+%dmp init
+D = struct('tau',1,'alphay',4,'betay',1,'alphax',.5,... %how fast cannonical system decays
+    'bfs',bfs,... %number of basis functions (psi activations)
+    'c',logspace(log10(1), log10(.01), bfs),... %basis function centers 
+    'sigma',logspace(log10(.3),log10(.002), bfs),... %basis function widths
+    'w',zeros(1,length(desiredtraj)),...
+    'psi',nan(S.Nt,bfs),'y',y0,'ydot',y0dot,'x',x0);
+
+%rmp init
+R = struct('tau',1,'alphay',4,'betay',1,'alpha',10,'beta',1,'nu',1.2,'bfs',bfs,...
+    'rho',logspace(log10(.3),log10(.002), bfs),...
+    'a',[1,0.1,10^-9*ones(1,bfs-2)],...
+    'ep',[10^-9*ones(1,bfs)],...
+    'w',zeros(1,length(desiredtraj)),'y',y0,'ydot',y0dot,'x',x0);
+ 
+%Init
+D.w = S.desiredtraj(round(linspace(1,length(S.desiredtraj),D.bfs)) ,:)'*(D.alphay+D.betay);
+R.w = S.desiredtraj(round(linspace(1,length(S.desiredtraj),R.bfs)) ,:)'*(R.alphay+R.betay);
+
+Ntrials = 500;
+desiredt =  linspace(0,10,length(S.desiredtraj))';% assumes constant velocity
+desiredydot = [0,0;diff(S.desiredtraj)./(diff(desiredt)*[1,1])]; % desired speed
+desiredydotdot = [0,0;diff(desiredydot)./(diff(desiredt)*[1,1])];
+fdesired = D.tau * desiredydotdot - (D.alphay * (D.betay*(S.g-S.desiredtraj) - desiredydot));
+
+expected_x = x0*exp(-D.alphax/D.tau*(desiredt-desiredt(1)));
+expectedPsi = exp(ones(length(desiredt),1)*(-1./(2*D.sigma.^2)) .*(expected_x - D.c).^2); %n traj points by n basis 
+% proveXexpworking % run this code to make a plot
+betaw =  (expected_x .* expectedPsi) \ (sum(expectedPsi')'.* fdesired);
+errors = (expected_x .* expectedPsi)*betaw - (sum(expectedPsi')'.* fdesired);
+
+D.w = betaw';
+
+index = 1;
+expected_SHCas = R.a;
+for t = desiredt(1): S.dt: desiredt(end)    
+    dW = sqrt(S.dt)*randn(1,R.bfs);
+    da = R.a .* (R.alpha - R.a*R.rho') *S.dt + R.ep.*dW; %wondering if I forgot tau
+    R.a = max(min(R.a + da, 1), .0005);
+    
+    if t>=desiredt(index)
+        index = index+1;
+    else
+        expected_SHCas(index, :) = R.a;
+    end
+end
+size(expected_SHCas)
+betaw =  (expected_SHCas) \ (sum(expected_SHCas')'.* fdesired);
+% meanerror = sum(sum(errors.^2))
+R.w = betaw';
+
+[dcost,rcost] = Eval(D,R,S);
+%DMP weight evaluation
+delta = 1;
+alldcosts = [];
+for passes = 1:10
+    bestw = D.w;
+    bestcost = dcost;
+%     for i = 1:Ntrials
+%         if rand(1)>.5
+%             % try random stuff
+%             D.dmp.w = 1-.5*rand(2, D.dmp.bfs);
+%             [dcost,~] = Eval(D,R,S);
+%             if dcost<bestcost
+%                bestw = D.dmp.w;
+%                bestcost = dcost;
+%                delta = 1;
+%             end
+%         else
+%             %try different random stuff based on size delta 
+%            D.w = bestw + 3*delta*randn(2, D.bfs);
+%            [dcost,~] = Eval(D,R,S);
+%            if dcost<bestcost
+%                bestw = D.w;
+%                bestcost = dcost;
+%            end
+%         end
+%     end
+%     D.w = bestw;
+%     dcost = bestcost;
+%     delta = delta*.99;
+%     Eval(D,R,S);
+%     alldcosts = [alldcosts,bestcost];
+    
+    %Gradient Descent on the best weights
+    pause(1)
+    chosenew = Inf;
+    chosedel = Inf;
+    while chosenew+chosedel>=3
+    chosenew = 0;
+    chosedel = 0;
+     for i = 1:size(D.w,1)
+         for j = 1:size(D.w,2)
+             wtrial = bestw;
+             wnew = bestw;
+             delt = delta*(1-2*rand(1));
+             wtrial(i,j) = bestw(i,j) + delt;
+             [dcosttrial,~] =  Eval(D,R,S,wtrial);
+             wnew(i,j) = bestw(i,j) + max(min(bestcost*delt/(bestcost - dcosttrial), delta*5), -delta*5);
+             [dcostnew,~] =  Eval(D,R,S,wnew);
+             if dcostnew < bestcost
+                 bestw = wnew;
+                 bestcost = dcostnew;
+                 chosenew = chosenew +1;
+             end
+             if dcosttrial < bestcost
+                 bestw = wtrial;
+                 bestcost = dcosttrial;
+                 chosedel = chosedel +1;
+             end
+         end
+     end
+    end
+
+    %at the end of one pass, save progress
+    alldcosts = [alldcosts, bestcost];
+    D.w = bestw;
+    dcost = bestcost;
+    ShowPlot(D,R,S)
+end
+
+%RMP weight evaluation
+delta = 1;
+allrcosts = [];
+for passes = 1:10
+    bestw = R.w;
+    bestcost = rcost;
+%     for i = 1:Ntrials
+%         if rand(1)>.5
+%             % try random stuff
+%             R.w = 1-.5*rand(2, R.bfs);
+%             [~,rcost] = Eval(D,R,S);
+%             if rcost<bestcost
+%                bestw = R.w;
+%                bestcost = rcost;
+%                delta = 1;
+%             end
+%         else
+%             %try different random stuff based on size delta 
+%            R.w = bestw + 3*delta*randn(2, R.bfs);
+%            rcost = Eval(D,R,S);
+%            if rcost<bestcost
+%                bestw = R.w;
+%                bestcost = rcost;
+%            end
+% 
+%         end
+% 
+%     end
+%     R.w = bestw;
+%     rcost = bestcost;
+%     delta = delta*.99;
+%     Eval(D,R,S);
+%     allrcosts = [allrcosts,bestcost];
+    
+    %Gradient Descent on the best weights
+    pause(1)
+    chosenew = Inf;
+    chosedel = Inf;
+    while chosenew+chosedel>=3
+    chosenew = 0;
+    chosedel = 0;
+     for i = 1:size(R.w,1)
+         for j = 1:size(R.w,2)
+             wtrial = bestw;
+             wnew = bestw;
+             delt = delta*(1-2*rand(1));
+             wtrial(i,j) = bestw(i,j) + delt;
+             [~,rcosttrial] =  Eval(D,R,S,wtrial);
+             wnew(i,j) = bestw(i,j) + max(min(bestcost*delt/(bestcost - rcosttrial), delta*5), -delta*5);
+             [~,rcostnew] =  Eval(D,R,S,wnew);
+             if rcostnew < bestcost
+                 bestw = wnew;
+                 bestcost = rcostnew;
+                 chosenew = chosenew +1;
+             end
+             if rcosttrial < bestcost
+                 bestw = wtrial;
+                 bestcost = rcosttrial;
+                 chosedel = chosedel +1;
+             end
+         end
+     end
+    end
+
+    %at the end of one pass, save progress
+    allrcosts = [allrcosts, bestcost];
+    R.w = bestw;
+    rcost = bestcost;
+end
+ShowPlot(D,R,S)
+Eval(D,R,S);
+toc
+
+%% Evaluate Cost
+function [dcost,rcost] = Eval(D,R,S,wnew)
+if nargin == 4
+    wdmp = wnew;
+    wrmp = wnew;
+end
+if nargin == 3
+    wdmp = D.w;
+    wrmp = R.w;
+end
+
+goaltol = 0.005;
+distance = 0;
+maxtime = (S.Nt+1)*S.dt;
+
+%DMP
+timetogoal = nan;
+trajpointdistancessq = Inf*ones(size(S.desiredtraj, 1), 1);
+for i = 1:S.Nt
+    D.x = D.x + ((-D.alphax)*D.x)/D.tau*S.dt;
+    D.psi(i,:) = exp(-1./(2*D.sigma.^2).*((D.x-D.c).^2));
+    D.psi(i,:)
+    forcing = (D.psi*wdmp') *D.x /sum(D.psi(i,:));
+    f = forcing(i,:);
+    
+    ydotdot = (D.alphay*(D.betay*(S.g-D.y)-D.ydot)+f)/D.tau;
+    D.ydot = ydotdot*S.dt + D.ydot;
+    D.y = D.ydot*S.dt+D.y;
+    
+    trajpointdistancessq = min ((D.y(1) - S.desiredtraj(:,1)).^2 + (D.y(2) - S.desiredtraj(:,2)).^2, ...
+                            trajpointdistancessq);
+
+    [dsquared, whichtrajpoint] = min( (D.y(1) - S.desiredtraj(:,1)).^2 + ...
+        (D.y(2) - S.desiredtraj(:,2)).^2);
+    distance = distance + sqrt(dsquared);
+    
+    %record the time if it's at the goal
+    if isnan(timetogoal) && sum((D.y-S.g).^2)<goaltol^2
+        timetogoal = i*S.dt;
+    end
+end
+
+if isnan(timetogoal) 
+    timetogoal = maxtime-S.dt;
+end
+dcost = 1*distance + 10*timetogoal/S.Nt + 1*sum(sqrt(trajpointdistancessq));
+
+%RMP
+timetogoal = nan;
+trajpointdistancessq = Inf*ones(size(S.desiredtraj, 1), 1);
+for i = 1:S.Nt
+    dW = sqrt(S.dt)*randn(1,R.bfs);
+    da = R.a .* (R.alpha - R.a*R.rho') *S.dt + R.ep.*dW; %tau for scaling?
+    R.a = max(min(R.a + da, 1), .0005); %enforcing boundaries on a
+    f = R.a*wrmp'/sum(R.a);
+
+    ydotdot = (R.alphay*(R.betay*(S.g-R.y)-R.ydot)+f)/R.tau;
+    R.ydot = ydotdot*S.dt + R.ydot;
+    R.y = R.ydot*S.dt+R.y;
+    
+    trajpointdistancessq = min ((R.y(1) - S.desiredtraj(:,1)).^2 + (R.y(2) - S.desiredtraj(:,2)).^2, ...
+                            trajpointdistancessq);
+
+    [dsquared, whichtrajpoint] = min( (R.y(1) - S.desiredtraj(:,1)).^2 + ...
+        (R.y(2) - S.desiredtraj(:,2)).^2);
+    distance = distance + sqrt(dsquared);
+    
+    %record the time if it's at the goal
+    if isnan(timetogoal) && sum((R.y-S.g).^2)<goaltol^2
+        timetogoal = i*S.dt;
+    end
+end
+
+if isnan(timetogoal) 
+    timetogoal = maxtime-S.dt;
+end
+
+rcost = 1*distance + 10*timetogoal/S.Nt + 1*sum(sqrt(trajpointdistancessq));
+
+end
+
+%% Plot
+function ShowPlot(D,R,S)
+
+xall = nan*ones(S.Nt,1);
+psiall =  nan*ones(S.Nt,length(D.sigma));
+aall = nan*ones(S.Nt, R.bfs);
+yall = nan*ones(S.Nt,2);
+ydotall = nan*ones(S.Nt,2);
+whichcolor = nan*ones(S.Nt,1);
+rmpcolor = whichcolor;
+dmpcolor = whichcolor;
+alldistanceerror = nan*ones(S.Nt,1);
+whichtraj = nan*ones(S.Nt,1);
+allverror = nan*ones(S.Nt,1);
+
+% desiredtrajdir = diff(S.desiredtraj);
+% desiredtrajdir = .5*( [desiredtrajdir;desiredtrajdir(end,:)] + [desiredtrajdir(1,:);desiredtrajdir]);%centerdiffs
+% desiredtrajdir = desiredtrajdir ./sqrt(sum(desiredtrajdir.^2,2)); %normalize
+for i = 1:S.Nt
+    [~, dmpcolor(i)] = max( [sum( abs(D.alphay*(D.betay*(S.g-D.y)-D.ydot)).^2)*.1;
+                               sum((abs((D.psi(i,:).*D.w) *D.x /sum(D.psi(i,:)))').^2,2)]);
+    [~, rmpcolor(i)] = max( [sum( abs(R.alphay*(R.betay*(S.g-R.y)-R.ydot)).^2)*.1;
+                               sum((abs((R.a.*R.w) /sum(R.a))').^2,2)]);
+%     [dsquared, whichtrajpoint] = min( (D.y(1) - S.desiredtraj(:,1)).^2 + ...
+%                                 (D.y(2) - S.desiredtraj(:,2)).^2);
+%     [dsquared, whichtrajpoint] = min( (R.y(1) - S.desiredtraj(:,1)).^2 + ...
+%                                 (R.y(2) - S.desiredtraj(:,2)).^2);
+%     desv = desiredtrajdir(whichtrajpoint, :);
+%     vnormalerror = (desv(1)*S.ydot(2) - desv(2)*S.ydot(1))/sqrt(S.ydot*S.ydot');
+%     alldistanceerror(i) = sqrt(dsquared);
+%     whichtraj(i) = whichtrajpoint;
+%     allverror(i) = vnormalerror;
+    yalldmp(i,:) = D.y;
+    yallrmp(i,:) = R.y;
+%     ydotall(i,:) = S.ydot;
+%     xall(i) = S.x;
+    psiall(i,:) = D.psi(i,:);
+    aall(i,:) = R.a;
+end
+dmpcolorstouse = setdiff(unique(dmpcolor)-1, 0);
+rmpcolorstouse = setdiff(unique(rmpcolor)-1, 0);
+psicolors = ones(1, size(D.w,2))'*[0.7, 0.7, 0.7];
+size(dmpcolorstouse)
+psicolors(dmpcolorstouse,:) = cool(length(dmpcolorstouse));
+acolors(rmpcolorstouse,:) = cool(length(rmpcolorstouse));
+
+figure(1)
+clf(1)
+figure(1)
+hold on 
+subplot(2,1,1)
+title ('Basis functions times weights (dmp)')
+for j = 1:size(D.psi,2)
+    plot((1:S.Nt)*S.dt, psiall(:,j)*D.w(1,j), '-c', 'Color', psicolors(j,:))
+    plot((1:S.Nt)*S.dt, psiall(:,j)*D.w(2,j), '-c.', 'Color', acolors(j,:))
+end
+subplot(2,1,2)
+title('Basis functions times weights (rmp)')
+for j = 1:size(R.a,2)
+    plot((1:S.Nt)*S.dt, aall(:,j)*R.w(1,j), '-c', 'Color', psicolors(j,:))
+    plot((1:S.Nt)*S.dt, aall(:,j)*R.w(2,j), '-c.', 'Color', psicolors(j,:))
+end
+
+figure(2)
+ clf(2)
+ figure(2)
+hold on
+subplot(2,1,1)
+plot(yalldmp(:,1), yalldmp(:,2), 'k-')
+plot(S.desiredtraj(:,1), S.desiredtraj(:,2), '-g.');
+plot(yalldmp(whichcolor==1,1), yalldmp(whichcolor==1,2), 'k.')
+for i = 1:size(D.w, 2)
+    plot(yalldmp(whichcolor==(1+i),1), yalldmp(whichcolor==(1+i),2), 'k.', 'Color', psicolors(i,:))
+    plot(D.w(1,i), D.w(2,i), 'k*', 'Color', psicolors(i,:))
+end
+axis equal
+subplot(2,1,2)
+plot(yallrmp(:,1), yallrmp(:,2), 'k-')
+plot(S.desiredtraj(:,1), S.desiredtraj(:,2), '-g.');
+plot(yallrmp(whichcolor==1,1), yallrmp(whichcolor==1,2), 'k.')
+for i = 1:size(R.w, 2)
+    plot(yallrmp(whichcolor==(1+i),1), yallrmp(whichcolor==(1+i),2), 'k.', 'Color', psicolors(i,:))
+    plot(R.w(1,i), R.w(2,i), 'k*', 'Color', psicolors(i,:))
+end
+axis equal
+
+figure(3)
+ clf(3)
+ figure(3)
+hold on
+subplot(2,1,1)
+for i = 1:size(D.w, 2)
+    plot(yalldmp(whichcolor==(1+i),1), yalldmp(whichcolor==(1+i),2), 'k.', 'Color', psicolors(i,:))
+end
+plot(S.desiredtraj(:,1),S.desiredtraj(:,2), '-k.');
+axis equal
+subplot(2,1,2)
+for i = 1:size(R.w, 2)
+    plot(yallrmp(whichcolor==(1+i),1), yallrmp(whichcolor==(1+i),2), 'k.', 'Color', psicolors(i,:))
+end
+plot(S.desiredtraj(:,1),S.desiredtraj(:,2), '-k.');
+axis equal
+
+
+
+
+end
+
diff --git a/Eval.m b/Eval.m
index c464728..ee688fb 100644
--- a/Eval.m
+++ b/Eval.m
@@ -1,4 +1,4 @@
-function d = Eval(w, Nt, y0dot, y0, x0, alphax, tau, dt, sigma_or_rho, c_or_alpha, alphay, betay,g, desiredtraj, ShowPlot, OneforDMPtwoforRMP, w1,w2,Nsaddles)
+function [d,dcomponents,ftrace] = Eval(w, Nt, y0dot, y0, x0, alphax, tau, dt, sigma_or_rho, c_or_alpha, alphay, betay,g, desiredtraj, ShowPlot, OneforDMPtwoforRMP)
 % %--------------------
 goaltol = .005;
 %ShowPlot = 1;
@@ -42,7 +42,7 @@
  
  else
 % %for RMP
-% Nsaddles = 10;
+Nsaddles = 10;
 alpha = 10;
 beta = 1;
 nu = 1.2;
@@ -67,10 +67,11 @@
 end
 
 distance = 0;
-
 ydot = y0dot;
 y = y0;
-fnum = 0;
+% fnum = 0;
+sumf = 0;
+ftrace = [];
 
 maxtime = (Nt+1)*dt;
 timetogoal = nan;
@@ -90,6 +91,7 @@
 
 trajpointdistancessq = Inf*ones(size(desiredtraj, 1), 1);
 
+xall = [];
 for i = 1:Nt
     % advance diff equation
     %DMP
@@ -110,8 +112,8 @@
     %RMP
     if OneforDMPtwoforRMP == 2
     dW = sqrt(dt)*randn(1,Nsaddles);
-    da = a .* (alpha - a*rho) *dt + ep.*dW; %%%DID I forget tau??
-    a = max(min(a + da, 1), .0005);
+    da = a .* (alpha - a*rho) *dt + ep.*dW; %tau for scaling?
+    a = max(min(a + da, 1), .0005); %enforcing boundaries on a
     f = a*w'/sum(a);
     end
     ydotdot = (alphay*(betay*(g-y)-ydot)+f)/tau;
@@ -157,13 +159,11 @@
         psiall(i,:) = psi;
         end
         if OneforDMPtwoforRMP == 2
-            if Nsaddles == 1
-                aall = a;
-            else
-                aall(i,:) = a;
-            end
+            aall(i,:) = a;
         end
     end
+    sumf = sumf + f;
+    ftrace = [ftrace;f];
 end
 
 % %evaluate trajectory
@@ -179,11 +179,10 @@
 if isnan(timetogoal) 
     timetogoal = maxtime-dt;
 end
-
 d = 1*distance+...
-    10*timetogoal/Nt+...
+    1e5*timetogoal/Nt+...
     1*sum(sqrt(trajpointdistancessq));
-
+dcomponents = [distance, timetogoal/Nt, sum(sqrt(trajpointdistancessq))];
 
 if ShowPlot
 %grayout colors that aren't used
@@ -194,7 +193,7 @@
 psicolors = ones(1, size(w,2))'*[0.7, 0.7, 0.7];
 psicolors(colorstouse,:) = cool(length(colorstouse));
 
-
+% majorpsi = exp(-1./(2*sum(sigma_or_rho.^2)).*(xall-sum(c_or_alpha)).^2);
 figure(3)
 clf(3)
 figure(3)
@@ -218,7 +217,7 @@
 hold on
 plot(yall(:,1), yall(:,2), 'k-')
 plot(desiredtraj(:,1), desiredtraj(:,2), '-g.');
-plot(w2(1,:), w2(2,:), '-r.')
+% plot(w2(1,:), w2(2,:), '-r.')
 plot(w(1,:), w(2,:), '-ro', 'Color', [.6, .6, .6])
 
 plot(yall(whichcolor==1,1), yall(whichcolor==1,2), 'k.')
@@ -253,5 +252,12 @@
 %     plot((1:Nt)*dt, 1.1+aall(:,j), '-c', 'Color', saddlecolors(j,:))
 %     plot((1:Nt)*dt, 1.1+aall(:,j), '-c.', 'Color', saddlecolors(j,:))
 % end
+
+% figure(7)
+% clf(7)
+% figure(7)
+% hold on
+% plot((1:Nt)*dt,ftrace(:,1),'Linewidth',2,'Color','r')
+% plot((1:Nt)*dt,ftrace(:,2),'Linewidth',2,'Color','b')
 end
 
diff --git a/RunbothRMP_and_DMP.m b/RunbothRMP_and_DMP.m
new file mode 100644
index 0000000..8bc9ecb
--- /dev/null
+++ b/RunbothRMP_and_DMP.m
@@ -0,0 +1,28 @@
+clear all
+close all
+clc
+
+% wdmp = cell2mat(struct2cell(load('wbestDMP1.mat','w')));
+% [wdmp,Nt,y0dot,y0,x0,alphax,tau,dt,sigma_or_rho,c_or_alpha,alphay,betay,g,desiredtraj,ShowPlot,OneforDMPtwoforRMP]...
+%     = cell2mat(struct2cell(load('wbestDMP1.mat')));
+load('wbestDMP1.mat')
+wdmp = w;
+[d,dcomponents,ftrace1] = Eval(wdmp, Nt, y0dot, y0, x0, alphax, tau, dt, sigma, c, alphay, betay,g, desiredtraj, 1, OneforDMPtwoforRMP);
+
+% wrmp = cell2mat(struct2cell(load('wbestRMP1.mat','w')));
+load('wbestRMP1.mat')
+wrmp = w;
+[d,dcomponents,ftrace2] = Eval(wrmp, Nt, y0dot, y0, x0, alphax, tau, dt, sigma, c, alphay, betay,g, desiredtraj, 1, OneforDMPtwoforRMP);
+
+figure(7)
+hold on
+plot((1:Nt)*dt,ftrace1(:,1),'ro','MarkerSize',0.7)
+plot((1:Nt)*dt,ftrace1(:,2),'bo','MarkerSize',0.7)
+
+plot((1:Nt)*dt,ftrace2(:,1),'Linewidth',1.5,'Color',[0.6 0 0])
+plot((1:Nt)*dt,ftrace2(:,2),'Linewidth',1.5,'Color',[0 0 0.6])
+
+figure(8)
+hold on
+plot(ftrace1(:,1),ftrace1(:,2),'.k')
+plot(ftrace2(:,1),ftrace2(:,2),'-k')
\ No newline at end of file
diff --git a/run8.m b/run8.m
index a451289..5b4f419 100644
--- a/run8.m
+++ b/run8.m
@@ -4,9 +4,8 @@
 tic
 rng(1)
 
-finalds = [];
-for Nsaddles = 5:20
-OneforDMPtwoforRMP = 1
+% finalds = [];
+OneforDMPtwoforRMP = 2
 
 Usepresents = 1;
 dt = .01;
@@ -43,14 +42,14 @@
 alphay = 4; % if I decrease these numbers weird stuff happens % damping and stiffness
 betay = alphay/4; %alphay/4; % if I decrease these numbers weird stuff happens % stiffnesses
 alphax = .5; %how fast cannonical system decays
-Npsi = Nsaddles;
-% Npsi = 10;
+% Npsi = Nsaddles;
+Npsi = 10;
 c = logspace(log10(1), log10(.01), Npsi); % these are the positions of DMP in X, and they do not change. 
 sigma = logspace(log10(.3),log10(.002), Npsi); %if I make this narrower (smaller sigma) it looks like it can get smaller turn radius, but best solution has a kink
 %Cz = 0 no coupling
 
 %-------------------
-Ktrials = 1000;
+Ktrials = 500;
 % w = 5-10*rand(2, Npsi); % these are the weights - they do change.
 %Bestw for ramp up wrap around
 %  w =[0.1682   -0.0909   -2.5292   -2.0515   -0.4892    1.4525    2.4341    1.8060   -0.0140   -2.3589
@@ -92,7 +91,7 @@
 else
     
     
-% Nsaddles = 10; %DUPLICATED IN EVAL
+Nsaddles = 10; %DUPLICATED IN EVAL
 alpha = 10; %DUPLICATED IN EVAL
 beta = 1;  %DUPLICATED IN EVAL
 nu = 1.2; %DUPLICATED IN EVAL
@@ -105,16 +104,19 @@
 index = 1;
 expected_SHCas = a;
 
-for t = desiredt(1): dt: desiredt(end)    
+for t = desiredt(1): dt: desiredt(end)  
+    t
+    index
     dW = sqrt(dt)*randn(1,Nsaddles);
-    da = a .* (alpha - a*rho) *dt + ep.*dW; %wondering if I forgot tau
-    a = max(min(a + da, 1), .0005);
+    da = a .* (alpha - a*rho) *dt + ep.*dW %wondering if I forgot tau
+    a = max(min(a + da, 1), .0005)
     
     if t>=desiredt(index)
         index = index+1;
     else
         expected_SHCas(index, :) = a;
     end
+    pause
 end
 
 
@@ -130,10 +132,11 @@
 
 
 %TWO main functions here, one to evaluate weights
-d = Eval(w, Nt, y0dot, y0, x0, alphax, tau, dt, sigma, c, alphay, betay,g, desiredtraj, 1, OneforDMPtwoforRMP, w1,w2,Nsaddles);
+[d,dcomponents] = Eval(w, Nt, y0dot, y0, x0, alphax, tau, dt, sigma, c, alphay, betay,g, desiredtraj, 1, OneforDMPtwoforRMP);
 
 delta = 1;
 allds = [];
+dcomponentsplot = [];
 for passes = 1:10
 bestw = w;
 bestd = d;
@@ -141,7 +144,7 @@
     if rand(1)>.5
         % try random stuff
        w = 1-.5*rand(2, Npsi);
-       d = Eval(w, Nt, y0dot, y0, x0, alphax, tau, dt, sigma, c, alphay, betay,g, desiredtraj, 0,OneforDMPtwoforRMP, w1,w2,Nsaddles);
+       [d,dcomponents] = Eval(w, Nt, y0dot, y0, x0, alphax, tau, dt, sigma, c, alphay, betay,g, desiredtraj, 0,OneforDMPtwoforRMP);
        if d<bestd
            bestw = w;
            bestd = d;
@@ -150,24 +153,23 @@
     else
         %try different random stuff based on size delta 
        w = bestw + 3*delta*randn(2, Npsi);
-       d = Eval(w, Nt, y0dot, y0, x0, alphax, tau, dt, sigma, c, alphay, betay,g, desiredtraj, 0,OneforDMPtwoforRMP, w1,w2,Nsaddles);
+       [d,dcomponents] = Eval(w, Nt, y0dot, y0, x0, alphax, tau, dt, sigma, c, alphay, betay,g, desiredtraj, 0,OneforDMPtwoforRMP);
        if d<bestd
            bestw = w;
            bestd = d;
        end
        
     end
- 
+
 end
 w = bestw;
 d = bestd;
 delta = delta*.99;
-Eval(w, Nt, y0dot, y0, x0, alphax, tau, dt, sigma, c, alphay, betay,g, desiredtraj, 1,OneforDMPtwoforRMP, w1,w2,Nsaddles);
+Eval(w, Nt, y0dot, y0, x0, alphax, tau, dt, sigma, c, alphay, betay,g, desiredtraj, 1,OneforDMPtwoforRMP);
 allds = [allds,bestd];
-
+dcomponentsplot = [dcomponentsplot;dcomponents];
 
 % now for a gradient desecent on the best one. 
-
 pause(1)
 chosenew = Inf;
 chosedel = Inf;
@@ -180,28 +182,30 @@
          wnew = bestw;
          delt = delta*(1-2*rand(1));
          wtrial(i,j) = bestw(i,j) + delt;
-         dtrial =  Eval(wtrial, Nt, y0dot, y0, x0, alphax, tau, dt, sigma, c, alphay, betay,g, desiredtraj, 0,OneforDMPtwoforRMP,w1,w2,Nsaddles);
+         [dtrial,dtrialcomponents] =  Eval(wtrial, Nt, y0dot, y0, x0, alphax, tau, dt, sigma, c, alphay, betay,g, desiredtraj, 0,OneforDMPtwoforRMP);
          wnew(i,j) = bestw(i,j) + max(min(bestd*delt/(bestd - dtrial), delta*5), -delta*5);
-         dnew =  Eval(wnew, Nt, y0dot, y0, x0, alphax, tau, dt, sigma, c, alphay, betay,g, desiredtraj, 0,OneforDMPtwoforRMP,w1,w2,Nsaddles);
+         [dnew,dnewcomponents] =  Eval(wnew, Nt, y0dot, y0, x0, alphax, tau, dt, sigma, c, alphay, betay,g, desiredtraj, 0,OneforDMPtwoforRMP);
          if dnew < bestd
              bestw = wnew;
              bestd = dnew;
+             dcomponents = dnewcomponents;
              chosenew = chosenew +1;
          end
          if dtrial < bestd
              bestw = wtrial;
              bestd = dtrial;
+             dcomponents = dtrialcomponents;
              chosedel = chosedel +1;
          end
          
      end
  end
-%  chosenew
-%  chosedel
+ chosenew
+ chosedel
 end
- 
-%at the end of one pass, save progress
+
 allds = [allds,bestd];
+dcomponentsplot = [dcomponentsplot;dcomponents];
 w = bestw;
 d = bestd;
 %ShowTraj(w, Nt, y0dot, y0, x0, alphax, tau, dt, sigma, c, alphay, betay,g, desiredtraj);
@@ -209,11 +213,14 @@
 %     pause
 % end
 end
-Eval(w, Nt, y0dot, y0, x0, alphax, tau, dt, sigma, c, alphay, betay,g, desiredtraj, 1,OneforDMPtwoforRMP,w1,w2,Nsaddles);
-
-% figure(4)
-% plot(allds)
+Eval(w, Nt, y0dot, y0, x0, alphax, tau, dt, sigma, c, alphay, betay,g, desiredtraj, 1,OneforDMPtwoforRMP);
 
+figure(4)
+hold on
+plot(allds,'k','Linewidth',2)
+plot(dcomponentsplot(:,1),'b')
+plot(dcomponentsplot(:,2),'m')
+plot(dcomponentsplot(:,3),'r')
 %  
 % pause
 % ShowTraj(w, Nt, y0dot, y0, x0, alphax, 2*tau, dt, sigma, c, alphay, betay,g, desiredtraj);
@@ -228,9 +235,272 @@
 % tau % changing this makes shape pretty different, sloppier if highter
 %dt  %seems like I can change this without changing shape
 
-finalds = [finalds,allds(end)]
-figure(6)
-plot(finalds)
+% finalds = [finalds,allds(end)]
+% figure(6)
+% plot(finalds)
+% end
+
+toc
+
+function [d,dcomponents] = Eval(w, Nt, y0dot, y0, x0, alphax, tau, dt, sigma_or_rho, c_or_alpha, alphay, betay,g, desiredtraj, ShowPlot, OneforDMPtwoforRMP)
+% %--------------------
+goaltol = .005;
+%ShowPlot = 1;
+% % initialize or variable descriptions (basics that are the same for RMP/DMP)
+%  Nt = 1000; % number of timesteps
+%  y0 = .001 * [1,1]; %system initial position
+%  y0dot= 0 * [1,1]; %system initial velocity
+%   dt = .01; %timestep 
+% tau = 1;   %time constant base system, also reused in canonical system        
+% alphay = 4; %damping base system
+% betay = alphay/4; %stiffness base system
+% alphax = .5; %how fast cannonical system decays
+% g = [1,0]; %goal position of base system
+%  desiredtraj = [0:.05:.5, fliplr(.5*cos((-pi/2):-.1:-(pi))+1);
+%                 0:.05:.5, fliplr(.5*sin((-pi/2):-.1:-(pi))+.5)]'; 
+
+% %shared naming
+% x0 = 1; %cannonical system initial state (scalar for DMP, vector for RMP)
+% w  = weights that are learned  
+    %for DMP w will be size 2 by Npsi (assuming planar trajectory)
+% w  =   [0.8726   -4.8226   -5.2870   -5.6955   11.9854   -1.6538    0.2696   -0.6114    2.2657   -0.8382;
+%        4.3342    1.8618    0.2612   -1.5936   -5.3375    5.2797   -3.1289    0.0051    0.4761   -1.3480];
+% for RMP w will be size 
+%
+% 
+
+% Nsaddles = 20;
+if OneforDMPtwoforRMP == 1
+% %for DMP
+%   Npsi = 10; %number of basis functions (use this generate c and sigma)
+ % c = logspace(log10(1), log10(.01), Npsi); % these are the positions of DMP in X, and they do not change. 
+ % sigma = 2*logspace(log10(.3),log10(.002), Npsi);
+ sigma = sigma_or_rho;
+ c = c_or_alpha;
+ 
+ x = x0;
+ if ShowPlot
+xall = nan*ones(Nt,1);
+psiall =  nan*ones(Nt,length(sigma));
+ end
+ 
+ else
+% %for RMP
+Nsaddles = 10;
+alpha = 10;
+beta = 1;
+nu = 1.2;
+rho = calculaterho_notacycle(alpha*ones(Nsaddles,1), beta*ones(Nsaddles,1), nu*ones(Nsaddles,1));
+ 
+ 
+ep = 10^-9*ones(1,Nsaddles);
+a0 = ep;
+a0(1) = 1;
+a0(2) = .01;
+a = a0;
+if ShowPlot
+aall = nan*ones(Nt, Nsaddles);
 end
+% %%rho = []; %coupling matrix
+% %%alpha = []; %growth factors
+% % for now P and zd are zero since not measuring contact
+% % and C is zero because there is no coupling and no measured z
+% rho = sigma_or_rho;
+% alpha = c_or_alpha;
+% 
+end
+
+distance = 0;
+ydot = y0dot;
+y = y0;
+% fnum = 0;
+sumf = 0;
+ftrace = [];
+
+maxtime = (Nt+1)*dt;
+timetogoal = nan;
+
+if ShowPlot
+yall = nan*ones(Nt,2);
+ydotall = nan*ones(Nt,2);
+whichcolor = nan*ones(Nt,1);
+alldistanceerror = nan*ones(Nt,1);
+whichtraj = nan*ones(Nt,1);
+allverror = nan*ones(Nt,1);
+end
+
+desiredtrajdir = diff(desiredtraj);
+desiredtrajdir = .5*( [desiredtrajdir;desiredtrajdir(end,:)] + [desiredtrajdir(1,:);desiredtrajdir]);%centerdiffs
+desiredtrajdir = desiredtrajdir ./sqrt(sum(desiredtrajdir.^2,2)); %normalize
+
+trajpointdistancessq = Inf*ones(size(desiredtraj, 1), 1);
+
+xall = [];
+for i = 1:Nt
+    % advance diff equation
+    %DMP
+    if OneforDMPtwoforRMP == 1        
+%         w = [w(1,:); zeros(1,length(w))];
+%         w = [zeros(1,length(w));w(2,:)];
+    x = x + ((-alphax)*x)/tau*dt;
+    psi = exp(-1./(2*sigma.^2).*(x-c).^2);
+%     fnum = fnum + ((psi*w')*x);
+    f = (psi*w') *x /sum(psi);
+%     f = [0,f(2)];
+%     f = [f(1),0];
+%     %new weights
+%         beta2 = ((alphay*(betay*(g-y)-ydot))*(dt^2)/tau) + ydot*dt + y;
+%         beta1 = (psi*w'*(dt^2))*x/tau;
+%         f = beta2; 
+    end
+    %RMP
+    if OneforDMPtwoforRMP == 2
+    dW = sqrt(dt)*randn(1,Nsaddles);
+    da = a .* (alpha - a*rho) *dt + ep.*dW; %tau for scaling?
+    a = max(min(a + da, 1), .0005); %enforcing boundaries on a
+    f = a*w'/sum(a);
+    end
+    ydotdot = (alphay*(betay*(g-y)-ydot)+f)/tau;
+    ydot = ydotdot*dt + ydot;
+    y = ydot*dt+y;    
+    
+    
+    if ShowPlot
+        % find out which part of solution has max contribution
+        if OneforDMPtwoforRMP == 1
+        [~, whichcolor(i)] = max( [sum( abs(alphay*(betay*(g-y)-ydot)).^2)*.1;
+                                   sum((abs((psi.*w) *x /sum(psi))').^2,2)]);
+        else 
+            [~, whichcolor(i)] = max( [sum( abs(alphay*(betay*(g-y)-ydot)).^2)*.1;
+                                       sum((abs((a.*w)/sum(a))').^2,2)]);
+                                  
+        end
+        
+    end
+    trajpointdistancessq = min ((y(1) - desiredtraj(:,1)).^2 + (y(2) - desiredtraj(:,2)).^2, ...
+                                trajpointdistancessq);
+       
+    [dsquared, whichtrajpoint] = min( (y(1) - desiredtraj(:,1)).^2 + ...
+        (y(2) - desiredtraj(:,2)).^2);
+    distance = distance + ...
+        sqrt( dsquared);
+    desv = desiredtrajdir(whichtrajpoint, :);
+    vnormalerror = (desv(1)*ydot(2) - desv(2)*ydot(1))/sqrt(ydot*ydot');
+    
+    % here if its at the goal, I record time
+    if isnan(timetogoal) && sum((y-g).^2)<goaltol^2
+        timetogoal = i*dt;
+    end
+    
+    if ShowPlot
+        alldistanceerror(i) = sqrt( dsquared);
+        whichtraj(i) = whichtrajpoint;
+        allverror(i) = vnormalerror;
+        yall(i,:) = y;
+        ydotall(i,:) = ydot;
+        if OneforDMPtwoforRMP == 1
+        xall(i) = x;
+        psiall(i,:) = psi;
+        end
+        if OneforDMPtwoforRMP == 2
+            aall(i,:) = a;
+        end
+    end
+    sumf = sumf + f;
+    ftrace = [ftrace;f];
+end
+
+% %evaluate trajectory
+% distance = nan*ones(Nt,1);
+% whichtraj = nan*ones(Nt,1);
+% for i = 1:Nt
+%     distance(i) = sqrt( min( (yall(i,1) - desiredtraj(:,1)).^2 + ... 
+%                 (yall(i,2) - desiredtraj(:,2)).^2));
+%     [~,whichtraj(i)] = min( (yall(i,1) - desiredtraj(:,1)).^2 + ... 
+%                 (yall(i,2) - desiredtraj(:,2)).^2);
+% end
+
+if isnan(timetogoal) 
+    timetogoal = maxtime-dt;
+end
+d = 1*distance+...
+    1e5*timetogoal/Nt+...
+    1*sum(sqrt(trajpointdistancessq));
+dcomponents = [distance, timetogoal/Nt, sum(sqrt(trajpointdistancessq))];
+
+if ShowPlot
+%grayout colors that aren't used
+colorstouse = setdiff(unique(whichcolor(1:i)-1), 0);
+% colorstouse = setdiff(unique(whichcolor(1:(timetogoal/dt)))-1, 0);
+colorstouse = setdiff(unique(whichcolor)-1, 0);
+
+psicolors = ones(1, size(w,2))'*[0.7, 0.7, 0.7];
+psicolors(colorstouse,:) = cool(length(colorstouse));
+
+% majorpsi = exp(-1./(2*sum(sigma_or_rho.^2)).*(xall-sum(c_or_alpha)).^2);
+figure(3)
+clf(3)
+figure(3)
+hold on 
+title ('Basis functions times weights')
+if OneforDMPtwoforRMP == 1
+    for j = 1:size(psi,2)
+        plot((1:Nt)*dt, psiall(:,j)*w(1,j), '-c', 'Color', psicolors(j,:))
+        plot((1:Nt)*dt, psiall(:,j)*w(2,j), '-c.', 'Color', psicolors(j,:))
+    end
+else
+    for j = 1:size(a,2)
+        plot((1:Nt)*dt, aall(:,j)*w(1,j), '-c', 'Color', psicolors(j,:))
+        plot((1:Nt)*dt, aall(:,j)*w(2,j), '-c.', 'Color', psicolors(j,:))
+    end
+end
+
+figure(2)
+ clf(2)
+ figure(2)
+hold on
+plot(yall(:,1), yall(:,2), 'k-')
+plot(desiredtraj(:,1), desiredtraj(:,2), '-g.');
+% plot(w2(1,:), w2(2,:), '-r.')
+plot(w(1,:), w(2,:), '-ro', 'Color', [.6, .6, .6])
+
+plot(yall(whichcolor==1,1), yall(whichcolor==1,2), 'k.')
+for i = 1:size(w, 2)
+    plot(yall(whichcolor==(1+i),1), yall(whichcolor==(1+i),2), 'k.', 'Color', psicolors(i,:))
+    plot(w(1,i), w(2,i), 'k*', 'Color', psicolors(i,:))
+end
+axis equal
+
+figure(5)
+ clf(5)
+ figure(5)
+hold on
+for i = 1:size(w, 2)
+    plot(yall(whichcolor==(1+i),1), yall(whichcolor==(1+i),2), 'k.', 'Color', psicolors(i,:))
+end
+
+plot(desiredtraj(:,1), desiredtraj(:,2), '-k.');
+axis equal
+
+% figure(6)
+% clf(6)
+% figure(6)
+% 
+% hold on 
+% for j = 1:size(psi,2)
+%     plot((1:Nt)*dt, psiall(:,j), '-c', 'Color', psicolors(j,:))
+%     plot((1:Nt)*dt, psiall(:,j), '-c.', 'Color', psicolors(j,:))
+% end
+% saddlecolors  = cool(Nsaddles);
+% for j = 1:Nsaddles
+%     plot((1:Nt)*dt, 1.1+aall(:,j), '-c', 'Color', saddlecolors(j,:))
+%     plot((1:Nt)*dt, 1.1+aall(:,j), '-c.', 'Color', saddlecolors(j,:))
+% end
 
-toc
\ No newline at end of file
+figure(7)
+clf(7)
+figure(7)
+hold on
+plot((1:Nt)*dt,ftrace(:,1),'Linewidth',2,'Color','r')
+plot((1:Nt)*dt,ftrace(:,2),'Linewidth',2,'Color','b')
+end
\ No newline at end of file
diff --git a/run9.m b/run9.m
new file mode 100644
index 0000000..d8eeb9c
--- /dev/null
+++ b/run9.m
@@ -0,0 +1,506 @@
+close all
+clear all
+clc
+tic
+rng(1)
+
+% finalds = [];
+OneforDMPtwoforRMP = 1
+
+Usepresents = 1;
+dt = .01;
+Nt = 1000;
+g = [1,0];
+y0 = .001 * [1,1];
+y0dot= 0 * [1,1];
+x0 = 1;
+
+% %ramp up bubble out
+% desiredtraj = [0:.05:.5, .5*cos((pi/2):-.1:0)+.5;
+%                0:.05:.5, .5*sin((pi/2):-.1:0)]'; 
+           
+% % %ramp up bubble in ------ 
+%    desiredtraj = [0:.05:.5, fliplr(.5*cos((-pi/2):-.1:-(pi))+1);
+%                   0:.05:.5, fliplr(.5*sin((-pi/2):-.1:-(pi))+.5)]'; 
+%-------             
+% %ramp up wrap around
+   desiredtraj = [(0:.05:.5)*0, .5*cos(pi/2:.1:pi*2.6);
+                   0:.05:.5, .5*sin(pi/2:.1:pi*2.6)]';
+  g = [0,0];
+% % ----- 
+%% IGNORE THIS
+%  bestw =[  3.2763   -7.9183    2.9071   -7.7999  -19.5263   -4.2977    7.2047   -0.1165  -1.7832    2.3749
+%           6.9902   -0.5676   -2.2115    2.5635   11.9405   -6.1655   -0.0285    0.6614 -0.6912    0.6141];
+%  bestw = [   2.5279   -6.3904    0.4768   -0.9810   -4.9774    3.7569 -0.7490    0.0487   -0.1125    0.3058;
+%             7.3744   -2.2085   -0.0588   -0.1458    0.6147   -0.3531   -0.0641    0.0472   -0.0146   -0.0088];
+% % After running all night! with all three factors
+% % bestw =  [-2.9178   -4.4224    0.7990   -0.0852
+%            2.1801    0.4769   -0.2220   -0.0464];
+% two factors (no direction)      
+%%
+tau = 1;
+alphay = 4; % if I decrease these numbers weird stuff happens % damping and stiffness
+betay = alphay/4; %alphay/4; % if I decrease these numbers weird stuff happens % stiffnesses
+alphax = .5; %how fast cannonical system decays
+% Npsi = Nsaddles;
+Npsi = 10;
+c = logspace(log10(1), log10(.01), Npsi); % these are the positions of DMP in X, and they do not change. 
+sigma = logspace(log10(.3),log10(.002), Npsi); %if I make this narrower (smaller sigma) it looks like it can get smaller turn radius, but best solution has a kink
+%Cz = 0 no coupling
+
+%-------------------
+Ktrials = 500;
+% w = 5-10*rand(2, Npsi); % these are the weights - they do change.
+%Bestw for ramp up wrap around
+%  w =[0.1682   -0.0909   -2.5292   -2.0515   -0.4892    1.4525    2.4341    1.8060   -0.0140   -2.3589
+%     1.8293    2.7509    0.6173   -1.1173   -2.4039   -1.9603   -0.0670    1.5410    2.4332    1.5410];
+% w =[    0.0990   -0.3065   -2.3560   -2.0671   -0.4662    1.4635    2.3638    1.8060   -0.0140   -1.7901;
+%     2.0122    2.6248    0.6573   -1.1035   -2.4039   -1.9603   -0.2377    1.6481    2.3701    1.7008];
+
+%What happens when w = desired traj (simple sampling)
+% w = desiredtraj(round(linspace(1,length(desiredtraj),Npsi)) ,:)'*(alphay+betay); %when alphay = 4, betay = 1nice to multiply by 5
+% w =   [  0.2060   -0.3065   -2.4163   -2.0671   -0.4646    1.4635    2.2877    1.8060   -0.1399   -2.0545;
+%     1.7634    2.6421    0.6093   -1.1801   -2.4039   -1.8916   -0.2974    1.6481    2.4332    1.5173];
+w = desiredtraj(round(linspace(1,length(desiredtraj),Npsi)) ,:)';
+w1 = w;
+w = desiredtraj(round(linspace(1,length(desiredtraj),Npsi)) ,:)'*(alphay+betay);
+w2 = w;
+
+%500 trials
+% wbest =   [  0.1682   -0.0909   -2.5292   -2.0515   -0.4892    1.4525    2.4341    1.8060   -0.0140   -2.3589;
+%     1.8293    2.7509    0.6173   -1.1173   -2.4039   -1.9603   -0.0670    1.5410    2.4332    1.5410];
+%1000 trials
+% wbest =  [   0.2060   -0.3065   -2.4163   -2.0671   -0.4646    1.4635    2.2877    1.8060   -0.1399   -2.0545;
+%     1.7634    2.6421    0.6093   -1.1801   -2.4039   -1.8916   -0.2974    1.6481    2.4332    1.5173];
+
+if Usepresents
+%%%%%%% $$$ Here I am inserting linear algebra approach to weight finding
+% where we know f = tau*ydotdot - (alphay*(betay*(g-y) - ydot);
+desiredt =  linspace(0,10,length(desiredtraj))';% [1:length(desiredtraj)]'; % assumes constant velocity linspace(0,10,length(desiredtraj))
+desiredydot = [0,0;diff(desiredtraj)./(diff(desiredt)*[1,1])]; % desired speeds
+desiredydotdot = [0,0;diff(desiredydot)./(diff(desiredt)*[1,1])];
+fdesired = tau * desiredydotdot - (alphay * (betay*(g-desiredtraj) - desiredydot));
+
+if OneforDMPtwoforRMP == 1
+    expected_x = x0*exp(-alphax/tau*(desiredt-desiredt(1)));
+    expectedPsi = exp(ones(length(desiredt),1)*(-1./(2*sigma.^2)) .*(expected_x - c).^2); %n traj points by n basis 
+    
+    % proveXexpworking % run this code to make a plot
+    betaw =  (expected_x .* expectedPsi) \ (sum(expectedPsi')'.* fdesired);
+    errors = (expected_x .* expectedPsi)*betaw - (sum(expectedPsi')'.* fdesired);
+else
+    
+    
+Nsaddles = 10; %DUPLICATED IN EVAL
+alpha = 10; %DUPLICATED IN EVAL
+beta = 1;  %DUPLICATED IN EVAL
+nu = 1.2; %DUPLICATED IN EVAL
+rho = calculaterho_notacycle(alpha*ones(Nsaddles,1), beta*ones(Nsaddles,1), nu*ones(Nsaddles,1));
+ep = 10^-9*ones(1,Nsaddles);  %DUPLICATED IN EVAL
+a0 = ep;  %DUPLICATED IN EVAL
+a0(1) = 1; %DUPLICATED IN EVAL
+a0(2) = .01; %DUPLICATED IN EVAL
+a = a0;
+index = 1;
+expected_SHCas = a;
+
+for t = desiredt(1): dt: desiredt(end)  
+    dW = sqrt(dt)*randn(1,Nsaddles);
+    da = a .* (alpha - a*rho) *dt + ep.*dW; %wondering if I forgot tau
+    a = max(min(a + da, 1), .0005);
+    
+    if t>=desiredt(index)
+        index = index+1;
+    else
+        expected_SHCas(index, :) = a;
+    end
+end
+
+
+    size(expected_SHCas)
+    betaw =  ( expected_SHCas) \ (sum(expected_SHCas')'.* fdesired);
+end
+
+
+% meanerror = sum(sum(errors.^2))
+w = betaw';
+%%%%%%% $$$ end insertion
+end
+
+
+%TWO main functions here, one to evaluate weights
+[d,dcomponents] = Eval(w, Nt, y0dot, y0, x0, alphax, tau, dt, sigma, c, alphay, betay,g, desiredtraj, 1, OneforDMPtwoforRMP);
+
+delta = 1;
+allds = [];
+dcomponentsplot = [];
+for passes = 1:10
+bestw = w;
+bestd = d;
+for i = 1:Ktrials
+    if rand(1)>.5
+        % try random stuff
+       w = 1-.5*rand(2, Npsi);
+       [d,dcomponents] = Eval(w, Nt, y0dot, y0, x0, alphax, tau, dt, sigma, c, alphay, betay,g, desiredtraj, 0,OneforDMPtwoforRMP);
+       if d<bestd
+           bestw = w;
+           bestd = d;
+           delta = 1;
+       end
+    else
+        %try different random stuff based on size delta 
+       w = bestw + 3*delta*randn(2, Npsi);
+       [d,dcomponents] = Eval(w, Nt, y0dot, y0, x0, alphax, tau, dt, sigma, c, alphay, betay,g, desiredtraj, 0,OneforDMPtwoforRMP);
+       if d<bestd
+           bestw = w;
+           bestd = d;
+       end
+       
+    end
+
+end
+w = bestw;
+d = bestd;
+delta = delta*.99;
+Eval(w, Nt, y0dot, y0, x0, alphax, tau, dt, sigma, c, alphay, betay,g, desiredtraj, 1,OneforDMPtwoforRMP);
+allds = [allds,bestd];
+dcomponentsplot = [dcomponentsplot;dcomponents];
+
+% now for a gradient desecent on the best one. 
+pause(1)
+chosenew = Inf;
+chosedel = Inf;
+while chosenew+chosedel>=2
+chosenew = 0;
+chosedel = 0;
+ for i = 1:size(w,1)
+     for j = 1:size(w,2)
+         wtrial = bestw;
+         wnew = bestw;
+         delt = delta*(1-2*rand(1));
+         wtrial(i,j) = bestw(i,j) + delt;
+         [dtrial,dtrialcomponents] =  Eval(wtrial, Nt, y0dot, y0, x0, alphax, tau, dt, sigma, c, alphay, betay,g, desiredtraj, 0,OneforDMPtwoforRMP);
+         wnew(i,j) = bestw(i,j) + max(min(bestd*delt/(bestd - dtrial), delta*5), -delta*5);
+         [dnew,dnewcomponents] =  Eval(wnew, Nt, y0dot, y0, x0, alphax, tau, dt, sigma, c, alphay, betay,g, desiredtraj, 0,OneforDMPtwoforRMP);
+         if dnew < bestd
+             bestw = wnew;
+             bestd = dnew;
+             dcomponents = dnewcomponents;
+             chosenew = chosenew +1;
+         end
+         if dtrial < bestd
+             bestw = wtrial;
+             bestd = dtrial;
+             dcomponents = dtrialcomponents;
+             chosedel = chosedel +1;
+         end
+         
+     end
+ end
+ chosenew
+ chosedel
+end
+
+allds = [allds,bestd];
+dcomponentsplot = [dcomponentsplot;dcomponents];
+w = bestw;
+d = bestd;
+%ShowTraj(w, Nt, y0dot, y0, x0, alphax, tau, dt, sigma, c, alphay, betay,g, desiredtraj);
+% if passes==1 || passes==5 || passes==10
+%     pause
+% end
+end
+Eval(w, Nt, y0dot, y0, x0, alphax, tau, dt, sigma, c, alphay, betay,g, desiredtraj, 1,OneforDMPtwoforRMP);
+
+figure(4)
+hold on
+plot(allds,'k','Linewidth',2)
+plot(dcomponentsplot(:,1),'b')
+plot(dcomponentsplot(:,2),'m')
+plot(dcomponentsplot(:,3),'r')
+%  
+% pause
+% ShowTraj(w, Nt, y0dot, y0, x0, alphax, 2*tau, dt, sigma, c, alphay, betay,g, desiredtraj);
+% w = weights
+% example 
+%     0.8726   -4.8226   -5.2870   -5.6955   11.9854   -1.6538    0.2696   -0.6114    2.2657   -0.8382
+%     4.3342    1.8618    0.2612   -1.5936   -5.3375    5.2797   -3.1289    0.0051    0.4761   -1.3480
+% Nt = number of timesteps
+% y0 initial conditions
+% x0 initial conditions
+% alphax % keep same as training
+% tau % changing this makes shape pretty different, sloppier if highter
+%dt  %seems like I can change this without changing shape
+
+% finalds = [finalds,allds(end)]
+% figure(6)
+% plot(finalds)
+% end
+% figure(7)
+% hold on
+% plot((1:Nt)*dt,ftrace(:,1),'Linewidth',2,'Color','r')
+% plot((1:Nt)*dt,ftrace(:,2),'Linewidth',2,'Color','b')
+
+toc
+
+function [d,dcomponents,ftrace] = Eval(w, Nt, y0dot, y0, x0, alphax, tau, dt, sigma_or_rho, c_or_alpha, alphay, betay,g, desiredtraj, ShowPlot, OneforDMPtwoforRMP)
+% %--------------------
+goaltol = .005;
+%ShowPlot = 1;
+% % initialize or variable descriptions (basics that are the same for RMP/DMP)
+%  Nt = 1000; % number of timesteps
+%  y0 = .001 * [1,1]; %system initial position
+%  y0dot= 0 * [1,1]; %system initial velocity
+%   dt = .01; %timestep 
+% tau = 1;   %time constant base system, also reused in canonical system        
+% alphay = 4; %damping base system
+% betay = alphay/4; %stiffness base system
+% alphax = .5; %how fast cannonical system decays
+% g = [1,0]; %goal position of base system
+%  desiredtraj = [0:.05:.5, fliplr(.5*cos((-pi/2):-.1:-(pi))+1);
+%                 0:.05:.5, fliplr(.5*sin((-pi/2):-.1:-(pi))+.5)]'; 
+
+% %shared naming
+% x0 = 1; %cannonical system initial state (scalar for DMP, vector for RMP)
+% w  = weights that are learned  
+    %for DMP w will be size 2 by Npsi (assuming planar trajectory)
+% w  =   [0.8726   -4.8226   -5.2870   -5.6955   11.9854   -1.6538    0.2696   -0.6114    2.2657   -0.8382;
+%        4.3342    1.8618    0.2612   -1.5936   -5.3375    5.2797   -3.1289    0.0051    0.4761   -1.3480];
+% for RMP w will be size 
+%
+% 
+
+% Nsaddles = 20;
+if OneforDMPtwoforRMP == 1
+% %for DMP
+%   Npsi = 10; %number of basis functions (use this generate c and sigma)
+ % c = logspace(log10(1), log10(.01), Npsi); % these are the positions of DMP in X, and they do not change. 
+ % sigma = 2*logspace(log10(.3),log10(.002), Npsi);
+ sigma = sigma_or_rho;
+ c = c_or_alpha;
+ 
+ x = x0;
+ if ShowPlot
+xall = nan*ones(Nt,1);
+psiall =  nan*ones(Nt,length(sigma));
+ end
+ 
+ else
+% %for RMP
+Nsaddles = 10;
+alpha = 10;
+beta = 1;
+nu = 1.2;
+rho = calculaterho_notacycle(alpha*ones(Nsaddles,1), beta*ones(Nsaddles,1), nu*ones(Nsaddles,1));
+ 
+ 
+ep = 10^-9*ones(1,Nsaddles);
+a0 = ep;
+a0(1) = 1;
+a0(2) = .01;
+a = a0;
+if ShowPlot
+aall = nan*ones(Nt, Nsaddles);
+end
+% %%rho = []; %coupling matrix
+% %%alpha = []; %growth factors
+% % for now P and zd are zero since not measuring contact
+% % and C is zero because there is no coupling and no measured z
+% rho = sigma_or_rho;
+% alpha = c_or_alpha;
+% 
+end
+
+distance = 0;
+ydot = y0dot;
+y = y0;
+% fnum = 0;
+sumf = 0;
+ftrace = [];
+
+maxtime = (Nt+1)*dt;
+timetogoal = nan;
+
+if ShowPlot
+yall = nan*ones(Nt,2);
+ydotall = nan*ones(Nt,2);
+whichcolor = nan*ones(Nt,1);
+alldistanceerror = nan*ones(Nt,1);
+whichtraj = nan*ones(Nt,1);
+allverror = nan*ones(Nt,1);
+end
+
+desiredtrajdir = diff(desiredtraj);
+desiredtrajdir = .5*( [desiredtrajdir;desiredtrajdir(end,:)] + [desiredtrajdir(1,:);desiredtrajdir]);%centerdiffs
+desiredtrajdir = desiredtrajdir ./sqrt(sum(desiredtrajdir.^2,2)); %normalize
+
+trajpointdistancessq = Inf*ones(size(desiredtraj, 1), 1);
+
+xall = [];
+for i = 1:Nt
+    % advance diff equation
+    %DMP
+    if OneforDMPtwoforRMP == 1        
+%         w = [w(1,:); zeros(1,length(w))];
+%         w = [zeros(1,length(w));w(2,:)];
+    x = x + ((-alphax)*x)/tau*dt;
+    psi = exp(-1./(2*sigma.^2).*(x-c).^2);
+%     fnum = fnum + ((psi*w')*x);
+    f = (psi*w') *x /sum(psi);
+%     f = [0,f(2)];
+%     f = [f(1),0];
+%     %new weights
+%         beta2 = ((alphay*(betay*(g-y)-ydot))*(dt^2)/tau) + ydot*dt + y;
+%         beta1 = (psi*w'*(dt^2))*x/tau;
+%         f = beta2; 
+    end
+    %RMP
+    if OneforDMPtwoforRMP == 2
+    dW = sqrt(dt)*randn(1,Nsaddles);
+    da = a .* (alpha - a*rho) *dt + ep.*dW; %tau for scaling?
+    a = max(min(a + da, 1), .0005); %enforcing boundaries on a
+    f = a*w'/sum(a);
+    end
+    ydotdot = (alphay*(betay*(g-y)-ydot)+f)/tau;
+    ydot = ydotdot*dt + ydot;
+    y = ydot*dt+y;    
+    
+    
+    if ShowPlot
+        % find out which part of solution has max contribution
+        if OneforDMPtwoforRMP == 1
+        [~, whichcolor(i)] = max( [sum( abs(alphay*(betay*(g-y)-ydot)).^2)*.1;
+                                   sum((abs((psi.*w) *x /sum(psi))').^2,2)]);
+        else 
+            [~, whichcolor(i)] = max( [sum( abs(alphay*(betay*(g-y)-ydot)).^2)*.1;
+                                       sum((abs((a.*w)/sum(a))').^2,2)]);
+                                  
+        end
+        
+    end
+    trajpointdistancessq = min ((y(1) - desiredtraj(:,1)).^2 + (y(2) - desiredtraj(:,2)).^2, ...
+                                trajpointdistancessq);
+    [dsquared, whichtrajpoint] = min( (y(1) - desiredtraj(:,1)).^2 + ...
+        (y(2) - desiredtraj(:,2)).^2);
+%     distance = distance + sqrt(dsquared);
+distance = sqrt(dsquared);
+    desv = desiredtrajdir(whichtrajpoint, :);
+    vnormalerror = (desv(1)*ydot(2) - desv(2)*ydot(1))/sqrt(ydot*ydot');
+    
+    % here if its at the goal, I record time
+    if isnan(timetogoal) && sum((y-g).^2)<goaltol^2
+        timetogoal = i*dt;
+    end
+    
+    if ShowPlot
+        alldistanceerror(i) = sqrt( dsquared);
+        whichtraj(i) = whichtrajpoint;
+        allverror(i) = vnormalerror;
+        yall(i,:) = y;
+        ydotall(i,:) = ydot;
+        if OneforDMPtwoforRMP == 1
+        xall(i) = x;
+        psiall(i,:) = psi;
+        end
+        if OneforDMPtwoforRMP == 2
+            aall(i,:) = a;
+        end
+        fall(i,:) = f;
+    end
+    sumf = sumf + f;
+    ftrace = [ftrace;f];
+end
+
+% %evaluate trajectory
+% distance = nan*ones(Nt,1);
+% whichtraj = nan*ones(Nt,1);
+% for i = 1:Nt
+%     distance(i) = sqrt( min( (yall(i,1) - desiredtraj(:,1)).^2 + ... 
+%                 (yall(i,2) - desiredtraj(:,2)).^2));
+%     [~,whichtraj(i)] = min( (yall(i,1) - desiredtraj(:,1)).^2 + ... 
+%                 (yall(i,2) - desiredtraj(:,2)).^2);
+% end
+
+if isnan(timetogoal) 
+    timetogoal = maxtime-dt;
+end
+d = 1*distance+...
+    1e5*timetogoal/Nt+...
+    1*sum(sqrt(trajpointdistancessq));
+dcomponents = [distance, timetogoal/Nt, sum(sqrt(trajpointdistancessq))];
+
+if ShowPlot
+%grayout colors that aren't used
+colorstouse = setdiff(unique(whichcolor(1:i)-1), 0);
+% colorstouse = setdiff(unique(whichcolor(1:(timetogoal/dt)))-1, 0);
+colorstouse = setdiff(unique(whichcolor)-1, 0);
+
+psicolors = ones(1, size(w,2))'*[0.7, 0.7, 0.7];
+psicolors(colorstouse,:) = cool(length(colorstouse));
+
+% majorpsi = exp(-1./(2*sum(sigma_or_rho.^2)).*(xall-sum(c_or_alpha)).^2);
+figure(3)
+clf(3)
+figure(3)
+hold on 
+title ('Basis functions times weights')
+if OneforDMPtwoforRMP == 1
+    for j = 1:size(psi,2)
+        plot((1:Nt)*dt, psiall(:,j)*w(1,j), '-c', 'Color', psicolors(j,:))
+        plot((1:Nt)*dt, psiall(:,j)*w(2,j), '-c.', 'Color', psicolors(j,:))
+    end
+    plot((1:Nt)*dt, fall(:,1), '-k')
+    plot((1:Nt)*dt, fall(:,2), '-k.')
+else
+    for j = 1:size(a,2)
+        plot((1:Nt)*dt, aall(:,j)*w(1,j), '-c', 'Color', psicolors(j,:))
+        plot((1:Nt)*dt, aall(:,j)*w(2,j), '-c.', 'Color', psicolors(j,:))
+    end
+    plot((1:Nt)*dt, fall(:,1), '-k')
+    plot((1:Nt)*dt, fall(:,2), '-k.')
+end
+
+figure(2)
+ clf(2)
+ figure(2)
+hold on
+plot(yall(:,1), yall(:,2), 'k-')
+plot(desiredtraj(:,1), desiredtraj(:,2), '-g.');
+% plot(w2(1,:), w2(2,:), '-r.')
+plot(w(1,:), w(2,:), '-ro', 'Color', [.6, .6, .6])
+
+plot(yall(whichcolor==1,1), yall(whichcolor==1,2), 'k.')
+for i = 1:size(w, 2)
+    plot(yall(whichcolor==(1+i),1), yall(whichcolor==(1+i),2), 'k.', 'Color', psicolors(i,:))
+    plot(w(1,i), w(2,i), 'k*', 'Color', psicolors(i,:))
+end
+axis equal
+
+figure(5)
+ clf(5)
+ figure(5)
+hold on
+for i = 1:size(w, 2)
+    plot(yall(whichcolor==(1+i),1), yall(whichcolor==(1+i),2), 'k.', 'Color', psicolors(i,:))
+end
+
+plot(desiredtraj(:,1), desiredtraj(:,2), '-k.');
+axis equal
+
+% figure(6)
+% clf(6)
+% figure(6)
+% 
+% hold on 
+% for j = 1:size(psi,2)
+%     plot((1:Nt)*dt, psiall(:,j), '-c', 'Color', psicolors(j,:))
+%     plot((1:Nt)*dt, psiall(:,j), '-c.', 'Color', psicolors(j,:))
+% end
+% saddlecolors  = cool(Nsaddles);
+% for j = 1:Nsaddles
+%     plot((1:Nt)*dt, 1.1+aall(:,j), '-c', 'Color', saddlecolors(j,:))
+%     plot((1:Nt)*dt, 1.1+aall(:,j), '-c.', 'Color', saddlecolors(j,:))
+% end
+
+end
+end
\ No newline at end of file
diff --git a/wbestDMP1.mat b/wbestDMP1.mat
new file mode 100644
index 0000000000000000000000000000000000000000..2b6b6725bcb36b4fe3d76faa3ec343d7969eff2a
GIT binary patch
literal 16023
zcmdVBV{m5O)-@cblXQ}ft&VNmcG9tJTOHfBZQHhO+xFMFpYz`5oae1`>izretFEg3
zV~(}PTzjsuYtLFkl2<{3mk*bbiUwDbSAo*d#N2=aSH@h&-q6a{f(=)OUrAJoiGc!_
z-_}6K-asGM%90IN(#jH7%)t_ujun@dk&T|7je!N1j)sl~_y6$%_&*n*s3h#4dx}4Q
z=d@itZS5)mT_)Uv-!r&fko)vMbsTXYs7Oa7#z+mdb+mP#fDl0dcx*w0zc569V`%;r
z(?bj8tHo<Wgb>vax(5t^FAD_o6;1q~q7i(xAb^3PqCC6Dx&Q#Mhk;PP!WsS!zxvm~
zf9l}jK1_;Bj#ArRLz9qBg46l91LK|~|0d%x1sGTaH~Yd;{f)K$cdR8Y45bt$i5R8S
zI5{a;sUFbC?>ztqK~VfJl;z*3qklyK(W1nG1rr|Z<skx_Na4Z&>;!67dOp`5v0$b(
z&{b)O4sOqv)qu`E*<)5<alAOgQ2Rtgc{wm8s||KP&g_P2HQ8fcu$LwLc+G__G+MxV
zCAyYh1q28{A`SkUmHh8n{}<ONbzLc5Aze^lU}6PqAnYYz0L4-e;x7o<-;k<*CHhn6
z9}q%dU<4UI&>?UDRz;x4f1l*v5IwZ?{JNU7e<pA@T(7o*=dQ;B5bOl|@df#hYZ#?0
zXKEt~8Fk<{GFis{Ah}c601Ti*1a|U;n){nt6JVb?&lq4n?{B0LBOqNWTtmMQ!hGb!
zxUWUj1n5iMQ7qa2g}u(*ukI}EKk4LFs#O^;-O+03g-gblYZRZvzr!j^96=cZK9lZ=
zwT}5YxFMo<z1X^b0)AoQNWG>@VVm`|{))kDfdr!<1<(Avu4}t>T(4!+!)(m5#u`uB
z<<-B<<GkH2dFIXqW96~nYR*K@jeDW;7*J0%w!MAYFMbKi%Bo<utnvA_xY&IceH~_b
ztq)EF7&+71JPqD)xA|giCtLvxfI+5!`JZz;1kxq?pWG4%68@1}p(<3N`G&Ror$zBD
z#)}ts&1dGu$%AR_jrUV2gxe;zJiClrQ>aBnMUVGu;=!@M>j^u-Xa$?w)cPRr>w!dJ
z6@m^7y_FT~Vi)#SlrzH+D?QCY7oy!ZClc4@xr1L?>7<wli@J7g+p5iFDCZ_LRzEWS
zG_aA?<;<OmmFpIq5zp8>6byKRr&><I-Sizmsb!IvNN{a0UH@kQUttU;NP$$Rbsgs<
zAb|UkEZJ9X|J<DaRv>>$>d(^_)g_EF1Pb}x2f$DO^jFb3{{5JZ|Fr`DY?UNKeG#yk
zG#<<@0AQRmBq`MKr{c#YO|G*8L(+iLhg2pUmPYd%6Sn}$+n6~vm?bOQGcQX=liGLY
zS2JNMEKSQ!-cf(rEX#;@8<_g6(VqzCrGg91+-0WK-~gzUgulPYlKwsW|04T?n-#y4
zg^`dk5P}2<9vFaU9k}EF1wdV&20_u&1pvTX0zC5t_zyd*hZdjztMEhqS@?{Rpfq0y
zvA;P5{rig0#~py}#O=MU`MkBwwZ-UGu}R@30O%sa2AD=_N(M%vMY1=1IO`hK)IH{|
zCCWbl{2qg|_!@Ea?-6OV^|iHuXE4A2KnGFIcK@@Lz;v_V!8~!LL?<RGhbG1Uh#A$;
zFtyGWNQei3Ztg)BG^V*K40StMg2YDNahKc_zHFz+Y`1aABYC&~x0i{1eD&sAMl80K
zrNg`NS^KBj?fpPu1UQCJaQ=GX%7Wg(ut6*}FOKS5<o2EV?HW;p$yF$Ks2i8}!@Bpd
zbd65znv90e%4w`dX$W4gHWYi?-c9)jM;!kVH$v&v&G+cHZT(477K^`AiVlhf|4Qkf
zTK8`c?#~N@U(8er;*Si0?mqC0HXu9z2)Tl0eL?<9Vg7-L{`^0MNeSfn6)pkz^%~=h
z<d)*&{Slam!Yjke^TG9F!%f*5(A){$6rl|Oi2%KPE&yr^l;yWGAQeC`V?5T%va)g#
zY8dSRs)yBMXsWrPk>kPRD^YG-i%p!EA8oODm{i&uNgJ(Y9Be>hAxdwzBAI(8%gJZh
z%w+oTiPWldB`k5NX6LKOM;=ZOg}c1hG*Ou5v)AnTvOSk(<?-v(qM4J+!bjexxUfe1
z2eTb_iy4?#vXZU&o}R7)X0-x_?){GLh$UayMM2Volw6soSeS&Bc-dqF#enQ?gmPzH
z8pd{*@}|a1t?qm6*#6cKZDt^G<OH@l=pE!0(llj{N{p%qYihz4<)|B?p+OV5ZpR_g
zQ>iURIAy7I^Pk`RR!O&FU@6QZmpXJ4x;5jrX^h*E+Z?AmN0Y~!`;&*e$HTWP*d^lM
z_}Z1o9Fz}c-43{3s<7{WZH7>guQjcYd1Xd)MhzrhBq2mvHE>v<?9Up=AZ>ISArlSu
zlUpvM5;3%?F;6=)Y?M34CTF)TlfYR1>13#`nTVyAQt<u2kV-4R0ZI-lCU!ic<`w76
zZo-c1&|`Bl@)8KrU$;a$8`ap3YF}?6i<|v&swCGTV!=f<7lv4f7&Aglj0N#F+bFMI
zE%K&Qr^yRqyn>~-S2T%`qEjIRnlocz)&?`rra4h?a^-qSHqgkjSA(Ublk?c*nRBi6
zxTU{ekaI|-f$&9u7YA}v&ut<J5sew9${vOEs^2{~(?|Fv`U7b`(@Ds`jdOH)quunb
zc2AN>oT2D-3+gAEI1Tv(Ew@7uuGK97w_b=g)~L#_O_8hlnS-zgMi71`3N3@}(r!w|
zokJ@w*<FR69|9<3HWg>Epy4D_BdlR$zlLd~*J!a7al1#@A0;hU?|&L<TBM0x3dO=>
zq&mF%4SBeCv3U>k2ZvVHWmkd1ik<8*CH>N=Aw6yN_e`{bzCRXv(FW9?sO-$GtpJ06
zFO&p*RqdvWNxjHef|%^1Zf6Ev_YWse3(y;A$<A_cW+Ny>5tuJ6l46r1X;dg?wP$6p
z=ag7KUxRHoT;IFTf;XR>w6TL#=q+lWY{7wNv`*=M#9S8pP4BL4v!wyV+k58^Hw!|d
zd0V;n{l}BX&<TOB=<ft(nhYB?V98%M@oE(%yq3M?D`&YtPPGMF(>b2nCzYS~h)OWo
zNCWJkog&#b9E(5w7QhAH<}B{`Xi0_$J8WXV{bHI7)eXPO4%e{xnD=E8@e+=ugen6k
zmkp&GJ7mLrgl|Zkp`Y?RKb(M>p|eMDq?4Fn-0AePA+!pRdrHEz&L5wmWry0*E5+a{
zGikTp1D;uK1rl&Fk{N#h`xyNdiV<r_2UHwAoM`rdu!YY0=#f(H-DtiayM@SjCI-b^
zrB&jSzukfDIQ<@_A%hy1`ZF-Mtdg;MHDCLghJEo|mRg;rNW=J&7_64x-4~1>E=YdO
zj)BTO=tj$}Y^4VJ14d=-&O(iHEoBc><X6=;{p~l+6D^eM9-D_HeN|98lqIk*YLfvm
z93?5E{>-ApD)zYu4{*@V>bnvpC;Zy8GsloTaPV{uiZUO9r`)GQbSm)x6P4s#1ar`V
z@N`dk#L_<9b^Q4UYK10Wv=sMMDPN!g#{6cq?u@}?O4of5ixB<>F0ayeE4;((Z8M7r
za+|P8{GwIcD*nTKF#LqZV<O2S+O_J!fl@Jl?2tOThI;rwOOnYwaFQ$0M6<j($p8x`
zG2fj-4^;7R8??w1Ug3}DZ3L~2L2akovk_gu4f9A2B-dKtU5;L#-c6h9jsA9+0Eb2z
z2E)WgH37&LddGlsR=Mxd2Zg`1@b7USLk$coECb)(BdH+NTeJxRIuh9kfreiQf~=7F
zfB9RyP5<V)I;X#5(IoP>-@Tk12;HxGXcxT@u;5lEjzg8<SJ3V>n>iItMjt5epe;Ex
z&I+lg)V(9{cFQLXQ}YF3%9l!yZ6}u5vsylz63i;?Eu4wx-NHFaRT}=1d5?cck_XKk
z1nOX7+rv)2<RZjzNGqMC-@?LhY*F1PYv1sij9i4gCR1P%d#pX71Il4lX=po(J+#<_
zp)-ZqZOvKDtK0IPf`XH6kzU*B!^KJFN9X035!Y?H%fa)hS)VqIggx4K&Twed&*#je
zjtegII!0p?<!rniRBEzqWdSjPvtqBy?~Ek&sGn*<Leo=JqV;B^pCiR*?+hUbD4G2j
zGeaUqig#K$RihUua)|^s%Xvom_Ca0VHSWit(U4{p<0rO!aZMTB7oAhnp{DP1(yZi&
zO>wE>W-h~1ZVF|cmTK&D&jkluu&T7J#*zy5f(V-HKOD6N+I{0^-gc6fGNW0s4v%#+
zBQyeqf!^}Dd+Hpr$KAXcx1LY^1cT0~Skx;SC!GlcisRZzA`^UCI2^6iWQoliTjk|4
z=7FyR(_JWPHLcHrrP--Yd`I)wx#yQke^(en^-95SC7F-fN)m1B^0{m=QbP*Z!5X;$
zBQh`z5}d6_@t+Z=rY*eASid@LoK%!EC>X@Il^GY}_HSxBm_dQsy>kMZbPx!@2w$aZ
zKn$_f7V=-oLF~38QzOH_b{R8u&K1Z524+wgEO4I({90bg51EF+ZTIP0J7L;7Y0bM)
z@4wQ)%R{^P0XXQsoo3~Z6y;;?(YOw(owu)n()0@dP*(+WuwK?@92odQ%kS_sw6(lz
zcx$LWnf7>aOQ?K5W{#a%m-}E8QK(F<e`JJ}vg;+{9Ub0d+#rg>B&u631R3Yt=myO0
zpzdFb##GqJglE%-mzTsUzSCcGov=*@NpDu(u6$}Nf&TjQiZpQmRJVg^B7FcP;N(3R
z4RZ7BXVwPUZ8HwE)#_(``{s!i=FP9o&lRf=$TzB5&!ADk?TaZOtk_f%*b%D^3x&v>
z!Uixs<`kQZADCEGCj79zxGEM3-~*Mqh3a9)LYhPNJ~k0RwILxBUdPjc50kz47WrZf
zDf*JFm4w#GQOTY?-hI#WLKktQ{sW=K2A21T8;|BHtc}q^ygAhiZLr}1>`YRmTr+2H
zO&ZGkc91&6y}Q~Z{>A%9OlKVMRyAfjK6c;Ugix<&nU(#weJ#k-gkP>+O7rHS*EG-6
zH?AtO8!$r^0F{~(G?vbG_jc|Q5d-HAgLf0vrp!l%bwBliH|*-neQF!4%s1P&TO#1i
zSzX>Iof-ss<9ie>nG+KEs_E8<`%932L6-M57-8_zAbV<;T9FGxu(!e}YybxviN<Lf
zp&TV=9y6ni;CWa}r(eBlqK-o*gRv9N+1r){Un=|<z#NSUg$SB_T&gPe_Osw?j{h`)
z<YJ*q>Q>9+@whmu3I&H21jlv?w>926#~gh!g@!KZW_^tbCDkTi-B`@P)#3Gw`4kEC
zTN~RcMu=3vT~EUF*l&Zs38NfOL3Y!Tn)+NtkOm!7C~3tJ(~r{FPeuN<2m>sPZBF&6
z4s;sSHxp;+H}2vKleWhebYp$Jl3Vt>XovTwyeDACI{QT_qkNF$33r;WdC9fa)3QeO
zmk34wLxfahVvP_3fD!p-a!4+K3}$m=u<B7sx`CAu#k0T9)vzEA!ZRWD;fjRj`~voa
z1U=@d0}&)4V}T5rBHHFb7U_w3>qJB<lTircmD4B9O0L$aLOJ)?e9mw=U^hBYZ-*j5
z49O?;*EOtSHQ2DD^>~G4&^GTH>H-3$rEu)g`6VGFVOz2-OTy4A@eZ@^C7y10Ww!YN
zSO6>>M^KyBkmkC%pGH>to#*8Jd1mO;+Q*5xA+Hb+66Wp|RLp;?d0Sphh@bq<?QLl8
zQB>zzyGz+!n4JQ`jyjXI!tcpDiln*%!MGpXd5w?^b%(wNA)?3|?gTlrCH{9gPVVhA
z3=qRM^=Kh;EYE=%vdLqt%w^d*92VYo#W1;w01X60u$i639Z1&v>`wQ0ATGP~tVq)N
zsXYDBMCfANS|W?@IF8-dh#HkDn&7C*9gPiE>M)!brT(x=^}KV_3#(dFgw~UhznY4_
z={PxpIu$fS6lnNJSDBq^@9C1pzh}W$WYBH8l#_sip{%ZG`16@#QZC(!$}k3~I>hfo
zFYLCX546@#dUbbg>sfYeQ=(oWq;t!V@NPq^+QL^t^H*G=uly+Fd8^vgHO&EDKTD+N
z<ZPkU9wClu<O}y2o;iZRH|01(@!U3m^9~#!i?F2-*)!Hort;~tu=kSx#TJY=ono|4
z0pAaE%H+frtqku`o}J~*qpH79wk!q|N~H4I*KMk=V5OIi?2v1y*VT?^NzAn~=i1Fj
zxH7v!N940Qy^($CFPel{?><P6e5z_=Woc^pUF&zM;ezoPxs_f-`J>g&;o?*P{?o*+
zIVo37f)*Q7S3rGu2d3Mj_?0yE#n5cZsFG~^=W3$gQZoY-Qo0=)a>>E?+oR^7yV<sq
z9eM`h!o;T8v_&?TnIwu{2tMVs4R!Du%VN<ixhRF&4nl7!fea4b7`K_k);ovjUTWmr
z5o>XE=6eH1tbW0q(o}FRo*I2B2;`V2qDPf&33TmwTts^;{8}QGaiNt7uO66ED)l?k
zGev00>5kgb$@|{5=xJ%vPFltcvwB#wPTIN)7jbsN>xw?-Io;V%otmAjCxQ^;CI!;_
z^tWK&-blh6!z`u-Y_bY?{<xpj`N|}|uOHZlT3S)P<>Yi4bhW@jEwd*aVl7?VmhCP_
zlVn#WBc59-l>w>UiXCmrKmkYeWeQk;K*8|@{h5B}zswWSnIMv%HK9&pMXm2ORbPN(
z{cvxQKts5I5VZm*zd_-44fQP?iFu=&e<(z@2A1FoGf!Fe9r^6M%=0Fg64sqFicLDi
z@V9WC$3zx>_o;JuU@Yy|_Sp3mm;i-CR3$8T@i>Kv6AWqimS*`|bZ-Eq-5*6qy35vU
zrNWnoliO8lW03!(uC>(56MA8l6xm;TV*qjza``C;VErkp@w=%Y)!}$gA(~v-I$9^m
z&wQ-KPp-PoMa5vvmxk!05-G$&&V8+m<ALR1c@?9d^Gg>o{a(7lTaWB@@D0Day5_m^
z=O+kEkt}bXSm4VG7mE|nT%|UtD951judUq1<;A{)>A!tuYnrnD`OH@OY-~`V#sUYB
zhy(kpIV<%C_CM|4|GRwwyc5?pck|B<1{j!BRuagM%!_rztD}^Eb_-hN$7!z5eCZzm
z4%Lw~|0@v$ez89-Ut)zeRsHnW-V$Ut0G1(;r!NTg-$bncwU6@E+Y}R(HPnUqf_Zug
zh|b?+uD}2oWr3rA!Tt+>WPqKxn4Gb`w0~ys7ynA~jU@VJrP*!0f!&C5?u;e>rZWm{
zL(V_Vm$M;+zZCxUFNLR!bz_)wK^^EUNOfFgl^4%m4Psa&4Om#NOMO%_x4=ga&Xp5K
zA5~Z?u5C$W5)~9>prXVd30o_Ki+AxFvQQo<7wwa2F)z<0ur+gNI82*#$#`r#d_Uwo
z)Pd)_D}<wauZ{hDuPyLMPAArp$ayp%{t@gW01lDNn#?E<p<+j0d%-V+W?SDKC#j6*
z8h8$QWX~PkO_cGt!AOQ@#8KRB&rf(Kqq1Sbe3DJ&XY+D5iC$zBG+YoKzs1eba3|(Q
zHE;`!u4!#Ql$vdEdgBq8kP}0DzYp=<Y5JgYgFW@dz*-b!c_qwVB;<7aiHQO7%045p
zNbveDp1-uygfFE2Gr}<$J=dT34e17a>9V#mp_hzbHAer)w1CeFlpi&!oToaarA0R$
z<X9DB6r5KM7pG>$j?>DQY9+TWOBqyQjhidx&34Lf57xT?sOi8mAk=)+n}+2hG|bBP
zF{}x@Req6n$%<jBrXJJeAX}qd+U)vuIU-l1KOS}RuH5=jbWloGN<AM9+Olylya9TS
zS6171$5IG-&jqxhqhvI|b0uPsCO_KFVmRBP9s0;34J&(HHqyo5H3RLEluu*jwUr)u
z6EJ;s|GBKubSli%Gvxgj^j&{ZAh#olw=&ug%Qh$~hxC%<z7n1qT8ep51#fuOgrf&R
zxIpgV)Q6Z)9<%u`=6LA3SV<YFO!%Wl(AM5aWB|^BnMW}(@SYbZDchDGA~%({FH-sV
z>#d;0H+*N{-M8eAKbFaDVe66-vhKwAPBNnH_bxjPZHmMJ)!mY1EES=nV|oQcn;op*
z_RbIvV->DA@EASmA7KK#3iSp+n0D7@aI-ffnC@gMIUYw^K%Wh$rWJ`EX+D&>38NDE
z;6{Duk5(Yr)86=|JAjv9ZIx_NGkE4M^##bxz)zvnG&>_^eFCf3M3b0d-~!~8h$$8e
z!yC}c7)wpO6M;_yz#lohUoqN+sHIN99ILPKH?N6t86!HQf6M{wR2S?EEh;W$+I%QX
zy%e%&bVNR+v;Pi6aYVnj%H%n8*azHiRYv^Gzfmn{fP041n~#oA6iH~fez_=#zN$Mo
zFhCkwxl-D&#>-*Q{%w%t`^2@E6W>2Zmgf%gwn2(sX8~t@p2)o8gIbW`t;pyaxax$1
z8QjmY&bqyuN!Gn;c0k<PKHha0AHdmQ8Q>gICcmcuD4^e&8WOF2_s)3v4n!u?IAU;B
zT7;gbfMW!$C5Lx?FA|m-1oknZEngi5wjN$d-KQeGVdK`<JVAzQd85OrNf#CQ(vN&k
z7sr-vu<$?<ZDIbbcA`KSbGKGU>f0EF7$OJmoGA0gVv!6a$LlnoZx~YYqi9jxBbAxK
z+6-8-4?~Q1kv6pXS1!Oq@)GPDt6W}EKyo!K@$31XW1V>gdM!{Zlg7Gwgn(m@w_7qS
z_)nDY(4=JATW3Eab#ee<dLxgM<)G_Qal>iRfhikinw}@gs;ziO`5wh3i|j<i>$nzv
z-}T(KyQ^@4feU<cUI&A%tmukssd7e<873YlqvKk6vg}NbfN0boUU0LCM>R2D+?AFH
zGQqSMIuyw+YQ$h?&^>2{3D3hEM2j@e!Bz!h_Qe|pP|I0*FKI=2$!B~MNYHnaYDVdI
z6c$pA6#RJ|veIxoDMQfx8fj+E=j~`;x3)pzElg2QMofY90sP+d+vQ7GLlM7T4Eu>D
z49Ht80iwWUaKp+0bg^T-hpo}ZKVqWvu|w!0<MBvq1>(l^0>;S5W{jxNLJ9N^j`<<;
zF%Qq)y*`&--&ju9%+^meFI6XBPumx6+hDOkLH%K%K>HuY^9r7XKN=g}Z`}=N>rxBW
z?j|JF+*noAT2$1In>3#@*-|8DYu|3+`+?{*hbaWo2z+I&%&CrVFg0Z}k;ko)z&v8A
zO8M78%BnmcMYc&C$C0%xDo_Iu9^3XuA^_E;mNh4Y&in;0syqo^iJNTuf?xWbc#I08
za{@2G>+FY%Q`wmDCOr(67b{?FFvYpemNt6MwXI)wUly+r+wU|4T7gQO^iOx_N3UN2
z)W{@FA?O77SA^yvud}-O{ZFm2Ge8(cJw2ntcPLvTFg(zl?GJo!+Jw=Yf)w^sc85g}
z(kCS!B!7?flC>8MM*hgFp7+0&!Q_gY!zsx11PGkDEXc2!9@^JnQB212mxxh9uu>-=
zIP_2^+UtpKgvV)7!$stVB6RGfK|PJkRWdfa;KQwCz?<2EaqO?tKs0~=!Eq4n_;3YC
zODP8V&SdG$=SwK!bBk<l(jE)!@Lnx1qFAOe0ZV17M72ouv?XEl?z>lziW@n;n$X_i
zO|?%kI&pBiG)s<2#eN8_J1OmZ8rYWY`I(XO$w&<Xjp?$twiOI60dp&ulkp?cy=*Pz
z08mwusawVp_N;RWe<tcOVn%$y*V{S}rPlb~I1^)oO;q3JsdXD*zuT2shxzQBcGGg{
zGKA>akEmN&iDKZGa_O|?=WlUgro>^S;JM;Sj#qWuc7D!o`c+VvoJ2H-bq#HEUKWk)
zZ6K&PuDUII?;@{(T2AhCJa>;|@~U36XUl3&Ji!SOO4Gfx&cm?I)k<gZ78d@UnrRel
zU?YleXzvnAwnB23$~RZr<1dG?8S`#w*=hD(+7|Vw=D;5k3gHBpiyfOc8^yq5$18!R
z2QN|NYNM1|&(QRqm{uzf64`3wo=`C3-raOXb_=oJQMLieKXLS{%4E$|PE!M`_XZA6
zBMq}3cC~jB?not&)T>izF~AYT7iAzXne&8AY=UiJvdM3iv{VjJ>_1F6wp^MpHUfoD
zBeBl=2G&(m)GTnUSxhp=m-dPL5|Te>R^<Y3l$d;ePB6+8!Ky2<TtUW+KGssZa0Gf*
z%c={gkYZzHtTva(gVcy_MYz;*_+eEj=rBzLoZor;LK*mu315CZ!XVub-aC(A-x7S(
zJcTfl`uwe+k#F1|r%(6#n&tRp0kx`{=`!IW63APJIoZgIAZZneA(o^^n?PsF*c3i2
zF&P?mmgT}{bblHTc$??Rl%7HT9)hQ5Hkos0u5k(rE=tCE5cXWy*a>s-u6r@?K~6n@
znP;(;sW@f_8oYoexwcHRXk(Yf`XC(Kb~B=V2HH+|BW#<Cyi$^D>`~Piq=_%)RiaHW
z0ZP?j+-DXnt(q>UQHSg-%mTO^9}G{biEeh@*?0oZO;WK(GmJ$$z-xNLU>ki5G&5m=
zM$=bzw=8tIxU`Aev0i|C1>4HJv@$|+x%*nMWzNEay^p-){N?EZvcu1vcDxHI&PoXz
z-MH;LAINU^5maI{8p<@?%{sEhj5k;7uDK4~9)~HQ+R!P?Dh~0gb}naFBKIhmOWwyN
zuOX92Mfghln1Qd{L(diE?n0JVN^Z82J~Y4MY|ByV0?7{6E0<hVy0c^3S!Hk;p-l$v
zVWoSEAItOjixBQT{52Q<E@mY+x7<#Bnnc=NR_UkN^F6QCcXxEA2)sc&QTpR@^la2R
z6E~CachRAxZq{hDIY9dBTp-W}t?oQtyGa3S#2B|C><I1*p!ZRWu)a7}5Xu(KR5D_G
zR;`li`q;3|e3Wvp^o+pyN=Vw_I<zP|?9wE33BCg6j=owi#Ag4{z&hZ7YTE#_2ICj<
z9S5{jwOu#slM{=3p$97KZ*U=NoAo=KhaV6(sYLE?qOtcEYLs9TtI8Q`a=u1vP8kwj
z6@Il!&o`@=`%#j=<}Qp#yf0NJNayR_<3uUjjB~wwh)F9Xi2~`Pj*;7a1)@U^a(s-)
zLL+rd-E7Qjh+$$cBhpkmk$eXd8pYYdWw)ZWrI!YN06IJ$h;LH+bsbSNbBf+m?V!4p
zBxGy|1tSPX2Y*s7&67SArr+`}J)?965L)%T7afFpYg?h-v%4w4aZZcIU9{S+VFBkT
zDXLytJm3|$3bG2JOp(xX;22x?DA4vbn6m7o$Aia)yfC6tK~1)7)C{h{aN6zrlZ(t(
z1&KnnhFL+D&Pz`&?cBQMGR@{P(G()m-X~Onotr25Y@?CSwTob2-qiOg=Rbk3vO&2k
zz9=U3SbBTt+B9ba9$c~L5Fb?Of=P|fdlOO9OH4Ckv(%4&Z+?`%42fmzr%LC)mBl6s
zg49y-(C~tg=jKEJa-rsxa$=*RnL$O*r_$(H4cm58%2}W}`gvC=R769sDPYd0x0{mg
zV{T^IxAStW+#FumQ|_|Nc#|)M@;$ZI6jA@zh5y)vw9FnnU3+SGq4YN<PL!CLPhag7
zq~#aF38mn^4Y=~NrlPuj6XNq}`If%-4RKGc8M>gD=Ro$}8ls?+)trg2Q)^#B*HNoU
zZLBf{x-|$4>p-W1{cJmMJz4aql@#~9Zv^0}4_uV+5_B9Ko+wVZfi@49m-m%22_zir
zsq`UC*p~gI*Cq^A;8ImJ<Gylv9Bq1jVd1RQl`Y0}bHSjO%DRU%J$p2TJKW(DW9jes
zySpO$u_+jM?!JbaLpGIryl*ka6g6#&p_iOMf~F5RvQ*c0ZJK3Rmd)yq<UT2Q@_YzG
zy{dsIMiM2ZwVSx^h1Y3=s-W(x2c1fcaoO}93A}5i-|8mrtC@70m6tL45p$v;5-aJ-
z<*16AdOgOJ9Ij~NQU(+wvBH484D+>$Wf?T7rpT7#g!o?0pr7e|@kI2?-gur*VLm)l
zB#Mloem@cS%-Fbi+0$~a+$g*eZM-Q<OitYP6qb*nnN+S-Gu`WE@vI}gTk^|s=ciR4
z8EZbc&H$q9OR<iPt?)0-$y}!$Bb}Yvw3HK?LNT`*t=FS3L`6lhco43`->FJmMvAF+
z9iAqqY9WtbIR@`SI7O3w*J?<`P|)BrVDAsVE+5D;L5E4cqxBJ|A>=1~1WjT|LeP}P
zz!h)(h<_6HSo7SlOMV!2v34ySw=X~0PV#=REi1wf5Y8BnU7O_n^%=;dl}c5k*T-!?
zdZOFQV53=dMR8VIVI@FVg2RAsqiB$cdSI|{eU4m(y(cZd<AyhuNBkC2VKyFQEhVu&
z6C~Z@uKH%6*w!~l>M5ENVA#BaTWkOJ9C|_OPLOQXxshNkgcYA#b#y(I)kM@;$?&Uo
z{2;(*?Q+eufj=R{3G>n%RT1)s!om_+9SZxmgnR#1Q0VdazV(SZ#jr=R_qt}~TW$X2
zA$LcrB0Jgy61KGGfaiGtS(ydHjnN1s4md0&D9ubC%6*3kTOp;T_>dHgjd0#@XQ^(D
z1HQ5mGr2gCm@3!kd2Z_t;Ge|MK&+v!coj(yP|dV;XSy`#W|=}Y;bEWcueaKwG6vA9
z#JgznMfQ_$bjDiaQ7R*baNGQ5Jmid^*)JyauIsdjC5&4a=Q`iqsmQ(`Zk$cpvfYT)
ztS@zpAWP+MdN-T$uJmSZ@^%r=hgIjnJhbX<j(d6n!(IGH$!<lOLc)f>S10j5&WDJJ
zH31~4Nmf@IsI{>WJySk0T{&PEG+tGg@{5+Pa#S$IH0=CbL8rtFpIFLIxiN#gy0OS7
zo&xDT^;PwI3S6z)?PUaH_L?JVI=Sy#`@IIC^)obq*zatYX#PcgySMOUx@{G$hCW*`
z!>NLoaAb77`UO&?g3yT_-opC#qP}cY52S1;JtX_;m~4fT;6(<L|0-XCh_Ej$L4%oo
zTn*~>Tt*T@H8SvA^Z8+>Id^1bOlezinX8CLgSIU_FA6E8mNarYYiRchLR>8nTdEyv
z%><7))cdCIyX&Rtj_c^j-oOm$)zFFrqhC5mf2Yh?wte77k9=HMD@$#Wmq)>|k(P10
zo_mP5NoI;IWN=;Qh^n}C$y(a2<W}%bn0N_C`AN45o(GqEA*kaOJL4_X^?Pyln|uNB
zh|xWpOMa?sThkc1w#cBOF3Gr8)@51eoLC&IJjyC<GWfnwH<1yB0r`hBrd{+Japej#
z%6Sw?aa8R?T`P|!PqpF4!Nbh0Sx5u%<s1ap7_obWyNpM(nBp;5d;y6Eh3N4vb2aM&
zwXVSQ0n>+xxAsn~oX5y?0@cs^y%GhdHBAL@tsF!#&InF}2lTRjsUM%BjD`b9;X7Hh
z=ZeulsFoC>X(+@x(qPGDMJo`yEqS*{jo?!;CbMuKF5l&SaG=B6jHbqI=8usH(PBB~
ztw5Pr+l`PM6&dZYgBMyB8D@>TqGxBF@9sg;9*;irrQZTRo`R*RvB0iVh2kT)kLgz^
zN;<XFP6XC>564c@DZj1d%f_k6M^Z1NU=L^&0UE6}qzqp{?s;6@mN}6N?wsG=O>jPc
zi};Yo=uH4MfQgNPOi*wRn^!<=cLfpdZ-{dKK$T#)XN#K<`aR3*_L$Iw`%SU_?B^U9
zE^f_&o9`-0ICmTZ3G-uEAG7p3(4t25MgA4*aRNe5;|GLTvqhxvpTlJ)&K5dJQVlF|
zrV!<nv1$F|hNzCQ!aX&#m={Cs$%rk#^~Qcuz|_6`E@SAdxn*^~wX#%^Wrw=Npk`4E
zsIL-0k>6f-M^{-s1vY;p=1nGiu{~^h>D)WpoOdhm+RL~LEv_MDU41-rxg}&FjFFmN
z(?%KYv(z#TR8%o+M0>q(5p4n2+*Vh8A%-SbEQ0L}+jyyFauv$5Oez%FxbzcwyJa7N
zMi7piS_Un1Fp9%zZ=39>rZ*L_J?JWB_ko$8amn+o81)aRNpH5a$Kn1pA~`TO5@ISb
zve@=v-!yG3)E#Tc_~4?E`_mtil)v(jNyjxJ_2b<}Gap^Jys=iXS*#dAV-_5GDVVkN
z3`2KjF&J5JU4oTuyiavfY2F*$F-L?cXAqAERGeRrc!!}<r!9f}@LB;8yQ2za$Ib&1
z&TOWTzyW@b{-qTQik<h=)miJ{p115w(@PPPE<*7E#J3%&mSsf3sdO<UtlRr=4%0&)
zb>MomNP2SHb-p(2vS9WpS9j($(E*c8>2-6A0k-!tb@0-H(Ie`r9WSZ-7J?Eok_eb?
zC9A&L%6g~;4uwUkIzPD0e->q&>Kr(JDV4M7$f+6;Ruh&rKXR{Io6UB)mI@f(vyH69
zk6_QZd2e!Ykgxkbo#Wk~M44@K=&}?lAl?kWbyOGNTW7j}CDv{Crg$tqEVm2Kxr<9O
zc>e*{3*DWYShr$b`;$jT>M*h&_^wAT0hD^1(b3ABsn1;maX@!A%G66Or&!k+hx!JE
z*y8zu;4*Ql@i|-hr^CdDyz3c1<;93i%n78GsM>sET6*TD9WA(*>Jqz9Rti$AyurI0
zm#0^O^u70K=p|a;!|UybT$ww|MEqE$hPjd)CmzvSZTW21)fH{^T?e-Bb71Wb4Dm+W
zDh?UBiC)jib)qDnD`i4+9;<`G0o*$NNj#R2?e6L+T8`W9J`O|LJf+jHQWXMX7>B_d
zPql59x;pb{khP{6RJhJ1G9j-52XtIVWgkLc(G!Cc__g_hq<HzcQgFE}HiP|l#y;v5
zNGPg#g{B2<FWu<`07h4^<Bv$CEj~rZ{;U)16D}l>2dRDh1G7MzRuqVg&f!MYyjB_4
zhNUof*Kf1on#h!e#9hL8o7HRVp*$W9M1e;74BU+Nh%0Z2Go#d%Pvw(*C0i~k*LMVO
zFjO}jvuJ|lt;X3x@3_i)h=fiTAKwSipaSYzV1DFP;B2{VeivUne<t9DcZ}|vdqnN<
zo9#Ic6VJ~-@*uj4relD$0X5yP#XlCgu!B|16649f5eN<t|86X>)N?r;=$lNcPsGms
zV?gLtya@YAcW~Tk_vn^KDW~XSc6XvmaciWarlI#pFg%C`qx6$L?Y*^x_Q>3xF*^&l
zseXYKv0q**W$D8HVN&HK)n?r1G*s;o1J<ZW%-CZGRULxVNgQp5%d}tIp+z`&((7)p
zj^puH_WP;)Erbbrj#pw}TnlV<$n2!8N`*O8jP^o`CVZK6^v?l#=?%@gSX$@l7Dl|I
zz_Q~pOhKuj0QbGdF*LN=QAi--j3C0r6tWQKRT&4wAOw;omvBX(KnV6x{cx3hj<^ns
zSMpVX<}!wQdi#3(8~EuRO#jViy?p`vXl#_05QiLlgTr8I^O~TNMvp8eQ((I5ih@^F
z5O=7bhk7};(dV~>HwN?&4k_*!iLWi%2VDwcxUkvV!&^i7z_D9D<qn<vC%V%Q10EyM
z=ze)I87Wl7(0W|?w3$49ht!K1X~bGhdn^z(4lEa@8Frl>GS7k~l?9Z=bi~1v^msFE
zyaIPGkm<zT)d*a}Z`FiYZUcM0MRl5Kr#lO6t!?unfeqV{4o==UE=p#*ZR{h)o`Y?F
zdkVSy?A$)%CiJvTM~iG8ZZHcjL9*wCYdw9SpRcOp(m7EG{qdc9eYilBLb~1oZcjIq
zOPu&2=XuKbsR*?{+6f3{?8CG%g*?>QKE$QOjRc!D!`;GXq4vJWi@$!oa#t<iRFv@f
zoQ{45vHQ>$o~g$G-@}t`YKVH+pE-ZFp8lGw4LcFG^?b6BB$1b9`BFr1=m&|rI*X#@
zQUNoTBgJh_jPP<KMypR?oaNaQ+OS5YV4aa|C$}A8;!%zTT_#hFL|ukK?CCQZVDK8J
z-^=K)r4k&HHq;+5pEr5JND<F0=;b`7_;We9R!90uFTxzsBZ(gA^xvVLeSn2Y3uvBI
z?L@>}vAwJ;=d8@5cva(9U8gLu^)SPPCf5tHn_4bb4Y<Lm<h*+=Zt+lSu(z%>F1m>?
zpeR{u&!4Ex8lGRiT9U;;UoA<>P$-H&y~%*dVwtp5VJx#>v(+3ka>Qfr;|dj_@)}2^
zl?f#9wp5jEVlC-VcCBs4`V1`*BltP$v{uu7SSHvyhRl(4`PWvP9q+53vEM-WNTYd{
z8Y~@p0zQpS4V_ek4PwtvpM(RRUIU(AT?8%97#5m&*Erw~;LuaVJ33fG6#{qk1ZKWF
z4uzYWrWI5hN=!?RXZUG&`C4?Wfb{rIu(Mxz{S-<uTCumr%rir}J4Dc1@(NkPBuDGN
z`rX@M;0;7Xkz<n)fJr0{DPH_QvvJQwDwhrIy)4-5Mty=|Xqo)Ugn{0b^<H6<@TwFh
zWstlo@yL5oe!La443$ur4Bg)jj{FFnW^%Azg-cVE=+KJ04T^(=;h1#;#HnsFAHnHE
z31;J|RrPcKduBxf9s#Za*-8@<KWKgUSxuz1-HnbKt!mbfOU|^m2S7UMkw+611Zy7s
zI?1)5)pCCd@zO_JS%<Ryz!P4nZ~GI?i#f0IXY>VElur`#K==FT3}sCvE^EbLipkSS
zd1QhSM}e!Gwh${|rFCh(nmTc%-711eU}rgIY?5T#B`6|`z2a$Qnq{+GY7tX4eo<R}
zbPOeVi7JorJ%=R+$;LsvC@l4ymZaVkS!wF08eu#a!QRgAKbJmZMtp@KX_$mNbY)W`
z8HF#GMIEtS8OT-3*6p?=@{@~&LaG!lO5NVVe;BHk;iJDS)9fck^yL}o6nDrpSFs~N
zwV~`qBoq$CIn6;}f6F3QHO0{_^ku3%Yc?JX#3<~`?MVy_4r2jhr+_Y%FqM(BA+v=%
z6hDH3oi5fR8EcR8SzI1ytBxj+dBNI!%Ylzh!`1&r3BNTjkxEFqn_cu#>sm_>vz}>D
z?v|r_P10jq=!Xy<WTy)Gt@#ODK#zF&uY&!@L6KMHITOsm&P^j?Av5~%0-c2hISN75
zc`20i!)oQ4S+P||`#9B~LO&Hu!^Wu?=`|z@Lv@2m<O=1@q%E&GTEr&msDkTMb;HCB
z{AdpvmL8;+9$7CZU*Fj>CnGae5Cv(CWa5Su<USAL4BfZxW5HqzxUHWis-A|Ls?rT(
z-=)FsJA-Yyt^1^=4!MNiMk?vjBo)kH2cbLXck{eoF0La<>1t%BI@KiMPO*q2bVwzg
zp-b;LxOrG&5}Xe)e)qW)*=VESs)X-ObAaLoF8-0sL|}L<bhD)n=qSDAe#lv&lqX{J
z3+r-|8iE!=i%Z)OmMRX9kY_M;C&3(?lbM~opw)}Y&Y3M!&(mlbh5&HgiLxo>jzQ7V
zSY6n2w?Os`8?t=2IOQHsb(7t>V5cfF*o+91p5hAi?a=kcs%6vEvf3T+Deqne*SUsn
zNb>0y(2aE(b2RaxNQ~doQZYH|Mo4LwsVs|xIv~GjVQTRsO0uOJJDIqgfkG1Zmp$pq
zd;=|AKtCD=zNN3Ig5A=S>d)hE&rCCfjL2)a_kbN0zck>AK*}g(i_6;L^BB5KG7;Vb
z&;5M5%bO>_xo_<dA_6?hnGc^jTL!>F7`F-z&5=!x8zJMC#uFS)vo_0s>nN{7x`R2<
z7HB=Lujztl(n?*_&ddpb)9h})p2nwH)6XI6*90|)xhd1g2Bvw&P?DVwu{37C?HFv&
zVLEE<UdbDN1eAbR)11U<^G8crrMSE!YU?Ux{(KpcqOe0Ktq6%jwY0tkEWWHNJ>GwS
zNqu}$c@>&$uo}Fg*2;{jyJ2wFff(5+o7>4<mXx&t1bf@mYU;e<j;;<2Cp*Z828l!4
zO4?nxYt2PW!Hk8*T_v^G&srgEV+WSp{Ur~QR)CHWTzF~t^Wv@tbG^=NkpK#;8lT&F
z^b7zi4(!guWn2mx#kTa6G6~;W%d0p}i58@?=+d6*qaMf9O$|EjQj336>Qs0`I+MRq
z@ILh=kC7&JvQ86SF~UsVG2P>*b3i&JRo{%GhI2wKt><_ISDRP@v3ryLH<VMH^pxZm
zY%MB<3usErUaNPKNVa6KQ0#q>`N}z+f;Luy!!|$^YO<)w_voMCD|bn?X+#EIc?LGF
zCbnyB<l&tdKW?-9z`55!{g&SvV`U(MdipVL*Z_h7hq;(<0i&s>BO#OkW3oM2T^SRJ
zO8g?zOf_ZlHW=T>yovCcMpYDghUWEVO;uHlO8YtS6R@e&25C#vZur%NY%C;(TW6>P
zY*gy^{d<y_m4Ef7IvK_SWwzR1ZhMpfXX2EYJ{}CwI;gkpUl(!5-O5|FtxyCCt_280
zSb#J5HD9aoBSm4rCwFi@L)VFDACqAQWD_T*FSU3e%H`cXI^gYEuOHG`MEN2kiwJ}|
zfVw(+2|6pD+u@JV-|D8jie@K@14}Qd=g9I5@gE1z18n0x8+C&M^moD;zO4bu<h6%%
zN<u{SgV25`0!KN&^A4uV0wf2nVN_mbPa6mpykj$?vtB+TR(oa1>^W6GzZXJRMMT@}
zZP8&#xKx|u372>uuF+RhIeq^CZYAaU%icrq_aO|Le?5c&M2qlc?*Xq(?z#a$5dxU{
z61V>l`(>3O`eV+Rk>EiGK#>6W%SrHGVv)aM)xTn$0am}p{SOwLuY(}Gx;8eZw11i0
zp#GTL*tCG5zYy+!vp)KF)?fZTB%^{uBA~zgd%E~~Kbp)hAFc&&K?d|5)H^p`+8dTk
zs87sX^&dhRAI`$u;pq@imO9t%>aRvKMIWMt^`Yfp8dq>S8lU<?B9R`{uur~qWG3i+
zI_(dNcZfnf0ASXU<$rM?{F}qSt=yN<PE?l^7$FLD2N(c1AF%E#c<S%qCV>BKmILXr
z#T|#}gZk|(-zHYSHj?{Vm;`lMeD!Q;b$?oaGwif@1}h+Fd{+i<C&EE|(pN2b&C@bl
znVY)%xrAXt@!{I2_TI%ge&MecCJ2Mr>E^XtP~PJ`BDzD=sU`u#(f)8M@dO4P$X-YX
z`c%n+`Hi}*F*tp4aJzg`HFDhW0KAX$bfap)kCnr{-TAm3mpIUrs`WJU^Mx&mi!NiD
zGUKd^U`y7Oh54l`@k0&A@?yvV0ATlCPWLOXrvH{#%?LCaDvS3S==Y}XvSeKAS*{tI
zV&}@O{P&z`Y$ns>#<OIcmNo8G8?Vnznr+T`@WvJLW8ovsHam|`Jr7`;{H)T~QBXsM
zYd6XD${LEX_xZ@z*Tv!W=GNJ_fOp>bRr66zjpa%0L3*<h-`9r)7*DutlhHF2?8+O4
zOs{FpQ=H4XOEs%ywREn7uGMC@&*0t)3`xw3RbYU*MxlZ){&oMIe>m#@@CWhkeF^rc
z#wqYz{anyJpFI!%4ioC!qQq=&z*QwItb0${yZTUBwxC$0`P}*$ai7Urn()5Dt=LE9
za+9X<T3IYTntYe~%oC?-eFVAa^?<V}b#|rnUjNxe_XtjFm4wwsMNr{(|5@i@v@dD$
z>%r=w@gaSw)ePSkIMgG25OOx~RnI8hvv8{W3;>|#yRi2c_u2o#U0+*=WKx{+5TphL
z;JUy$-n2Ph_)(e`WkY`Zc75Yy(QH5VvSJx0C}iqLh43KCS!ym=FLcM-zEDe;dGNEE
zc}Dr|-m>w<+bv}ssu495lf?7!V?a{PA2>d?Ptvn73Dep2d@=b78Xd+-$^i08%ZBBL
zrn40SduC*>T4p_V>i8IJklRALPC0~~^ONiKj2o{M%)GkeBKPOL1C571N3I&jvbShg
z?wz&eoj3J+BiCBoS6Ow6%ENx8w(!r?{%>#TNPHrq0nq^sjWy8n{|`1zLZ|C}=i@#b
z0DumqeBW1g|4|ze_(5gx{zo{_(E;JUV(I=~iGMpJ_m@8F7BOgRZ-LZ+{(hPqiA;2l
q1v=AV00<6(^!q}R{Ee*oca22&qmhc}d3%Y!`;)p(0O)Fg4E{fVxQhG$

literal 0
HcmV?d00001

diff --git a/wbestRMP1.mat b/wbestRMP1.mat
new file mode 100644
index 0000000000000000000000000000000000000000..0a2f17f96ca030219b1ed1237934217430e618d4
GIT binary patch
literal 18572
zcma&Ob9`jo(l#7t;)y1<ZQFJ-v29xuPi#9A+s4GUZQJbRo80I5?&qBIz304Nckll7
zA9dBbR&`aay=$-9G6Krd0)qG~wDkBg0?ITd=2phk`0`fzPA0Yv)|~kALaGw7tjyH-
zLJr3IPR2&~wl<vjGPXAOlFl~xj2!q3ES!w=e?It(^nbqof4+eJ-xsii48ot^)PD}Q
zj9o)Ly=ou>R{Vn>Gx)!u_n8RwUGN`hDMqBnC`|P9^$easP{4rr9l*psF{FQE=>Hkh
z!vGVYD_~EG9MccJ2MR=_00RFhP5HO<zm4#9vx=R8goLai1&X}q6TzneTKS0}`y26Z
z`jG>4p+G@lVtl&CeggsF4uhb7inIJJe)Z3E2=xi@A10+_M(G@`;m9c_AsPL>K=Dsf
ze^93UU<Mjkg|zwv)BX*%`&Y0HK0J*q4Y?$Z>^Kz#M42JT$d5fBXAy9sPn6)_D6@Y?
zfiR$@m>7wJ&1CRn1NHfS_i5it-_w($Jzvw+9d#q_^O=Zx^)D^kSOo>TU89Nm)WrK=
zG#Q99>*;NQ)q<kyZ3TEY3~@e&sL(tpRh4vXEg;tuB|`vF;NtpyYO(oS%hA8_Nq`6?
zIoQib0X3J!hX>k`wiVg#qYH+oU)dY|m07C^i*w<8Wt++}J(auA=PyI}MZlHUREL{k
z`nSO3%~&hHE@;Tje7Y$I?SkvqAv9;?%<@YhpeI^<h|f{U{~q<<{@EBc16cu4!Wa{f
zFgsv#AZAjKfKPd<zvchU)-hV2@&@3bpk&IpAh=7QKq_TmWS@}#=sf=eApr$NmJb9U
zf&}7F0eSq_LH>nQ4+E2sfeyo;0lcnB!1bqxU*LgwXTW}ZLjI!<#HcA+*h@gi90ZQQ
z?1i_6$}YG90V$P$MSP;>|DH`VaGw?b7;u5W57ZG;5Cd9#lfW>Nf^W$QpGMaK>Ql<j
zf?<AY>-K3gC{kO~hJXx0y*2KVvfcER->(DYe^A3K+|qG(T8ukv?XZM<bKXavYiECO
zYpxF4<=Fd%_FLu3y`sr%>R;PGt^+m(5fYa)nDE%==_B=h7F<19wI2jS+I?T!kf>a5
zRT#O(Hebp*j=OUKj<z&#88=T&-jdIIG6!c}XX^{TUz9V~>pu3{bU4NxOdw+owMu->
zyB-e*Cg*w5u4Zw8-eA8AeNK+#@5#~svqSv}aUn$u2}0x;@SQ&qZUO@NpDl3+VnF&2
zOC%8|{hK9t4KD$jJ-4)%(P_Th*D97tpp}!pSrz9-?A2y3@Zr&Imlu5hGr!l$zL9mW
z#4-5s2&&B2?Dra1%?|>3XO~Hz|CfF8<!brnV`BGq&WP!&#D_XItxnxik<!OfC$nFX
zPw-jt-!@B$ammY}efaPJG^`H?w;Vo5t)DR;{d_g{z0t19FyW}{ZGnmdDI4?o6QIEA
z-@QL4_rJ;LU;K$s|I;}n3`jnm{6`-Ub0J9Rr?|`ChWkHd28hqh@EZse?+jHIef+89
zaY={w?7)N~`1B#26_35i>c-qNnC3QajuU>#*5S<8#>Ko2!1ih>MvJ3k(<Lw(#E@eX
z1+a&2$QdO>J}(nlXyGfjsDT7Rry=>vlPUfl{olxb<>MggVrL;?34x;S`3(fbzYN;(
zN%#K-plitZ2SA_{bmkNA9|E$6fk@~xPQv~-PRNl!{z7>F&FTMZ5d(23)4wv@kEd1h
zx;iTm&GRmF7(UKA`$L~=IK0ThvZ_W`4UF_+U*Fw#bt!b1jS#%ek2h@Hd*w)u*CNJX
zY4;U%(=K|?w0g(&MgW^4yzE@bNrP=Bo{!tdTND6Dg^;TCleWWuB_Uz-4-)L#UBQ`m
zSQ7wTDoHkg$4JY^hEB~|75@sE+4xL0Kj-kJ-mNZvW|orm6R5bWaLxNGM80$Dh!+3J
z?0X&OAO_|6A;kle@f344Bv3dGMf&H+Qh$&9^PeLLgn=pH0AeR$?`_Tht$nUN&Y+r8
z7C#ASXA7nz+x2Y0{wd|U+3+-)%mt>>7yjJCdlVO_1qUVbQ{~j(D(Ur%^z=YyuwlPq
zf~n<t{mDEK202I&KKQZ{6O%MUlTv@yF501CI{huMFmE7(it&(A*FX&R-$#j$#+w_-
zjCNb4XD?1SnVr31bb!f?1)`&PuJ0GJJb9<p*5_T9Nia@05v-!ptG>#E1W$q|hd6D{
z*t{m(7fUa@p*Rgk$!E#BFxK8y2)_);Xwzzx05u)LO{RM;O@j|Rm1p*nw9#AjOT$7N
zSXG-%n{53Cby1hzug|#G>+o1Mt8~rp9QCL7G@d~E@G*0r^ZMWQ;a|M>GwDcL$U>1R
zXXIWQsnx%Ndjg3#gNJ-V{!4WJfk^zd1bs#)4T#UDxHRzR*$z)MpRAz3*N|j10eJ!b
zciyiXo@#c$R<1}E$n8L=#F!Ox!7y9k>_6OqX@Npn5^+|R)znhZBNztIy=@=E)2&QQ
zT@D^!Nq;4@+9yZ`GL%?F$Y#8ew=>uzAOt5Dq4jpFP<myvpL|5jOlA(B$ga9oA&`~n
zbiInd=i~KIdnx%&lSb$~`_7&(JMrpN9luU3TDrO~yyt&NiD`GdvpMp$T0;1ysybNh
z85%fa*C=Bd-0v8S*a((i6sA1LDwg|5Mo8OAl}|QO4=C(Lsdd$7U~NaJZEC;N835|W
z_P2%@vO~zCCvdgE@1U<xr)hfB<21xM(v!AmMm<qXjGL(pIuB8w${esFY0B(c{=Db6
zO1o7;%HS7ywcwgD?O3)g;yjN$=Xkuho4wz>p1i%hAHG~6ERp>n(yRK$P4i&c?TqiM
zfdKd`zQe&jZQ2m`%7)^G9zwQAPJ+5>?7TqJpEHm}(d0TpDG?f|xLi&vZem|+m2qa$
zq<D@?#pO^gjkQeZYNDl+jANKq2zy{ct6R_rqlgn1KOR;4ig)HX;mCXFy*U|u2?8Bt
zP%4*;Zsth4Z#a>|$8|YX`pY?L!Cm7QJeepNc9f(fJIZaYX?}xd^i7w3voF+mC3|nL
zL<$LYmvR^!Pu9Y$J$Al*OR~u1%Jq^$h^bAlHhXCo&$0P4&sy7YYk$88&yac}$&0W6
z9`vT6=R^t$1{-{}6B@-;zt^v9f3cU?chvc8SJ9w$p3&uv4vV|GJsDCd=Hk~a7(z}d
zda4NqKIfnKwzoihhGBX*qw3!`#joaP4k8{{z=T+-b&a>nx@lN;4sCfAc9na+3Zqfl
zSDqn&N0LvCa70jkAEuXEW589x?;hcLl(AX8Cp6Kq&XBwmjYq;tcYX~V^7i`8={GDC
z8eUbOTMdpNd9uTr@?F1{;<PQuC)pnE{#f)y57=m;s_R!>B?J<nNCx~>qnj}<{UU1#
zYO;^6gAIH=D3U58*l?gVH^<qHleh>?c)qMymQ#_uNx6i>iG$gRM|%By4WYwieeXU8
z$!c=a-Vs5$x42`n6%UEUF0K0!ds*@alb4?TmNtk$?_Ch$EEu`YZPgy^*C+3x6XF1g
zA4zQVS@xQsGT(0!H7iR6Y<eqJ&VGTo))j6|=lSTJRDIl|sKV!>4sd~YiRapLFA@eW
zK#IK0S>FjVkPne`+Q)zS&N>-x5P6jwscrv0AHXW^D;7@!Qw~X`5Y9Mu$cg=k)R;WO
zH05)CH~~My=!ERTC_TZl)8%VVVjHaZl!9$nFh0e=1+!&XhQ(WM-eI=~I<wpcBJ64^
zKmGvmKKeZzE8c_=q$GAY+42E-3zOr~JFUX6$!b4-3x(xO5{9i>x75F2yA#)C8W5u`
zkDicD81k#Uilt_?K<}BJYw=uxPK&-++w77IqE5&w073}yr_!1uGp*Op8(q)xm0GxW
zc=feAYfYN9v^{X~@73E(w_kKlbkVMR>>rklG{70rmLMYN%m*a#RAo*3vx}3fx#pt0
zA;G(9?n+f%iR#YIT*C4pAv3k9%l(O;emxyx(n<xJtEc`#wgMlB%=A%0DeE&>Cz^kt
zQ*I8xNb_2i4FDNnDQLmy&KgXm@z@8m4ijqR^(_O~5*+4kTUt+0*+)zg6|Xu}3mq0f
z5G6Golgbn`tko0^lt~8RhSf7RHXw!AkWcPGl3z(ATjtNn1Y5I82J9Sqqf15FV?>__
zh`m2=BkOJq>bc&YjTiuLSVeQAdenjLa`*c8ZrWdO^mo7qJ2%lYn<O`B3PZOtxdfkc
zD8j}b6n)nvy2pPEH#VuX33&rV(?V&r>JbNbCUcU248IWnwEZUZJ;?fP`UmgTIny1x
z4r!3n?&ah__<r?6hs1@jHJ=(;0=m4AvR;?v%&9~w=0HU!L+PPePFMqt!5y)mX8}co
zW&jv#fox`c2buhy?ef`_NKRRA(M+Pi7T!_1>hNcp_x_)h<V`;Zg+7?v{;*RZvj}w@
z*2ZWPxUeuBUtB-R(Koy%uNbAI!y1yp74JmijCNRE7T&?`1SdIR;zn(GTYFaf>bbn9
ztnBJgY}kJKaB-6T-gP-<%6psXe(-#1*{4S@?Sui#6A6d@@tl3sdBKZW&thh-mP@dM
zPDi<|CM+p@R^odJ%R=si{-OC(bb5+bqQR2lW2EE^z#N8*mfepvGbC=Ra;KYDJ$iwr
zm`rTHoNrp-^z*l0t=BPlEVO0i_=$sHLUUI4Mc34HxCLNNj)MxNIU!xj(tUW!Q@Om$
zMw5&2x$uA&L4%>qOh(yB1X*YOtBdYHM?m7t+fK?-b}R?Z;juw>ly-<1$XfwlPrY;Q
zxThb>*7Iqg$j>uccC9LwNjH+<l7tTO=p_GEZWmik1v0CqHYLTZdC=>SOn2%!9lNto
zIWF3hfYE|=zWJrHAC)FBy|PGKDORHnGNjuEg6>-^bkM?%2&V3!D9o&b#AhqALT6;@
z84Is7cCW4*CzTb<%EpQ9<z_|r{hK<@mN4Ls03KlTPGVtrv8zmNs3Fd}BB3iqsNIfl
zbl;F(f19y(%@ryEhh$M3FYuiQe_viH2%Cn-@9^(iJ7L{AY0JOS>c7$_$j7+&3Owkw
zonh;R8sl%}-Lwv_m%p!#*8Ga}P+tvyuwLG0783HpAmscsw6(lza%-YBneljVOQLo^
zW`&zw|Legtsz{B_=*Sc&ZP!=aFE+BrtWg4wRl=Y`6gt7J$rF^zSu3axgSDuOmB7A_
zAU}meYNx;UI%%5`n#r=FL+#W|8uOL#iXwRc+@O<nB69#N_yiD&0lj%fn6p87+kyvY
zyZX`4v3X*Pee-?uW5xCz`i-{E=jW)%_QezkPJB8!!ia6BwQ_V`Q6q#QTbg~=S8SYW
zb0LHPe06JO$bl+@BCUvHQJo<tfBPtqy0EYb-{a|!hsj<d>jFvUG$Wa|DiXWYm{gx0
zzrN>r(TfC%pn>obW1D-FjYlhWj;2^qfxMc9c7(`aE>>9z-kGzvW^J{7M`(Sr-d#QN
zppt!5)-!G-+gi&Vf5$IxqUcu)Y-&N<0oGI*VlP)OW%=`PYdU9I8&{RNjo9JJz^W}t
z+Dm7<dpmc@C?Ru)p}WbNQ&uCx1|LSC8;<o>{&h{&R+}B$tx-r;9PWTg*GA#q#2ytJ
zwxndi8pbuU{?c#Xp)2|tO|b;%zxn8u*-{BdakarKZ-9oHN+jr*q8+7XAG4v25O~|k
zWnR7NppQeRLU57HIXRSvUMhbbz#feYhx$4BxKv%@7icZmlK5c`&CAZ1(yf`#?|pGp
z9S#X60*UJy>0q{Xjy?Kh0S8yu&G8x+PN7H4v9XwiuP@*e_aPn<xHh&^f*h@kzn+5a
zz2A;>6G1bc_RUjYcIsmlSq^+mxwH*W(kMoIKmFVHMR-s#Tr1iSEs)cneKQF*f#dE0
z@ELoYKX2@=SALcMDBckuRPq7tT<5wdV^R7kbHbMqU{!i;`?RcG^O=NV|0fBl%g39d
z1cRao&g79_fEdr_$>TJjlXruvp-APz&egJ`3?i|j_Th_%=Y0naga$w6uLlz$r(}l?
zn<Cxj|0doO_tu4iS}v~~A)shPk&{}ZUyXL|z4@Hwe!yjVpw$6GjuKWt5oBOe&0)OZ
z$l(2oz^rHWd+0X^C<C=ikN$UQQE7+LZ3S}XUTFaQzOPiK$(7~iJ8&VWSORffeq)Bm
z=6(ic87%(^;CW`~)Xv|Ptuem{7#jZW6<jiCt7ThBQ&foR&hu?(?omSjTJN`-mlzi{
zlp|d>M`hrXUkrIoC9+vRq}v(^CHf9iEpk+`AL7Zk?AF8|6?ngHr{O`Ex9LWU*y8yQ
zEWepQ#>-!ppCjTBY*!9bsSDFXL4{g6THk@?%+Kz0!-DWSX68gw%unSTjV8mD;Mb8_
z!{WJgU!!POsp~+ZFLyRI+G@e`V3h?Ss5S`9O)sqKPLbG6Mt^TE`J(UY0`6Ma0#&H(
zFIR1Os<&rAkqF2^s?1{Cbgv+Xgg{$e(GC){!lqfem5^r%)^JYThg;a~z#M36nDp)b
zy=`dIxlMz9g`CMJPcE<xr{RE91t(N_iMjH%i2tp6)4(DRbp0%uiHE0^L2raCrb#f;
zcX;LqipYZd49#cT7|}0efHKN~T71t;FO}B6&)UgX={sjA!E~DGJ~dK5{3)v|XRI2M
zcSUZFAHRmtLiw^JNI0qbYhSm8k+Q8}<~QeGCWgN|2yDoBcji301xZ$BR~SkCSEn~}
zFM}jfP#U}jnZBKB*xTAz*ud)kNH<w98>6x{jH-CF-8o#G3MP7**tMeItxeM9Wc?l7
z5ZQ_C`6zWIM|Uwan>MPd(DAXF9JtiN41=2Ki1DrTVEpY-XVA-XTh$RW3uR$q({kE6
zm)BAT%`l9JX4;-Ebd7znc$P|nT5|`vw~SaGk6?_?QhE!(EwPs#J$J-WQj-m6#ELg6
zoKu|&{Y9Y3lnw?x=7Zv0?NAC=cb*W{5s$Q%jAK@0Yc602p_)zyKz*hTFFoDSJURjF
zT}zynrR-#6&9G@jwCHE7yYrIeCcUm0@tiZB4b^KpD)=CavTRbL0;a!&2J}Xg<eB8K
zHsVrNA_*lB))c6b2fV)H9_s4G^j1(YYBSb>inh+4a7(uS=CkQ=KboYxG9U5TQm+b5
z?^fw-R|5$?Vk%e00R{<8B<{}+JpXQ$jL8a>`m6(U8ZTjYucPq-8Xt&%iwYjb3xc8>
zO!EZ>|MyVe!jYsOrq#Q0bX!O%z8Kq-P2Z9K&da<&k_AcqIg8|^bDU5s?|EEw5v+f`
z^8-s+zn=GQfbax3B8mn{g}e7De1b?=<ChGZ9};^5XdOXl`f|S=d{-(1_<8s|WH$zd
zPU_pre0|^+Rw=#($!!ckPeLz0{RCQn%4veN5TQLB?<vAiEMLdyA}1`sS^VIw?^;v|
z)p==*Jt~z&DdO4Jy*M6N4pmY&C7fTni0k(?5Zij>sz+-4@%#5YZ^8Tou?4El%@aFP
zMbTnO5{8HDCN0ev9MQF{=eUyOXJYzqm)SZN9AwJvH4j_EmZuTeKr)&T<)0AQKgU1r
z68wLQ7@#`|?Q=K(TgQNakj+T}`>Jcxdi<Nj;OL5B*M4vTPwuQS<UKhR{dox&XgD3&
z@xKy57Lxo^=l?@Q!zlB$xAdDQ5c?3=(<g-PZz6X8Tuu2?jnk5pP%tro|AhHG?sgRQ
z<-uVCS@=SJ{lD=42DFn9mp9gz@xL?pi$5O^RTfo~$l|u)z;09p-=B-AiF(`D{Hx!%
zKu2dN)Sp@z|0eT)s%+8xqjvwp?S!QPZOy~qWiAi6>=y6g6Z!hr+CUTU$vo$dKG0Q|
z?y}0EB$d1RlX;aQcwxCd{ZZA*ng}!Wm!cHrsPa-tU28h4gop$)Ee+8~#99$zqPy>q
zwc0?1M4x=CRmCr22TSM1!;DGytjG34z#-3}K9b;F5h5d?F8%{hSLmIZNv1EI_h?M^
zHPl}i5-OD=l|>0k-I1y8LP!+Dp`klLMvcHD<Q)3Qi7&L9H0yDLg_6LOyQJGmh~!RQ
zeZ!pXB$qbO{^f2Gv)J_KaA9QP79V%xounu2z%3l6j-As`dam{9jdw^=UL3>yJ`}9$
z^g-1ISNe;wodnqON`#Ym*y;8YD>L+!Q&w`Z$n{;KP+6C`U|7RPluIh+uOPBF)Emg9
z%etzhUP>X2IHMzrLP1+_A@rCE{+hJbR)a*aV-2iPNC8EByxJ8<9@_xgm0$HaYT(Li
ze7tdQ4pV`92!4ei%?CEY;Z~!5^z8595w-!35zV-5N{b9jw#-wt4cO)fx!N6ame;S#
zQNOhN6VWH{D(oI524xjwwF)rcY?=lm8{y^z6!ZdiY((Mq+`$_=OGkr!R-y*!3t}Cu
zhjXnv;Et>_aB|lbqTP*OvoJ0x1hrRQ+nBy>0%xx7KbJRIOhtJ3gaN+8-SroT@VSut
zsbLJUZ-b+A%Pq<5s}g8pq*)bL3Pe^<xOfvs3jaEsdYAOiXS4dwmIzlLFC#CTjdaum
z-qsua4Tz_3=221-vggHB)}i&Q_)XRAi);bWdK-Ajjo=w%_bt`q*JUaPg!+`EoI6Ru
zldM>$y~{2W`(i0zEzeYW8x^?NxL%R)7H7M+y))#)c;zc@0u~>pNBCghBEtbN*4?!k
z{M^kb);sws?#Gc<@MmM%X%*5(`gb)xl9*&c#8H2yqZMebj5opQPS7O;2UYv@EdIGm
zBVkHQ$Ws_iovx@^|B#wBi4-<?#9$>=GU~;m$VSX^mNIj{WYE)K$VYC!SF8?EI@wbQ
zmzry$&1*7zmZ+}SuX8{<HHG`4iz-Xm_V3D5FGcLyozV}OTt7n4Trlr#v-uC5_ks7@
z)KESOZZrxT5uXta=VPN(#FHAYUoJ{xuj&sDj8TVHu2eVd2=bWqei)|&Jn`=3CH9X|
z=6iv?ZBSs=TO-<?C$sJNqZelRsjzs2th(Z1hxT)?b8PQsQ+BUf9+0(ljQ>7N4CZOH
z33iJrSK3ns7B=ci4~x~i1F&2IKq%##MvSk@iZSz*@l4@#6$!5I#Us*xLcCAtDb++k
ztVdSS^{LBk*n9T1Oi<$6-01V@Fvdi`^nbf&OyJBkUU(pnwYGZJJW(czyIZTL2xy8y
z2~&i0OO}6Qw@w976mXp{FbOODTD+*`ozBK=X9+6Phb1Yn$PnK0{TI+f>Jq{mhhlz8
zaB2+#+3We9OTASkW*tZytM<B9l(0*WpJyrpBq16s90jG`*4fu+{XAg!-st00MY#HO
z{742&P@2Y>=I2Su8e4%;!AB{XVn=bQdftT}cRjZqUh2FMkis84*P#$AD+W@!8ay$S
zCdtRCnE19n>^qYqVA>6b7kr#jG0kijcV(p_tnjTS&czCg+Hts9jL+E-V)O6^vEog0
z2-Tt3eTgQ)bc!~9OS&<>N?BiolZ-rNThRJl#6&fsMF_9MRvM2d<%zprqb;oj{al>t
z*EY!g#Hbr6$*57^K>^J_+&@!bILhaevY%|u{B5f>SOSz1aab*wF@CJ~ur1c?Yg~*G
zZWv>9A^}C6aKe~j@E9fKj43TfII-cuu@H0t_Td@8_haexjs0}Za{W~2Qe*P<v}576
z9RUX%JO~~Jy#HZ5zwk-yy{Qp!>t!-qpI*3jHzA|x$)T><s;+t5tn-}BnI<z^_jZfa
z55lN3Of8&29H3xpMSFaMt)r0rZQM2m!aJ_IOlTdfyxQkce4E^5{F|<IC3-OOWBdL{
z6tI@;vd)C)S&+y@wGZ(tS+hf5=u5vVziClyUdRPxz0+_>Iwu>!q_^?%VkNviwiKV`
z(nim@p55#2%i<ME$DOut8%U|E(diD;==Cd*CZ&u86r-@vis(G_bxyZX(5W4678r|!
zk56pm4ozzmmN$l*(?P&ZyBKEkPv!lz-C=R$%t@ICnIB`llpTeG(O>gx=7X-~v3V2b
z@CtwV0ENt478cY_5AAEStEA!tNyn)o+iDRLA9|~i?)AhrA>p-Z;-m1vkht{Hqn}3q
zQZ=)@5X7%yCYafRcj>R!Mlps0!*iDCeD?s#NGkz@Wwr4W3?LErzx`%q-VqP#45(2O
zS1H$?K%liyrCp?b+LE>h1nd>2<42FLCUtcB(e6`^P8^&r%~E00avj1MOv(kEhP3DU
zd}O74u+V|QVY~0GZG}Qg!{3VJWqpnIDql-G0M?LU?UuJeIO|#>nu)oLnvq%v@Usg+
zt24Vd%f{N^lrXY?YTE|d@Ajb6XFEG**tA)?3?qFGB<)sHr5-q@SvqYc{2?XAnmmje
zI#)8u{i=oEA;i<ov<eQNmyF@OuB~S!z^<LU4FZ$ETfgPxSL{1b$HSLN;N_i4Ro#p6
zY*XVyATl9NW3iXfbr{jLTIB}W$}Y51JB@}5YD)bD10b#HAgXw&c5}5o{&E<fHSd{`
zo8jcEXWf8q1^O<n97&A5*tvPLQ35)4yb@w@@Df9%IZC7Z49DbyZM*UyovS(S0|P(q
z*Ueb$xDXGEwhc`6foD`*u3)8pnjTWKH*k0wZIb)2tGAPMM<I=>Rg=zu1&J)RC=Y$f
zmM>;*AL;;~OLeQNtA2>)^lr|*<=%|75h8XPjdR{Nu&$A&X^m&cZk|29v`-qCl=?BV
zsu*&k%IZ%z!6ILbpry)w1syl~SV!m19pY1?pe3wMfs2#1+ES_nRx7a;<zB}fh*PPo
z&pHu&e&_uiZ2%S<sp5EqS*{<lcOKcPHT0-u3V9^``AcDw;CK*TpTYGt`|-&FdUXx!
zWzt17h@U=Ns;Mt=$|@>zJb8~EvHq5s1yV$EDjdQr`-T7L{xkvTHvf|a6SLMm6o1cb
zD$mYb(-aP3jJ(?*!nv54EB4}D_hQI{qE;{)|6&_!N!$(`WFdWOUAa#2#xA?vK_sNZ
zW>m)voTJ!A#5ONgl??CLqlOt+vtZn-bh}6rjE3{L|13mC4P#!DKIK`2HE0D91c7Wb
z<Ltbf*#x4ej7pDA1iN0a@AQW8Hs%<3cG3d9PJqI0dH8ZkSu>wYgD~F;uB}yBRg}zf
z_q9mtoV7JqA64o3%hLmNXP_6u_;1t%TUA_4vvycPu-)z>nB-Utv}wkh^>5ZQe!OkF
zR{D&4+!nx^L#Obocx0=3zjz{&`9>k!^WQH8OjyM$BUd`ci~|%Odah`87jk^le&wnf
z!3nv{wjQM~kniBU@+wwmy1BHURfU$5*k|D%R(Z7svOkZ%h~nQPUGoa<Vps9;Deg36
zNN3#Tlzmt}-wW8ndSS9g5eyPYFdbK5=Aze|dzwcAB!-r{Ibt#9fSIm;fq*yacIOK?
zP72$h#CaCuM)75V07k7N`Vu(6Xj*mBDanX9bW3X*;v+T-&?<a0vqI*ppc#hiF=8BX
z%Th3<1q<0a`|5mAT7tqu>Oq5R9D*$y&0eT>oH5chcRlS+POR@mA875qAcn1NHtg^m
zzC+!llX|^L#NS_N(m+hCs%5Py2AFcXW=Z>22G*%Q->hEl$H;u2yD%m9yVRJVm~Ze(
zkf3Qd`{nCTMo}q48p0HF{H-HEI5zAc&)<|XJX+tv)849<3_kucDnp|SHDDmANs2R4
zVJlWoZfW2vu=DeQ)Fxfv?;|=k9*KL}9d!57q^u3mP-KzV&<~oWd5Wi^%v+(QXSA+h
z65Ad?@j<wso-O)4m!~o!&$L9sMVrGK4rrdLipHh&13{sO2!|-z6gdMoo|#>bGDBaZ
z1^Z5BB4m8n3ky0e%w+3E?cf?bkK=w2mH2%1PYIZ|2wUi~dAZ4@om<aeth2vZ>5EVp
z?vtt^&aG1Xw=t;ZI>fQCZyNg43Z5WWIbpn1UQ|+gZ2Y_p>|1hy53bnt$quRwAY{kq
z{YdGUq^H?%*&D`TTOQ>u!{S-`X)}dx6>v#^LhGt}Yx_be@$n#oxYG&9x^mLe&!A%#
z&}w(CMr?bk<}J`45#Cja7Sl872wMpn?xtn>TUpxl?YtbTwL}*6RJbp*+!V;7!KT+)
zpcs9>2pYSPlix#P>`3n}lKaBSgBCaQ5umq%y8J>ip&Ht^fmm_YTwLF8PIf-6)Y=Ew
zkn+);Vf-2Q9KzLGOZxL<HE$x~)GmO;W7Kw152sw2aSaN~F2uEPKi3h`PysV$CCw}U
z3o&H+11}AdG$S{+51K1}i2cLm<$aZW5;^yJI#U=cu1!D1wfUd><FeJY;{l5K-0g;e
z5s@5pRjp=>bD`juY6ge&J$v*;JA9E7W0?S=-Cgnh_%tj6uK*LBA^WO5fwwp_>e}|j
z@Jk*L5sL>r1=?%JcAat@n-;A{D*rSBB|+q&UX2hmQ|Z#Ox=nnqqU(%74REj3gDzE;
zgj^=?B!RWEFZC1mHLM0LYRg#tD0#6^$yJOMinJxoz20N0&Q}ZxX#*<JI1!+}CIz}B
z3d}mRQ<TdIqJl4HaL-Hu1mZ^JZ~V`v@b5lp(#2*lKc2{XX6)U4ofvplZj|3hH{R5w
zCns)uiYmr1%&XRFSnmyT_}5VZHbRPg1sOF*W;zcZGr(y3vK(V$D?*EN^4A&1sAs44
ztra8|Fl=q6>kXI-F)=ah-X!ZtcN)@{(UKa!4^LCmb-#^YxrFXQxyDk!>NcihDQgQF
zbM;4FR}AEsW5TE2G5AZ-lL(PKf~T;jAnV9s;Y+o>Cq9XJulZ~^rap|i+j$g?J5`))
zr}#ZMlo#U$i)D?+uT2Vk{|I5#O{c9j?BjD9Ju&EIw$~}XqCP9Dv=t^P#bYM2S24~;
zKQLanKL1vYyC<i#<4G`<Pxcm8X*vGWPF8w-=BHe%m&Tj1N_*cRg^xsDuu01fex1|X
zbNB^?7jdd>*G7_^C{E(9>Z9waoMzIlD(3HX;|Ib1YnN*ljY3IbuGp7W=qk`(l^2#M
z>(RKrB;5zKfy0d__N`CUt3*6f0_t1TZuNvxhrC>9iyaw~$T>5fgP-St6yz67Hb$dR
zxe;+xVRW+nY4)8b97I)@62sE4HX;Qg-DJD94+P6cEEN;P<ElMk=lSe9K?%v=Ksdr*
z2`W>dU|JaJ&kX1>Ewe>yBO^XKUT^gz<c;Ap$aXQ5ik&7A8O?OZW7J1X5VwUa`KegI
zb6?DvJk}XdN?Eop&h@`|(Ne-5Zk$azaNbDPt}k_te3LEM^lPyYSn18)6!=XxA5oJJ
z|IlW*Iqu^Fig@ugEw>GI3KbXWUW+{FxBx0H-W-^`HdRY=pw8Y};!N$tV&#BK#B5bd
zHZWGM+C|v{+obDb1(OCla$>0<?Zy)E>c+Z&YznORG(aQpDP*;Jx0eN&&3BHp`Q*ND
z?Z+CFE@5~QS>V|&>HLe<c5l(ibo(ksEmN*YmTRQ|$;jw>%?q@6C5bB+lC|BBMI(in
z9%zMdCTOnJF@;K1k&7(WpjE*naj^gf;zmoOgj)3NxvUiCns1<UE$4^XR(#P_ab@kH
z<sRbRjd~7D0%#O8x^mw#Il{YFkQ3@aIMW?DYA5)mU;vu|0FO(H9goqIy@45utDzNX
zmcUG~{x12kT&Ixt9;JkcHukz=U+=<WQ(dzTL$5GD^XxPS=+OGE5e+H3(zT3RnXS;B
z2&q!;ij!`20&iZgB5;>0E|y!EYd}fvn^Ga!i0M72dqKKFd-E8Tp7@}O0r|LZ&SiPm
zoMZxr651+5D&)RsH>oL>G1a>pwqxuXS=9;~+Ib9lNle{CeH*_He~roe!Nbh0WmqHG
z<s2067@1e4m%Mk2q{=ZwVj;OVwZ!o*TMfqpoq_Q50qeWDpWaTqqW8#j5-s8VUa7L{
znvOE0ZXSvxPZW>w17>-@?AH$o7Lx(g$ekR9bCp;SbQ@}k3^X!*If&Hq;uWaf*8E%4
zCdjEc^I61qcUUEVJh;eq)2VU$`QvXS81dZmw&1KB9j2%*DlCq;p$n~x%(JGyV`pdG
z?(V@d9*;f><lchcpF-v6a3HSJMH8d=j+s`dOS^P6PlVTZ564b2X}+u#C?seqMbj;#
z;ST5)1DmcjrVU>~?|EO{mb+4k?3~};P4GN_iF#MU>P-SShL4YfPEvM@m{&&W@BkC*
zZ;Ww!N0(;4=S-Lo{V^-x`Iyv<|3#(YjBpMDAHR0NGhh`hk}m<7ob559k4+8$vZ!5i
zQE<g^oP^xd^bRH2VjV43$?0kCX04wh+sK|^0aZa0pV2>Vg6<M8)>F%XeKFLLiqiVS
zaEy=|zW(J$IdfO-Er-{wt&O?@7tAFV9lK_5L$xrP()PL+ruy<JsMQ;pKq|?L!(sDF
z*WTIYyl0{BUe;ZBNi7A(>f@RFEeShGob2?P9@=o9jjlzAin>V?#_NT<L@T7uwwA^V
z861^LF+x|w#!C&WhiHyXN|E@+WuW-mE!PMfvRL%gGI+VOX#!qH`($SglZCj$!S6CI
zfB5+s_xym$(V*bk%oZCbJihNEG6QoXVHVOOi|y}D&C_P0-SH+Y5ANE(2!o(0gsKi%
z^*y4}-vRbI1(;$LO?4_Qk|oI6vygC0p&VsrSOznTq2EN-r8zjq`!pt1=lvjE^2Ay5
z1_}7VrGyO0c9^U5+momcua!}7JFC%l9KE3tEoX{|osssKUfOVAxCBl;+;k7_1<KEK
zd{wX+qf{Qi0y;qI*hi#Y%ND~Ty8RC4u)U4Y2d+np<tDd1=IbIZ3um8x8O*#UJ7ZI-
zzHW{&BlKRT4_;ccc*k6I5Ttb9LeW4+lY%m?<TTXS+6}cLqOr@?6oj@1&7zIdo`WVX
zrSmi&xz?Z{=pb+uMDKO$aXKv5(Sj2Bv{SYQ67QL{>`g8X3U=RTasz_M)i^hYF3Vto
z6D^6_Mh$?zbY%+L;M{g^O2rc)@VWD!ySt}C_8;)RFy47ecB|BPJb7oO4}S}U-1YuN
z45QU<dbBcU;eQuJ7TleSHuX}+Bl-J`TWf<_a`AjYWSK18?3}aW!+GLe$>U6j=3>M?
z?gZLaLUX<;BQtx`kpa?IV~I;NCk-`T$r#|t>*HG}ckg!^eu>fd@Ot~MSnkC>kvNvE
zZKbNnLqNJ#S26qh>WZP}t`j%lIizj}o@}Fi6_1k2+_2~5I$1{0gC?mZpTk-C0CAn@
zBoRl{VR!WuBhPbpACEa>p2l@pwHg^Eg4_6wzs4a)ON(vvr=5-^Or-whHxdD5Zn%Wb
zsy^ht;wNTT$ZM+w8L5hM)zAtBTxKU&mOi=_Xc*dg<>m!FUxVo+AQlgZ<M(LQEkPBR
z{+tus6JAuX2ibk11IrNmHZ-WLuHhz){5E-y#-#`^k1w;4I^SrD$bO3vY}TxCh4Xti
zl7^TXG4ruFp{%?m&y3PlJylE!mTtMLU*8eG!PDMw&tiyFw3+3K0`S%LP)J-a-eCtY
zV1nyg;lJir;%#|u!b&ZkKNIsIxy1I(J)(C8&h{KfNEH+ud6VA7GBP9BgIny^5gm(P
zI3lRzNb=|22!{qs!I}v#^;`~z1f)_Jk#g~U9T0t$D#m>>7#w%qJ-X#r%`3i`-JPgb
z*&3;=ZR|Y~iTp{ARrbM@0ca~_II?nL$<4uUZdl+z=~vQCTe@(1m{fmBw;%UE4cC0c
zLNG0sH1pm;*Mg#OmBQHJwdj{}ZWRli^t~Ib=YIU23piD}g)+y?^Gyy(Xho<Ao1Jt}
zue5@R(_2W>K`NJvB^*$a+t8_xXK<TtWg$2UDL)>=7LolK?6uc4hJjHx3JpS*^^;^V
zjWWz_Ro)rpCo*}nd!!0T2o%?-QKWhScS5K2E7huSOF44`lT!oH4bt=ucF^Xt;l40Y
zEG}AWm~)<!@nI;PRqfBxChr_p3sA=E%EDI-FfSOwL&Ln=*z;SG8)GIY=QJ;@<kwcc
zgWt-M_z1b%!&^fIpz&LTiifU26Wy7I!H>}xjNg4(O_i(T7`(6i+sz+gp$+3kns8P#
z9t*|HLMp`Qhdri;ta1=26o3`5UGNBGyx%OEt{}Y%<-72AwL{j3+H|0n+aX?W(OqXc
z7|+7n>e_wD5h8ZvLQ^-6i&HsooBGIb=MXyHp299ay0*{wNPHYJF``?B8!baiQJn-3
z+fE;t=Bw*@^-q+;zrymZ4;M;M%QZM7?iqyhN|8O}Jx`fE6{GjZx`LpMy<0S;QH7g1
zg}Ik{lH+n@d0G1})ZG{R3N@@(?P?ZSNRT|AGcwJfbRPyFvGy1fdHXO<4bcq;u@%fV
zFkMr&<0d1tolh2#CkxOoUy6$ieI@tOVpow_DrCcPp}x(F6I+hPYV!|CusM6e7}l;5
zsW)}#;&UWPKFYIZ%x0~XuFo=#KYgYI4qf93d>Q?|REkI5j{X(?<0fAWHR_oivx47(
zXf6-m_Q*)}MT}c+B-uNY2^QwrA5@H@kp5Z2QC!jk*Voo&&ekeMKqGP0W6B2C5IaJ2
za=kFOx%Fbzm=A(h(XZF~mH@pLck4>~qMPgjhK8f={E5!8@%iPmB-sG*S&}?obeB#r
z#f7|5Km-R9b<$H72BQS&Euj8$IVKDttcOTPUJM)Z13l3&#{o7+q|$-ec&d~iOAi9U
zsAb`s+QlobXpB&TP>iJ;qLrFO$(hy42b+32iKctJF0Xr5`z7DxtxTtTfjIFD6lEwe
zr4!LH+XtZh!`qq&+kU$oF)G21R_C$hXrTS*!s}#70nHalLb_S3@ckX}n8phmfl2G-
zi~KGI-(GpDU%Uu_BWM^(KAu@;P89YRf8|S-m@gS=q_YP}bb_9IH^;?$i*nBJw5gN^
zdWt6p18p9xFjWnhjytKE2m4@?y|(lGChT&WB?C4#)Mn)4V2f^{p>(oA<8<AZ2-@2K
zi36GH8rgN{?Dgp1!MIrIYYeb=x12;uMKv8|W5iRDzr6};7gTTRj>LEPBuT%hE$Sf8
zn`LyzS|>_@Co0J|e~Xg{$LjC+X&j)lc{n7aw`z}8HdXn0yY9e8tEE=)IF!k7#bDd2
z+cb%V(R2y}I4l4Dru7|QDC6E~Dkx}KckSyjSytix;uc0xc!sP(e&E7>O8mM%%8C#(
zJ`n-$60%)glNVSn04+Pvs4Z1IEO3(xVdJgG;xj#`$M)#Ft?wc&m@;U&_HrL}?^#9>
zsNmW_e_}#^FYK&-$m183*NhWdTldp}uhTS~$j`KsnhKN!n3v5RX?VmtfLQZ_WAp&H
zy&Zs@*)<1tKiccGKjn2N|6V)k!ho?Nae0<62eGAMn@TD5a2J++TKb_q!Mk8Do2M|<
zO2nSIIjc2o0Isp{97Hby;dVQS?Ai+PqvTR9sVxRb?%SgP3<=C@xiVdmHl2X-#B!%3
zFM>pC+zeYqaExH3QsPV+{|{Dg{+l46)e~8c3IG?dRAi>(`AFSUnSOctv>`~R`dcR7
zm)8&N0)e4JDP(qq-C+q?yaAP<b(w65Yl78|iyS~N5;gjHO{8aLSQH^JAXR}qZ?GMc
z3In#jt?#VJhNpJt@z(U(H?fXSQslC%bzsyf0^$*t@@R?Zt<jCM>Kb~_g^~k&Rb<Ds
z`;{sD<hc1aiKUA<s(_3}|C~-aRO2d@%}Dd3Va8?(ikB&@V=i)mqr|MdY=MmLfi%C#
z#HxqaAhl!#woQHqNsPx$a!2%vsh&90W_f>lkC(f+5|$~-OOWp64)wj>`4NTaN3Mn0
zJ{qfHTZ?=-x$lMPHyGA)k!!U|Hr5CaqcUF2^N)|k4Da&i@$~_r+`_NtDGHde4ZSX+
zu>03=2nnOBfL1pOSOFu|4nklo%dU-fwq6oQLTofdm|qrwrRu^wJ5Ks8j2XIV93ZC%
z@XyG>O5$ryw}>rX5H;$H8Zu~55nVC_Y7TYmSG#g0-DqDTFuYzLDzktD{9C0>09egN
zN<DR~?O>N3?vd{$CQkNqRuhW{M5;W`7rF6PeXwRwU?RZ98hV;DslHk7A9D^5HN3_{
zmM5s`KMJ4b^;{xQ+wY)pIUl%)Ucf$PKv;lKn3EaMc5@9ZOmOE@p(6LmB-528<@c;M
zXM4AEHCJmo6db>VP)XKyC8z)RnO^aVoCV~HTOHwe17$a(+|U*X(8WkV|C%I~^IdWE
zm!ubjiiLoM#hwJ%Ay1)1B3$o>WU<bv0<TU3j(0^<0Sbfh3=GoC=CA2VtzQXfQI{!>
zV`o|rl3_OON^Q+IfUdN<#G38z8>E@0PS|OSUG+XU)U0Q`IQ!=w;N0_`onAHKYo*)u
z-kOQQIdpR&LPaq4yxE-Jw5NZz7e-hIrd@q(?qtVy<{Dm6RGv1rFhtgp3vD)tf=O~e
zHeGyu@LjxM+?;a<2^~Q)>c7B983@IZ#>?<GD}Sr`S*n1}?h@XL_0ZFH{(FH5ao&zP
zja%L@C2|90C1fQ|Ef7;8IMrNxAaSb)Z~(th#_=j#z*7*n*qkoNA#^F0d5s)nZ+A=V
zdl+>e8*Ji(r>0l-lbvr|Pl&XcxA#W^==0KT)8T?jFXP}5o~n179<hchx1A*zQVa4N
z`8-92Y}st{trNtc$2`^%3|K-GTCtmwY3!7_BFl;7kFv6nIC{jb-#@pr&Qxu$3e8>n
zNN<zbUnx7g35qb1Uy?vIhr!WHZ;5QFjea%%*@vTW<2O}<lV=&WT6J)S?5z7`c8qlr
zEV*tr<H(6hK4XWh<1JHQcD_RLT(76LAQR_ShI_BoeVP!e=iSx|n{Zko0?{<OHhs}u
zzH%$ri-;%io>h{$mj(H1F*)~(2H4kEe;Ourx!=l;#f9^>-_8}i9oulnc64!AXp}w|
zuM&{Y#fv5!hWgKqwcyDT`HXZi9Lw*sHQY0bWMhDF<;rj+?IVh+ByuDN+NyTR)=U`3
zw)SHq=GLeYqC71I+vz?Wb6kBBwrIwJYun9E!0Jc*7cfDJSpKC(8|R+j57Sc<S9LMt
z`18{zvEZlI;OAF&5t}pSg%*J|Zo~sb%rvRaPWEu+kR3ze8CaL0NGpqs!Wt9lX_@h?
zKyBXu>&_Lho`4B1t}9<c(KOQ)Cp+wXOVqnVWWy!juqA9NjQ*=1y`9E>Af(iJ_F2K$
zq*BmQB@gr)_q-H}xp02VA}yYDCs-ynsUNIZn7?xXmG()msu8lrsjJeD0v8p>TXD-U
zNkyq}{T-0s9^o>~57w*k>5G${+wix+@ldf`a&AC)w9MzDc>HM~?0s~r3HM>ME0YL_
z@r@~0no)(o8zRqYqwO4T^fehYa=u>jWVAm3Gs=xTnyVw*@f+34to>ZA2%?rMd&F08
zF5eG15s>||Khd(7_o{TpRCq=6Bs~vuzmLgW-dyUwRsx}tI-QbFDH3%QvbyO2wE|IA
zpAn#=pHSAVE`kbimS@Q+L%CgwCcfA!l~Jx!KFg~aHB}oJv(?AQT$-P({+QTvSbC6Z
z_EP|jy@AJu!jC#9L+eyKg8w4a&kaDh^bt1_AO=m(D%NSBkRHt<cDXF!g6qLdrBS}_
zxFubXS|S=&t$b1D`4;)rM5CMt^JSTSKRK!|-&nt-Q@*8|3mK*zZ7(XRXehyT4hr{6
z4wZ%lo<UInYt>nc*<c7(QQxng<dDz^b_gzNxH4%Ac}06l2k1koBN&9~5<~K_js*Y3
z<$?B^SYr7XoZYuPq}U96qc1c_Tl3QCBow>3#qV_<bxiQ<+13@Fc?Q?yJq|^I$dNxC
zHK4z=JV6Q@k}dz`u>Z3^@|A7Q9DA^9)09lql4-n9f1y#4T0~=B7A^CzMy+;Ma@EBt
zL4#0~P}w44oR)=2Te>LRAe3CONXb&p=9;@za-yC#v|hs?LdrOh;h=HpL2l`h<8ty9
zz?nT6ovn@{!eA<&Fs!Wjagbo*wRIm45nsq>_cT%cG}K(3X%Y{RgShVswePm;lbt%`
z6?+@0V$6_HwnP|&>zd!q_j|dxj;3I&Rha71ltDbjA(hsrka2@6yW{5LXOByAJH-0Y
z=U!~DhlZ~nxjW4bjvuo4=X^gI6bT2@a;Xz8#&EeGdR8>;i460?uEM;QxRu2E(jkn!
zn%g_<8A8idBoFUoW+(sW>P1!8%$9}EX{<bRFr>jm`IK7cphQ``0m8Xw2v?RpWr1gc
zT91#G`R-h(Yc(Z8R+M>9Nu}0y_<B?IvPF7%-45iGUoW%ATw^yh)$|Ma#yY(fhSX3r
z)(<(^xI8UW)Qrn?_C*qXu<s1;bwp96xpGZith{a@VafZ;K8)o7AvW&d?~MarGFLPp
zZkZ^I=83jvrkTS=l(fBiAdX628u7)U<yCW~6dZ{7O*|)AN$x@C2%qlq=ZW#|+d4%_
zfsgX$Bd5-mfpCz=Z9~KJ6jBpLDEZ_FM20i$EVB?hE9z12;7{~~+m0J*e?v9vrZ4Jc
z=LI9`baz}&6Vb03<x%$QfE&l%lxydL(m!LVD$Ix3m~q{94tC_R9<_C^<PSdrOCxFO
zOyac%VWh26U*3_n|1M+uco~tUc0?|#3`;<_vAYE>xvVZb-hY5ke|%DZ6`gFf9lWB`
z&5o<TVRqAp8rdkH+xfLDqhJpV@wTbk+;zhjTN4sVc~AfcmVmL9vb%8C_6sEqJ01>y
zmBPs=XN97j3sh$JyAoJNAtrKY(WMFD#a$2fdcEZ$F$_cv5ue-W84yka#GSYMxGWr+
zL)j@!3Xz?zZ%KkG16Wn@r4#LY1D=JaCS1m)uF$6Jsn~{GwosGEefmp23w``#y$+^I
zl%<kOrZ=Hma3&3H-;9g4TT&f^&v+DXyJQlXSF_O<v{SsywA2?|U0US}I2!C;TL5`9
zXDUQE?mpOj)tr7|I|uP$J1`m@Wy~ZXmJo8~E~PGm)Yv!Q*xtk3VXd7ivJ30$ZB8I0
z-#U2U@>^59Jk-ygeykf#pitmpUbb7{Si0$GC{^INTptb(mSob>z~~GM9r^qX7QmPv
zDG}?ax^mCZyy2{chPr84KMzq7F0JMuLs`a+kfx};we)b?3|+9jdc%HDPYRpb_uh0@
zlX#HqHmA#N?^4igyi$wDgCPcIt@i!vV!ni1CENBD>JX8&VBsihNanznYfT~47%ZgJ
zPM&ADdU3sDO6=fVvgFLAR&Nx={JTeIf?d1yLq_YEfN$T#g~Oe}J=}an+*HmTiN=_2
z4Kh6>a+9S%<(9Pa6!?dPjzi~xwh5k1yTO6_yAVy@)_~>nJHonTpkn&L7~WMtW844&
zgP96IsUd4vRhPNb#v+9PTsBOO%SV(N-yHcp*P3TQ5nOdttmEDmBaXCtjd{LUso&um
zQw^;v>^o>11^-{Iy~KZ3*?(+;r2pr=hae2dpH2Ueb*aB^fY3yNraqI~e~|rblO_Gr
zF+3y9j|qe(4fIz7-G6DLH_?%_0RQYTe)`i!&x!%^SK|CHy1#wu*80@#2DJLg<3H%`
zVL%qrq4?Y@xrPqQ`={HPQ4F;IQ~p1+_kQlL6fm&2w_y0I-yY^qzdffeDBLH4|IhIc
zt9;Qj`Ilx?auXvx6Y`NE(?5?eKuyUFLs!PeuZQZt3sb?r8ugi5Q*aCkXaPEMRd@Z=
z<Mp?mqkpw7+Mh=lpqdsQl7js82m=Em!>`R&mk-w>w_pQ?4_aLtFCC3b=5!~P9!3x0
zEDvW9UPz3{XiHt|jty6%*%A-2Vn%R^@J%atolQ@DVbQ1$nz$!lI<u1uKV0_*r8*^`
z9)PgxDGNS1{1?kY{cYJRp#4wT|JWhVBqU)#0s4;)dHe$4`cL8MzbDrW^zY9K0PAtU
zABXCL`Lj`U*3l2bmu<;b^$aDvZFDQA+Mx5y_4xc1q13A}`!2hmz~Vh<P_;rGUaQ@t
zMy}|+rsMQB+tiW^1D^SUt|o=%&io_qn2VbCqO)!D^(hxg)A9hc2(yr3Z1TlsvT#)A
zDCW`DSJhzOrc=@+<XO|VfsKc~-uI%Sz)uPHqfw3`yL~tVkleBQg5XzL=lPKBwMbQ@
zK5>Tc5J5Fx1$ca4dunMFL-tt#1?oFc{wpS$|J%fBN8m8f+5OJIVVk?lQ}OL)d1vfP
z+^V(;0D056tQM(FXQ_CtYkaHrz8{<P+dT7-O)FH#Vn;gdj@}=J-k|mcIc2Y-;3mx1
zo-*rIwbWyP`RLcz#o_gqw%NB}fI#A^)u@j4@}%A%ljTUj>%#)P4`QzQ=ouPr)eUpD
z@3hV--evuzrtPw3Chx)T)fUf>(B4Wc8SINc{yx_vTKM_L)c<$>k?8;659ZhV66#%p
zSLn0)v0!jMdmaf3AMVzw%4TKETP-GLa8J{_`cPHApi-^#-1ZT5pUqL01X$ry>7#YO
z$<ThSDv=vay-R=QPtdSC`hUiCz5bj0eZMy!!vAjlf57NFyJ7we**$tH>#D#0-%^|Y
z)HgHiTmHB0-{#)fovqx=++Evyoa=J?@8tBkb;~ate8a$Sh{qj#=*Ja2Zcj-_)Hv_0
z`<!KyIK#&)%bzpx{>$5Nf87mDvGVB$9TSh%+<brO_ILS^pE>p~?Yo_?{Ne6jAGKL#
zN5$Es`F?+stFJ$|KYy*vNG9V>e3D`Gi_8C9FPBHu+^H?!|NW!0>b7fV*!@g>l4s2S
zdUu}h^t~4p-{1cSJE!6E_J3LVRRtOU??ihuFSq%ZJ3q*-a<b)b{p#&s<(Wd$zrV5n
zxh~vq^PHQGPb9)u#s7Z)H;%DwX7S&hzzCVkA94;9FV_)%!#rH$)YR!BscO#*3<{WT
z4ux#9z5UP3=OTZ%vNL#PgmZzshqIs&c4iGwgp?gpQjCHiwTM;bC`;bK)lSk1$O$p5
w4L{DG>YrXSndwcU2}5TGR|`ldj=~?P6Ii5%N-cRZz2WGL1Fsm2HZ!FH0DP)mRR910

literal 0
HcmV?d00001


From 39065874982b5a8ff949d574aeb65014fd44b2ef Mon Sep 17 00:00:00 2001
From: Natasha Rouse <nar50@case.edu>
Date: Fri, 10 Jul 2020 11:40:30 -0400
Subject: [PATCH 3/7] Still not plotting correctly

---
 DMPandRMP1.m | 404 +++++++++++++++++++++++++++------------------------
 1 file changed, 211 insertions(+), 193 deletions(-)

diff --git a/DMPandRMP1.m b/DMPandRMP1.m
index af2b47f..69ac490 100644
--- a/DMPandRMP1.m
+++ b/DMPandRMP1.m
@@ -6,7 +6,7 @@
 clc
 tic
 
-rng(2) %seeding different random numbers to check validity of cost evaluation
+rng(1) %seeding different random numbers to check validity of cost evaluation
 
 %TRAJECTORY CHOICES
 % %ramp up bubble out -----
@@ -21,15 +21,17 @@
 desiredtraj = [(0:.05:.5)*0, .5*cos(pi/2:.1:pi*2.6);
                0:.05:.5, .5*sin(pi/2:.1:pi*2.6)]';
 g = [0,0];
-y0 = .001 * [1,1];
-y0dot= 0 * [1,1];
-x0 = 1;
-bfs = 10;
 
 %system init
 S = struct('desiredtraj',desiredtraj,'dt',0.01,'Nt',1000,'g',g);
 %trajectory start, velocity at trajectory start, canonical state vector,
 %size of timestep, number of timesteps, goal
+y0 = [];
+y0(1,:) = .001 * [1,1];
+y0dot = [];
+y0dot(1,:) = 0 * [1,1];
+x0 = 1;
+bfs = 10;
 
 %dmp init
 D = struct('tau',1,'alphay',4,'betay',1,'alphax',.5,... %how fast cannonical system decays
@@ -41,35 +43,54 @@
 
 %rmp init
 R = struct('tau',1,'alphay',4,'betay',1,'alpha',10,'beta',1,'nu',1.2,'bfs',bfs,...
-    'rho',logspace(log10(.3),log10(.002), bfs),...
-    'a',[1,0.1,10^-9*ones(1,bfs-2)],...
+    'rho',zeros(bfs),...
+    'a',[],...
     'ep',[10^-9*ones(1,bfs)],...
-    'w',zeros(1,length(desiredtraj)),'y',y0,'ydot',y0dot,'x',x0);
- 
+    'w',zeros(1,length(desiredtraj)),'y',y0,'ydot',y0dot);
+
 %Init
-D.w = S.desiredtraj(round(linspace(1,length(S.desiredtraj),D.bfs)) ,:)'*(D.alphay+D.betay);
-R.w = S.desiredtraj(round(linspace(1,length(S.desiredtraj),R.bfs)) ,:)'*(R.alphay+R.betay);
+% D.w = S.desiredtraj(round(linspace(1,length(S.desiredtraj),D.bfs)) ,:)'*(D.alphay+D.betay);
+% R.w = S.desiredtraj(round(linspace(1,length(S.desiredtraj),R.bfs)) ,:)'*(R.alphay+R.betay);
 
-Ntrials = 500;
+Ntrials = 100;
 desiredt =  linspace(0,10,length(S.desiredtraj))';% assumes constant velocity
 desiredydot = [0,0;diff(S.desiredtraj)./(diff(desiredt)*[1,1])]; % desired speed
 desiredydotdot = [0,0;diff(desiredydot)./(diff(desiredt)*[1,1])];
-fdesired = D.tau * desiredydotdot - (D.alphay * (D.betay*(S.g-S.desiredtraj) - desiredydot));
+fdesiredd = D.tau * desiredydotdot - (D.alphay * (D.betay*(S.g-S.desiredtraj) - desiredydot));
 
 expected_x = x0*exp(-D.alphax/D.tau*(desiredt-desiredt(1)));
 expectedPsi = exp(ones(length(desiredt),1)*(-1./(2*D.sigma.^2)) .*(expected_x - D.c).^2); %n traj points by n basis 
 % proveXexpworking % run this code to make a plot
-betaw =  (expected_x .* expectedPsi) \ (sum(expectedPsi')'.* fdesired);
-errors = (expected_x .* expectedPsi)*betaw - (sum(expectedPsi')'.* fdesired);
-
-D.w = betaw';
+betawd =  (expected_x .* expectedPsi) \ (sum(expectedPsi')'.* fdesiredd);
+errors = (expected_x .* expectedPsi)*betawd - (sum(expectedPsi')'.* fdesiredd);
+
+D.w = betawd';
+
+R.a(1,:) = [1,0.1,10^-9*ones(1,R.bfs-2)];
+alpha_rho = R.alpha*ones(R.bfs,1);
+beta_rho = R.beta*ones(R.bfs,1);
+nu_rho = R.nu*ones(R.bfs,1);
+R.rho = zeros(bfs);
+
+for i = 1:bfs
+    for j = 1:bfs
+        if i==j
+            R.rho(i,j) = alpha_rho(i)/beta_rho(i);
+        else if 0 == j-1-i
+                R.rho(i,j) = (alpha_rho(i) - alpha_rho(j)/nu_rho(j))./beta_rho(j);
+            else
+                R.rho(i,j) = (alpha_rho(i) + alpha_rho(j))/beta_rho(j);
+            end
+        end
+    end
+end
 
 index = 1;
 expected_SHCas = R.a;
 for t = desiredt(1): S.dt: desiredt(end)    
     dW = sqrt(S.dt)*randn(1,R.bfs);
-    da = R.a .* (R.alpha - R.a*R.rho') *S.dt + R.ep.*dW; %wondering if I forgot tau
-    R.a = max(min(R.a + da, 1), .0005);
+    da = R.a .* (R.alpha - R.a*R.rho) *S.dt + R.ep.*dW; %tau for scaling?
+    R.a = max(min(R.a + da, 1), .0005); %enforcing boundaries on 'a'
     
     if t>=desiredt(index)
         index = index+1;
@@ -77,156 +98,153 @@
         expected_SHCas(index, :) = R.a;
     end
 end
+R.a
+pause
 size(expected_SHCas)
-betaw =  (expected_SHCas) \ (sum(expected_SHCas')'.* fdesired);
+fdesiredr = R.tau * desiredydotdot - (R.alphay * (R.betay*(S.g-S.desiredtraj) - desiredydot));
+betawr =  (expected_SHCas) \ (sum(expected_SHCas')'.* fdesiredr);
 % meanerror = sum(sum(errors.^2))
-R.w = betaw';
+R.w = betawr';
+
+'before optimization'
+[dcost,rcost,D,R] = Eval(D,R,S);
+ShowPlot(D,R,S)
+pause
 
-[dcost,rcost] = Eval(D,R,S);
 %DMP weight evaluation
 delta = 1;
 alldcosts = [];
+allrcosts = [];
 for passes = 1:10
-    bestw = D.w;
-    bestcost = dcost;
-%     for i = 1:Ntrials
-%         if rand(1)>.5
-%             % try random stuff
-%             D.dmp.w = 1-.5*rand(2, D.dmp.bfs);
-%             [dcost,~] = Eval(D,R,S);
-%             if dcost<bestcost
-%                bestw = D.dmp.w;
-%                bestcost = dcost;
-%                delta = 1;
-%             end
-%         else
-%             %try different random stuff based on size delta 
-%            D.w = bestw + 3*delta*randn(2, D.bfs);
-%            [dcost,~] = Eval(D,R,S);
-%            if dcost<bestcost
-%                bestw = D.w;
-%                bestcost = dcost;
-%            end
-%         end
+    'enter optimization'
+    passes
+    bestdw = D.w;
+    bestdcost = dcost;
+    bestrw = R.w;
+    bestrcost = rcost;
+    for i = 1:Ntrials
+        if rand(1)>.5
+            % try random stuff
+            D.w = 1-.5*rand(2, D.bfs);
+            R.w = 1-.5*rand(2, R.bfs);
+            [dcost,rcost,D,R] = Eval(D,R,S);
+            ShowPlot(D,R,S)
+            if dcost<bestdcost
+               bestdw = D.w;
+               bestdcost = dcost;
+               delta = 1;
+            end
+            if rcost<bestrcost
+               bestrw = R.w;
+               bestrcost = rcost;
+               delta = 1;
+            end
+        else
+            %try different random stuff based on size delta 
+           D.w = bestdw + 3*delta*randn(2, D.bfs);
+           R.w = bestrw + 3*delta*randn(2, R.bfs);
+           [dcost,rcost,D,R] = Eval(D,R,S);
+           ShowPlot(D,R,S)
+           if dcost<bestdcost
+               bestdw = D.w;
+               bestdcost = dcost;
+           end
+           if rcost<bestrcost
+               bestrw = R.w;
+               bestrcost = rcost;
+           end
+        end
+    end
+    D.w = bestdw;
+    dcost = bestdcost;
+    R.w = bestrw;
+    rcost = bestrcost;
+    delta = delta*.99;
+%     [~,~,D,R] = Eval(D,R,S);
+    
+    alldcosts = [alldcosts,bestdcost];
+    allrcosts = [allrcosts,bestrcost];
+    
+%     %Gradient Descent on the best weights
+%     pause(1)
+%     chosenew = Inf;
+%     chosedel = Inf;
+%     while chosenew+chosedel>=3
+%     chosenew = 0;
+%     chosedel = 0;
+%      for i = 1:size(D.w,1)
+%          for j = 1:size(D.w,2)
+%              wtrial = bestw;
+%              wnew = bestw;
+%              delt = delta*(1-2*rand(1));
+%              wtrial(i,j) = bestw(i,j) + delt;
+%              [dcosttrial,~,D,~] =  Eval(D,R,S,wtrial);
+%              ShowPlot(D,R,S)
+%              wnew(i,j) = bestw(i,j) + max(min(bestcost*delt/(bestcost - dcosttrial), delta*5), -delta*5);
+%              [dcostnew,~,D,~] =  Eval(D,R,S,wnew);
+%              if dcostnew < bestcost
+%                  bestw = wnew;
+%                  bestcost = dcostnew;
+%                  chosenew = chosenew +1;
+%              end
+%              if dcosttrial < bestcost
+%                  bestw = wtrial;
+%                  bestcost = dcosttrial;
+%                  chosedel = chosedel +1;
+%              end
+%          end
+%      end
 %     end
+
+%     %at the end of one pass, save progress
+%     alldcosts = [alldcosts, bestcost];
 %     D.w = bestw;
 %     dcost = bestcost;
-%     delta = delta*.99;
-%     Eval(D,R,S);
-%     alldcosts = [alldcosts,bestcost];
-    
-    %Gradient Descent on the best weights
-    pause(1)
-    chosenew = Inf;
-    chosedel = Inf;
-    while chosenew+chosedel>=3
-    chosenew = 0;
-    chosedel = 0;
-     for i = 1:size(D.w,1)
-         for j = 1:size(D.w,2)
-             wtrial = bestw;
-             wnew = bestw;
-             delt = delta*(1-2*rand(1));
-             wtrial(i,j) = bestw(i,j) + delt;
-             [dcosttrial,~] =  Eval(D,R,S,wtrial);
-             wnew(i,j) = bestw(i,j) + max(min(bestcost*delt/(bestcost - dcosttrial), delta*5), -delta*5);
-             [dcostnew,~] =  Eval(D,R,S,wnew);
-             if dcostnew < bestcost
-                 bestw = wnew;
-                 bestcost = dcostnew;
-                 chosenew = chosenew +1;
-             end
-             if dcosttrial < bestcost
-                 bestw = wtrial;
-                 bestcost = dcosttrial;
-                 chosedel = chosedel +1;
-             end
-         end
-     end
-    end
 
-    %at the end of one pass, save progress
-    alldcosts = [alldcosts, bestcost];
-    D.w = bestw;
-    dcost = bestcost;
-    ShowPlot(D,R,S)
-end
-
-%RMP weight evaluation
-delta = 1;
-allrcosts = [];
-for passes = 1:10
-    bestw = R.w;
-    bestcost = rcost;
-%     for i = 1:Ntrials
-%         if rand(1)>.5
-%             % try random stuff
-%             R.w = 1-.5*rand(2, R.bfs);
-%             [~,rcost] = Eval(D,R,S);
-%             if rcost<bestcost
-%                bestw = R.w;
-%                bestcost = rcost;
-%                delta = 1;
-%             end
-%         else
-%             %try different random stuff based on size delta 
-%            R.w = bestw + 3*delta*randn(2, R.bfs);
-%            rcost = Eval(D,R,S);
-%            if rcost<bestcost
-%                bestw = R.w;
-%                bestcost = rcost;
-%            end
-% 
-%         end
-% 
+% %RMP weight evaluation
+ 
+%     Gradient Descent on the best weights
+%     pause(1)
+%     chosenew = Inf;
+%     chosedel = Inf;
+%     while chosenew+chosedel>=3
+%     chosenew = 0;
+%     chosedel = 0;
+%      for i = 1:size(R.w,1)
+%          for j = 1:size(R.w,2)
+%              wtrial = bestw;
+%              wnew = bestw;
+%              delt = delta*(1-2*rand(1));
+%              wtrial(i,j) = bestw(i,j) + delt;
+%              [~,rcosttrial,~,R] =  Eval(D,R,S,wtrial);
+%              wnew(i,j) = bestw(i,j) + max(min(bestcost*delt/(bestcost - rcosttrial), delta*5), -delta*5);
+%              [~,rcostnew,~,R] =  Eval(D,R,S,wnew);
+%              if rcostnew < bestcost
+%                  bestw = wnew;
+%                  bestcost = rcostnew;
+%                  chosenew = chosenew +1;
+%              end
+%              if rcosttrial < bestcost
+%                  bestw = wtrial;
+%                  bestcost = rcosttrial;
+%                  chosedel = chosedel +1;
+%              end
+%          end
+%      end
 %     end
+% 
+%     at the end of one pass, save progress
+%     allrcosts = [allrcosts, bestcost];
 %     R.w = bestw;
 %     rcost = bestcost;
-%     delta = delta*.99;
-%     Eval(D,R,S);
-%     allrcosts = [allrcosts,bestcost];
-    
-    %Gradient Descent on the best weights
-    pause(1)
-    chosenew = Inf;
-    chosedel = Inf;
-    while chosenew+chosedel>=3
-    chosenew = 0;
-    chosedel = 0;
-     for i = 1:size(R.w,1)
-         for j = 1:size(R.w,2)
-             wtrial = bestw;
-             wnew = bestw;
-             delt = delta*(1-2*rand(1));
-             wtrial(i,j) = bestw(i,j) + delt;
-             [~,rcosttrial] =  Eval(D,R,S,wtrial);
-             wnew(i,j) = bestw(i,j) + max(min(bestcost*delt/(bestcost - rcosttrial), delta*5), -delta*5);
-             [~,rcostnew] =  Eval(D,R,S,wnew);
-             if rcostnew < bestcost
-                 bestw = wnew;
-                 bestcost = rcostnew;
-                 chosenew = chosenew +1;
-             end
-             if rcosttrial < bestcost
-                 bestw = wtrial;
-                 bestcost = rcosttrial;
-                 chosedel = chosedel +1;
-             end
-         end
-     end
-    end
-
-    %at the end of one pass, save progress
-    allrcosts = [allrcosts, bestcost];
-    R.w = bestw;
-    rcost = bestcost;
+ShowPlot(D,R,S)
 end
 ShowPlot(D,R,S)
 Eval(D,R,S);
 toc
 
 %% Evaluate Cost
-function [dcost,rcost] = Eval(D,R,S,wnew)
+function [dcost,rcost,D,R] = Eval(D,R,S,wnew)
 if nargin == 4
     wdmp = wnew;
     wrmp = wnew;
@@ -244,25 +262,24 @@
 timetogoal = nan;
 trajpointdistancessq = Inf*ones(size(S.desiredtraj, 1), 1);
 for i = 1:S.Nt
-    D.x = D.x + ((-D.alphax)*D.x)/D.tau*S.dt;
-    D.psi(i,:) = exp(-1./(2*D.sigma.^2).*((D.x-D.c).^2));
-    D.psi(i,:)
-    forcing = (D.psi*wdmp') *D.x /sum(D.psi(i,:));
-    f = forcing(i,:);
+    D.x(i+1,:) = D.x(i,:) + ((-D.alphax)*D.x(i,:))/D.tau*S.dt;
+    D.psi(i,:) = exp(-1./(2*D.sigma.^2).*((D.x(i+1,:)-D.c).^2));
+    f(i,:) = (D.psi(i,:)*wdmp') *D.x(i+1,:) /sum(D.psi(i,:));
+%     f = forcing(i,:);
     
-    ydotdot = (D.alphay*(D.betay*(S.g-D.y)-D.ydot)+f)/D.tau;
-    D.ydot = ydotdot*S.dt + D.ydot;
-    D.y = D.ydot*S.dt+D.y;
+    ydotdot = (D.alphay*(D.betay*(S.g-D.y(i,:))-D.ydot(i,:))+f(i,:))/D.tau;
+    D.ydot(i+1,:) = ydotdot*S.dt + D.ydot(i,:);
+    D.y(i+1,:) = D.ydot(i+1,:)*S.dt+D.y(i,:);
     
-    trajpointdistancessq = min ((D.y(1) - S.desiredtraj(:,1)).^2 + (D.y(2) - S.desiredtraj(:,2)).^2, ...
+    trajpointdistancessq = min ((D.y(i+1,1) - S.desiredtraj(:,1)).^2 + (D.y(i+1,2) - S.desiredtraj(:,2)).^2, ...
                             trajpointdistancessq);
 
-    [dsquared, whichtrajpoint] = min( (D.y(1) - S.desiredtraj(:,1)).^2 + ...
-        (D.y(2) - S.desiredtraj(:,2)).^2);
+    [dsquared, whichtrajpoint] = min( (D.y(i+1,1) - S.desiredtraj(:,1)).^2 + ...
+        (D.y(i+1,2) - S.desiredtraj(:,2)).^2);
     distance = distance + sqrt(dsquared);
     
     %record the time if it's at the goal
-    if isnan(timetogoal) && sum((D.y-S.g).^2)<goaltol^2
+    if isnan(timetogoal) && sum((D.y(i+1,:)-S.g).^2)<goaltol^2
         timetogoal = i*S.dt;
     end
 end
@@ -277,23 +294,23 @@
 trajpointdistancessq = Inf*ones(size(S.desiredtraj, 1), 1);
 for i = 1:S.Nt
     dW = sqrt(S.dt)*randn(1,R.bfs);
-    da = R.a .* (R.alpha - R.a*R.rho') *S.dt + R.ep.*dW; %tau for scaling?
-    R.a = max(min(R.a + da, 1), .0005); %enforcing boundaries on a
-    f = R.a*wrmp'/sum(R.a);
+    da = R.a(i,:) .* (R.alpha - R.a(i,:)*R.rho) *S.dt + R.ep.*dW; %tau for scaling?
+    R.a(i+1,:) = max(min(R.a(i,:) + da, 1), .0005); %enforcing boundaries on a
+    f(i,:) = R.a(i+1,:)*wrmp'/sum(R.a(i+1,:));
 
-    ydotdot = (R.alphay*(R.betay*(S.g-R.y)-R.ydot)+f)/R.tau;
-    R.ydot = ydotdot*S.dt + R.ydot;
-    R.y = R.ydot*S.dt+R.y;
+    ydotdot = (R.alphay*(R.betay*(S.g-R.y(i,:))-R.ydot(i,:))+f(i,:))/R.tau;
+    R.ydot(i+1,:) = ydotdot*S.dt + R.ydot(i,:);
+    R.y(i+1,:) = R.ydot(i+1,:)*S.dt+R.y(i,:);
     
-    trajpointdistancessq = min ((R.y(1) - S.desiredtraj(:,1)).^2 + (R.y(2) - S.desiredtraj(:,2)).^2, ...
+    trajpointdistancessq = min ((R.y(i+1,1) - S.desiredtraj(:,1)).^2 + (R.y(i+1,2) - S.desiredtraj(:,2)).^2, ...
                             trajpointdistancessq);
 
-    [dsquared, whichtrajpoint] = min( (R.y(1) - S.desiredtraj(:,1)).^2 + ...
-        (R.y(2) - S.desiredtraj(:,2)).^2);
+    [dsquared, whichtrajpoint] = min( (R.y(i+1,1) - S.desiredtraj(:,1)).^2 + ...
+        (R.y(i+1,2) - S.desiredtraj(:,2)).^2);
     distance = distance + sqrt(dsquared);
     
     %record the time if it's at the goal
-    if isnan(timetogoal) && sum((R.y-S.g).^2)<goaltol^2
+    if isnan(timetogoal) && sum((R.y(i+1,:)-S.g).^2)<goaltol^2
         timetogoal = i*S.dt;
     end
 end
@@ -303,7 +320,6 @@
 end
 
 rcost = 1*distance + 10*timetogoal/S.Nt + 1*sum(sqrt(trajpointdistancessq));
-
 end
 
 %% Plot
@@ -324,11 +340,12 @@ function ShowPlot(D,R,S)
 % desiredtrajdir = diff(S.desiredtraj);
 % desiredtrajdir = .5*( [desiredtrajdir;desiredtrajdir(end,:)] + [desiredtrajdir(1,:);desiredtrajdir]);%centerdiffs
 % desiredtrajdir = desiredtrajdir ./sqrt(sum(desiredtrajdir.^2,2)); %normalize
+
 for i = 1:S.Nt
-    [~, dmpcolor(i)] = max( [sum( abs(D.alphay*(D.betay*(S.g-D.y)-D.ydot)).^2)*.1;
-                               sum((abs((D.psi(i,:).*D.w) *D.x /sum(D.psi(i,:)))').^2,2)]);
-    [~, rmpcolor(i)] = max( [sum( abs(R.alphay*(R.betay*(S.g-R.y)-R.ydot)).^2)*.1;
-                               sum((abs((R.a.*R.w) /sum(R.a))').^2,2)]);
+    [~, dmpcolor(i)] = max( [sum( abs(D.alphay*(D.betay*(S.g-D.y(i,:))-D.ydot(i,:))).^2)*.1;
+                               sum((abs((D.psi(i,:).*D.w) *D.x(i,:) /sum(D.psi(i,:)))').^2,2)]);
+    [~, rmpcolor(i)] = max( [sum( abs(R.alphay*(R.betay*(S.g-R.y(i,:))-R.ydot(i,:))).^2)*.1;
+                               sum((abs((R.a(i,:).*R.w) /sum(R.a(i,:)))').^2,2)]);
 %     [dsquared, whichtrajpoint] = min( (D.y(1) - S.desiredtraj(:,1)).^2 + ...
 %                                 (D.y(2) - S.desiredtraj(:,2)).^2);
 %     [dsquared, whichtrajpoint] = min( (R.y(1) - S.desiredtraj(:,1)).^2 + ...
@@ -338,35 +355,36 @@ function ShowPlot(D,R,S)
 %     alldistanceerror(i) = sqrt(dsquared);
 %     whichtraj(i) = whichtrajpoint;
 %     allverror(i) = vnormalerror;
-    yalldmp(i,:) = D.y;
-    yallrmp(i,:) = R.y;
+    yalldmp(i,:) = D.y(i,:);
+    yallrmp(i,:) = R.y(i,:);
 %     ydotall(i,:) = S.ydot;
 %     xall(i) = S.x;
     psiall(i,:) = D.psi(i,:);
-    aall(i,:) = R.a;
+    aall(i,:) = R.a(i,:);
 end
 dmpcolorstouse = setdiff(unique(dmpcolor)-1, 0);
 rmpcolorstouse = setdiff(unique(rmpcolor)-1, 0);
 psicolors = ones(1, size(D.w,2))'*[0.7, 0.7, 0.7];
-size(dmpcolorstouse)
+acolors = ones(1, size(R.w,2))'*[0.7, 0.7, 0.7];
 psicolors(dmpcolorstouse,:) = cool(length(dmpcolorstouse));
 acolors(rmpcolorstouse,:) = cool(length(rmpcolorstouse));
 
+
 figure(1)
-clf(1)
-figure(1)
-hold on 
+ clf(1)
+ figure(1)
+hold on
 subplot(2,1,1)
 title ('Basis functions times weights (dmp)')
 for j = 1:size(D.psi,2)
     plot((1:S.Nt)*S.dt, psiall(:,j)*D.w(1,j), '-c', 'Color', psicolors(j,:))
-    plot((1:S.Nt)*S.dt, psiall(:,j)*D.w(2,j), '-c.', 'Color', acolors(j,:))
+    plot((1:S.Nt)*S.dt, psiall(:,j)*D.w(2,j), '-c.', 'Color', psicolors(j,:))
 end
 subplot(2,1,2)
 title('Basis functions times weights (rmp)')
 for j = 1:size(R.a,2)
-    plot((1:S.Nt)*S.dt, aall(:,j)*R.w(1,j), '-c', 'Color', psicolors(j,:))
-    plot((1:S.Nt)*S.dt, aall(:,j)*R.w(2,j), '-c.', 'Color', psicolors(j,:))
+    plot((1:S.Nt)*S.dt, aall(:,j)*R.w(1,j), '-c', 'Color', acolors(j,:))
+    plot((1:S.Nt)*S.dt, aall(:,j)*R.w(2,j), '-c.', 'Color', acolors(j,:))
 end
 
 figure(2)
@@ -376,19 +394,19 @@ function ShowPlot(D,R,S)
 subplot(2,1,1)
 plot(yalldmp(:,1), yalldmp(:,2), 'k-')
 plot(S.desiredtraj(:,1), S.desiredtraj(:,2), '-g.');
-plot(yalldmp(whichcolor==1,1), yalldmp(whichcolor==1,2), 'k.')
+plot(yalldmp(dmpcolor==1,1), yalldmp(dmpcolor==1,2), 'k.')
 for i = 1:size(D.w, 2)
-    plot(yalldmp(whichcolor==(1+i),1), yalldmp(whichcolor==(1+i),2), 'k.', 'Color', psicolors(i,:))
+    plot(yalldmp(dmpcolor==(1+i),1), yalldmp(dmpcolor==(1+i),2), 'k.', 'Color', psicolors(i,:))
     plot(D.w(1,i), D.w(2,i), 'k*', 'Color', psicolors(i,:))
 end
 axis equal
 subplot(2,1,2)
 plot(yallrmp(:,1), yallrmp(:,2), 'k-')
 plot(S.desiredtraj(:,1), S.desiredtraj(:,2), '-g.');
-plot(yallrmp(whichcolor==1,1), yallrmp(whichcolor==1,2), 'k.')
+plot(yallrmp(rmpcolor==1,1), yallrmp(rmpcolor==1,2), 'k.')
 for i = 1:size(R.w, 2)
-    plot(yallrmp(whichcolor==(1+i),1), yallrmp(whichcolor==(1+i),2), 'k.', 'Color', psicolors(i,:))
-    plot(R.w(1,i), R.w(2,i), 'k*', 'Color', psicolors(i,:))
+    plot(yallrmp(rmpcolor==(1+i),1), yallrmp(rmpcolor==(1+i),2), 'k.', 'Color', acolors(i,:))
+    plot(R.w(1,i), R.w(2,i), 'k*', 'Color', acolors(i,:))
 end
 axis equal
 
@@ -398,13 +416,13 @@ function ShowPlot(D,R,S)
 hold on
 subplot(2,1,1)
 for i = 1:size(D.w, 2)
-    plot(yalldmp(whichcolor==(1+i),1), yalldmp(whichcolor==(1+i),2), 'k.', 'Color', psicolors(i,:))
+    plot(yalldmp(dmpcolor==(1+i),1), yalldmp(dmpcolor==(1+i),2), 'k.', 'Color', psicolors(i,:))
 end
 plot(S.desiredtraj(:,1),S.desiredtraj(:,2), '-k.');
 axis equal
 subplot(2,1,2)
 for i = 1:size(R.w, 2)
-    plot(yallrmp(whichcolor==(1+i),1), yallrmp(whichcolor==(1+i),2), 'k.', 'Color', psicolors(i,:))
+    plot(yallrmp(rmpcolor==(1+i),1), yallrmp(rmpcolor==(1+i),2), 'k.', 'Color', acolors(i,:))
 end
 plot(S.desiredtraj(:,1),S.desiredtraj(:,2), '-k.');
 axis equal

From f69bebca28ab553c66090faf13a808a68c2d4f0a Mon Sep 17 00:00:00 2001
From: Natasha Rouse <nar50@case.edu>
Date: Fri, 24 Jul 2020 13:39:32 -0400
Subject: [PATCH 4/7] Plotting works! Functional code.

Includes: linear algebra formulation for weights, stochastic learning for weights, & gradient descent for learning weights.
---
 DMPandRMP1.m | 387 +++++++++++++++++++++++++++++++--------------------
 1 file changed, 236 insertions(+), 151 deletions(-)

diff --git a/DMPandRMP1.m b/DMPandRMP1.m
index 69ac490..261708e 100644
--- a/DMPandRMP1.m
+++ b/DMPandRMP1.m
@@ -32,21 +32,29 @@
 y0dot(1,:) = 0 * [1,1];
 x0 = 1;
 bfs = 10;
+rbfs = 10;
+sigma = logspace(log10(.3),log10(.002), bfs); %original
+% sigma = 0.002*ones(1,bfs); %this gives bf #10 the same size as rmps
+% sigma = logspace(log10(0.205),log10(0.002),bfs); %closest overall
+% sigma = logspace(log10(0.3),log10(0.0005),bfs);
 
 %dmp init
 D = struct('tau',1,'alphay',4,'betay',1,'alphax',.5,... %how fast cannonical system decays
     'bfs',bfs,... %number of basis functions (psi activations)
     'c',logspace(log10(1), log10(.01), bfs),... %basis function centers 
-    'sigma',logspace(log10(.3),log10(.002), bfs),... %basis function widths
+    'sigma',sigma,... %basis function widths
     'w',zeros(1,length(desiredtraj)),...
+    'f',zeros(S.Nt,2),...
     'psi',nan(S.Nt,bfs),'y',y0,'ydot',y0dot,'x',x0);
 
 %rmp init
-R = struct('tau',1,'alphay',4,'betay',1,'alpha',10,'beta',1,'nu',1.2,'bfs',bfs,...
-    'rho',zeros(bfs),...
+R = struct('tau',1,'alphay',4,'betay',1,'alpha',10,'beta',1,'nu',1.2,'bfs',rbfs,...
+    'rho',zeros(rbfs),...
+    'a0',[1,0.1,10^-9*ones(1,rbfs-2)],...
     'a',[],...
-    'ep',[10^-9*ones(1,bfs)],...
-    'w',zeros(1,length(desiredtraj)),'y',y0,'ydot',y0dot);
+    'ep',10^-9*ones(1,rbfs),...
+    'w',zeros(1,length(desiredtraj)),...
+    'f',zeros(S.Nt,2),'y',y0,'ydot',y0dot);
 
 %Init
 % D.w = S.desiredtraj(round(linspace(1,length(S.desiredtraj),D.bfs)) ,:)'*(D.alphay+D.betay);
@@ -66,17 +74,19 @@
 
 D.w = betawd';
 
-R.a(1,:) = [1,0.1,10^-9*ones(1,R.bfs-2)];
+R.a(1,:) = R.a0;
 alpha_rho = R.alpha*ones(R.bfs,1);
 beta_rho = R.beta*ones(R.bfs,1);
 nu_rho = R.nu*ones(R.bfs,1);
-R.rho = zeros(bfs);
+% nu_rho(2) = 2.4;
+R.rho = zeros(R.bfs);
 
-for i = 1:bfs
-    for j = 1:bfs
+for i = 1:R.bfs
+    for j = 1:R.bfs
         if i==j
             R.rho(i,j) = alpha_rho(i)/beta_rho(i);
-        else if 0 == j-1-i
+        else
+            if 0 == j-1-i
                 R.rho(i,j) = (alpha_rho(i) - alpha_rho(j)/nu_rho(j))./beta_rho(j);
             else
                 R.rho(i,j) = (alpha_rho(i) + alpha_rho(j))/beta_rho(j);
@@ -87,20 +97,21 @@
 
 index = 1;
 expected_SHCas = R.a;
+a = R.a(1,:);
+a*R.rho
+
 for t = desiredt(1): S.dt: desiredt(end)    
     dW = sqrt(S.dt)*randn(1,R.bfs);
-    da = R.a .* (R.alpha - R.a*R.rho) *S.dt + R.ep.*dW; %tau for scaling?
-    R.a = max(min(R.a + da, 1), .0005); %enforcing boundaries on 'a'
+    da = a .* (R.alpha - a*R.rho) *S.dt + R.ep.*dW; %tau for scaling?
+    a = max(min(a + da, 1), .0005); %enforcing boundaries on 'a'
     
     if t>=desiredt(index)
         index = index+1;
     else
-        expected_SHCas(index, :) = R.a;
+        expected_SHCas(index, :) = a;
     end
 end
-R.a
-pause
-size(expected_SHCas)
+
 fdesiredr = R.tau * desiredydotdot - (R.alphay * (R.betay*(S.g-S.desiredtraj) - desiredydot));
 betawr =  (expected_SHCas) \ (sum(expected_SHCas')'.* fdesiredr);
 % meanerror = sum(sum(errors.^2))
@@ -122,125 +133,130 @@
     bestdcost = dcost;
     bestrw = R.w;
     bestrcost = rcost;
-    for i = 1:Ntrials
-        if rand(1)>.5
-            % try random stuff
-            D.w = 1-.5*rand(2, D.bfs);
-            R.w = 1-.5*rand(2, R.bfs);
-            [dcost,rcost,D,R] = Eval(D,R,S);
-            ShowPlot(D,R,S)
-            if dcost<bestdcost
-               bestdw = D.w;
-               bestdcost = dcost;
-               delta = 1;
-            end
-            if rcost<bestrcost
-               bestrw = R.w;
-               bestrcost = rcost;
-               delta = 1;
-            end
-        else
-            %try different random stuff based on size delta 
-           D.w = bestdw + 3*delta*randn(2, D.bfs);
-           R.w = bestrw + 3*delta*randn(2, R.bfs);
-           [dcost,rcost,D,R] = Eval(D,R,S);
-           ShowPlot(D,R,S)
-           if dcost<bestdcost
-               bestdw = D.w;
-               bestdcost = dcost;
-           end
-           if rcost<bestrcost
-               bestrw = R.w;
-               bestrcost = rcost;
-           end
-        end
-    end
-    D.w = bestdw;
-    dcost = bestdcost;
-    R.w = bestrw;
-    rcost = bestrcost;
-    delta = delta*.99;
+%     for i = 1:Ntrials
+%         if rand(1)>.5
+%             % try random stuff
+%             D.w = 1-.5*rand(2, D.bfs);
+%             R.w = 1-.5*rand(2, R.bfs);
+%             [dcost,rcost,D,R] = Eval(D,R,S);
+%             if dcost<bestdcost
+%                bestdw = D.w;
+%                bestdcost = dcost;
+%                delta = 1;
+%             end
+%             if rcost<bestrcost
+%                bestrw = R.w;
+%                bestrcost = rcost;
+%                delta = 1;
+%             end
+%         else
+%             %try different random stuff based on size delta 
+%            D.w = bestdw + 3*delta*randn(2, D.bfs);
+%            R.w = bestrw + 3*delta*randn(2, R.bfs);
+%            [dcost,rcost,D,R] = Eval(D,R,S);
+%            if dcost<bestdcost
+%                bestdw = D.w;
+%                bestdcost = dcost;
+%            end
+%            if rcost<bestrcost
+%                bestrw = R.w;
+%                bestrcost = rcost;
+%            end
+%         end
+%     end
+%     D.w = bestdw;
+%     dcost = bestdcost;
+%     R.w = bestrw;
+%     rcost = bestrcost;
+%     delta = delta*.99;
 %     [~,~,D,R] = Eval(D,R,S);
+%     
+%     alldcosts = [alldcosts,bestdcost];
+%     allrcosts = [allrcosts,bestrcost];
     
-    alldcosts = [alldcosts,bestdcost];
-    allrcosts = [allrcosts,bestrcost];
-    
-%     %Gradient Descent on the best weights
-%     pause(1)
-%     chosenew = Inf;
-%     chosedel = Inf;
-%     while chosenew+chosedel>=3
-%     chosenew = 0;
-%     chosedel = 0;
-%      for i = 1:size(D.w,1)
-%          for j = 1:size(D.w,2)
-%              wtrial = bestw;
-%              wnew = bestw;
-%              delt = delta*(1-2*rand(1));
-%              wtrial(i,j) = bestw(i,j) + delt;
-%              [dcosttrial,~,D,~] =  Eval(D,R,S,wtrial);
-%              ShowPlot(D,R,S)
-%              wnew(i,j) = bestw(i,j) + max(min(bestcost*delt/(bestcost - dcosttrial), delta*5), -delta*5);
-%              [dcostnew,~,D,~] =  Eval(D,R,S,wnew);
-%              if dcostnew < bestcost
-%                  bestw = wnew;
-%                  bestcost = dcostnew;
-%                  chosenew = chosenew +1;
-%              end
-%              if dcosttrial < bestcost
-%                  bestw = wtrial;
-%                  bestcost = dcosttrial;
-%                  chosedel = chosedel +1;
-%              end
-%          end
-%      end
-%     end
-
-%     %at the end of one pass, save progress
-%     alldcosts = [alldcosts, bestcost];
-%     D.w = bestw;
-%     dcost = bestcost;
+%Gradient Descent on the best weights
+pause(1)
+chosenew = Inf;
+chosedel = Inf;
+while chosenew+chosedel>=3
+    chosenew = 0;
+    chosedel = 0;
+     for i = 1:size(D.w,1)
+         for j = 1:size(D.w,2)
+             wtrial = bestdw;
+             wnew = bestdw;
+             delt = delta*(1-2*rand(1));
+             wtrial(i,j) = bestdw(i,j) + delt;
+             [dcosttrial,~,D,~] =  Eval(D,R,S,wtrial);
+             wnew(i,j) = bestdw(i,j) + max(min(bestdcost*delt/(bestdcost - dcosttrial), delta*5), -delta*5);
+             [dcostnew,~,D,~] =  Eval(D,R,S,wnew);
+             if dcostnew < bestdcost
+                 bestdw = wnew;
+                 bestdcost = dcostnew;
+                 chosenew = chosenew +1;
+             end
+             if dcosttrial < bestdcost
+                 bestdw = wtrial;
+                 bestdcost = dcosttrial;
+                 chosedel = chosedel +1;
+             end
+         end
+     end
+end
 
-% %RMP weight evaluation
+%at the end of one pass, save progress
+alldcosts = [alldcosts, bestdcost];
+D.w = bestdw;
+dcost = bestdcost;
  
-%     Gradient Descent on the best weights
-%     pause(1)
-%     chosenew = Inf;
-%     chosedel = Inf;
-%     while chosenew+chosedel>=3
-%     chosenew = 0;
-%     chosedel = 0;
-%      for i = 1:size(R.w,1)
-%          for j = 1:size(R.w,2)
-%              wtrial = bestw;
-%              wnew = bestw;
-%              delt = delta*(1-2*rand(1));
-%              wtrial(i,j) = bestw(i,j) + delt;
-%              [~,rcosttrial,~,R] =  Eval(D,R,S,wtrial);
-%              wnew(i,j) = bestw(i,j) + max(min(bestcost*delt/(bestcost - rcosttrial), delta*5), -delta*5);
-%              [~,rcostnew,~,R] =  Eval(D,R,S,wnew);
-%              if rcostnew < bestcost
-%                  bestw = wnew;
-%                  bestcost = rcostnew;
-%                  chosenew = chosenew +1;
-%              end
-%              if rcosttrial < bestcost
-%                  bestw = wtrial;
-%                  bestcost = rcosttrial;
-%                  chosedel = chosedel +1;
-%              end
-%          end
-%      end
-%     end
-% 
-%     at the end of one pass, save progress
-%     allrcosts = [allrcosts, bestcost];
-%     R.w = bestw;
-%     rcost = bestcost;
-ShowPlot(D,R,S)
+% Gradient Descent on the best weights
+pause(1)
+chosenew = Inf;
+chosedel = Inf;
+while chosenew+chosedel>=3
+    chosenew = 0;
+    chosedel = 0;
+     for i = 1:size(R.w,1)
+         for j = 1:size(R.w,2)
+             wtrial = bestrw;
+             wnew = bestrw;
+             delt = delta*(1-2*rand(1));
+             wtrial(i,j) = bestrw(i,j) + delt;
+             [~,rcosttrial,~,R] =  Eval(D,R,S,wtrial);
+             wnew(i,j) = bestrw(i,j) + max(min(bestrcost*delt/(bestrcost - rcosttrial), delta*5), -delta*5);
+             [~,rcostnew,~,R] =  Eval(D,R,S,wnew);
+             if rcostnew < bestrcost
+                 bestrw = wnew;
+                 bestrcost = rcostnew;
+                 chosenew = chosenew +1;
+             end
+             if rcosttrial < bestrcost
+                 bestrw = wtrial;
+                 bestrcost = rcosttrial;
+                 chosedel = chosedel +1;
+             end
+         end
+     end
+end
+
+% at the end of one pass, save progress
+allrcosts = [allrcosts, bestrcost];
+R.w = bestrw;
+rcost = bestrcost;
+% ShowPlot(D,R,S)
 end
-ShowPlot(D,R,S)
 Eval(D,R,S);
+ShowPlot(D,R,S)
+
+figure(8)
+hold on
+title('All Costs')
+plot(alldcosts,'-k','Linewidth',1.5)
+plot(allrcosts,'--k','Linewidth',1.5)
+legend('DMP','RMP')
+xlabel('Iterations')
+ylabel('Cost')
+
 toc
 
 %% Evaluate Cost
@@ -253,7 +269,6 @@
     wdmp = D.w;
     wrmp = R.w;
 end
-
 goaltol = 0.005;
 distance = 0;
 maxtime = (S.Nt+1)*S.dt;
@@ -264,17 +279,17 @@
 for i = 1:S.Nt
     D.x(i+1,:) = D.x(i,:) + ((-D.alphax)*D.x(i,:))/D.tau*S.dt;
     D.psi(i,:) = exp(-1./(2*D.sigma.^2).*((D.x(i+1,:)-D.c).^2));
-    f(i,:) = (D.psi(i,:)*wdmp') *D.x(i+1,:) /sum(D.psi(i,:));
+    D.f(i,:) = (D.psi(i,:)*wdmp') *D.x(i+1,:) /sum(D.psi(i,:));
 %     f = forcing(i,:);
     
-    ydotdot = (D.alphay*(D.betay*(S.g-D.y(i,:))-D.ydot(i,:))+f(i,:))/D.tau;
+    ydotdot = (D.alphay*(D.betay*(S.g-D.y(i,:))-D.ydot(i,:))+D.f(i,:))/D.tau;
     D.ydot(i+1,:) = ydotdot*S.dt + D.ydot(i,:);
     D.y(i+1,:) = D.ydot(i+1,:)*S.dt+D.y(i,:);
     
     trajpointdistancessq = min ((D.y(i+1,1) - S.desiredtraj(:,1)).^2 + (D.y(i+1,2) - S.desiredtraj(:,2)).^2, ...
                             trajpointdistancessq);
 
-    [dsquared, whichtrajpoint] = min( (D.y(i+1,1) - S.desiredtraj(:,1)).^2 + ...
+    [dsquared,~] = min( (D.y(i+1,1) - S.desiredtraj(:,1)).^2 + ...
         (D.y(i+1,2) - S.desiredtraj(:,2)).^2);
     distance = distance + sqrt(dsquared);
     
@@ -287,25 +302,32 @@
 if isnan(timetogoal) 
     timetogoal = maxtime-S.dt;
 end
+dcomponents = [1*distance, 10*timetogoal/S.Nt, 1*sum(sqrt(trajpointdistancessq))];
 dcost = 1*distance + 10*timetogoal/S.Nt + 1*sum(sqrt(trajpointdistancessq));
 
 %RMP
+distance = 0;
 timetogoal = nan;
 trajpointdistancessq = Inf*ones(size(S.desiredtraj, 1), 1);
 for i = 1:S.Nt
     dW = sqrt(S.dt)*randn(1,R.bfs);
-    da = R.a(i,:) .* (R.alpha - R.a(i,:)*R.rho) *S.dt + R.ep.*dW; %tau for scaling?
-    R.a(i+1,:) = max(min(R.a(i,:) + da, 1), .0005); %enforcing boundaries on a
-    f(i,:) = R.a(i+1,:)*wrmp'/sum(R.a(i+1,:));
+    if i == 1
+        da = R.a0 .* (R.alpha - R.a0*R.rho) *S.dt + R.ep.*dW;
+        R.a(i,:) = max(min(R.a0 + da, 1), 0.0005);
+    else
+        da = R.a(i-1,:) .* (R.alpha - R.a(i-1,:)*R.rho) *S.dt + R.ep.*dW; %tau for scaling?
+        R.a(i,:) = max(min(R.a(i-1,:) + da, 1), .0005); %enforcing boundaries on a
+    end
+    R.f(i,:) = (R.a(i,:)*wrmp')/sum(R.a(i,:));
 
-    ydotdot = (R.alphay*(R.betay*(S.g-R.y(i,:))-R.ydot(i,:))+f(i,:))/R.tau;
+    ydotdot = (R.alphay*(R.betay*(S.g-R.y(i,:))-R.ydot(i,:))+R.f(i,:))/R.tau;
     R.ydot(i+1,:) = ydotdot*S.dt + R.ydot(i,:);
     R.y(i+1,:) = R.ydot(i+1,:)*S.dt+R.y(i,:);
     
     trajpointdistancessq = min ((R.y(i+1,1) - S.desiredtraj(:,1)).^2 + (R.y(i+1,2) - S.desiredtraj(:,2)).^2, ...
                             trajpointdistancessq);
 
-    [dsquared, whichtrajpoint] = min( (R.y(i+1,1) - S.desiredtraj(:,1)).^2 + ...
+    [dsquared,~] = min( (R.y(i+1,1) - S.desiredtraj(:,1)).^2 + ...
         (R.y(i+1,2) - S.desiredtraj(:,2)).^2);
     distance = distance + sqrt(dsquared);
     
@@ -318,7 +340,7 @@
 if isnan(timetogoal) 
     timetogoal = maxtime-S.dt;
 end
-
+rcomponents = [1*distance, 10*timetogoal/S.Nt, 1*sum(sqrt(trajpointdistancessq))];
 rcost = 1*distance + 10*timetogoal/S.Nt + 1*sum(sqrt(trajpointdistancessq));
 end
 
@@ -362,27 +384,28 @@ function ShowPlot(D,R,S)
     psiall(i,:) = D.psi(i,:);
     aall(i,:) = R.a(i,:);
 end
+
 dmpcolorstouse = setdiff(unique(dmpcolor)-1, 0);
 rmpcolorstouse = setdiff(unique(rmpcolor)-1, 0);
-psicolors = ones(1, size(D.w,2))'*[0.7, 0.7, 0.7];
+psicolors = ones(1,size(D.w,2))'*[0.7, 0.7, 0.7];
 acolors = ones(1, size(R.w,2))'*[0.7, 0.7, 0.7];
 psicolors(dmpcolorstouse,:) = cool(length(dmpcolorstouse));
 acolors(rmpcolorstouse,:) = cool(length(rmpcolorstouse));
 
-
 figure(1)
  clf(1)
  figure(1)
-hold on
 subplot(2,1,1)
 title ('Basis functions times weights (dmp)')
 for j = 1:size(D.psi,2)
+    hold on
     plot((1:S.Nt)*S.dt, psiall(:,j)*D.w(1,j), '-c', 'Color', psicolors(j,:))
     plot((1:S.Nt)*S.dt, psiall(:,j)*D.w(2,j), '-c.', 'Color', psicolors(j,:))
 end
 subplot(2,1,2)
 title('Basis functions times weights (rmp)')
 for j = 1:size(R.a,2)
+    hold on
     plot((1:S.Nt)*S.dt, aall(:,j)*R.w(1,j), '-c', 'Color', acolors(j,:))
     plot((1:S.Nt)*S.dt, aall(:,j)*R.w(2,j), '-c.', 'Color', acolors(j,:))
 end
@@ -390,20 +413,19 @@ function ShowPlot(D,R,S)
 figure(2)
  clf(2)
  figure(2)
-hold on
 subplot(2,1,1)
-plot(yalldmp(:,1), yalldmp(:,2), 'k-')
-plot(S.desiredtraj(:,1), S.desiredtraj(:,2), '-g.');
-plot(yalldmp(dmpcolor==1,1), yalldmp(dmpcolor==1,2), 'k.')
+hold on
+title('Weights plotted spatially (dmp)')
+plot(D.w(1,:),D.w(2,:), '-ro', 'Color', [.6, .6, .6])
 for i = 1:size(D.w, 2)
     plot(yalldmp(dmpcolor==(1+i),1), yalldmp(dmpcolor==(1+i),2), 'k.', 'Color', psicolors(i,:))
     plot(D.w(1,i), D.w(2,i), 'k*', 'Color', psicolors(i,:))
 end
 axis equal
 subplot(2,1,2)
-plot(yallrmp(:,1), yallrmp(:,2), 'k-')
-plot(S.desiredtraj(:,1), S.desiredtraj(:,2), '-g.');
-plot(yallrmp(rmpcolor==1,1), yallrmp(rmpcolor==1,2), 'k.')
+hold on
+title('Weights plotted spatially (rmp)')
+plot(R.w(1,:),R.w(2,:), '-ro', 'Color', [.6, .6, .6])
 for i = 1:size(R.w, 2)
     plot(yallrmp(rmpcolor==(1+i),1), yallrmp(rmpcolor==(1+i),2), 'k.', 'Color', acolors(i,:))
     plot(R.w(1,i), R.w(2,i), 'k*', 'Color', acolors(i,:))
@@ -413,22 +435,85 @@ function ShowPlot(D,R,S)
 figure(3)
  clf(3)
  figure(3)
-hold on
 subplot(2,1,1)
+hold on
+title('Desired traj (black) and produced traj (color) - dmp')
 for i = 1:size(D.w, 2)
     plot(yalldmp(dmpcolor==(1+i),1), yalldmp(dmpcolor==(1+i),2), 'k.', 'Color', psicolors(i,:))
 end
 plot(S.desiredtraj(:,1),S.desiredtraj(:,2), '-k.');
 axis equal
 subplot(2,1,2)
+hold on
+title('Desired traj (black) and produced traj (color) - rmp')
 for i = 1:size(R.w, 2)
     plot(yallrmp(rmpcolor==(1+i),1), yallrmp(rmpcolor==(1+i),2), 'k.', 'Color', acolors(i,:))
 end
 plot(S.desiredtraj(:,1),S.desiredtraj(:,2), '-k.');
 axis equal
 
+figure(4)
+ clf(4)
+ figure(4)
+subplot(2,1,1)
+hold on
+title('DMP forcing function')
+plot((1:S.Nt)*S.dt,D.f(:,1),'-r')
+plot((1:S.Nt)*S.dt,D.f(:,2),'--r')
+legend('x-component','y-component')
+subplot(2,1,2)
+hold on
+title('RMP forcing function')
+plot((1:S.Nt)*S.dt,R.f(:,1),'-b')
+plot((1:S.Nt)*S.dt,R.f(:,2),'--b')
+legend('x-component','y-component')
+
+figure(5)
+ clf(5)
+ figure(5)
+subplot(2,1,1)
+hold on
+title('Unweighted basis functions (dmp)')
+for i = 1:size(psiall,2)
+    plot((1:S.Nt)*S.dt, psiall(:,i),'-c', 'Color', psicolors(i,:))
+end
+subplot(2,1,2)
+hold on
+title('Unweighted basis functions (rmp)')
+for i = 1:size(aall,2)
+    plot((1:S.Nt)*S.dt, aall(:,i),'-c','Color',acolors(i,:))
+end
 
-
+figure(6)
+ clf(6)
+ figure(6)
+subplot(2,1,1)
+hold on
+title('DMP Basis Functions')
+plot((1:S.Nt)*S.dt, psiall(:,5),'-k','Linewidth', 1.5)
+set(gca,'xtick',[])
+subplot(2,1,2)
+hold on
+title('RMP Basis Functions')
+plot((1:S.Nt)*S.dt,aall(:,5),'-k','Linewidth',1.5)
+% legend('DMP','RMP')
+set(gca,'xtick',[])
+% dmparea5 = trapz(psiall(:,5))
+% rmparea5 = trapz(aall(:,5))
+
+figure(7)
+ clf(7)
+ figure(7)
+hold on
+title('Basis Functions')
+plot((1:S.Nt)*S.dt, psiall(:,5),'-k','Linewidth', 1.5)
+plot((1:S.Nt)*S.dt,aall(:,5),'--k','Linewidth',1.5)
+legend('DMP','RMP')
+set(gca,'xtick',[])
+
+%Looking for area under curve
+dmparea = trapz(psiall,1)
+rmparea = trapz(aall,1)
 
 end
 

From 71b00602673d85b34539516ea37a5b12524da72d Mon Sep 17 00:00:00 2001
From: Natasha Rouse <nar50@case.edu>
Date: Fri, 24 Jul 2020 13:41:12 -0400
Subject: [PATCH 5/7] Update README.md

---
 README.md | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/README.md b/README.md
index fba87d4..fbf7e5a 100644
--- a/README.md
+++ b/README.md
@@ -1 +1,3 @@
-# DMPandRMP
\ No newline at end of file
+# DMPandRMP
+Includes a LOT of files. Most relevant is DMPandRMP1.m
+This file combines run8.m & Eval.m into one file and allows for plotting of multiple variables without saving Matlab Workspace (this was the issue that created RunbothRMP_and_DMP.m.

From 34425616d2f55b86b03f4cb9cdc0664093be4666 Mon Sep 17 00:00:00 2001
From: Natasha Rouse <nar50@case.edu>
Date: Wed, 29 Jul 2020 11:12:43 -0400
Subject: [PATCH 6/7] Gradient descent conditions corrected.

---
 DMPandRMP1.m | 9 ++++-----
 1 file changed, 4 insertions(+), 5 deletions(-)

diff --git a/DMPandRMP1.m b/DMPandRMP1.m
index 261708e..b2016bc 100644
--- a/DMPandRMP1.m
+++ b/DMPandRMP1.m
@@ -98,7 +98,6 @@
 index = 1;
 expected_SHCas = R.a;
 a = R.a(1,:);
-a*R.rho
 
 for t = desiredt(1): S.dt: desiredt(end)    
     dW = sqrt(S.dt)*randn(1,R.bfs);
@@ -178,7 +177,7 @@
 pause(1)
 chosenew = Inf;
 chosedel = Inf;
-while chosenew+chosedel>=3
+while chosenew+chosedel>=2
     chosenew = 0;
     chosedel = 0;
      for i = 1:size(D.w,1)
@@ -213,7 +212,7 @@
 pause(1)
 chosenew = Inf;
 chosedel = Inf;
-while chosenew+chosedel>=3
+while chosenew+chosedel>=2
     chosenew = 0;
     chosedel = 0;
      for i = 1:size(R.w,1)
@@ -512,8 +511,8 @@ function ShowPlot(D,R,S)
 set(gca,'xtick',[])
 
 %Looking for area under curve
-dmparea = trapz(psiall,1)
-rmparea = trapz(aall,1)
+% dmparea = trapz(psiall,1)
+% rmparea = trapz(aall,1)
 
 end
 

From b20641800d6feec26e2c29d29f13462055dbd503 Mon Sep 17 00:00:00 2001
From: Natasha Rouse <nar50@case.edu>
Date: Fri, 22 Jan 2021 12:28:34 -0500
Subject: [PATCH 7/7] Update README.md

---
 README.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/README.md b/README.md
index fbf7e5a..c19a76f 100644
--- a/README.md
+++ b/README.md
@@ -1,3 +1,3 @@
 # DMPandRMP
 Includes a LOT of files. Most relevant is DMPandRMP1.m
-This file combines run8.m & Eval.m into one file and allows for plotting of multiple variables without saving Matlab Workspace (this was the issue that created RunbothRMP_and_DMP.m.
+This file combines run8.m & Eval.m into one file and allows for plotting of multiple variables without saving Matlab Workspace (this was the issue that created RunbothRMP_and_DMP.m).