%% Solve Analytical Inverse Kinematics for Robot Manipulator
% Generate closed-form inverse kinematics (IK) solutions for a desired end effector. Load the provided robot model and inspect details about the feasible kinematic groups of base and end-effector bodies. Generate a function for your desired kinematic group. Inspect the various configurations for a specific end-effector pose.
%% Robot Model
% Load the ABB IRB 120 robot model into the workspace. Display the model.

robot = loadrobot('abbIrb120','DataFormat','row');
show(robot);

%
%% Analytical IK
% Create the analytical IK solver. Show details for the robot model, which lists the different kinematic groups available for closed-form analytical IK solutions. Select the second kinematic group by clicking the Use this kinematic group link in the second row of the table.

aik = analyticalInverseKinematics(robot);
showdetails(aik)

% Inspect the kinematic group, which lists the base and end-effector body names. For this robot, the group uses the 'base_link' and 'tool0' bodies, respectively.

aik.KinematicGroup

%
%% Generate Function
% Generate the IK function for the selected kinematic group. Specify a name for the function, which is generated and saved in the current directory.

generateIKFunction(aik,'robotIK');

% Specify a desired end-effector position. Convert the xyz-position to a homogeneous transformation.

eePosition = [0 0.5 0.5];
eePose = trvec2tform(eePosition);
hold on
plotTransforms(eePosition,tform2quat(eePose))
hold off

%% Generate Configuration for IK Solution
% Specify the homogeneous transformation to the generated IK function, which generates all solutions for the desired end-effector pose. Display the first generated configuration to verify that the desired pose has been achieved.

ikConfig = robotIK(eePose); % Uses the generated file

show(robot,ikConfig(1,:));
hold on
plotTransforms(eePosition,tform2quat(eePose))
hold off

% Display all of the closed-form IK solutions sequentially.

figure;
numSolutions = size(ikConfig,1);

for i = 1:size(ikConfig,1)
    subplot(1,numSolutions,i)
    show(robot,ikConfig(i,:));
end

% Copyright 2020 The MathWorks, Inc.
%