Skip to content

Instantly share code, notes, and snippets.

@Kdoje Kdoje/dhOutput.m
Created Sep 18, 2018

Embed
What would you like to do?
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
%% 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
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.