added simple example of linear mpc applied to the minimum-time proble…

…m for the double integrator

drake/examples/DoubleIntegrator.m

@@ -78,6 +78,44 @@ function drawfun(J,PI)
       subplot(2,1,2); colorbar;
     end
 
+    function runConvexDirtran
+      % Solve the min-time problem as a bisection search of 
+      % linear programs
+
+      x0 = [-2; -2];
+      xf = zeros(2,1);
+      dt = .1;
+
+      A = [ 0, 1; 0 0]; B = [0 ; 1];
+      % x[n+1] = Ax[n] + Bu[n]
+      Ad = eye(2)+dt*A; Bd = dt*B;
+      % Ad = expm(A*dt); Bd = ... % would be slightly better...
+      discrete_time_system = LinearSystem([],[],Ad,Bd,eye(2),[]);
+
+      for N = 50:100 % increase N until i find a feasible solution
+        [prog,x_inds,u_inds] = dirtranModelPredictiveControl(discrete_time_system,N);
+
+        % add initial value constraint:
+        prog = addLinearConstraint(prog,LinearConstraint(x0,x0,eye(2)),x_inds(:,1));
+
+        % add final value constraint:
+        prog = addLinearConstraint(prog,LinearConstraint(xf,xf,eye(2)),x_inds(:,end));
+
+        % add input limit constraints
+        prog = addBoundingBoxConstraint(prog,BoundingBoxConstraint(-1*ones(N,1),1*ones(N,1)),u_inds);
+
+        prog = prog.setSolver('quadprog');
+        [x,objval,exitflag]=prog.solve();
+        if (exitflag>0)
+          N
+          plot(x(x_inds(1,:)),x(x_inds(2,:)),'b',x(x_inds(1,:)),x(x_inds(2,:)),'b.','LineWidth',2,'MarkerSize',15);
+          u = x(u_inds)
+          return
+        end
+      end
+
+    end
+
     function runDirtran
       % Simple example of Direct Transcription trajectory optimization
 

drake/solvers/QuadraticProgram.m

@@ -218,8 +218,14 @@
     result = gurobi(model,params);
 
     info = strcmp(result.status,'OPTIMAL');
-    x = result.x;
-    objval = result.objval;
+    if (info)
+      x = result.x;
+      objval = result.objval;
+    else
+      x=[];
+      objval=[];
+      return
+    end
 
     if (size(obj.Ain,1)>0 || ~isempty(obj.x_lb) || ~isempty(obj.x_ub))
       % note: result.slack(for Ain indices) = bin-Ain*x

drake/systems/AffineSystem.m

@@ -386,6 +386,42 @@
 
       sys = setSampleTime(sys,obj.getSampleTime);
     end
+
+
+    function [prog,x_inds,u_inds] = dirtranModelPredictiveControl(obj,N)
+      % sets up a Quadratic Program with the dynamics constraints
+      % x[n+1] = Ad*x[n] + Bd*u[n] + xdn0;
+      if ~isDT(obj)
+        error('not implemented yet');
+      end
+
+      % decision variables:
+      % x[0],x[1],...,x[N], u[0],...,u[N-1]
+      num_x = obj.num_xd;
+      x_inds = reshape(1:num_x*(N+1),num_x,N+1);
+      u_inds = numel(x_inds) + reshape(1:obj.num_u*N,obj.num_u,N);
+
+      num_vars = numel(x_inds)+numel(u_inds);
+
+      Aeq = zeros(num_x*N,num_vars);
+      beq = zeros(num_x*N,1);
+
+      for i=1:N % todo: vectorize this
+        c_inds = num_x*(i-1)+(1:num_x);
+
+        % x[n+1]
+        Aeq(c_inds,x_inds(:,i+1)) = eye(num_x);
+        % -Ad*x[n]
+        Aeq(c_inds,x_inds(:,i)) = -obj.Ad;
+        % -Bd*u[n]
+        Aeq(c_inds,u_inds(:,i)) = -obj.Bd;
+
+        % xdn0
+        beq(c_inds) = obj.xdn0;
+      end
+
+      prog = QuadraticProgram(zeros(num_vars),zeros(num_vars,1),[],[],Aeq,beq);
+    end
   end
 
   properties