%% Add Collision Meshes and Check Collisions for Manipulator Robot Arm
% Load a robot model and modify the collision meshes. Clear existing collision meshes, add simple collision object primitives, and check whether certain configurations are in collision.
%% Load Robot Model
% Load a preconfigured robot model into the workspace using the loadrobot function. This model already has collision meshes specified for each body. Iterate through all the rigid body elements and clear the existing collision meshes. Confirm that the existing meshes are gone.

robot = loadrobot('kukaIiwa7','DataFormat','column');

for i = 1:robot.NumBodies
    clearCollision(robot.Bodies{i})
end

show(robot,'Collisions','on','Visuals','off');

%
%% Add Collision Cylinders
% Iteratively add a collision cylinder to each body. Skip some bodies for this specific model, as they overlap and always collide with the end effector (body 10).

collisionObj = collisionCylinder(0.05,0.25);

for i = 1:robot.NumBodies
    if i > 6 && i < 10
        % Skip these bodies.
    else
        addCollision(robot.Bodies{i},collisionObj)
    end
end

show(robot,'Collisions','on','Visuals','off');

%
%% Check for Collisions
% Generate a series of random configurations. Check whether the robot is in collision at each configuration. Visualize each configuration that has a collision.

figure
rng(0) % Set random seed for repeatability.
for i = 1:20
    config = randomConfiguration(robot);
    isColliding = checkCollision(robot,config);
    if isColliding
        show(robot,config,'Collisions','on','Visuals','off');
        title('Collision Detected')
    else
        % Skip non-collisions.
    end
end

% Copyright 2020 The MathWorks, Inc.