# Inverse kinematics showing incorrect results for 4 dof robot in MATLAB using Robotics Toolbox

I'm doing inverse kinematics for 4 dof robot using robotics toolbox matlab. The code is given below:

    preach = [0.326 0.223 0.342]; % reach point of end-effector
% create links using D-H parameters
% Link('d', 0.15005, 'a', 0.0203, 'alpha', -pi/2)
L(1) = Link([0  0     0.15   pi/2    0], 'standard');
L(2) = Link([0  0     0.15   0       0], 'standard');
L(3) = Link([0  0     0.15   0       0], 'standard');
L(4) = Link([0  0     0.15   0       0], 'standard');
% set limits for joints
%build the robot model
qready = [0 0 0 0]; % initial position of robot
T1= transl(preach); % convert of reach point of 4x4 homogenous matrix
[qreach,err] =  rob.ikcon(T1, qready); % find inverse kinematics with error


Matlab shows results like this(using robotics toolbox ):

    >> [qreach,err] =  rob.ikcon(T1, qready)
qreach =
2.7925    0.7854    1.0472    0.8727
err =
9.6055


I'm not taking preach = [0.326 0.223 0.342]; randomly. Infact, first I do forward kinemtics to get these points. code is below:

    % to find forward kinemtics
qreadyrr = [0.6 0.45 0.63 0.22]; % setting the four angles randomly within range to get preach


then, I got T0 as

     >> T0
T0 =
0.2208   -0.7953    0.5646    0.3267
0.1510   -0.5441   -0.8253    0.2235
0.9636    0.2675    0.0000    0.3421
0         0         0    1.0000


Also, when I put this T0 in place of T1 in inverse kinematics code as given above, the values I got is very accurate with negligible error.

    >> [qreach,err] =  rob.ikcon(T0, qready)
qreach =
0.6002    0.4502    0.6296    0.2204
err =
4.6153e-07


The point is, in my case, I have only px, py and pz values for transformation matrix but with this, inverse kinematics is not solving it correctly. I want to do inverse kinematics px, py and pz values. how can I do it correctly.

Thanks.

The T0 matrix in your code has an orientation component which (since it comes from the forward kinematics) is a reachable orientation.

The T1 matrix in your code has been created with the command:

T1= transl(preach);


It most probably look like this

T1 =
1.0000   0.0000    0.0000    0.3267
0.0000   1.0000    0.0000    0.2235
0.0000   0.0000    1.0000    0.3421
0         0        0    1.0000


This specifies that the orientation of the end-effector is [0, 0, 0]. This orientation might not be possible to reach in the position you have specified.

• Thanks for response@50k4, but, I tries for many points and it shows error components every time high. So, what should I do to remove these error. I'm not concern about end-effector orientation, I just concern about end-effector position. Commented Dec 18, 2016 at 13:52
• You are not concerned about the orientation, however by specifying it to be zero, you tell the solver that you need zero orientation. There is a discrepancy between what you are trying to do and what you implemented. It is not important how many points you try, you still are in the same situation. (I think if you give a point that lies on the Oz axis you might be able to get a good solution.)
– 50k4
Commented Dec 18, 2016 at 13:57
• Since you have 4 dof and you only care about 3, you could calculate an orientation that is not zero and is reachable for the given position and use that in the IK instead of zero orientation. Since the critical part is rotation around the Z axis I would start by calculating what is the "natural" orientation of the robot around the Z axis for a given px and py and use that instead of 0 for the IK. That should bring you one step further. After, you can do the same for X and y orientations
– 50k4
Commented Dec 18, 2016 at 14:00
• Can you please tell me or suggest me any pdf to calculate natural orientation of the robot around the Z axis for a given px and py. I have read about IK, rotation matrix, DH parameters etc. but I struck here. Can you please explain me how to calculate orientation matrix which will work accurately for every position in robot workspace. Commented Dec 19, 2016 at 10:48
• If I am not mistakeing, the Z axis orientation (gamma) will be gamam = atan2(px, py). you will need to set another orientation which is reachable (aside from this), then the solver will be able to cope with the IK. I did not recreate your mechanism, but to solve FK for a few points and see if any of the orientations are constant. If yes then use that constant orientation and the Z orientation to set a reachable pose for the IK
– 50k4
Commented Dec 19, 2016 at 10:54

I wrote a short article about the problems with IK for 4-axis robots which might help explain the issues.

You should probably use the ikine() function with a 'mask' option.

You will get much fast answers to questions regarding the Robotics Toolbox for MATlAB at its own support forum.