%% Solve Analytical IK for Large-DOF Robot
% Some manipulator robot models have large degrees-of-freedom (DOFs). To reach certain end-effector poses, however, only six DOFs are required. Use the analyticalInverseKinematics object, which supports six-DOF robots, to determine various valid kinematic groups for this large-DOF robot model. Use the showdetails object function to get information about the model.
%
%% Load Robot Model and Generate IK Solver
% Load the robot model into the workspace, and create an analyicalInverseKinematics object. Use the showdetails object function to see the supported kinematic groups.

robot = loadrobot('willowgaragePR2','DataFormat','row');
aik = analyticalInverseKinematics(robot);
opts = showdetails(aik);

% Select a group programmically using the output of the showdetails object function, opts. The selected group uses the left shoulder as the base with the left wrist as the end effector.

aik.KinematicGroup = opts(1).KinematicGroup;
disp(aik.KinematicGroup)

% Generate the IK function for the selected group.

generateIKFunction(aik,'willowRobotIK');

%
%% Solve Analytical IK
% Define a target end-effector pose using a randomly-generated configuration.

rng(0);
expConfig = randomConfiguration(robot);

eeBodyName = aik.KinematicGroup.EndEffectorBodyName;
baseName = aik.KinematicGroup.BaseName;
expEEPose = getTransform(robot,expConfig,eeBodyName,baseName);

% Solve for all robot configurations that achieve the defined end-effector pose using the generated IK function. To ignore joint limits, specify false as the second input argument.

ikConfig = willowRobotIK(expEEPose,false);

%
% To display the target end-effector pose in the world frame, get the transformation from the base of the robot model, rather than the base of the kinematic group. Display all of the generated IK solutions by specifying the indices for your kinematic group IK solution in the configuration vector used with the show function.

eeWorldPose = getTransform(robot,expConfig,eeBodyName);

generatedConfig = repmat(expConfig, size(ikConfig,1), 1);
generatedConfig(:,aik.KinematicGroupConfigIdx) = ikConfig;

for i = 1:size(ikConfig,1)
    figure;
    ax = show(robot,generatedConfig(i,:));
    hold all;
    plotTransforms(tform2trvec(eeWorldPose),tform2quat(eeWorldPose),'Parent',ax);
    title(['Solution ' num2str(i)]);
end

% Copyright 2020 The MathWorks, Inc.
%