Add Quadrotor6D implementation, correcting its errors

drone.m

@@ -0,0 +1,106 @@
+function [data, tau2] = drone()
+%% Grid
+grid_min = [-8; -8; -5; -3]; % Lower corner of computation domain
+grid_max = [8; 8; 5; 3];    % Upper corner of computation domain
+N = 21*ones(4,1);         % Number of grid points per dimension
+
+g = createGrid(grid_min, grid_max, N);
+
+%% target set
+R = 3;
+data0 = shapeCylinder(g, [2, 4], [0; 0; 0; 0], R); %x-z
+
+%% time vector
+t0 = 0;
+tMax = 3;
+dt = 0.1;
+tau = t0:dt:tMax;
+
+%% problem parameters
+uMode = 'max';
+
+%% Pack problem parameters
+dCar = Quad6D([0,0,0,0]);
+
+% Put grid and dynamic systems into schemeData
+schemeData.grid = g;
+schemeData.dynSys = dCar;
+schemeData.accuracy = 'veryHigh'; %set accuracy
+schemeData.uMode = uMode;
+
+%% additive random noise
+% HJIextraArgs.addGaussianNoiseStandardDeviation = [0; 0; 0.5; 0];
+% Try other noise coefficients, like:
+%    [0.2; 0; 0]; % Noise on X state
+%    [0.2,0,0;0,0.2,0;0,0,0.5]; % Independent noise on all states
+%    [0.2;0.2;0.5]; % Coupled noise on all states
+%    {zeros(size(g.xs{1})); zeros(size(g.xs{1})); (g.xs{1}+g.xs{2})/20}; % State-dependent noise
+
+%% Compute value function
+HJIextraArgs.visualize = true;
+HJIextraArgs.fig_num = 1;
+HJIextraArgs.deleteLastPlot = true;
+HJIextraArgs.stopConverge = true;
+HJIextraArgs.makeVideo = true;
+
+[data, tau2] = HJIPDE_solve(data0, tau, schemeData, 'zero', HJIextraArgs);
+
+deriv = computeGradients(g, data);
+
+%% Compute optimal trajectory from some initial state
+compTraj = true;
+if compTraj
+
+  %set the initial state
+  xinit = [-4.8; 0; -0.52; -0.6];
+
+  %check if this initial state is in the BRS/BRT
+  %value = eval_u(g, data, x)
+  value = eval_u(g,data(:,:,:,:,end),xinit);
+  if value <= 0 %if initial state is in BRS/BRT
+    % find optimal trajectory
+
+    dCar.x = xinit; %set initial state of the dubins car
+
+    TrajextraArgs.uMode = uMode; %set if control wants to min or max
+    TrajextraArgs.dMode = 'max';
+    TrajextraArgs.visualize = true; %show plot
+    TrajextraArgs.fig_num = 2; %figure number
+
+    %we want to see the dimensions (x and z)
+    TrajextraArgs.projDim = [1 0 1 0]; % computes optimal traj and visualizes
+
+    %flip data time points so we start from the beginning of time
+    dataTraj = flip(data,4);
+
+    % [traj, traj_tau] = ...
+    % computeOptTraj(g, data, tau, dynSys, extraArgs)
+    [traj, traj_tau] = ...
+      computeOptTraj(g, dataTraj, tau2, dCar, TrajextraArgs);
+
+    figure(6)
+    clf
+    h = visSetIm(g, data(:,:,:,:,end));
+    hold on
+    s = scatter3(xinit(1), xinit(2), xinit(3));
+    s.SizeData = 70;
+    title('The reachable set at the end and xinit')
+    hold off
+
+    %plot traj
+    figure(4)
+    plot(traj(1,:), traj(2,:))
+    hold on
+    xlim([-8 8])
+    ylim([-8 8])
+    % add the target set to that
+    [g2D, data2D] = proj(g, data0, [0 1 0 1]);
+    visSetIm(g2D, data2D, 'green');
+    title('2D projection of the trajectory & target set')
+    hold off
+  else
+    error(['Initial state is not in the BRS/BRT! It has a value of ' num2str(value,2)])
+  end
+end
+
+end

dynSys/@DynSys/updateState.m

@@ -73,6 +73,16 @@
 obj.x = x1;
 obj.u = u;
 
-obj.xhist = cat(2, obj.xhist, x1);
-obj.uhist = cat(2, obj.uhist, u);
-end
+try
+    obj.xhist = cat(2, obj.xhist, x1);
+catch
+    x1 = transpose(x1);
+    obj.xhist = cat(2, obj.xhist, x1);
+end
+try
+    obj.uhist = cat(2, obj.uhist, u);
+catch
+    u = transpose(u);
+    obj.uhist = cat(2, obj.uhist, u);
+end
+end

dynSys/@Quad6D/dynamics.m

@@ -1,11 +1,4 @@
 function dx = dynamics(obj, ~, x, u, ~)
-% Dynamics of the Dubins Car
-%    \dot{x}_1 = v * cos(x_3)
-%    \dot{x}_2 = v * sin(x_3)
-%    \dot{x}_3 = w
-%   Control: u = w;
-%
-% Mo Chen, 2016-06-08
 dx = cell(obj.nx,1);
 
 dims = obj.dims;
@@ -57,9 +50,15 @@
   case 1
     dx = x{dims==2};
   case 2
-    dx = (-(1/obj.m)*obj.transDrag*x{dims==2})+...
-          ((-1/obj.m)*sin(x{dims==5})*u{1})+...
-          ((-1/obj.m)*sin(x{dims==5})*u{2});
+    if iscell(u)        
+        dx = (-(1/obj.m)*obj.transDrag*x{dims==2})+...
+             ((-1/obj.m)*sin(x{dims==5})*u{1})+...
+             ((-1/obj.m)*sin(x{dims==5})*u{2});
+    else 
+        dx = (-(1/obj.m)*obj.transDrag*x{dims==2})+...
+          ((-1/obj.m)*sin(x{dims==5})*u(1))+...
+          ((-1/obj.m)*sin(x{dims==5})*u(2));
+    end
   case 3
     dx = x{dims==4};
   case 4
@@ -69,9 +68,15 @@
   case 5
     dx = x{dims==6};
   case 6
-    dx = ((-1/obj.Iyy)*obj.rotDrag*x{dims==6})+...
-      ((-obj.l/obj.Iyy)*u{1})+...
-      ((obj.l/obj.Iyy)*u{2});
+    if iscell(u)
+        dx = ((-1/obj.Iyy)*obj.rotDrag*x{dims==6})+...
+          ((-obj.l/obj.Iyy)*u{1})+...
+          ((obj.l/obj.Iyy)*u{2});
+    else 
+        dx = ((-1/obj.Iyy)*obj.rotDrag*x{dims==6})+...
+          ((-obj.l/obj.Iyy)*u(1))+...
+          ((obj.l/obj.Iyy)*u(2));
+    end
   otherwise
     error('Only dimension 1-6 are defined for dynamics of Quad6D!')
 end

dynamics.m

@@ -0,0 +1,83 @@
+function dx = dynamics(obj, ~, x, u, ~)
+dx = cell(obj.nx,1);
+
+dims = obj.dims;
+
+returnVector = false;
+if ~iscell(x)
+  returnVector = true;
+  x = num2cell(x);
+  u = num2cell(u);
+end
+
+for i = 1:length(dims)
+  dx{i} = dynamics_cell_helper(obj, x, u, dims, dims(i));
+end
+
+if returnVector
+  dx = cell2mat(dx);
+end
+end
+% 
+% if iscell(x)
+%   dx = cell(length(dims), 1);
+%   
+%   for i = 1:length(dims)
+%     dx{i} = dynamics_cell_helper(obj, x, T1, T2, dims, dims(i));
+%   end
+% else
+%   dx = zeros(obj.nx, 1);
+%   
+%   dx(1) = x(2);
+%   dx(2) = (-(1/obj.m)*obj.transDrag*x(2))+...
+%           ((-1/obj.m)*sin(x(5))*T1)+...
+%           ((-1/obj.m)*sin(x(5))*T2);
+%   dx(3) = x(4);
+%   dx(4) = (-1/obj.m)*(obj.m*obj.grav + obj.transDrag*x(4)) +...
+%           ((1/obj.m)*cos(x(5))*T1)+...
+%           ((1/obj.m)*cos(x(5))*T2);
+%   dx(5) = x(6);
+%   dx(6) = ((-1/obj.Iyy)*obj.rotDrag*x(6))+...
+%           ((-obj.l/obj.Iyy)*T1)+...
+%           ((obj.l/obj.Iyy)*T2);
+%   
+% end
+% end
+
+function dx = dynamics_cell_helper(obj, x, u, dims, dim)
+
+switch dim
+  case 1
+    dx = x{dims==2};
+  case 2
+    if iscell(u)        
+        dx = (-(1/obj.m)*obj.transDrag*x{dims==2})+...
+             ((-1/obj.m)*sin(x{dims==5})*u{1})+...
+             ((-1/obj.m)*sin(x{dims==5})*u{2});
+    else 
+        dx = (-(1/obj.m)*obj.transDrag*x{dims==2})+...
+          ((-1/obj.m)*sin(x{dims==5})*u(1))+...
+          ((-1/obj.m)*sin(x{dims==5})*u(2));
+    end
+  case 3
+    dx = x{dims==4};
+  case 4
+    dx = (-1/obj.m)*(obj.m*obj.grav + obj.transDrag*x{dims==4}) +...
+      ((1/obj.m)*cos(x{dims==5})*u{1})+...
+      ((1/obj.m)*cos(x{dims==5})*u{2});
+  case 5
+    dx = x{dims==6};
+  case 6
+    if iscell(u)
+        dx = ((-1/obj.Iyy)*obj.rotDrag*x{dims==6})+...
+          ((-obj.l/obj.Iyy)*u{1})+...
+          ((obj.l/obj.Iyy)*u{2});
+    else 
+        dx = ((-1/obj.Iyy)*obj.rotDrag*x{dims==6})+...
+          ((-obj.l/obj.Iyy)*u(1))+...
+          ((obj.l/obj.Iyy)*u(2));
+    end
+  otherwise
+    error('Only dimension 1-6 are defined for dynamics of Quad6D!')
+end
+end

updateState.m

@@ -0,0 +1,88 @@
+function x1 = updateState(obj, u, T, x0, d)
+% x1 = updateState(obj, u, T, x0, d)
+% Updates state based on control
+%
+% Inputs:   obj - current quardotor object
+%           u   - control (defaults to previous control)
+%           T   - duration to hold control
+%           x0  - initial state (defaults to current state if set to [])
+%           d   - disturbance (defaults to [])
+%
+% Outputs:  x1  - final state
+%
+% Mo Chen, 2015-05-24
+
+% If no state is specified, use current state
+if nargin < 4 || isempty(x0)
+  x0 = obj.x;
+end
+
+% If time horizon is 0, return initial state
+if T == 0
+  x1 = x0;
+  return
+end
+
+% Default disturbance
+if nargin < 5
+  d = [];
+end
+
+% Do nothing if control is empty
+if isempty(u)
+  x1 = x0;
+  return;
+end
+
+% convert u to vector if needed
+if iscell(u)
+  u = cell2mat(u);
+end
+
+if ~isempty(d) && iscell(d)
+  d = cell2mat(d);
+end
+
+% Do nothing if control is not a number
+if isnan(u)
+  warning('u = NaN')
+  x1 = x0;
+  return;
+end
+
+% Make sure control input is valid
+if ~isnumeric(u)
+  error('Control must be numeric!')
+end
+
+% Convert control to column vector if needed
+if ~iscolumn(u)
+  u = u';
+end
+
+% Check whether there's disturbance (this is needed since not all vehicle
+% classes have dynamics that can handle disturbance)
+if isempty(d)
+  [~, x] = ode113(@(t,x) obj.dynamics(t, x, u), [0 T], x0);
+else
+  [~, x] = ode113(@(t,x) obj.dynamics(t, x, u, d), [0 T], x0);
+end
+
+% Update the state, state history, control, and control history
+x1 = x(end, :)';
+obj.x = x1;
+obj.u = u;
+
+try
+    obj.xhist = cat(2, obj.xhist, x1);
+catch
+    x1 = transpose(x1);
+    obj.xhist = cat(2, obj.xhist, x1);
+end
+try
+    obj.uhist = cat(2, obj.uhist, u);
+catch
+    u = transpose(u);
+    obj.uhist = cat(2, obj.uhist, u);
+end
+end