%% Check for Manipulator Self Collisions using Collision Meshes
% This example shows how to check for manipulator self collisions using the collision meshes in the URDF source folder. The following related examples show how to define collision meshes in other ways, and how to check for environmental collisions:
% Create Collision Objects for Manipulator Collision Checking
% Check for Environmental Collisions with Manipulators
%
%% Create KUKA IIWA Robot Representation
% Create a rigidBodyTree object for the KUKA® IIWA-14 serial manipulator.

iiwa = importrobot('iiwa14.urdf');
iiwa.DataFormat = 'column';

%
%% Create Collision Meshes for the Rigid Body Tree
% Generate an array of collision meshes for each link of the rigid body tree. These links will be stored in a cell array with n+1 rows, for the base and n bodies in the rigid body tree. Each row will have two elements: the collision object, and a transform indicating the relationship of the object to the associated rigid body tree origin.

collisionArray = cell(iiwa.NumBodies+1,2);

% There are several ways to fill this array. Create Collision Objects for Manipulator Collision Checking covers three different ways to generate collision objects for the links in a rigid body tree. Since the IIWA manipulator provides specific collision meshes in the URDF directory, this example creates collision meshes from the information in the mesh path.
% Create a rigidBodyTree object and specify the path of the folder containing the collision meshes. This syntax associated the rigid body objects with the STL mesh files based on their specified name.

collisionMeshPath = fullfile(matlabroot,'toolbox','robotics',...
        'robotexamples','robotmanip','data','iiwa_description',...
        'meshes','iiwa14','collision');
iiwaCollision = importrobot('iiwa14.urdf','MeshPath',collisionMeshPath);
iiwaCollision.DataFormat = 'column';

% Create the collision mesh definitions for each rigid body from the source directory:
% Convert each STL file to faces and vertices by reading the STL: stldata = stlread(file.STL)
% Create a collisionMesh from the vertices: mesh = collisionMesh(stldata.Points);
% Assign the mesh to the ith body by placing it in the (i+1)th element of the collisionArray cell array. The base mesh occupies the first element of the cell array.

% For each body, read the corresponding STL file
robotBodies = [{iiwaCollision.Base} iiwaCollision.Bodies];
for i = 1:numel(robotBodies)
    if ~isempty(robotBodies{i}.Visuals)
        % As a simplifying assumption, assume that the first visual is the 
        % only associated collision mesh.
        visualDetails = robotBodies{i}.Visuals{1};
        
        % Extract the part of the visual that actually specifies the STL name
        visualParts = strsplit(visualDetails,':');
        stlFileName = visualParts{2};
        
        % Read the STL file
        stlData = stlread(fullfile(collisionMeshPath,stlFileName));
        
        % Create a collisionMesh object from the vertices
        collisionArray{i,1} = collisionMesh(stlData.Points);
        
        % For imported meshes, the origin of the imported mesh and the
        % rigid body origin are the same
        collisionArray{i,2} = eye(4);
    end
end

%
%% Generate Trajectory and Check for Collisions
% Specify a start and end configuration and find a joint space trajectory that connects them:

% Define collision-free start & goal configurations
startConfig = [0 -pi/4 pi 3*pi/2 0 -pi/2 pi/8]';
goalConfig = [0 -pi/4 pi 3*pi/4 0 -pi/2 pi/8]';

% Define a trapezoidal velocity trajectory between the two configurations
q = trapveltraj([startConfig goalConfig],100,'EndTime',3);

% To verify this output trajectory doesn't contain self-collisions, iterate over the output samples and see if any points are in collision. Collision checking is performed using an example helper that uses two for loops to verify every body against every other body. The following specific assumptions are considered here:
% Bodies are not checked against neighboring bodies, since they are always in contact at the joint. It is assumed that joint limits would be set to prevent bodies from colliding with their immediate neighbors
% All bodies are checked against each other only once (i.e. body 1 is checked for collision with body 5, but not vice versa).
% When the robot has self collisions and an m-by-2 matrix of the pairs of collisions, collisionPairIdx, the example outputs a flag, isInCollision, as true. The elements of this matrix are indices that map the collisions to the bodies in the associated collisionArray cell array of collision meshes.

isConfigInCollision = false(100,1);
configCollisionPairs = cell(100,1);
for i = 1:length(q)
    [isConfigInCollision(i),configCollisionPairs{i}] = exampleHelperManipCheckSelfCollisions(iiwaCollision,collisionArray,q(:,i),false);
end

% The planned trajectory goes through a series of collisions. Visualize the configuration where the first collision occurs and highlight the bodies.

any(isConfigInCollision)
firstCollisionIdx = find(isConfigInCollision,1);

% Visualize the first configuration that causes problems
figure;
show(iiwaCollision,q(:,firstCollisionIdx));
exampleHelperHighlightCollisionBodies(iiwaCollision,configCollisionPairs{firstCollisionIdx},gca);

%
%% Generate a Collision-Free Trajectory
% This first collision actually occurs at the starting configuration because a joint position is specified past its limits. Call wrapToPi to limit the starting positions of the joints.
% Generate a new trajectory and check for collisions again. To visualize parts of the trajectory, uncomment the show function call.

newStartConfig = wrapToPi(startConfig);
q = trapveltraj([newStartConfig goalConfig],100,'EndTime',3);

isConfigInCollision = false(100,1);
configCollisionPairs = cell(100,1);
for i = 1:length(q)
    [isConfigInCollision(i),configCollisionPairs{i}] = exampleHelperManipCheckSelfCollisions(iiwaCollision,collisionArray,q(:,i),false);
%     if rem(i,10) == 0
%         show(iiwaCollision,q(:,i));
%         drawnow
%     end
end

%
% After checking the whole trajectory, no collisions are found.

any(isConfigInCollision)

% Copyright 2019 The MathWorks, Inc.