r = loadrobot('abbIrb120','DataFormat','row');

q0 = zeros(1,6);
figure;
  ax = show(r,q0, ...
           'Visuals','on', ...
           'PreservePlot',0, ...
           'Fastupdate',1); hold all;
  drawFloor();

%% ----------------------------------------------------------------------------
% Create a desired end-effector pose

% Desired position
xd = 0.25;
yd = 0;
zd = 0.25;
ad = pi/4;
bd = pi/4;
gd = 0;
Td = trvec2tform([xd;yd;zd]')*eul2tform([ad,bd,gd]);

% Show desired position using 'plotTransform', 'plot3'
figure;
  ax = show(r,q0, ...
    'Visuals','on', ...
    'PreservePlot',0, ...
    'Fastupdate',1); hold all;
  drawFloor();
  plotTransforms(Td(1:3,4)',tform2quat(Td), ...
                'Parent',ax, ...
                'framesize',0.25)
  plot3(ax,[0,xd],[0,yd],[0,zd],'m','LineWidth',5);
  title('Demo using plotTransforms')


%% ----------------------------------------------------------------------------
% Analytical IK

% create object for analytic IK
aik = analyticalInverseKinematics(r);

% generate a function called 'robotIK' which computes the IK
generateIKFunction(aik,'robotIK');

% find the pose for the target end-effector position
q = robotIK(Td);

% Choose solution closest to home position
[~,idx] = min(vecnorm((q - q0)'));
q = q(idx,:);

figure;
  ax = show(r,q, ...
    'Visuals','on', ...
    'PreservePlot',0, ...
    'Fastupdate',1); hold all;
  drawFloor();
  plotTransforms(Td(1:3,4)',tform2quat(Td), ...
                'Parent',ax, ...
                'framesize',0.25)
  plot3(ax,[0,xd],[0,yd],[0,zd],'m','LineWidth',5);
  th = title('Analytic IK');
  pos = th.Position;
  th.Position = [pos(1), pos(2)-0.2, pos(3)];

%% ----------------------------------------------------------------------------
% Numerical IK

% BFGSGradientProjection IK object
ik = inverseKinematics('RigidBodyTree',r);

% find pose with numerical
[q,solnInfo] = ik('tool0',Td,ones(1,6),zeros(1,6));

figure;
  ax = show(r,q, ...
    'Visuals','on', ...
    'PreservePlot',0, ...
    'Fastupdate',1); hold all;
  drawFloor();
  plotTransforms(Td(1:3,4)',tform2quat(Td), ...
                'Parent',ax, ...
                'framesize',0.25)
  plot3(ax,[0,xd],[0,yd],[0,zd],'m','LineWidth',5);
  title('Numeric IK')

%% ----------------------------------------------------------------------------
% Generalized IK with cup

% Add a cup
cupHeight = 0.2;
cupRadius = 0.05;
xd = 0.35
yd = 0;
cupPosition = [xd, yd, 0];
body = rigidBody('cupFrame');

% Scale visual
addVisual(body,"Mesh",'cup.stl',[[0.0015*eye(3), zeros(3,1)]; 0 0 0 1]);

setFixedTransform(body.Joint,trvec2tform(cupPosition))
addBody(r,body,r.BaseName);

% Generalized IK object
gik = generalizedInverseKinematics('RigidBodyTree',r);
gik.ConstraintInputs = {'cartesian','position','aiming'};

% Add Floor Constraint
heightAboveFloor = constraintCartesianBounds('tool0');
heightAboveFloor.Bounds = [-inf, inf; ...
                           -inf, inf; ...
                           0.005, inf];

% Add Position Constraint
distanceFromCup = constraintPositionTarget('cupFrame');
distanceFromCup.ReferenceBody = 'tool0';
distanceFromCup.PositionTolerance = 0.25;

% Aim at the cup
alignWithCup = constraintAiming('tool0');
alignWithCup.TargetPoint = [100 0 0];

% Get IK
[q,solnInfo] = gik(q0,heightAboveFloor, ...
                      distanceFromCup, ...
                      alignWithCup);

figure;
  ax = show(r,q, ...
    'Visuals','on', ...
    'PreservePlot',0, ...
    'Fastupdate',1);
  drawFloor();
  title('Generalized IK')

% -----------------------------------------------------------------------------
function drawFloor()
  ax = gca;
  ax.CameraViewAngle = 5;
  p = patch([1 -1 -1 1].*0.5, [1 1 -1 -1]*0.5, [0 0 0 0]);
  p.FaceColor = [0.8, 0.8, 0.8];
  axis off;
  xlim([-0.75,0.75]);
  ylim([-0.75,0.75]);
  zlim([0,0.75]);
end
