%% Modify Connection Types for Reeds-Shepp Path
% Create a reedsSheppConnection object.

reedsConnObj = reedsSheppConnection;

%
% Define start and goal poses as [x y theta] vectors.

startPose = [0 0 0];
goalPose = [1 1 pi];

%
% Calculate a valid path segment to connect the poses.

[pathSegObj,pathCosts] = connect(reedsConnObj,startPose,goalPose);

%
% Show the generated path. Notice the direction of the turns.

show(pathSegObj{1})
pathSegObj{1}.MotionTypes
pathSegObj{1}.MotionDirections

%
% Disable this specific motion sequence in a new connection object. Reduce the MinTurningRadius if the robot is more maneuverable. Increase the reverse cost to reduce the likelihood of reverse directions being used. Connect the poses again to get a different path.

reedsConnObj = reedsSheppConnection('DisabledPathTypes',{'LpRnLp'});
reedsConnObj.MinTurningRadius = 0.5;
reedsConnObj.ReverseCost = 5;

[pathSegObj,pathCosts] = connect(reedsConnObj,startPose,goalPose);
pathSegObj{1}.MotionTypes
show(pathSegObj{1})
xlim([0 1.5])
ylim([0 1.5])

% Copyright 2019 The MathWorks, Inc.