Mpc with direct collocation

.gitignore

@@ -0,0 +1 @@
+.DS_Store

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

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

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

goat2D_mpc/.gitignore

@@ -0,0 +1 @@
+.DS_Store