%% Optimization-Based Path Smoothing
% This example shows how to optimize a path in the form [x y theta] by maintaining the curvature bound and a safe distance from obstacles. This example uses the plannerHybridAStar object to plan a path and the fmincon solver algorithm to optimize a path. This solver algorithm requires an Optimization Toolbox™.
%
%% Path Plan Using Hybrid A* Path Planner
% Create a binaryOccupancyMap object from the complex map.

load complexMap.mat;
map = binaryOccupancyMap(complexMap);

% Create a validatorOccupancyMap object for collision checking.

validator = validatorOccupancyMap;

% Assign the map to the state validator.

validator.Map = map;

% Set the validation distance for the validator.

validator.ValidationDistance = 0.01;

% Create a plannerHybridAStar object with the state validator.

planner = plannerHybridAStar(validator);

% Set the start and goal poses.

start = [6 3 pi/2];
goal = [44 35 pi];

% Plan a path with the default settings.

pathObj = plan(planner,start,goal);

% Visualize the path.

show(planner,"Tree","off")
hold on

%
%% Configure Parameters to Optimize the Path
% Extract the poses from the path object to optimize.

inputPath = pathObj.States;

% Set the state bounds so they are the same as the map limits.

lb = repmat([map.XWorldLimits(1) map.YWorldLimits(1) -pi],size(inputPath,1),1);
ub = repmat([map.XWorldLimits(2) map.YWorldLimits(2) pi],size(inputPath,1),1);

% Extract the minimum turning radius from the planner object.

minTurningRadius = planner.MinTurningRadius;

% Set the minimum safe distance to maintain from the obstacles.

safeDistance = 2;

% Tune the weights for path optimization. Increase the weight to the constraint for giving more priority.

weightMinTurningRadius = 5;
weightObstacle = 2;
weightSmoothness = 3;

% Optimize the path by converging to the lowest cost value using the tuned weights.

cosFunCall = @(inputPath) exampleHelperCostFunction(inputPath,map,start,goal, ...
    minTurningRadius,safeDistance,weightMinTurningRadius,weightObstacle,weightSmoothness);

% Create the optimization options.

options = optimoptions('fmincon','Algorithm','interior-point','Display','iter');

% Find the minimum value of the cost function given the path, state bounds, and optimization options.

outPath = fmincon(cosFunCall,inputPath,[],[],[],[],lb,ub,[],options);

% Set the start and goal poses so they are the same as the input.

outPath(1,:) = inputPath(1,:);
outPath(end,:) = inputPath(end,:);

% Plot the optimized path.

plotPoints = exampleHelperDrawPath(outPath);
plot(plotPoints(:,1),plotPoints(:,2),'Color',[0 0 1],'LineWidth',3);

%% Reference
% Rosmann, Christoph, Frank Hoffmann, and Torsten Bertram. “Kinodynamic  Trajectory Optimization and Control for Car-like Robots.” In 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 5681–86. Vancouver, BC: IEEE, 2017. https://doi.org/10.1109/IROS.2017.8206458.
% Copyright 2020 The MathWorks, Inc.