# Inverse kinematic solution not giving the same forward kinematics answer

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.

• None of the code you've posted means anything at all unless you can provide some documentation (drawings, your initial work, etc.) to illustrate/explain what the various terms are. At a glance, though, it looks like your code is probably not correct. You calculate a D term as a bunch of squares, like when you find a radius to a point, but you don't take the square root of it. What's more, you're using link lengths (I'm assuming l is a vector of lengths - again, no explanation given as to what anything means) but there's no angles included with any of the links to correct for orientation. – Chuck Mar 23 '17 at 10:14
• You calculate an angle using the D term, but I can't think of any physical meaning that D would have as written. You then have D2 defined as sqrt(1-D^2) and then check to see if it's negative, where it looks like you fail the function. Being a square root, it'll never be negative. It might be imaginary, but you check if it's negative. You should try if imag(D2)~=0. Also, I'm assuming solution = 0 means the simulation fails, so you could just put error('IK - D2 is imaginary.'); but that's a personal preference. theta4 uses atan2(sin,cos), which is a redundant statement. – Chuck Mar 23 '17 at 10:17
• Finally, you state, "I'm kind of fixing the last two angles for the tool so I can avoid using the subs command." I can see that there was a subs command, but it's based on R35 which isn't defined anywhere. I can show you how to use subs to generate an expression you can use in code generation, but again, I don't know what any of your variables mean, how they're defined, or what your process was to derive the equations you're using. – Chuck Mar 23 '17 at 10:20
• @Chuck thanks for the reply. I'll edit my question and demonstrate some things. meanwhile, would you please tell me how to use subs command in simulink? the R35 matrix was used in .m file in matlab to get the orientation of the gripper. – DieDen Mar 23 '17 at 10:58
• Use the solve command. Make a vector of equations, then use solve(eqns,[theta1, theta2, theta3]). Once you have the expressions for the terms you're trying to calculate, variable assignment becomes the "subs" you use. I can show you with your own problem as an example, but I need to know what the equations you're using are. – Chuck Mar 23 '17 at 11:10