Kent Libby's inverse kinematic solution for pre-lab3

ikin.m

function [joints]= ikin(x, y, z) 
    %% TODO update these with the robot params
    theta1=0;
    theta2=0;
    theta3=90;
    curPos=zeros(3,1);
    L1 = 135; L2 = 175; L3 = 169.28; 
    %% Creates a jacobian then uses the difference between the locations to give the theta values to move to
    J1=cross([0;0;1], [L2*cosd(theta2) + L3*cosd(theta3); L2*sind(theta2)+L3*sind(theta3); L1]);
    J2=cross([sind(theta1); -cosd(theta1); 0], [L2*cosd(theta2) + L3*cosd(theta3); L2*sind(theta2)+L3*sind(theta3); 0]);
    J3=cross([-sind(theta1); cosd(theta1); 0], [L3*cosd(theta3); L3*sind(theta3); 0]);
    Jfinal=[J1 J2 J3];
    %% calculates the d-theta and error checks
    tempJoints=transpose(Jfinal)*([x;y;z]-curPos);
    tempJoints=tempJoints';
    for i=1:size(tempJoints)
        if abs(tempJoints(i))>90
            error('bad points called');
        end
    end
    joints=tempJoints;
end