dhOutput.m

function [DH] = dhOutput(a, alp, d, th)
     DH = [cosd(th), -sind(th)*cosd(alp), sind(th)*sind(alp), a*cosd(th);
     sind(th), cosd(th)*cosd(alp), -cosd(th)*sind(alp), a*sind(th);
     0, sind(alp), cosd(alp), d;
     0, 0, 0, 1];
end

hwk3.m

%% Kent Libby RBE hwk3

%% part B: dh symbols
syms A B C q1 q2 q3
symVars = [A, B, C, q1, q2, q3];

%% part C&D: dh functions and solutions wo/t
% these calculations are done without t to allow for direct substitiution
T01 = dhOutput(0, 90, A, q1+90);
disp('T01:');
disp(T01);
T12 = dhOutput(B, 0, 0, q2);
disp('T12:');
disp(T12);
T23 = dhOutput(C, 0, 0, q3);
disp('T23:');
disp(T23);

%% part E: symbolic T03 matrix
T03=T01*T12*T23;
disp('T03:');
disp(T03);

%% part F: numeric kinematic matrix for defined config
posnConds= [60, 40, 20, 15, 30, -15];
T03numeric = subs(T03, symVars, posnConds);
T03numeric = double(T03numeric);
disp('T03 numeric:');
disp(T03numeric);

%% part G: approach vector

x3Approach=T03numeric(1:3, 1);
disp('X approach:');
disp(x3Approach);

%% Part H: derivative of position vector
tipPosition= T03(1:3 ,4);
diff(tipPosition, t); 
disp(tipPosition);

%% Part I: full forward velocity kinemeatics
jakobianPosition=[tipPosition