diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..e43b0f9
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1 @@
+.DS_Store
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
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
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
diff --git a/Readme.txt b/Readme.txt
new file mode 100644
index 0000000..e69de29
diff --git a/goat2D_mpc/.DS_Store b/goat2D_mpc/.DS_Store
new file mode 100644
index 0000000..a79538a
Binary files /dev/null and b/goat2D_mpc/.DS_Store differ
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
diff --git a/goat2D_mpc/GoatMPCDC.m b/goat2D_mpc/GoatMPCDC.m
new file mode 100644
index 0000000..6af3a46
--- /dev/null
+++ b/goat2D_mpc/GoatMPCDC.m
@@ -0,0 +1,143 @@
+clear;
+clc;
+close all;
+profile on
+addpath('lib')
+
+%Sample Time
+Ts = 0.001;
+params.Ts = Ts;
+
+%Prediction Horizon
+N = 10;
+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] + [0.12; 0.56; -0.58; 0; 0; 0];
+x = [q20; dq20] + [0.012; 0; 0; 0; 0; 0];
+
+%Final State
+xf = [q20; dq20];
+
+%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',500000,...
+                       'DiffMinChange',1e-3,'Display','iter','Algorithm','sqp');
+                   
+%Input Bounds
+lb_input = -100;
+ub_input = 100;
+% 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 ,LB_];
+    UB = [UB ,UB_];
+end
+
+%Store Data
+xHist = zeros(length(x),N);
+
+%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
+%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 = [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;
+    xk1 = xk;
+
+
+    for i = 1:N
+        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(7:8,i-1))' * R * (uk-p(7:8,i-1));
+        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..148a9b2
--- /dev/null
+++ b/goat2D_mpc/goatConstraintFCNDC.m
@@ -0,0 +1,27 @@
+function [c, ceq] = goatConstraintFCNDC(p, x, xref, Ts, N, link_length)
+%% Nonlinear MPC design parameters
+% Range of theta posn
+%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);
+    
+    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
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);