%% Plan Path To A Workspace Goal Region
% Specify a goal region in your workspace and plan a path within those bounds. The workspaceGoalRegion object defines the bounds on the XYZ-position and ZYX Euler orientation of the robot end effector. The manipulatorRRT object plans a path based on that goal region and samples random poses within the bounds.
% Load an existing robot model as a rigidBodyTree object.

robot = loadrobot("kinovaGen3", "DataFormat", "row");
ax = show(robot);

%
%% Create Path Planner
% Create a rapidly-exploring random tree (RRT) path planner for the robot. This example uses an empty environment, but this workflow also works well with cluttered environments. You can add collision objects to the environment like the collisionBox or collisionMesh object.

planner = manipulatorRRT(robot,{});

%
%% Define Goal Region
% Create a workspace goal region using the end-effector body name of the robot.
% Define the goal region parameters for your workspace. The goal region includes a reference pose, XYZ-position bounds, and orientation limits on the ZYX Euler angles. This example specifies bounds on the XY-plane in meters and allows rotation about the Z-axis in radians.

goalRegion = workspaceGoalRegion(robot.BodyNames{end}); 
goalRegion.ReferencePose = trvec2tform([0.5 0.5 0.2]);
goalRegion.Bounds(1, :) = [-0.2 0.2];    % X Bounds
goalRegion.Bounds(2, :) = [-0.2 0.2];    % Y Bounds
goalRegion.Bounds(4, :) = [-pi/2 pi/2];  % Rotation about the Z-axis

% You can also apply a fixed offset to all poses sampled within the region. This offset can account for grasping tools or variations in dimensions within your workspace. For this example, apply a fixed transformation that places the end effector 5 cm above the workspace.

goalRegion.EndEffectorOffsetPose = trvec2tform([0 0 0.05]);
hold on
show(goalRegion);

%
%% Plan Path To Goal Region
% Plan a path to the goal region from the robot's home configuration. Due to the randomness in the RRT algorithm, this example sets the rng seed to ensure repeatable results.

rng(0)
path = plan(planner,homeConfiguration(robot),goalRegion);

% Show the robot executing the path. To visualize a more realistic path, interpolate points between path configurations.

interpConfigurations = interpolate(planner,path,5);

for i = 1 : size(interpConfigurations)
    show(robot,interpConfigurations(i,:),"PreservePlot",false);
    set(ax,'ZLim',[-0.05 0.75],'YLim',[-0.05 1],'XLim',[-0.05 1],...
        'CameraViewAngle',5)
  
    drawnow
end
hold off

%
%% Adjust End-effector Pose
% Notice that the robot arm approaches the workspace from the bottom. To flip the orientation of the final position, add a pi rotation to the Y-axis for the reference pose.

goalRegion.EndEffectorOffsetPose = ... 
    goalRegion.EndEffectorOffsetPose*eul2tform([0 pi 0],"ZYX");

% Replan the path and visualize the robot motion again. The robot now approaches from the top.

hold on
show(goalRegion);
path = plan(planner,homeConfiguration(robot),goalRegion);

interpConfigurations = interpolate(planner,path,5);

for i = 1 : size(interpConfigurations)
    show(robot, interpConfigurations(i, :),"PreservePlot",false);
    set(ax,'ZLim',[-0.05 0.75],'YLim',[-0.05 1],'XLim',[-0.05 1])
    drawnow;
end
hold off

% Copyright 2020 The MathWorks, Inc.