Skip to content

Instantly share code, notes, and snippets.

@Kdoje Kdoje/ikin.m
Created Sep 13, 2018

Embed
What would you like to do?
Kent Libby's inverse kinematic solution for pre-lab3
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
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.