%% Sample Multiple Poses In A Workspace Goal Region
% Sample various poses within the bounds of a workspace goal region for a manipulator arm. Some end-effector poses may not be desirable due to the positioning of the arm bodies and obstacles in the scene. The workspaceGoalRegion object defines the bounds on the XYZ-position and ZYX Euler orientation of the robot end effector. The sample object function uniformly samples random poses within the bounds. Find configurations that achieve these end-effector poses and determine the best by visualization.
% Load an existing robot model as a rigidBodyTree object.

robot = loadrobot("kinovaGen3","DataFormat","row");
show(robot,"Collisions","on","Visuals","off");

% Add a can as a collisionCylinder object to the robot arm.

can = collisionCylinder(0.05, 0.1); 
can.Pose = trvec2tform([0.2, 0.3, 0.5]); 

addCollision(robot.Bodies{end},"cylinder", [0.05, 0.1], trvec2tform([0, 0, 0.02])); 

% The goal of this example is to place this can on a table with other cans. Add the table and other cans to the environment by creating a cell array of collision objects. Show the entire env cell array.

table = collisionBox(0.7, 0.5, 0.04); 
table.Pose = trvec2tform([0, 0.5, 0.43]); 
env = {can, copy(can), copy(can), table}; 
env{2}.Pose = trvec2tform([-0.1, 0.3, 0.5]); 
env{3}.Pose = trvec2tform([-0.1, 0.5, 0.5]); 

hold on 
for i = 1: length(env) 
    show(env{i}) 
end
show(robot,homeConfiguration(robot),"Collisions","on","Visuals","off"); 

%
%% 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 specifiies XYZ bounds within the table dimensions and fixes rotation to a small range in the Y and X axis.

tableRegion = workspaceGoalRegion("EndEffector_Link",...
    "ReferencePose",table.Pose);
tableRegion.EndEffectorOffsetPose(1:3,1:3) = eul2rotm([0, 0, pi]); 
tableRegion.EndEffectorOffsetPose(3, end) = 0.1; 

tableRegion.Bounds = ... 
    [-table.X/2, table.X/2; % X Bounds
    -table.Y/2, table.Y/2;  % Y Bounds
    0.04, 0.10;             % Z Bounds
    -pi, pi;                % Rotation about Z-axis
    -0.01, 0.01;            % Y-Axis
    -0.01, 0.01;];          % X-Axis

show(tableRegion);
view(165,50)
camzoom(3.5)

%
%% Sample Poses
% Uniformly sample poses within the table region using the sample object function. In this example, set the rng seed to get repeatible results. Create vectors for storing valid and invalid poses.

rng(0)
poses = sample(tableRegion,10);
validPoses = [];
invalidPoses = [];

%
%% Check for Collisions
% To find configurations for those poses, create an inverse kinematics (IK) solver.

ik = inverseKinematics('RigidBodyTree',robot);
config = cell(10);

% Test the sampled poses by iterating through the sampled poses, solving for configurations using IK, and checking for collisions. Show the valid configurations.

for i = 1:length(poses)
    % Solve for robot configuraiton using IK.
    config{i} = ik("EndEffector_Link",poses(:,:,i),ones(6,1),homeConfiguration(robot));
    % Check for collisions.
    isColliding = checkCollision(robot,config{i},env);
    
    if ~isColliding % If not in collision, show robot configuration and save valid pose.
        show(robot,config{i},"PreservePlot",false,"Collisions","on","Visuals","off");
        drawnow
        validPoses = [validPoses; i];
    else
        invalidPoses = [invalidPoses; i];
    end
    
end
disp(string(validPoses'))

%
%% Visualize A Singe Valid Pose
% Plot all valid poses as transforms. The final valid configuration from checking collisions is still visible in the figure.

translations = tform2trvec(poses(:,:,validPoses));
rotations = tform2quat(poses(:,:,validPoses));

plotTransforms(translations,rotations,"FrameSize",0.1)

%
% Show a valid configuration from the list. Change the index in validPoses to look at different poses. Call hold off to stop preserving figure elements. To manually inspect poses and configurations, comment out the final line when running.

poseIndex = validPoses(1);
show(robot,config{poseIndex},"PreservePlot",false,"Collisions","on","Visuals","off");
hold off

% Copyright 2020 The MathWorks, Inc.
%