Add functional everything. Receding slightly finnicky

.gitignore

@@ -31,3 +31,9 @@ codegen/
 
 # Cloud based storage dotfile
 .MATLABDriveTag
+
+
+barrier/*
+normal/*
+receding/*
+.DS_Store

full_quadrotor.m → Barrier_full_quadrotor.m

@@ -1,4 +1,4 @@
-function [dyn] = full_quadrotor(dt)
+function [dyn] = full_quadrotor_barrier(dt, xf)
 %   Detailed explanation goes here
 
 % Get parameters
@@ -10,15 +10,18 @@
 Iyy = 0.0032;
 Izz = 0.0055;
 
-syms x y z phi theta ps p q r vx vy vz u1 u2 u3 u4
+syms x y z phi theta ps p q r vx vy vz u1 u2 u3 u4 w;  % psi called "ps" for namespace reasons
 
-coord = [x; y; z; phi; theta; ps];
-vel = [vx; vy; vz; p; q; r];
-state = [coord; vel];
+pos = [x; y; z];
+coord_vel = [vx; vy; vz];
+euler_angle = [phi; theta; ps];
+body_rate = [p; q; r];
+state = [pos; coord_vel; euler_angle; body_rate; w];
 controls = [u1; u2; u3; u4];
 vars = [state; controls];
 
 %% Get equations of motion
+% Linear EOM first
 % Starting with R
 % Define individual rotation matrices
 R1_phi = [1, 0, 0;
@@ -37,19 +40,35 @@
 R = R1_phi * R2_theta * R3_psi;
 
 % Linear acceleration
-lin_acc = (1/m)*R*[0;0; u1 + u2 + u3 + u4] + [0;0;-m*g];
+lin_acc = (1/m)*R*[0;0; u1 + u2 + u3 + u4] + [0;0;-g];
+
+%% Rotational EOM
 
 % Rotational acceleration by Euler's eqn
-rot_acc = [(1/Ixx)*(sqrt(2)/2)*(u1+u3-u2-u4)*l - (Izz - Iyy)*q*r;
-    (1/Iyy)*(sqrt(2)/2)*(u3+u4-u1-u2)*l - (Izz - Ixx)*p*r;
+rot_acc = [(1/Ixx)*((sqrt(2)/2)*(u1+u3-u2-u4)*l - (Izz - Iyy)*q*r);
+    (1/Iyy)*((sqrt(2)/2)*(u3+u4-u1-u2)*l - (Izz - Ixx)*p*r);
     (1/Izz)*kt*(u1+u4-u2-u3)];
 
 % Set eom
 eom = [lin_acc; rot_acc];
 
-f = [coord + dt * vel;
-     vel + dt * eom];
+% body rates to euler rates
+P = [1, tan(theta)*sin(phi), tan(theta)*cos(phi);
+     0, cos(phi), -sin(phi);
+     0, sin(phi)/cos(theta), cos(phi)/cos(theta)];
+
+% Euler integrate to get next state
+f = [pos + dt * coord_vel;
+    coord_vel + dt * lin_acc;
+    euler_angle + dt * P * body_rate;
+    body_rate + dt * rot_acc;
+    0];  % placeholder
+
+% now update barrier state based on new state
+barrier_func = get_barrier_func();
+f(13) = barrier_func(f) - barrier_func(xf);
 
+% take derivatives and convert to matlab functions
 fx = jacobian(f, state);
 fu = jacobian(f, controls);
 
@@ -101,18 +120,19 @@
     u3 = clip(u3,-bound,bound);
     u4 = clip(u4,-bound,bound);
 
-    wrapper_f = num_f(x,y,z,phi,theta,psi,vx,vy,vz,p,q,r,u1,u2,u3,u4);
-    wrapper_fx = num_fx(x,y,z,phi,theta,psi,vx,vy,vz,p,q,r,u1,u2,u3,u4);
-    wrapper_fu = num_fu(x,y,z,phi,theta,psi,vx,vy,vz,p,q,r,u1,u2,u3,u4);
+    wrapper_f = num_f(x,y,z,vx,vy,vz,phi,theta,psi,p,q,r,u1,u2,u3,u4);
+    wrapper_fx = num_fx(x,y,z,vx,vy,vz,phi,theta,psi,p,q,r,u1,u2,u3,u4);
+    wrapper_fu = num_fu(x,y,z,vx,vy,vz,phi,theta,psi,p,q,r,u1,u2,u3,u4);
+	%wrapper_f = num_f(x,y,z,vx,vy,vz,phi,theta,psi,p,q,r,u1,u2,u3,u4);
 
     wrapper_fxx = zeros(12, 12, 12);
     wrapper_fxu = zeros(12, 12, 4);
     wrapper_fuu = zeros(12, 4, 4);
 
     for i = 1:12
-        wrapper_fxx(i, :, :) = num_fxx{i}(x,y,z,phi,theta,psi,vx,vy,vz,p,q,r,u1,u2,u3,u4);
-        wrapper_fxu(i, :, :) = num_fxu{i}(x,y,z,phi,theta,psi,vx,vy,vz,p,q,r,u1,u2,u3,u4);
-        wrapper_fuu(i, :, :) = num_fuu{i}(x,y,z,phi,theta,psi,vx,vy,vz,p,q,r,u1,u2,u3,u4);
+        wrapper_fxx(i, :, :) = num_fxx{i}(x,y,z,vx,vy,vz,phi,theta,psi,p,q,r,u1,u2,u3,u4);
+        wrapper_fxu(i, :, :) = num_fxu{i}(x,y,z,vx,vy,vz,phi,theta,psi,p,q,r,u1,u2,u3,u4);
+        wrapper_fuu(i, :, :) = num_fuu{i}(x,y,z,vx,vy,vz,phi,theta,psi,p,q,r,u1,u2,u3,u4);
     end
 end
 

back_pass.m

@@ -1,4 +1,4 @@
- function [controller, V] = back_pass(states, controls, dyn, costfn, term_costfn, regularizer, mode)
+ function [controller, V] = back_pass(states, controls, dyn, costfn, term_costfn, regularizer, mode, xf)
 %BACK_PASS The backward pass of iLQR / DDP.
 %
 %   states: A tf-by-n matrix where the row t contains the state of the
@@ -75,6 +75,14 @@
     regularizer = 0;
 end
 
+initial_mu = 1e-2;
+delta_0 = 2;
+
+mu = initial_mu;
+mu_min = 1e-6;
+delta = delta_0;
+
+
 
 % Some basic setup code for you
 horizon = size(controls, 1);
@@ -89,6 +97,10 @@
 
 %% Fill in your code below
 
+% add w to 13th pos in state vec
+barrier_func = get_barrier_func();
+states(horizon, 13) = barrier_func(states(horizon, :)) - barrier_func(xf);
+
 % Initialize with final values
 [~, cx, ~, Q, ~, ~] = costfn(states(horizon,:).', controls(horizon,:).');
 next_P = Q;
@@ -104,51 +116,90 @@
     % Evaluate cost and function
     [~, cx, cu, cxx, cxu, cuu] = costfn(states(t,:).', controls(t,:).');
     [f, fx, fu, fxx, fxu, fuu] = dyn(states(t,:).', controls(t,:).');
+    reg = regularizer;
 
-    % Get first-order derivatives of Q
-    Qx = cx + (fx.')*Vx_next - regularizer*(fx.')*(states(t+1,:).' - f);
-    Qu = cu + (fu.')*Vx_next - regularizer*(fu.')*(states(t+1,:).' - f);
+	delta = delta_0;
+
+    % automatically increment regularizer if Quu not invertible
+    for i = 1:1
+        % Get first-order derivatives of Q
+        Qx = cx + (fx.')*Vx_next - reg*(fx.')*(states(t+1,:).' - f);
+        Qu = cu + (fu.')*Vx_next - reg*(fu.')*(states(t+1,:).' - f);
+
+        if strcmp(mode, 'ddp')
+            % Get second-order derivatives of Q
+            Qxx = cxx + (fx.')*Vxx_next*fx + fake_tensor_prod(Vx_next,fxx);% + reg*(fx.')*fx;
+            Qxu = cxu + (fx.')*(Vxx_next + reg * eye(length(Qxx)))*fu + fake_tensor_prod(Vx_next,fxu);
+            Qux = Qxu.';
+            Quu = cuu + (fu.')*(Vxx_next + reg * eye(length(Qxx)))*fu + fake_tensor_prod(Vx_next,fuu);
+
+        elseif strcmp(mode,'ilqr')
+            % Get second-order derivatives of Q without tensor prod
+            Qxx = cxx + (fx.')*Vxx_next*fx;
+            Qxu = cxu + (fx.')*Vxx_next*fu;
+            Qux = Qxu.';
+            Quu = cuu + (fu.')*Vxx_next*fu;
+		end
+
+		Quu = (Quu + Quu.')/2;
+		%Quu = Quu + mu * eye(size(Quu));
+
+        % check if Quu is positive definite
+        if all(eig(Quu) > 1e-6) && rcond(Quu) > 1e-13
+            break
+		end
+
+        % Increase mu if Quu is not positive definite
+        delta = max(delta_0, delta * delta_0);
+        mu = max(mu_min, mu * delta);
+
+        if reg == 0 || isinf(reg)
+            disp('Reg 0 or inf');
+            break
+        end
+        if isnan(rcond(Quu))
+            disp('NaN rcond(Quu)');
+            break
+        end
+        % otherwise, increment regularizer
+        %reg = reg * 2;
 
-    if strcmp(mode, 'ddp')
-        % Get second-order derivatives of Q
-        Qxx = cxx + (fx.')*Vxx_next*fx + fake_tensor_prod(Vx_next,fxx) + regularizer*(fx.')*fx;
-        Qxu = cxu + (fx.')*Vxx_next*fu + fake_tensor_prod(Vx_next,fxu) + regularizer*(fx.')*(fu);
-        Qux = Qxu.';
-        Quu = cuu + (fu.')*Vxx_next*fu + fake_tensor_prod(Vx_next,fuu) + regularizer*(fu.')*(fu);
-
-    elseif strcmp(mode,'ilqr')
-        % Get second-order derivatives of Q without tensor prod
-        Qxx = cxx + (fx.')*Vxx_next*fx;
-        Qxu = cxu + (fx.')*Vxx_next*fu;
-        Qux = Qxu.';
-        Quu = cuu + (fu.')*Vxx_next*fu;
-    end
+    end % regularizer update loop
 
     % Get new controls
-    K = -inv(Quu)*Qux;
-    k = -inv(Quu)*Qu;
+
+    K = -Quu \ Qux;
+    k = -Quu \ Qu;
 
     % Get new value function
-    Vx_next = Qx - (K.')*Quu*k;
-    Vxx_next = Qxx - (K.')*Quu*K;
+	Vx_next = Qx + K.'*Quu*k + K.'*Qu + Qux.'*k;
+	Vxx_next = Qxx + K.'*Quu*K + K.'*Qux + Qux.'*K;
+
+    % Decrease mu for next iteration if successful
+    delta = min(1 / delta_0, delta / delta_0);
+    if mu * delta > mu_min
+        mu = mu * delta;
+    else
+        mu = 0;
+    end
+
+	%disp(sort(eig(Quu)))
 
     % Assign
     controller.K(t,:,:) = K;
     controller.k(t,:,:) = k;
 end
 
-
-
-
-V = 0; % don't bother defining V
+V = -0.5 * (Qu.' * pinv(Quu) * Qu);  % cost to go
 end
 
 function out = fake_tensor_prod(A,B)
     out = 0;
-    for i = 1:6
+	n = length(A);
+    for i = 1:n
         out = out + squeeze(A(i)*B(i,:,:));
-    end
-    out = out/3;
+	end
+	out = out / 3;
 end
 
 %% Fill in your code below

ddp.m

@@ -1,4 +1,4 @@
-function [controller, total_costs] = ddp(ic, initial_controls, iters, regularizer, dyn, costfn, term_costfn, mode)
+function [controller, total_costs] = ddp(ic, initial_controls, iters, regularizer, dyn, costfn, term_costfn, mode, line_search_iters, xf)
 %DDP Solves for a locally optimal controller using iLQR / DDP.
 %
 %   ic: An n-by-1 state serving as the initial condition for the control
@@ -36,6 +36,11 @@
 %       mode = 'ddp'
 %
 %   The default is to use iLQR.
+% line_search_iters (int): max num of line search iterations. Min 1.
+
+if nargin < 10
+    xf = zeros(12,1);
+end
 
 % Setup variables
 tf = size(initial_controls, 1);
@@ -50,14 +55,27 @@
 
 total_costs = zeros(iters, 1);
 
+% alphas = 1:-1/line_search_iters:0;  % decreasing step size sequence
+alphas = exp(-(0:line_search_iters));
+
 %% Your code below
 
-[states, controls, ~] = fwd_pass(ic, controller, dyn, costfn, term_costfn);
+[states, controls, best_costs] = fwd_pass(ic, controller, dyn, costfn, term_costfn, alphas(1), xf);
 
 for i = 1:iters
-    [controller, ~] = back_pass(states, controls, dyn, costfn, term_costfn, regularizer, mode);
-    [states, controls, costs] = fwd_pass(ic, controller, dyn, costfn, term_costfn);
-    total_costs(i,1) = norm(costs); %maybe sum(costs)
+    [controller, ~] = back_pass(states, controls, dyn, costfn, term_costfn, regularizer, mode, xf);
+    %disp(sum(best_costs))
+    for j = 1:line_search_iters-1
+        % line search logic
+        [states, controls, new_costs] = fwd_pass(ic, controller, dyn, costfn, term_costfn, alphas(j), xf);
+        if sum(new_costs) < sum(best_costs)
+            best_costs = new_costs;
+        end
+	end
+
+    % update to a newer controller
+    [controller, ~] = back_pass(states, controls, dyn, costfn, term_costfn, regularizer, mode, xf);
+    total_costs(i,1) = sum(best_costs); 
 end
 
 end