%% Solve Generalized Inverse Kinematics for a Set of Constraints
% Create a generalized inverse kinematics solver that holds a robotic arm at a specific location and points toward the robot base. Create the constraint objects to pass the necessary constraint parameters into the solver.
% Load predefined KUKA LBR robot model, which is specified as a rigidBodyTree object.

load exampleRobots.mat lbr

%
% Create the System object™ for solving generalized inverse kinematics.

gik = generalizedInverseKinematics;

%
% Configure the System object to use the KUKA LBR robot.

gik.RigidBodyTree = lbr;

%
% Tell the solver to expect a PositionTarget object and a constraintAiming and constraintPositionTarget object as the constraint inputs.

gik.ConstraintInputs = {'position','aiming'};

%
% Create the two constraint objects.
% The origin of the body named tool0 is located at [0.0 0.5 0.5] relative to the robot's base frame.
% The z-axis of the body named tool0 points toward the origin of the robot's base frame.

posTgt = constraintPositionTarget('tool0');
posTgt.TargetPosition = [0.0 0.5 0.5];

aimCon = constraintAiming('tool0');
aimCon.TargetPoint = [0.0 0.0 0.0];

%
% Find a configuration that satisfies the constraints. You must pass the constraint objects into the System object in the order in which they were specified in the ConstraintInputs property. Specify an initial guess at the robot configuration.

q0 = homeConfiguration(lbr); % Initial guess for solver
[q,solutionInfo] = gik(q0,posTgt,aimCon);

%
% Visualize the configuration returned by the solver.

show(lbr,q);
title(['Solver status: ' solutionInfo.Status])
axis([-0.75 0.75 -0.75 0.75 -0.5 1])

%
% Plot a line segment from the target position to the origin of the base. The origin of the tool0 frame coincides with one end of the segment, and its z-axis is aligned with the segment.

hold on
plot3([0.0 0.0],[0.5 0.0],[0.5 0.0],'--o')
hold off

% Copyright 2019 The MathWorks, Inc.