0
$\begingroup$

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.

Improve this question
$\endgroup$
6
  • 1
    $\begingroup$ 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. $\endgroup$
    – Chuck ♦
    Commented Mar 23, 2017 at 10:14
  • $\begingroup$ 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. $\endgroup$
    – Chuck ♦
    Commented Mar 23, 2017 at 10:17
  • $\begingroup$ 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. $\endgroup$
    – Chuck ♦
    Commented Mar 23, 2017 at 10:20
  • $\begingroup$ @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. $\endgroup$
    – DieDen
    Commented Mar 23, 2017 at 10:58
  • $\begingroup$ 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. $\endgroup$
    – Chuck ♦
    Commented Mar 23, 2017 at 11:10

1 Answer 1

Reset to default
1
$\begingroup$

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.

Improve this answer
$\endgroup$
1
  • $\begingroup$ 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. $\endgroup$
    – DieDen
    Commented Mar 23, 2017 at 8:10

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.