Merge pull request #7 from mathworks-robotics/MPC_BugFix_And_Improvement

MPC improvement + CreatePathFollowingMPCController example MPPI behav…

Author: roquarrel

CreatePathFollowingMPCController.mlx

@@ -20,16 +20,31 @@
     % Find the closes point on the reference path to the current position
     [~, ii] = min(vecnorm(path-pos,2,2));
 
-    % Find the next 10 points on the path, make sure we don't run over the
-    % end of the path    
-    for jj=1:length(ind)
-        ind(jj) = min(ii+(jj-1), size(refPath,1));
+    remainingPath = refPath(ii:end,:);
+    numStates = size(remainingPath,1);
+    refEndPose = refPath(end,:);
+    dist = vecnorm(curPose(1:2)-refEndPose(1:2),2,2);
+    if numStates < predictionHorizon 
+        if numStates >= 2 && dist > 2
+            % Interpolate evenly between curPose and end pose at reference
+            % path to have predictionHorizon number of states for MPC.
+            xBounds = [min(remainingPath(:,1))-10, max(remainingPath(:,1))+10];
+            yBounds = [min(remainingPath(:,2))-10, max(remainingPath(:,2))+10];
+            ss = stateSpaceSE2([xBounds;yBounds;[-pi pi]]);
+            pathObj = navPath(ss, remainingPath);
+            interpolate(pathObj, 10);
+            refPathToMPC = pathObj.States;      
+        else
+            refPathToMPC = repmat(refEndPose,predictionHorizon,1);
+        end
+    else
+        refPathToMPC = remainingPath(1:predictionHorizon,:);
     end
-
+    
     % Assemble trajectory. In the first stage, the trajectory is not part of the cost function, therefore we set the first row to zero.
     trajectory = [ zeros(1,3);
                    curPose(:)';
-                   refPath(ind(2:end),:)
+                   refPathToMPC(2:end,:)
                  ];
     % MPC needs a smooth pose angle trajectory. If the pose angle is
     % wrapping around e.g. 2pi, we need to catch it and fix it to avoid the discontinuity.

Helpers/MPCController/exampleHelperGenerateMPCReferenceSegment.m

@@ -8,10 +8,10 @@
     persistent mppiObj
     persistent mapObj
     persistent vehicleObj;
-    persistent constraintViolated;
+    persistent numberOfViolations;
 
-    if isempty(constraintViolated)
-        constraintViolated = 0;
+    if isempty(numberOfViolations)
+        numberOfViolations = 0;
     end
 
     if isempty(mapObj)
@@ -30,10 +30,15 @@
         mppiObj.ReferencePath = refPathXY;
     end
 
-    [velcmds,optpath,info,numTrajs,needLocalReplan,needGlobalReplan] = stepImp(mppiObj, controllerParams, constraintViolated,curpose,curvel);
+    [velcmds,optpath,info,numTrajs,needLocalReplan,needGlobalReplan,resetNumViolations] = stepImp(mppiObj, controllerParams, numberOfViolations,curpose,curvel);
     sampleTime = mppiObj.SampleTime;
     trajs = info.Trajectories;   
     lookaheadPoses = info.LookaheadPoses;
+    if resetNumViolations
+        numberOfViolations = 0;
+    else       
+        numberOfViolations = numberOfViolations + 1;
+    end
 end
 
 
@@ -65,38 +70,44 @@
 mppi.Parameter.VehicleCollisionInformation = struct("Dimension",[length, width],"Shape","Rectangle");
 end
 
-function [velcmds,optpath,info,numTrajs,needLocalReplan,needGlobalReplan] = stepImp(mppiObj, controllerParams, constraintViolated,curpose,curvel)
+function [velcmds,optpath,info,numTrajs,needLocalReplan,needGlobalReplan,resetNumViolations] = stepImp(mppiObj, controllerParams, numberOfViolations,curpose,curvel)
 % stepImp return the control commands from the MPPI controller.
 
 needLocalReplan = 0;
 needGlobalReplan = 0;
-
-if constraintViolated == 1
+resetNumViolations = true;
+if numberOfViolations > 0
     % When constraints are violated in previous MPPI output, relax the
     % parameters in MPPI temporarily. The number of trajectories are doubled
     % increase the standard deviation, and keep the PathFollowing cost
     % to 0 and restore them back after velocity commands are generated.
     mppiObj.NumTrajectory = 2*controllerParams.NumTrajectory;
     stdDev = controllerParams.StandardDeviation;
     mppiObj.StandardDeviation = [stdDev(1) 2*stdDev(2)];
-    mppiObj.Parameter.CostWeight.PathFollowing = 0.1;
+    mppiObj.Parameter.CostWeight.PathAlignment = 0;
     numTrajs = mppiObj.NumTrajectory;
     % Generate optimal local path
     [velcmds,optpath,info] = mppiObj(curpose(:)',curvel(:)');
     mppiObj.NumTrajectory = controllerParams.NumTrajectory;
     mppiObj.StandardDeviation = stdDev;
-    mppiObj.Parameter.CostWeight.PathFollowing = controllerParams.PathFollowingCost;   
+    mppiObj.Parameter.CostWeight.PathAlignment = controllerParams.PathAlignmentCost;    
 else
     [velcmds,optpath,info] = mppiObj(curpose(:)',curvel(:)');
     numTrajs = mppiObj.NumTrajectory;
 end
 
-% Trigger local replan if the mppi output trajectory has constraints violated. 
 if info.ExitFlag == 1
-    needLocalReplan = 1;
-% Trigger global replan if the vehicle is far from reference path or it did
-% not move using previous controls. 
+    if numberOfViolations == 0
+        % Trigger local replan if the mppi output trajectory has constraints 
+        % violated and previous mppi call did not result in violation.      
+        needLocalReplan = 1;
+        resetNumViolations = false; 
+    elseif numberOfViolations >= 1
+        % Trigger global replan if constraint violation occured on consecutive MPPI calls 
+        needGlobalReplan = 1;
+    end
 elseif info.ExitFlag == 2
+    % Trigger global replan if the vehicle is far from reference path.
     needGlobalReplan = 1;
 end