2.0 Changes

- Unreal integration in ModelAndControlAutonomousHaulTruck+Simulink model
- MPC variant introduced in path following module
- CreatePathFollowingMPCController.mlx writeup added
- Visualization helpers added for MATLAB/Simulink
- Improved Stateflow/model organization
- General cleanup & performance improvements

Author: roquarrel

.gitignore

@@ -31,3 +31,11 @@ codegen/
 
 # Cloud based storage dotfile
 .MATLABDriveTag
+
+# Bus/ROS-msg folders
+**/+bus_conv_fcns/*
+**/*_msg/*
+**/*_msg_gen/*
+**/*.slx.original
+**/*.msg
+**/*.srv

CreateLocalPlannerToNavigateGlobalPath.mlx

@@ -45,7 +45,7 @@
     onrampIdx = nan;
     onrampPath = zeros(0,2);
     halfCellSize = 1/(2*binMap.Resolution);
-    states = exampleHelperSmoothPath(path);
+    states = exampleHelperSmoothReferencePath(path);
     for i = 1:numel(iSorted)
         idx = iSorted(i);
         rsConnection = reedsSheppConnection("MinTurningRadius",minTurnRadius);

CreatePathFollowingMPCController.mlx

@@ -1,5 +1,5 @@
-function resampledPath = exampleHelperSmoothPath(path,step)
-%exampleHelperSmoothPath Attempts to smooth the path by fitting a cubic polynomial
+function resampledPath = exampleHelperSmoothReferencePath(path,step)
+%exampleHelperSmoothReferencePath Attempts to smooth the path by fitting a cubic polynomial
 % Fits a cubic polynomial through every third point along a path. 
 % Path is then resampled and with orientation computed via 
 % finite-differencing. This provides the TEB controller with a 

CreateRoutePlannerUsingDigitalElevationData.mlx

@@ -0,0 +1,27 @@
+function [S,R,dTh,c,in] = exampleHelperArcDist(poses)
+%exampleHelperArcDist Compute arc parameters between successive poses
+%
+% Copyright 2024 The MathWorks, Inc.
+
+    % Assumes each segment is tangent with origination pose and intersects 
+    % the XY coordinate of the following pose.
+
+    % Prepare outputs
+    nSeg = size(poses,1)-1;
+    S = zeros(nSeg,1);
+    R = zeros(nSeg,1);
+    dTh = zeros(nSeg,1);
+    c = zeros(nSeg,1);
+
+    % Compute arc parameters for each segment. Assume segment is tangent with
+    % origination pose and intersects the next pose.
+    for i = 1:nSeg
+        [R(i),c(i),dTh(i),S(i)] = nav.algs.internal.ControlPolicies.posePointArc(poses(i,:),poses(i+1,1:2));
+    end
+
+    % Handle degenerate line case
+    in = isnan(S);
+    in = in | any(abs(mod(dTh,pi)-[0 pi]) < sqrt(eps),2);
+    idx = find(in);
+    S(in) = vecnorm(poses(idx+1,1:2)-poses(idx,1:2),2,2);
+end

CreateTerrainAwareGlobalPlanners.mlx

@@ -0,0 +1,60 @@
+function [pArcInterp,kOut,id] = exampleHelperArcInterp(poses,nv)
+%exampleHelperArcInterp Fit arcs between SE2 poses and evenly interpolate wrt arclength
+%
+% Copyright 2024 The MathWorks, Inc.
+
+    arguments
+        poses (:,3)
+        nv.StepSize (1,1) {mustBePositive, mustBeReal} = 1;
+        nv.Direction (:,1) {mustBeMember(nv.Direction,[0 1])} = 1;
+    end
+    
+    % Assumes each segment is tangent with origination pose and intersects 
+    % the XY coordinate of the following pose.
+
+    % Dirs must be scalar or equal number of poses
+    dirs = repmat(nv.Direction,size(poses,1)/numel(nv.Direction),1);
+    
+    poses(~dirs,3) = poses(~dirs,3)+pi;
+
+    % Compute distances between poses when fitting a tangent arc
+    [S,R,dTh,~,in] = exampleHelperArcDist(poses);
+    
+    % Compute segment arclengths
+    S = cumsum([0;S]);
+    
+    % Identify segment for interpolated points
+    s = 0:(nv.StepSize):S(end);
+    if size(poses,1) > 1
+        id = reshape(discretize(s,S),[],1);
+    else
+        id = ones(numel(s),1);
+    end
+    dS = s(:)-S(id);
+    pArcInterp = zeros(0,3);
+    kOut = 1./R(id);
+    
+    % Interpolate each segment
+    for i = 1:numel(R)
+        ds = dS(id==i);
+        if ~isempty(ds)
+            if in(i)
+                % Handle degenerate line-segment case
+                p0 = poses(i,1:2);
+                v = normalize(poses(i+1,1:2)-p0,'norm');
+                pts = p0 + v.*ds;
+                dth = zeros(size(pts,1),1);
+            else
+                pts = nav.algs.internal.ControlPolicies.evaluateArc(poses(i,:),R(i),ds')';
+                dth = dTh(i)*ds(:)/diff(S(i+[0 1]));
+            end
+            
+            th = poses(i,3)+dth;
+            if ~dirs(i)
+                th = th - pi;
+            end
+            
+            pArcInterp = [pArcInterp; [pts th]]; %#ok<AGROW>
+        end
+    end
+end

Helpers/Graph/exampleHelperConnectRSToPath.m

@@ -0,0 +1,35 @@
+function cost =  exampleHelperBicycleCost_MultiStage(i,x,u,udot,params)
+%exampleHelperBicycleCost_MultiStage Compute cost for bicycle kinematic model
+%
+%   i:                                        (current timestep)
+%   x:      [x y theta]                       (i'th state)
+%   u:      [v steeringAngle]                 (i'th control)
+%   dmv:    [v steeringAngle]                 (rate of change of the manipulated variables)
+%   params: [refState(1:3) predictionHorizon] (the target state and total number of timesteps)
+%
+% Copyright 2024 The MathWorks, Inc.
+    
+    %#codegen
+
+    arguments
+        i (1,1) double {mustBePositive}
+        x (:,1) double {mustBeReal}
+        u (:,1) double {mustBeReal} %#ok<INUSA>
+        udot (:,1) double {mustBeReal}
+        params (:,1) double {mustBeReal}
+    end
+    
+    cost = 0;
+
+    if i > 1
+        W = diag([3 3 3].^2); %weight vector squared
+        err = x-params(1:3);
+        cost = cost + err'*W*err;
+    end 
+    
+    if i <= params(4)
+        Wdmv = diag([0.1 .1].^2); %weight vector squared
+        % dmv is used to penalize high-frequency control movement
+        cost = cost + udot'*Wdmv*udot;
+    end
+end