From 45e3148b38a8d0cb5138d57449b93636fe90ec89 Mon Sep 17 00:00:00 2001
From: Akshay Kumar <33128582+Manbeast-95@users.noreply.github.com>
Date: Thu, 9 Nov 2017 17:50:26 -0500
Subject: [PATCH 01/12] Add files via upload

Implemented Direct Collocation Algorithm with MPC
---
 GoatMPCDC.m | 127 ++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 127 insertions(+)
 create mode 100644 GoatMPCDC.m

diff --git a/GoatMPCDC.m b/GoatMPCDC.m
new file mode 100644
index 0000000..f6f21c9
--- /dev/null
+++ b/GoatMPCDC.m
@@ -0,0 +1,127 @@
+clear;
+clc;
+
+addpath('lib')
+
+%Sample Time
+Ts = 0.001;
+
+%Prediction Horizon
+N = 10;
+
+global global_link_length
+global_link_length = [1.0 0.5 0.2];
+
+%Initial State
+q20 = [1.0781, 1.0701, 0.9933]';
+dq20 = [0, 0, 0]';
+q10 = [2.0634, -1.0701, -0.9933]';
+x = [q20; dq20; q10] + [+0.12; 0.35; -0.76; 0.43; -0.8; 0.65; 0 ; 0; 0];
+%x = 2*[1;1;1;1;1;1;0;0;0];
+%Final State
+xf = [q20; dq20];
+
+%Initial Optimized Input Values over the trajectory
+optimal_param = zeros(11,N);
+
+%Fmincon Options
+options = optimoptions(@fmincon,'TolFun',0.001,'MaxIter',100,'MaxFunEvals',1000,...
+                       'DiffMinChange',0.001,'Display','iter','Algorithm','sqp');
+                   
+%Parameter Bounds
+lb_input = -10;
+ub_input = 10;
+LB = [-Inf;-Inf;-Inf;-Inf;-Inf;-Inf;-Inf;-Inf;-Inf;lb_input;lb_input];
+UB = [Inf;Inf;Inf;Inf;Inf;Inf;Inf;Inf;Inf;ub_input;ub_input];
+for i = 2:1:N
+    LB = [LB [-Inf;-Inf;-Inf;-Inf;-Inf;-Inf;-Inf;-Inf;-Inf;lb_input;lb_input]];
+    UB = [UB [Inf;Inf;Inf;Inf;Inf;Inf;Inf;Inf;Inf;ub_input;ub_input]];
+end
+%Store Data
+xHist = zeros(length(x),N);
+%Solving
+for ct = 1:N
+    ct
+    xHist(:,ct) = x;
+    costfun = @(p) cost(p, xf, N, optimal_param(10:11,1), Ts);
+    constrfun = @(p) constraint(p,xf,Ts,N);
+    optimal_param = fmincon(costfun,optimal_param,[],[],[],[],LB,UB,constrfun,options);
+
+    [x, y] = goatDynamicsDT(x, optimal_param(10:11,1), Ts); 
+%     x = [(x(1:6) + dx*ts); x10];
+end
+
+
+% %Plot
+% x0 = x;
+% xHist = x;
+% for i = 1:N
+%     uk = optimal_input(:,i);
+%     xint = goatDynamicsDT(x0,uk,Ts);
+%     xHist = [xHist xint];
+%     x0 = xint;
+% end
+
+
+
+%Cost Function
+function J = cost(p, xref, N, u0, Ts)
+
+    Q = [10 0 0 0 0 0;...
+            0 10 0 0 0 0;...
+            0 0 10 0 0 0;...
+            0 0 0 1 0 0;...
+            0 0 0 0 1 0;...
+            0 0 0 0 0 1];
+        Q = 10*eye(6);
+    R = 0.01*eye(2);
+
+    J = 0;
+    xk1 = p(1:9,1);
+
+
+    for i = 1:N
+        uk = p(10:11,i);
+        xk1 = goatDynamicsDT(xk1, uk, Ts)
+        xkd = xk1(1:6);
+        
+        J = J + (xkd-xref)'*Q*(xkd-xref);
+
+        if i ==1
+            J = J + (uk-u0)' * R * (uk-u0);
+        else
+            J = J + (uk-p(10:11,i-1))' * R * (uk-p(10:11,i-1));
+        end
+    end  
+%     if(norm(xkd-xf)>0.001)
+%         J = NaN;
+%     end
+    
+end
+
+%Constraint Function
+function [c,ceq] = constraint(p, xf, Ts, N)
+c = [];
+% ceq = [];
+
+for tk = 0:1:N-2  
+    
+    xk = p(1:9,tk+1);
+    uk = p(10:11,tk+1);
+    xdotk = goatDynamicsCT(xk,uk);
+    
+    xk1 = p(1:9,tk+2);
+    uk1 = p(10:11,tk+2);
+    xdotk1 = goatDynamicsCT(xk1,uk1);
+    
+    xkc = (xk+xk1)/2 + Ts*(xdotk - xdotk1)/8;
+    ukc = (uk+uk1)/2;
+    xdotkc = goatDynamicsCT(xkc,ukc);
+    %Defect
+    delk = (xk - xk1) + Ts*(xdotk+4*xdotkc+xdotk1)/6;
+    ceq = delk(1:6);
+end    
+    %Constrain Final point
+    ceq = [ceq p(1:6,N)-xf(1:6)];
+
+end
\ No newline at end of file

From fda35d4f78ddccfe7a5786745cf002fd58eb4b73 Mon Sep 17 00:00:00 2001
From: Akshay Kumar <33128582+Manbeast-95@users.noreply.github.com>
Date: Thu, 9 Nov 2017 17:56:18 -0500
Subject: [PATCH 02/12] Delete GoatMPCDC.m

---
 GoatMPCDC.m | 127 ----------------------------------------------------
 1 file changed, 127 deletions(-)
 delete mode 100644 GoatMPCDC.m

diff --git a/GoatMPCDC.m b/GoatMPCDC.m
deleted file mode 100644
index f6f21c9..0000000
--- a/GoatMPCDC.m
+++ /dev/null
@@ -1,127 +0,0 @@
-clear;
-clc;
-
-addpath('lib')
-
-%Sample Time
-Ts = 0.001;
-
-%Prediction Horizon
-N = 10;
-
-global global_link_length
-global_link_length = [1.0 0.5 0.2];
-
-%Initial State
-q20 = [1.0781, 1.0701, 0.9933]';
-dq20 = [0, 0, 0]';
-q10 = [2.0634, -1.0701, -0.9933]';
-x = [q20; dq20; q10] + [+0.12; 0.35; -0.76; 0.43; -0.8; 0.65; 0 ; 0; 0];
-%x = 2*[1;1;1;1;1;1;0;0;0];
-%Final State
-xf = [q20; dq20];
-
-%Initial Optimized Input Values over the trajectory
-optimal_param = zeros(11,N);
-
-%Fmincon Options
-options = optimoptions(@fmincon,'TolFun',0.001,'MaxIter',100,'MaxFunEvals',1000,...
-                       'DiffMinChange',0.001,'Display','iter','Algorithm','sqp');
-                   
-%Parameter Bounds
-lb_input = -10;
-ub_input = 10;
-LB = [-Inf;-Inf;-Inf;-Inf;-Inf;-Inf;-Inf;-Inf;-Inf;lb_input;lb_input];
-UB = [Inf;Inf;Inf;Inf;Inf;Inf;Inf;Inf;Inf;ub_input;ub_input];
-for i = 2:1:N
-    LB = [LB [-Inf;-Inf;-Inf;-Inf;-Inf;-Inf;-Inf;-Inf;-Inf;lb_input;lb_input]];
-    UB = [UB [Inf;Inf;Inf;Inf;Inf;Inf;Inf;Inf;Inf;ub_input;ub_input]];
-end
-%Store Data
-xHist = zeros(length(x),N);
-%Solving
-for ct = 1:N
-    ct
-    xHist(:,ct) = x;
-    costfun = @(p) cost(p, xf, N, optimal_param(10:11,1), Ts);
-    constrfun = @(p) constraint(p,xf,Ts,N);
-    optimal_param = fmincon(costfun,optimal_param,[],[],[],[],LB,UB,constrfun,options);
-
-    [x, y] = goatDynamicsDT(x, optimal_param(10:11,1), Ts); 
-%     x = [(x(1:6) + dx*ts); x10];
-end
-
-
-% %Plot
-% x0 = x;
-% xHist = x;
-% for i = 1:N
-%     uk = optimal_input(:,i);
-%     xint = goatDynamicsDT(x0,uk,Ts);
-%     xHist = [xHist xint];
-%     x0 = xint;
-% end
-
-
-
-%Cost Function
-function J = cost(p, xref, N, u0, Ts)
-
-    Q = [10 0 0 0 0 0;...
-            0 10 0 0 0 0;...
-            0 0 10 0 0 0;...
-            0 0 0 1 0 0;...
-            0 0 0 0 1 0;...
-            0 0 0 0 0 1];
-        Q = 10*eye(6);
-    R = 0.01*eye(2);
-
-    J = 0;
-    xk1 = p(1:9,1);
-
-
-    for i = 1:N
-        uk = p(10:11,i);
-        xk1 = goatDynamicsDT(xk1, uk, Ts)
-        xkd = xk1(1:6);
-        
-        J = J + (xkd-xref)'*Q*(xkd-xref);
-
-        if i ==1
-            J = J + (uk-u0)' * R * (uk-u0);
-        else
-            J = J + (uk-p(10:11,i-1))' * R * (uk-p(10:11,i-1));
-        end
-    end  
-%     if(norm(xkd-xf)>0.001)
-%         J = NaN;
-%     end
-    
-end
-
-%Constraint Function
-function [c,ceq] = constraint(p, xf, Ts, N)
-c = [];
-% ceq = [];
-
-for tk = 0:1:N-2  
-    
-    xk = p(1:9,tk+1);
-    uk = p(10:11,tk+1);
-    xdotk = goatDynamicsCT(xk,uk);
-    
-    xk1 = p(1:9,tk+2);
-    uk1 = p(10:11,tk+2);
-    xdotk1 = goatDynamicsCT(xk1,uk1);
-    
-    xkc = (xk+xk1)/2 + Ts*(xdotk - xdotk1)/8;
-    ukc = (uk+uk1)/2;
-    xdotkc = goatDynamicsCT(xkc,ukc);
-    %Defect
-    delk = (xk - xk1) + Ts*(xdotk+4*xdotkc+xdotk1)/6;
-    ceq = delk(1:6);
-end    
-    %Constrain Final point
-    ceq = [ceq p(1:6,N)-xf(1:6)];
-
-end
\ No newline at end of file

From abcf2ca8536f627325336d1632740e4508d43d54 Mon Sep 17 00:00:00 2001
From: Akshay Kumar <33128582+Manbeast-95@users.noreply.github.com>
Date: Thu, 9 Nov 2017 17:57:24 -0500
Subject: [PATCH 03/12] Implemented Direct Collocation algorithm with MPC

This code still uses fmincon to find feasible configuration.
---
 goat2D_mpc/GoatMPCDC.m | 127 +++++++++++++++++++++++++++++++++++++++++
 1 file changed, 127 insertions(+)
 create mode 100644 goat2D_mpc/GoatMPCDC.m

diff --git a/goat2D_mpc/GoatMPCDC.m b/goat2D_mpc/GoatMPCDC.m
new file mode 100644
index 0000000..f6f21c9
--- /dev/null
+++ b/goat2D_mpc/GoatMPCDC.m
@@ -0,0 +1,127 @@
+clear;
+clc;
+
+addpath('lib')
+
+%Sample Time
+Ts = 0.001;
+
+%Prediction Horizon
+N = 10;
+
+global global_link_length
+global_link_length = [1.0 0.5 0.2];
+
+%Initial State
+q20 = [1.0781, 1.0701, 0.9933]';
+dq20 = [0, 0, 0]';
+q10 = [2.0634, -1.0701, -0.9933]';
+x = [q20; dq20; q10] + [+0.12; 0.35; -0.76; 0.43; -0.8; 0.65; 0 ; 0; 0];
+%x = 2*[1;1;1;1;1;1;0;0;0];
+%Final State
+xf = [q20; dq20];
+
+%Initial Optimized Input Values over the trajectory
+optimal_param = zeros(11,N);
+
+%Fmincon Options
+options = optimoptions(@fmincon,'TolFun',0.001,'MaxIter',100,'MaxFunEvals',1000,...
+                       'DiffMinChange',0.001,'Display','iter','Algorithm','sqp');
+                   
+%Parameter Bounds
+lb_input = -10;
+ub_input = 10;
+LB = [-Inf;-Inf;-Inf;-Inf;-Inf;-Inf;-Inf;-Inf;-Inf;lb_input;lb_input];
+UB = [Inf;Inf;Inf;Inf;Inf;Inf;Inf;Inf;Inf;ub_input;ub_input];
+for i = 2:1:N
+    LB = [LB [-Inf;-Inf;-Inf;-Inf;-Inf;-Inf;-Inf;-Inf;-Inf;lb_input;lb_input]];
+    UB = [UB [Inf;Inf;Inf;Inf;Inf;Inf;Inf;Inf;Inf;ub_input;ub_input]];
+end
+%Store Data
+xHist = zeros(length(x),N);
+%Solving
+for ct = 1:N
+    ct
+    xHist(:,ct) = x;
+    costfun = @(p) cost(p, xf, N, optimal_param(10:11,1), Ts);
+    constrfun = @(p) constraint(p,xf,Ts,N);
+    optimal_param = fmincon(costfun,optimal_param,[],[],[],[],LB,UB,constrfun,options);
+
+    [x, y] = goatDynamicsDT(x, optimal_param(10:11,1), Ts); 
+%     x = [(x(1:6) + dx*ts); x10];
+end
+
+
+% %Plot
+% x0 = x;
+% xHist = x;
+% for i = 1:N
+%     uk = optimal_input(:,i);
+%     xint = goatDynamicsDT(x0,uk,Ts);
+%     xHist = [xHist xint];
+%     x0 = xint;
+% end
+
+
+
+%Cost Function
+function J = cost(p, xref, N, u0, Ts)
+
+    Q = [10 0 0 0 0 0;...
+            0 10 0 0 0 0;...
+            0 0 10 0 0 0;...
+            0 0 0 1 0 0;...
+            0 0 0 0 1 0;...
+            0 0 0 0 0 1];
+        Q = 10*eye(6);
+    R = 0.01*eye(2);
+
+    J = 0;
+    xk1 = p(1:9,1);
+
+
+    for i = 1:N
+        uk = p(10:11,i);
+        xk1 = goatDynamicsDT(xk1, uk, Ts)
+        xkd = xk1(1:6);
+        
+        J = J + (xkd-xref)'*Q*(xkd-xref);
+
+        if i ==1
+            J = J + (uk-u0)' * R * (uk-u0);
+        else
+            J = J + (uk-p(10:11,i-1))' * R * (uk-p(10:11,i-1));
+        end
+    end  
+%     if(norm(xkd-xf)>0.001)
+%         J = NaN;
+%     end
+    
+end
+
+%Constraint Function
+function [c,ceq] = constraint(p, xf, Ts, N)
+c = [];
+% ceq = [];
+
+for tk = 0:1:N-2  
+    
+    xk = p(1:9,tk+1);
+    uk = p(10:11,tk+1);
+    xdotk = goatDynamicsCT(xk,uk);
+    
+    xk1 = p(1:9,tk+2);
+    uk1 = p(10:11,tk+2);
+    xdotk1 = goatDynamicsCT(xk1,uk1);
+    
+    xkc = (xk+xk1)/2 + Ts*(xdotk - xdotk1)/8;
+    ukc = (uk+uk1)/2;
+    xdotkc = goatDynamicsCT(xkc,ukc);
+    %Defect
+    delk = (xk - xk1) + Ts*(xdotk+4*xdotkc+xdotk1)/6;
+    ceq = delk(1:6);
+end    
+    %Constrain Final point
+    ceq = [ceq p(1:6,N)-xf(1:6)];
+
+end
\ No newline at end of file

From 3108919c6e82286c2c8174892a12b44e3330961a Mon Sep 17 00:00:00 2001
From: Akshay Kumar <33128582+Manbeast-95@users.noreply.github.com>
Date: Fri, 10 Nov 2017 10:01:18 -0500
Subject: [PATCH 04/12] Direct Collocation Updated - Working

---
 goat2D_mpc/GoatMPCDC.m | 173 ++++++++++++++++++++++-------------------
 1 file changed, 91 insertions(+), 82 deletions(-)

diff --git a/goat2D_mpc/GoatMPCDC.m b/goat2D_mpc/GoatMPCDC.m
index f6f21c9..84d2869 100644
--- a/goat2D_mpc/GoatMPCDC.m
+++ b/goat2D_mpc/GoatMPCDC.m
@@ -1,71 +1,107 @@
 clear;
 clc;
-
+profile on
 addpath('lib')
 
 %Sample Time
 Ts = 0.001;
+params.Ts = Ts;
 
 %Prediction Horizon
-N = 10;
-
-global global_link_length
-global_link_length = [1.0 0.5 0.2];
+N = 30;
+params.N = N;
+link_length = [1.0 0.5 0.2];
 
 %Initial State
 q20 = [1.0781, 1.0701, 0.9933]';
 dq20 = [0, 0, 0]';
 q10 = [2.0634, -1.0701, -0.9933]';
-x = [q20; dq20; q10] + [+0.12; 0.35; -0.76; 0.43; -0.8; 0.65; 0 ; 0; 0];
-%x = 2*[1;1;1;1;1;1;0;0;0];
+%x = [q20; dq20] + [0.12; 0.56; -0.58; 0; 0; 0];
+x = [q20; dq20] + [0.12; 0.56; -0.58; 1; 1; 1];
+
 %Final State
 xf = [q20; dq20];
 
 %Initial Optimized Input Values over the trajectory
-optimal_param = zeros(11,N);
+optimal_parameters = zeros(8,N);
+p0 = zeros(8,N);
 
 %Fmincon Options
-options = optimoptions(@fmincon,'TolFun',0.001,'MaxIter',100,'MaxFunEvals',1000,...
-                       'DiffMinChange',0.001,'Display','iter','Algorithm','sqp');
+options = optimoptions(@fmincon,'StepTolerance',1e-15,'TolFun',1e-8,'MaxIter',10000,'MaxFunEvals',10000,...
+                       'DiffMinChange',1e-3,'Display','iter','Algorithm','sqp');
                    
-%Parameter Bounds
-lb_input = -10;
-ub_input = 10;
-LB = [-Inf;-Inf;-Inf;-Inf;-Inf;-Inf;-Inf;-Inf;-Inf;lb_input;lb_input];
-UB = [Inf;Inf;Inf;Inf;Inf;Inf;Inf;Inf;Inf;ub_input;ub_input];
-for i = 2:1:N
-    LB = [LB [-Inf;-Inf;-Inf;-Inf;-Inf;-Inf;-Inf;-Inf;-Inf;lb_input;lb_input]];
-    UB = [UB [Inf;Inf;Inf;Inf;Inf;Inf;Inf;Inf;Inf;ub_input;ub_input]];
+%Input Bounds
+lb_input = -100;
+ub_input = 100;
+LB = [-Inf;-Inf;-Inf;-Inf;-Inf;-Inf;lb_input;lb_input];
+UB = [Inf;Inf;Inf;Inf;Inf;Inf;ub_input;ub_input];
+for i = 4:3:3*N
+    LB = [LB [-Inf;-Inf;-Inf;-Inf;-Inf;-Inf;lb_input;lb_input]];
+    UB = [UB [Inf;Inf;Inf;Inf;Inf;Inf;ub_input;ub_input]];
 end
+
 %Store Data
 xHist = zeros(length(x),N);
-%Solving
-for ct = 1:N
-    ct
-    xHist(:,ct) = x;
-    costfun = @(p) cost(p, xf, N, optimal_param(10:11,1), Ts);
-    constrfun = @(p) constraint(p,xf,Ts,N);
-    optimal_param = fmincon(costfun,optimal_param,[],[],[],[],LB,UB,constrfun,options);
-
-    [x, y] = goatDynamicsDT(x, optimal_param(10:11,1), Ts); 
-%     x = [(x(1:6) + dx*ts); x10];
-end
 
-
-% %Plot
-% x0 = x;
-% xHist = x;
-% for i = 1:N
-%     uk = optimal_input(:,i);
-%     xint = goatDynamicsDT(x0,uk,Ts);
-%     xHist = [xHist xint];
-%     x0 = xint;
+%Solving
+% for ct = 1:N
+%     sprintf('iteration #: %d', ct)
+%     xHist(:,ct) = x;
+    COSTFUN = @(p) cost(p, x, xf, N, optimal_parameters(7:8,1), Ts, link_length);
+    CONSTFUN = @(p) goatConstraintFCNDC(p,x,xf,Ts,N, link_length);
+    optimal_parameters = fmincon(COSTFUN,p0,[],[],[],[],LB,UB,CONSTFUN,options);
+
+%     [x, y] = goatDynamicsDT(x, optimal_parameters(7:8,1), Ts, link_length); 
+% %     x = [(x(1:6) + dx*ts); x10];
 % end
-
-
-
-%Cost Function
-function J = cost(p, xref, N, u0, Ts)
+%save('results', 'xHist', 'optimal_input');
+%%
+t = linspace(0,params.Ts*params.N, params.N);
+figure();
+subplot(2,3,1);
+hold on;
+plot(t, xf(1)*ones(1,params.N), 'k--');
+plot(t,optimal_parameters(1,:),'b');
+hold off;
+
+subplot(2,3,2);
+hold on;
+plot(t, xf(2)*ones(1,params.N), 'k--');
+plot(t,optimal_parameters(2,:),'b');
+hold off;
+
+subplot(2,3,3);
+hold on;
+plot(t, xf(3)*ones(1,params.N), 'k--');
+plot(t,optimal_parameters(3,:),'b');
+hold off;
+
+
+subplot(2,3,4);
+hold on;
+plot(t, xf(4)*ones(1,params.N), 'k--');
+plot(t,optimal_parameters(4,:),'b');
+hold off;
+
+subplot(2,3,5);
+hold on;
+plot(t, xf(5)*ones(1,params.N), 'k--');
+plot(t,optimal_parameters(5,:),'b');
+hold off;
+
+subplot(2,3,6);
+hold on;
+plot(t, xf(4)*ones(1,params.N), 'k--');
+plot(t,optimal_parameters(6,:),'b');
+hold off;
+
+figure();
+hold on
+plot(t, optimal_parameters(7,:));
+plot(t, optimal_parameters(8,:));
+
+%% Cost Function
+function J = cost(p, xk, xref, N, u0, Ts, link_length)
 
     Q = [10 0 0 0 0 0;...
             0 10 0 0 0 0;...
@@ -73,55 +109,28 @@
             0 0 0 1 0 0;...
             0 0 0 0 1 0;...
             0 0 0 0 0 1];
-        Q = 10*eye(6);
     R = 0.01*eye(2);
 
     J = 0;
-    xk1 = p(1:9,1);
+    xk1 = xk;
 
 
     for i = 1:N
-        uk = p(10:11,i);
-        xk1 = goatDynamicsDT(xk1, uk, Ts)
-        xkd = xk1(1:6);
-        
-        J = J + (xkd-xref)'*Q*(xkd-xref);
+        uk = p(7:8,i);
+        xk1 = goatDynamicsDT(xk1, uk, Ts, link_length);
+
+%      J = J + (xk1-xref)'*Q*(xk1-xref);
+%      J = J + (uk)' * R * (uk);
+%     end
 
+        
+        
+        J = J + (xk1-xref)'*Q*(xk1-xref);
         if i ==1
             J = J + (uk-u0)' * R * (uk-u0);
         else
-            J = J + (uk-p(10:11,i-1))' * R * (uk-p(10:11,i-1));
+            J = J + (uk-p(7:8,i-1))' * R * (uk-p(7:8,i-1));
         end
-    end  
-%     if(norm(xkd-xf)>0.001)
-%         J = NaN;
-%     end
+    end
     
 end
-
-%Constraint Function
-function [c,ceq] = constraint(p, xf, Ts, N)
-c = [];
-% ceq = [];
-
-for tk = 0:1:N-2  
-    
-    xk = p(1:9,tk+1);
-    uk = p(10:11,tk+1);
-    xdotk = goatDynamicsCT(xk,uk);
-    
-    xk1 = p(1:9,tk+2);
-    uk1 = p(10:11,tk+2);
-    xdotk1 = goatDynamicsCT(xk1,uk1);
-    
-    xkc = (xk+xk1)/2 + Ts*(xdotk - xdotk1)/8;
-    ukc = (uk+uk1)/2;
-    xdotkc = goatDynamicsCT(xkc,ukc);
-    %Defect
-    delk = (xk - xk1) + Ts*(xdotk+4*xdotkc+xdotk1)/6;
-    ceq = delk(1:6);
-end    
-    %Constrain Final point
-    ceq = [ceq p(1:6,N)-xf(1:6)];
-
-end
\ No newline at end of file

From 8e3a96a3b97f1365a7e56b8b51bb429d0c812eb9 Mon Sep 17 00:00:00 2001
From: Akshay Kumar <33128582+Manbeast-95@users.noreply.github.com>
Date: Fri, 10 Nov 2017 10:03:30 -0500
Subject: [PATCH 05/12] Constraint function file for Direct Collocation

---
 goat2D_mpc/GoatMPCDC.m           |  2 +-
 goat2D_mpc/goatConstraintFCNDC.m | 66 ++++++++++++++++++++++++++++++++
 2 files changed, 67 insertions(+), 1 deletion(-)
 create mode 100644 goat2D_mpc/goatConstraintFCNDC.m

diff --git a/goat2D_mpc/GoatMPCDC.m b/goat2D_mpc/GoatMPCDC.m
index 84d2869..b06fa8e 100644
--- a/goat2D_mpc/GoatMPCDC.m
+++ b/goat2D_mpc/GoatMPCDC.m
@@ -133,4 +133,4 @@
         end
     end
     
-end
+end
\ No newline at end of file
diff --git a/goat2D_mpc/goatConstraintFCNDC.m b/goat2D_mpc/goatConstraintFCNDC.m
new file mode 100644
index 0000000..9dbf773
--- /dev/null
+++ b/goat2D_mpc/goatConstraintFCNDC.m
@@ -0,0 +1,66 @@
+function [c, ceq] = goatConstraintFCNDC(p, x, xref, Ts, N, link_length)
+%% Constraint function of nonlinear MPC for pendulum swing-up and balancing control
+%
+% Inputs:
+%   u:      optimization variable, from time k to time k+N-1 
+%   x:      current state at time k
+%   Ts:     controller sample time
+%   N:      prediction horizon
+%
+% Output:
+%   c:      inequality constraints applied across prediction horizon
+%   ceq:    equality constraints (empty)
+%
+% Copyright 2016 The MathWorks, Inc.
+
+%% Nonlinear MPC design parameters
+% Range of cart position: from -10 to 10
+zMin = [0; 0; 0; pi/2; -pi/2; -pi/2];
+zMax = [pi/2; pi; pi/2; pi; 0; 0];
+
+%% Inequality constraints calculation
+c = [];
+% c = zeros(12,N);
+% % Apply 2*N cart position constraints across prediction horizon, from time
+% % k+1 to k+N
+% xk = x;
+% uk = u(:,1);
+% for ct=1:N
+%     xk1 = goatDynamicsDT(xk, uk, Ts, link_length);
+%     xk2 = findFeasibleConfigurationAnalytical(xk1, link_length);
+% %     c(1:6,ct) = -[xk1(1:3);xk2] + zMin;
+% %     c(7:12,ct) =  [xk1(1:3);xk2] - zMax;
+% %     c(1:6,ct) = -[xk1(1:3);xk1(7:9)] + zMin;
+% %     c(7:12,ct) =  [xk1(1:3);xk1(7:9)] - zMax;
+% %     
+%      % update plant state and input for next step
+%     xk = xk1;
+%     
+%     if ct<N
+%         uk = u(:,ct+1);
+%     end
+% end
+%% Equality constraints
+%Constarian Initial Point
+ceq = p(1:6,1) - x;
+for tk = 1:N-1   
+    
+    xk = [p(1:6,tk)];
+    uk = p(7:8,tk);
+    xdotk = goatDynamicsCT(xk,uk,link_length);
+    
+    xk1 = [p(1:6,tk+1)];
+    uk1 = p(7:8,tk+1);
+    xdotk1 = goatDynamicsCT(xk1,uk1,link_length);
+    
+    xkc = (xk+xk1)/2 + Ts*(xdotk - xdotk1)/8;
+    ukc = (uk+uk1)/2;
+    xdotkc = goatDynamicsCT(xkc,ukc,link_length);
+    %Defect
+    delk = (xk - xk1) + Ts*(xdotk+4*xdotkc+xdotk1)/6;
+    ceq = [ceq delk];
+end    
+    %Constrain Final point
+    ceq = [ceq [p(1:6,N)-xref]];
+    ceq = real(ceq);
+end

From ebec2243a7b5b8568f4f658952e4b0ac3722a383 Mon Sep 17 00:00:00 2001
From: Manbeast-95 <akv.nitk@gmail.com>
Date: Fri, 10 Nov 2017 11:19:06 -0500
Subject: [PATCH 06/12] Random Test

---
 Readme.txt | 0
 1 file changed, 0 insertions(+), 0 deletions(-)
 create mode 100644 Readme.txt

diff --git a/Readme.txt b/Readme.txt
new file mode 100644
index 0000000..e69de29

From 08c0b835f3da5d04b7f7059dae23e9d686df9ea7 Mon Sep 17 00:00:00 2001
From: Manbeast-95 <akv.nitk@gmail.com>
Date: Fri, 10 Nov 2017 21:26:43 -0500
Subject: [PATCH 07/12] Acrobot Dynamics File

---
 AcrobotDC/acrobotDynamics.m | 50 +++++++++++++++++++++++++++++++++++++
 1 file changed, 50 insertions(+)
 create mode 100644 AcrobotDC/acrobotDynamics.m

diff --git a/AcrobotDC/acrobotDynamics.m b/AcrobotDC/acrobotDynamics.m
new file mode 100644
index 0000000..a4ecce3
--- /dev/null
+++ b/AcrobotDC/acrobotDynamics.m
@@ -0,0 +1,50 @@
+function dz = acrobotDynamics(z,u)
+% dz = acrobotDynamics(z,u,p)
+%
+% This function computes the dynamics of the acrobot: double pendulum, two
+% point masses, torque motor between links, no friction.
+%
+% INPUTS:
+%   z = [4,1] = state vector
+%   u = [1,1] = input torque
+%   p = parameter struct:
+%       .m1 = elbow mass
+%       .m2 = wrist mass
+%       .g = gravitational acceleration
+%       .l1 = length shoulder to elbow
+%       .l2 = length elbow to wrist
+%
+% OUTPUTS:
+%   dz = [4,1] = dz/dt = time derivative of the state
+% 
+% NOTES:
+%   
+%   states:
+%       1 = q1 = first link angle
+%       2 = q2 = second link angle
+%       3 = dq1 = first link angular rate
+%       4 = dq2 = second link angular rate
+%
+%   angles: measured from negative j axis with positive convention
+%
+
+q1 = z(1,:);
+q2 = z(2,:);
+dq1 = z(3,:);
+dq2 = z(4,:);
+
+% Vectorized call to the dynamics
+[M11,M12,M21,M22,f1,f2] = autoGen_acrobotDynamics(...
+    q1, q2, dq1, dq2, u,...
+    1, 1, 9.81, 1, 1);
+
+% Numerically invert the mass matrix at each time step
+nTime = length(q1);
+ddq = zeros(2, nTime);
+for i=1:nTime
+    ddq(:,i) = [M11(i), M12(i); M21(i), M22(i)] \ [f1(i); f2(i)]; 
+end
+
+dz = [dq1;dq2;ddq];
+
+end
\ No newline at end of file

From a41802681ce27ef946b9d6fe690e5264f9eddf8a Mon Sep 17 00:00:00 2001
From: Manbeast-95 <akv.nitk@gmail.com>
Date: Fri, 10 Nov 2017 21:28:10 -0500
Subject: [PATCH 08/12] Dynamics Generation File

---
 AcrobotDC/autoGen_acrobotDynamics.m | 48 +++++++++++++++++++++++++++++
 1 file changed, 48 insertions(+)
 create mode 100644 AcrobotDC/autoGen_acrobotDynamics.m

diff --git a/AcrobotDC/autoGen_acrobotDynamics.m b/AcrobotDC/autoGen_acrobotDynamics.m
new file mode 100644
index 0000000..b2c0438
--- /dev/null
+++ b/AcrobotDC/autoGen_acrobotDynamics.m
@@ -0,0 +1,48 @@
+function [M11,M12,M21,M22,f1,f2] = autoGen_acrobotDynamics(q1,q2,dq1,dq2,u,m1,m2,g,l1,l2)
+%AUTOGEN_ACROBOTDYNAMICS
+%    [M11,M12,M21,M22,F1,F2] = AUTOGEN_ACROBOTDYNAMICS(Q1,Q2,DQ1,DQ2,U,M1,M2,G,L1,L2)
+
+%    This function was generated by the Symbolic Math Toolbox version 6.2.
+%    11-Jul-2015 20:41:44
+
+t2 = cos(q1);
+t3 = l1.^2;
+t4 = sin(q1);
+t5 = cos(q2);
+t6 = l1.*t2;
+t7 = l2.*t5;
+t8 = t6+t7;
+t9 = sin(q2);
+t10 = l1.*t4;
+t11 = l2.*t9;
+t12 = t10+t11;
+M11 = -m1.*t2.^2.*t3-m1.*t3.*t4.^2-l1.*m2.*t2.*t8-l1.*m2.*t4.*t12;
+if nargout > 1
+    M12 = -l2.*m2.*t5.*t8-l2.*m2.*t9.*t12;
+end
+if nargout > 2
+    M21 = -l1.*l2.*m2.*t2.*t5-l1.*l2.*m2.*t4.*t9;
+end
+if nargout > 3
+    t13 = l2.^2;
+    M22 = -m2.*t5.^2.*t13-m2.*t9.^2.*t13;
+end
+if nargout > 4
+    t14 = dq1.^2;
+    t15 = dq2.^2;
+    t16 = l1.*t2.*t14;
+    t17 = l2.*t5.*t15;
+    t18 = t16+t17;
+    t19 = l1.*t4.*t14;
+    t20 = l2.*t9.*t15;
+    t21 = t19+t20;
+    t22 = m2.*t12.*t18;
+    t23 = g.*m2.*t12;
+    t24 = g.*l1.*m1.*t4;
+    f1 = t22+t23+t24-m2.*t8.*t21;
+end
+if nargout > 5
+    t25 = l2.*m2.*t9.*t18;
+    t26 = g.*l2.*m2.*t9;
+    f2 = t25+t26-u-l2.*m2.*t5.*t21;
+end

From c365384e020b184d8835142df0c2fc7b045871f7 Mon Sep 17 00:00:00 2001
From: Manbeast-95 <akv.nitk@gmail.com>
Date: Fri, 10 Nov 2017 21:28:36 -0500
Subject: [PATCH 09/12] Direct Allocation Code

---
 AcrobotDC/DCAlgorithmMain.m | 176 ++++++++++++++++++++++++++++++++++++
 1 file changed, 176 insertions(+)
 create mode 100644 AcrobotDC/DCAlgorithmMain.m

diff --git a/AcrobotDC/DCAlgorithmMain.m b/AcrobotDC/DCAlgorithmMain.m
new file mode 100644
index 0000000..8c46558
--- /dev/null
+++ b/AcrobotDC/DCAlgorithmMain.m
@@ -0,0 +1,176 @@
+%Sample Time
+ts = 0.1;
+
+%Prediction Horizon
+N = 50;
+
+%Initial State
+x0 = [0;0;0;0];
+
+%Final State
+xf = [pi;0;0;0];
+
+%Initial Parameter Values over the trajectory
+p0 = zeros(5,N);
+
+%Fmincon Options
+options = optimoptions(@fmincon,'TolFun',1e-6,'MaxIter',10000,'MaxFunEvals',100000,'Display','iter',...
+                       'DiffMinChange',1e-3,'Algorithm','interior-point');
+                   
+%Bounds
+lb_input = -100;
+ub_input = 100;
+LB = [-Inf;-Inf;-Inf;-Inf;lb_input];
+UB = [Inf;Inf;Inf;Inf;ub_input];
+for i = 1:N-1
+    LB = [LB [-Inf;-Inf;-Inf;-Inf;lb_input]];
+    UB = [UB [Inf;Inf;Inf;Inf;ub_input]];
+end
+
+%Store Data
+xHist = x0;
+uHist = 0;
+
+%Solving
+costfun = @(p) cost(p,N,xf,ts);
+constrfun = @(p) constraint(x0,xf,p,ts,N);
+optimal_parameters = fmincon(costfun,p0,[],[],[],[],LB,UB,constrfun,options);
+
+
+%Extracting states
+xHist = optimal_parameters(1:4,:);
+uHist = optimal_parameters(5,:);
+
+figure
+title('Parameters Obtained from Direct Collocation on Discretized Dynamics');
+subplot(2,2,1)
+plot(1:N,xHist(1,:))
+title('X1 State trajectory')
+xlabel('Time(t)')
+ylabel('$\theta_1$','interpreter','latex')
+ylim([-pi 5*pi/4])
+yticks([-pi:pi/4:2*pi])
+yticklabels({'-\pi','-\3*pi/4','-\pi/2','-\pi/4','0','\pi/4','\pi/2','3*\pi/4','\pi','5*\pi/4'})
+grid on
+
+subplot(2,2,2)
+plot(1:N,xHist(2,:))
+title('X2 State trajectory')
+xlabel('Time(t)')
+ylabel('$\theta_2$','interpreter','latex')
+ylim([-pi 5*pi/4])
+yticks([-pi:pi/4:2*pi])
+yticklabels({'-\pi','-3*\pi/4','-\pi/2','-\pi/4','0','\pi/4','\pi/2','3*\pi/4','\pi','5*\pi/4'})
+grid on
+
+subplot(2,2,3)
+plot(1:N,xHist(3,:))
+title('X3 State trajectory')
+xlabel('Time(t)')
+ylabel('$\dot{\theta}_1$','interpreter','latex')
+grid on
+
+subplot(2,2,4)
+plot(1:N,xHist(4,:))
+title('X4 State trajectory')
+xlabel('Time(t)')
+ylabel('$\dot{\theta}_2$','interpreter','latex')
+grid on
+
+figure
+plot(1:N,uHist(1,:))
+title('Control Input')
+xlabel('Time(t)')
+ylabel('$u$','interpreter','latex')
+grid on
+
+xk = x0;
+xHist1 = xk;
+uk = uHist(1,1);
+for i = 1:N
+    xint = DCdynamics(xk,uk,ts);
+        if i<N
+        uk = uHist(1,i+1);
+    end
+    xHist1 = [xHist1 xint];
+    xk = xint;
+end
+
+figure
+for i = 1:N+30
+    
+    if i>N
+        xHist(1,i) = xHist(1,N);
+        xHist(2,i) = xHist(2,N);
+    end
+    hold on
+    cla
+    plot([0 sin(xHist(1,i))] ,[0 -cos(xHist(1,i))],'bs-');
+    plot([sin(xHist(1,i)) (sin(xHist(1,i))+sin(xHist(2,i)+xHist(1,i)))] ,[-cos(xHist(1,i)) (-cos(xHist(1,i))-cos(xHist(2,i)+xHist(1,i)))],'bo-');
+    axis([-2 2 -2 2])
+    title(['Simulation of Cart : Actual Time = ' num2str(i*ts)]);
+    grid on
+    pause(0.2)
+end
+
+
+
+%Cost Function
+function J = cost(p,N,xf,ts)
+J = 0;
+Q = 10*eye(4);
+R = 1;
+uk = p(5,1);
+uk1 = p(5,2);
+xk = p(1:4,1);
+xk1 = p(1:4,2);
+for i = 1:N-1 
+    %Discretized Cost
+    J = J + (xk)'*Q*(xk) + uk'*R*uk;
+    %J = J + (xk'*Q*xk+xk1'*Q*xk1+uk*R*uk+uk1*R*uk1);
+    if i<N-2
+        uk = p(5,i+1);
+        uk1 = p(5,i+2);
+        xk = p(1:4,i+1);
+        xk1 = p(1:4,i+2);
+    end
+end    
+end
+
+%Constraint Function
+function [c,ceq] = constraint(x0,xf,p,ts,N)
+c = [];
+%Constarian Initial Point
+ceq = p(1:4,1) - x0;
+for tk = 1:N-1   
+    
+    xk = p(1:4,tk);
+    uk = p(5,tk);
+    xdotk = acrobotDynamics(xk,uk);
+    
+    xk1 = p(1:4,tk+1);
+    uk1 = p(5,tk+1);
+    xdotk1 = acrobotDynamics(xk,uk);
+    
+    xkc = (xk+xk1)/2 + ts*(xdotk - xdotk1)/8;
+    ukc = (uk+uk1)/2;
+    xdotkc = acrobotDynamics(xkc,ukc);
+    
+    %Defect
+    delk = (xk - xk1) + ts*(xdotk+4*xdotkc+xdotk1)/6;
+    ceq = [ceq delk];
+end    
+    %Constrain Final point
+    ceq = [ceq [p(1:4,N)-xf]];
+
+end
+    
+%System Dynamics
+function xk1 = DCdynamics(x0,u,ts)
+M = 1000;
+delta = ts/M;
+xk1 = x0;
+for ct=1:M
+    xk1 = xk1 + delta*acrobotDynamics(xk1,u);
+end
+end

From cc50a45ed584e0b4635b686df14fd60a91aa0f79 Mon Sep 17 00:00:00 2001
From: Akshay Kumar <33128582+Manbeast-95@users.noreply.github.com>
Date: Sat, 11 Nov 2017 21:23:22 -0500
Subject: [PATCH 10/12] Updated with inequality constraints

---
 goat2D_mpc/goatConstraintFCNDC.m | 73 ++++++++++++++------------------
 1 file changed, 32 insertions(+), 41 deletions(-)

diff --git a/goat2D_mpc/goatConstraintFCNDC.m b/goat2D_mpc/goatConstraintFCNDC.m
index 9dbf773..e001979 100644
--- a/goat2D_mpc/goatConstraintFCNDC.m
+++ b/goat2D_mpc/goatConstraintFCNDC.m
@@ -1,25 +1,38 @@
 function [c, ceq] = goatConstraintFCNDC(p, x, xref, Ts, N, link_length)
-%% Constraint function of nonlinear MPC for pendulum swing-up and balancing control
-%
-% Inputs:
-%   u:      optimization variable, from time k to time k+N-1 
-%   x:      current state at time k
-%   Ts:     controller sample time
-%   N:      prediction horizon
-%
-% Output:
-%   c:      inequality constraints applied across prediction horizon
-%   ceq:    equality constraints (empty)
-%
-% Copyright 2016 The MathWorks, Inc.
-
 %% Nonlinear MPC design parameters
-% Range of cart position: from -10 to 10
-zMin = [0; 0; 0; pi/2; -pi/2; -pi/2];
-zMax = [pi/2; pi; pi/2; pi; 0; 0];
+% Range of theta posn
+zMin = [0; 0; 0];
+zMax = [pi/2; pi; pi/2];
+c = [];
+%% Equality constraints
+%Constarian Initial Point
+ceq = p(1:6,1) - x;
+for tk = 1:N-1   
+    
+    xk = [p(1:6,tk)];
+    uk = p(7:8,tk);
+    xdotk = (goatDynamicsCT(xk,uk,link_length));
+    
+    xk1 = [p(1:6,tk+1)];
+    uk1 = p(7:8,tk+1);
+    xdotk1 = (goatDynamicsCT(xk1,uk1,link_length));
+    ineq = [-xk1(1:3)+zMin;xk1(1:3)-zMax];
+    c = [c ineq];
+    
+    xkc = (xk+xk1)/2 + Ts*(xdotk - xdotk1)/8;
+    ukc = (uk+uk1)/2;
+    xdotkc = (goatDynamicsCT(xkc,ukc,link_length));
+    %Defect
+    delk = (xk - xk1) + Ts*(xdotk+4*xdotkc+xdotk1)/6;
+    ceq = [ceq delk];
+end    
+    %Constrain Final point
+    ceq = [ceq [p(1:6,N)-xref]];
+    ceq = real(ceq);
+
+
 
 %% Inequality constraints calculation
-c = [];
 % c = zeros(12,N);
 % % Apply 2*N cart position constraints across prediction horizon, from time
 % % k+1 to k+N
@@ -40,27 +53,5 @@
 %         uk = u(:,ct+1);
 %     end
 % end
-%% Equality constraints
-%Constarian Initial Point
-ceq = p(1:6,1) - x;
-for tk = 1:N-1   
-    
-    xk = [p(1:6,tk)];
-    uk = p(7:8,tk);
-    xdotk = goatDynamicsCT(xk,uk,link_length);
-    
-    xk1 = [p(1:6,tk+1)];
-    uk1 = p(7:8,tk+1);
-    xdotk1 = goatDynamicsCT(xk1,uk1,link_length);
-    
-    xkc = (xk+xk1)/2 + Ts*(xdotk - xdotk1)/8;
-    ukc = (uk+uk1)/2;
-    xdotkc = goatDynamicsCT(xkc,ukc,link_length);
-    %Defect
-    delk = (xk - xk1) + Ts*(xdotk+4*xdotkc+xdotk1)/6;
-    ceq = [ceq delk];
-end    
-    %Constrain Final point
-    ceq = [ceq [p(1:6,N)-xref]];
-    ceq = real(ceq);
+
 end

From 268c153675ff58ffdf59f100f546154b20005d68 Mon Sep 17 00:00:00 2001
From: Ashwin Khadke <ashwin@Ashwins-MacBook-Pro.local>
Date: Sat, 18 Nov 2017 13:30:11 -0500
Subject: [PATCH 11/12] Changed bounds for states. Still converges to an
 infeasible point

---
 goat2D_mpc/.DS_Store             | Bin 0 -> 8196 bytes
 goat2D_mpc/GoatMPCDC.m           |  35 ++++++++++++++++++-------------
 goat2D_mpc/goatConstraintFCNDC.m |   2 +-
 goat2D_mpc/goatDynamicsCT.m      |   3 +++
 4 files changed, 25 insertions(+), 15 deletions(-)
 create mode 100644 goat2D_mpc/.DS_Store

diff --git a/goat2D_mpc/.DS_Store b/goat2D_mpc/.DS_Store
new file mode 100644
index 0000000000000000000000000000000000000000..67cff6f3cabd79334b5e88bd7e3321ed5f6e9f10
GIT binary patch
literal 8196
zcmeHM(M}UV6unbW3#c?f0*TS=3yBYeP!xGECZ(+?Q4Ct54{FM8TcL5;ZQ3mj)RMmV
z59%NIDL(n=hxh?{?#x)XyGv|*Fh*v`%$@Gub7$tvy|a54A`<y_HA^&4L}^r(nJF|0
zh3|8Xm6X0R3n_pnIwYF{s*_J%Oxs*A3>XFs1BL;^fMMWYU;xi-F6mRA`|7S~4FiUO
zGsys-4;CuRoR)nh<<WsgrT~!RxGW3$$O9zC(lV!IUr7mtO_e<e4HY`Y5DJd_7OO+%
zwCpRX;3O2BgdSPw3`NM%fwQDKiJX$AH4GRA;tX)^ex0gRqbBn9aQ<GV3i9|S{%Yt2
zR7TH-{FUJ^A<K6*UBBYEz5_cPdUsH7e3u11qDk03f}TwWuwS7TEzv;)^C^P4O0S{o
z0l9&(ZD8u;kc)mnep%qVd(_5!!gGmO6>|}*3x8mBUC!La3izu=1>?E#`HOdvk!COw
zGuMHB6RUVAS5pH6YQBjqh;I=q*G8~1()?0(#PST-^W|I=pDnD8xn#}*a9*It@l~e2
zM6Aq?V5JCK@%?5Wc^~qT%)nAH;wuH(Nx-wejD{~J;iYGU@e_1k^RY$SNi&u^97aq<
z5Q*!QI~qn^Oh(bka6B+hq(l6S!RrtK!&-7z!`5e|mM0^IWBFKn8-I&qKdI6qdO}a*
ze9E1TTJpij8^l;cM!br?4e8Y@RHQBR_{?yJ=fu%JH1kPlu44}#PzeZ**b3Dm80_Zn
zF;1t|(5K92r{UIH!SeD~ks2F6cm6_}%caY&O8b7N6xcz#mA9KO<ga?Q=IH*8@4JJ(
z?L4d3I_1ojg6{=(-K#Z2>Mx(yy{g~I`)#k<l6JQ3U?12CF6GR8x0_vETeNQ7?k!r~
z?DER$qP2E2+v}y%a|_wKo4W@`$KAKRlMirH6rtZ(Xj=Uqv>(w+B<Qfz^jzQDW3bd|
zU~6rJhJ&_(ZHa{xyYvu=(>OGJpboJ~(V&W>aexh>2U9ei|Cxa?wYYiC|L>jt{r`Wa
zz^u(MaON2h<Hb@j4;p?xsiKT<u5F?2qH<xqzLFAxMyBJCG98B;{KF7^3#yFCY1vm2
cS5W@?hXA}K?3m|&IBDb8Tl4&v-gtJJA2RLRb^rhX

literal 0
HcmV?d00001

diff --git a/goat2D_mpc/GoatMPCDC.m b/goat2D_mpc/GoatMPCDC.m
index b06fa8e..6af3a46 100644
--- a/goat2D_mpc/GoatMPCDC.m
+++ b/goat2D_mpc/GoatMPCDC.m
@@ -1,5 +1,6 @@
 clear;
 clc;
+close all;
 profile on
 addpath('lib')
 
@@ -8,7 +9,7 @@
 params.Ts = Ts;
 
 %Prediction Horizon
-N = 30;
+N = 10;
 params.N = N;
 link_length = [1.0 0.5 0.2];
 
@@ -17,7 +18,7 @@
 dq20 = [0, 0, 0]';
 q10 = [2.0634, -1.0701, -0.9933]';
 %x = [q20; dq20] + [0.12; 0.56; -0.58; 0; 0; 0];
-x = [q20; dq20] + [0.12; 0.56; -0.58; 1; 1; 1];
+x = [q20; dq20] + [0.012; 0; 0; 0; 0; 0];
 
 %Final State
 xf = [q20; dq20];
@@ -25,19 +26,26 @@
 %Initial Optimized Input Values over the trajectory
 optimal_parameters = zeros(8,N);
 p0 = zeros(8,N);
+p0(1:6,:) = x + (xf-x)*(0:1:(N-1))/(N-1);
 
 %Fmincon Options
-options = optimoptions(@fmincon,'StepTolerance',1e-15,'TolFun',1e-8,'MaxIter',10000,'MaxFunEvals',10000,...
+options = optimoptions(@fmincon,'StepTolerance',1e-15,'TolFun',1e-8,'MaxIter',10000,'MaxFunEvals',500000,...
                        'DiffMinChange',1e-3,'Display','iter','Algorithm','sqp');
                    
 %Input Bounds
 lb_input = -100;
 ub_input = 100;
-LB = [-Inf;-Inf;-Inf;-Inf;-Inf;-Inf;lb_input;lb_input];
-UB = [Inf;Inf;Inf;Inf;Inf;Inf;ub_input;ub_input];
+% LB_ = [pi/5  ;pi/5  ;pi/6  ;-inf;-inf;-inf;lb_input;lb_input];
+% UB_ = [4*pi/5;4*pi/5;5*pi/6;inf ;inf ;inf ;ub_input;ub_input];
+LB_ = [0    ;0  ;0  ;-inf;-inf;-inf;lb_input;lb_input];
+UB_ = [pi/2 ;pi ;pi/2 ;inf ;inf ;inf ;ub_input;ub_input];
+
+LB = LB_;
+UB = UB_;
+
 for i = 4:3:3*N
-    LB = [LB [-Inf;-Inf;-Inf;-Inf;-Inf;-Inf;lb_input;lb_input]];
-    UB = [UB [Inf;Inf;Inf;Inf;Inf;Inf;ub_input;ub_input]];
+    LB = [LB ,LB_];
+    UB = [UB ,UB_];
 end
 
 %Store Data
@@ -76,7 +84,6 @@
 plot(t,optimal_parameters(3,:),'b');
 hold off;
 
-
 subplot(2,3,4);
 hold on;
 plot(t, xf(4)*ones(1,params.N), 'k--');
@@ -103,12 +110,12 @@
 %% Cost Function
 function J = cost(p, xk, xref, N, u0, Ts, link_length)
 
-    Q = [10 0 0 0 0 0;...
-            0 10 0 0 0 0;...
-            0 0 10 0 0 0;...
-            0 0 0 1 0 0;...
-            0 0 0 0 1 0;...
-            0 0 0 0 0 1];
+    Q = [100 0 0 0 0 0;...
+       	 0 100 0 0 0 0;...
+         0 0 100 0 0 0;...
+         0 0 0 1 0 0;...
+         0 0 0 0 1 0;...
+         0 0 0 0 0 1];
     R = 0.01*eye(2);
 
     J = 0;
diff --git a/goat2D_mpc/goatConstraintFCNDC.m b/goat2D_mpc/goatConstraintFCNDC.m
index 9dbf773..4f74c08 100644
--- a/goat2D_mpc/goatConstraintFCNDC.m
+++ b/goat2D_mpc/goatConstraintFCNDC.m
@@ -44,7 +44,7 @@
 %Constarian Initial Point
 ceq = p(1:6,1) - x;
 for tk = 1:N-1   
-    
+%     tk
     xk = [p(1:6,tk)];
     uk = p(7:8,tk);
     xdotk = goatDynamicsCT(xk,uk,link_length);
diff --git a/goat2D_mpc/goatDynamicsCT.m b/goat2D_mpc/goatDynamicsCT.m
index 0f43a56..faf2183 100644
--- a/goat2D_mpc/goatDynamicsCT.m
+++ b/goat2D_mpc/goatDynamicsCT.m
@@ -5,6 +5,9 @@
     dq2 = q(4:6);
 %   q1 = fsolve(@(q1) constraints(q1,q2), q10);
 %     q1 = findFeasibleConfiguration(q2, q10, link_length);
+    if (norm(q2) > 5)
+        fprintf('Too big q2\n')
+    end
     q1 = findFeasibleConfigurationAnalytical(q2, link_length);
 %     q1(2:3) = wrapToPi(q1(2:3));
 %     q1 = wrapToPi(q1);

From 86110d952912aaf787d7e1a87a23d1ccc1218adc Mon Sep 17 00:00:00 2001
From: Ashwin Khadke <ashwin@Ashwins-MacBook-Pro.local>
Date: Sat, 18 Nov 2017 13:59:50 -0500
Subject: [PATCH 12/12] gitignore modified

---
 goat2D_mpc/.DS_Store  | Bin 8196 -> 8196 bytes
 goat2D_mpc/.gitignore |   1 +
 2 files changed, 1 insertion(+)
 create mode 100644 goat2D_mpc/.gitignore

diff --git a/goat2D_mpc/.DS_Store b/goat2D_mpc/.DS_Store
index 67cff6f3cabd79334b5e88bd7e3321ed5f6e9f10..a79538a5fc6f0c0179d3329100ac2126243c1546 100644
GIT binary patch
delta 40
wcmZp1XmOa}&nU7nU^hRb$YvgaZ061Dg(FxdHdt(Cm-xoA*;Mor)5Hc-01z$>mjD0&

delta 80
zcmZp1XmOa}&nUVvU^hRb=w=>)Y-U{^26u*hhD3%E2498%24@BrAl75ZWvKJa$xlwo
h$xmWnU=UznVEhck9Gi25lUX*iOMGM5d{l&;82|;}6Al0X

diff --git a/goat2D_mpc/.gitignore b/goat2D_mpc/.gitignore
new file mode 100644
index 0000000..e43b0f9
--- /dev/null
+++ b/goat2D_mpc/.gitignore
@@ -0,0 +1 @@
+.DS_Store