Skip to content

Instantly share code, notes, and snippets.

@Kdoje Kdoje/fwkin.m
Created Sep 6, 2018

Embed
What would you like to do?
Kent Libby forward kinematic script
%% For this function, z is defined to be the axis coming out of the plane the base is on (z is up)
%This uses degrees
function p = fwkin(t1, t2, t3)
L1=135;
L2=175;
L3=169.28;
TX12=[cosd(t1) -sind(t1) 0 0;
sind(t1) cosd(t1) 0 0;
0 0 1 L1;
0 0 0 1];
TX23=[cosd(t2) 0 sind(t2) sind(t2)*L2;
0 1 0 0;
-sind(t2) 0 cosd(t2) cosd(t2)*L2;
0 0 0 1];
TX34= [cosd(t3) 0 sind(t3) sind(t3)*L3;
0 1 0 0;
-sind(t3) 0 cosd(t3) cosd(t3)*L3;
0 0 0 1];
%x-> y-> z
initp=TX12*TX23*TX34;
p=initp(1:3,4);
end
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.