I'm working on an assignment where I need to derive IK for 5 DOF Youbot kuka robotic arm manipulatorofficial website. I'm using inverse kinematics decoupling and following a geometrical approach using simulink and matlab. The answer of the IK problem is 5 angles but when I apply those angles to the forward kinematics I'm receiving a different coordinates. Is that normal or am I supposed to get the exact coordinates? I'm using the following Matlab code:
function [angles, gripperOut, solution] = IK(pos,toolangle,gripperIn) l=[0.147 0.155 0.135 0.081 0.137]; x = pos(1); y= pos(2); z = pos(3); surfangle = toolangle(2); theta1= atan2(y,x); % atan2(y/x) for theta1 s=z-l(2); % S = Zc - L2 r=sqrt(x.^2+y.^2)-l(1); D = (r^2 + s^2 -l(3)^2 -l(4)^2)/2*l(3)*l(4); %D = (pow2(r)+pow2(s) -pow2(l(3)) -pow2(l(4)))/2*l(3)*l(4); D2 = sqrt(1-D.^2); if D2<0 theta3 = 0; theta2 = 0; solution = 0; else theta3=atan2(D2,D); theta2=atan2(r,s)-atan2(l(4)*sin(theta3),l(3)+l(4)*cos(theta3)); solution = 1; end % R35 = subs(R35,[theta(1) theta(2) theta(3)],[theta1 theta2 theta3]); % theta4=atan2(R35(1,3),R35(3,3)); % theta5=atan2(R35(2,1),R35(2,2)); theta4 = surfangle-theta2-theta3; theta4 = atan2(sin(theta4),cos(theta4)); theta5 = toolangle(1); angles=[theta1 theta2 theta3 theta4 theta5]; if(solution == 0) angles = [0 0 0 0 0]; end gripperOut = gripperIn; end
as shown I'm kind of fixing the last two angles for the tool so I can avoid using the subs command which is not supported for code generation. Any help would be very appreciated.