# 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

## 1 Answer

Debugging this situation can be done pretty easily - or at least you can easily narrow down the possible problems.

First, plug the joint angles you received from your inverse kinematics algorithm into your forward kinematics equations. Do they provide the same end effector coordinates that you entered into your inverse kinematics routine?

If not, you have an error in either the forward or inverse equations.

If, however, the end effector coordinates are the same, then you have found a different pose of the robot. Perhaps your solution has the robot's base turned 180 degrees and it is reaching back over its shoulder. Or, the elbow may be either below or above the line connecting the shoulder and wrist. Or, the wrist may be angled either down or up. You need to pay attention to the multiple angles which can result in the same answer for atan2, acos, and asin.

• thanks for the reply Steve. I'm aware of the multiple solutions provided by the IK but the issue here is the first part of your reply which is different coordinates between the generated angles when plugged in FK and the coordinates given to the robot. I'll further check it and update the question. – DieDen Mar 23 '17 at 8:10