There are a couple of possible variations of this position. I chose it so that positive rotations around the horizontal joint axes raised the link (instead of lowering it). Note that all the X axes are parallel. From this sketch, we can get the DH parameters as follows:
| Link | angle - theta | offset - d | length - a | twist - alpha |
| 1 | variable | d1 | 0 | 90 |
| 2 | variable | 0 | a2 | 0 |
| 3 | variable | 0 | a3 | 0 |
| 4 | variable | 0 | 0 | 90 |
| 5 | variable | d5 | 0 | 0 |
From these parameters, we can get the following A matrices:
The link constants are (all in mm): d1 = 214, a2 = 200, a3 = 150, and d5 = 85.
For all joint angles set to 0, this gives the following transform to the end effector. Compare this with the figure above - it gives the direction of the axes of frame 5 and the location of the origin with respect to frame 0.
Furthermore, the following (laborious!) checks can be made for the elements of T5:
Other computational checks should be made as well. These are discussed below when the forward kinematics is implemented in a MATLAB routine.
the result of the matrix multiplication is
From the (3,3) and (3,4) elements we get
from which we get
The best solution to use involves px and py, unless these are both zero. Then we could use ax and ay. If these are both zero, then the pose in T5 is on the Z0 axis, with the hand pointing straight up. In this case, set theta_1 = 0. The second solutions (180 degrees away) can be used if the joint limits are exceeded by the first solution.
From the (1,3) and (2,3) elements we get
from which (since theta_1 is known)
Now, comparing the (1,4) and (2,4) elements and using the dummy variables
we have the system of equations
If we square both equations and add them, and use the trig identity for cos((A+B)-A) = cos(B), we get
Now we are stuck because we cannot find an independent expression for S3. We are forced to use
For the Mitsubishi, the elbow angle must always be negative, so we can always take the negative square root here. If C3 is larger than 1 (note that the square root function would fail), it indicates that the desired position is out of reach (t1 and/or t2, that depend on px and py, are too big).
Now we can return to the system of equations above. If we expand the terms in C23 and S23 we will get the following
These can be solved using Cramer's rule to get expressions for S2 and C2. We then use these with the atan2 function to get
Now that we know theta_2 and theta_3 we can use a previous result
The last joint angle is obtained from the (3,1) and (3,2) elements
so that
This completes the inverse kinematics. Along the way we should be checking for illegal operations (for example, square root of a negative number), or for angles that exceed the joint limits.
It is interesting to note that we obtained all the relations required for this solution from the first matrix multiplication above. This is not generally the case - we got lucky!
Here is a (slightly edited) diary file of some MATLAB operations involving the above routines:
>mitsudh
>dh
dh =
1.5708 0 0 214.0000 0
0 200.0000 0 0 0
0 150.0000 0 0 0
1.5708 0 0 0 0
0 0 0 85.0000 0
Note that the last column is necessary for ikine() - it identifies all joints as revolute.
>M'
ans =
1 1 1 0 1 1
This selection vector tells ikine() that the Mitsubishi does not have yaw capability (rotation around the X axis of the end effector).
>mittest
Starting joint angles (degrees):
-64.3013 50.4792 -68.3258 72.6446 175.4369
----- FORWARD KINEMATICS -----
direct solution using mitsufk:
T5 =
-0.3209 0.8783 0.3543 147.2246
0.4833 0.4736 -0.7363 -305.9273
-0.8145 -0.0650 -0.5765 273.3094
0 0 0 1.0000
flops = 58
indirect solution using fkine:
T5 =
-0.3209 0.8783 0.3543 147.2246
0.4833 0.4736 -0.7363 -305.9273
-0.8145 -0.0650 -0.5765 273.3094
0 0 0 1.0000
flops = 690
These results are identical. Notice the difference in flop count. fkine() wastes a lot of time regenerating the A matrices from scratch and multiplying them out each time it is called. The symbolic solution is more than 10 times faster.
Continuing on with mittest:
----- INVERSE KINEMATICS -----
direct solution using mitsuik:
flops = 65
solution:
-64.3013 50.4792 -68.3258 72.6446 175.4369
numerical solution using Toolbox:
original guess:
-61.6261 52.4876 -66.2201 71.5652 173.4223
ikine iterations = 104
flops = 556455
solution using ikine:
-64.3013 50.4799 -68.3292 72.6520 175.4351
maxerror =
0.0075
The solution found with mitsuik is exactly the same as the original vector used to generate T5. The first guess for the iterative solution implemented in ikine() is very close (within 3 degrees for each joint) to the original vector. The convergence tolerance in ikine() was changed from the value 1e-12 used in the original script to a value set by the user (more on this below). Even so, it took over 100 iterations, and more than half a million flops to produce the solution compared to 65 for the direct solution in mitsuik!
The performance of ikine() was, to say the least, unpredictable! It sometimes found the solution in as few as 3 or 4 iterations, while at other times it exceeded the maximum limit of 1000 iterations without converging. Tightening the convergence tolerance generally caused it to take longer to converge, as expected. Of more interest was the effect of the first guess. As mentioned above, this first guess vector was obtained from the known solution and a randomly generated perturbation. The amplitude of this perturbation is set in mittest. The question was how close the first guess has to be to the actual solution. I tried amplitudes corresponding to 3, 6, 12, and 60 degrees. Unfortunately, my study was inconclusive. We would expect ikine() to take longer to converge if the first guess was farther away from the solution. There was some evidence of this in the runs I did (50 in total), but because the first guesses were not identical for each run, it is hard to make a firm conclusion. I did find that if the first guess was very far away, the solution would sometimes converge to values more than a full revolution away (note that any joint angle can be incremented by 2*pi and still satisfy T5).
In all these runs, ikine() averaged over 5000 flops per iteration. In each iteration, it must evaluate the forward kinematics, the manipulator Jacobian, and its inverse - this is a lot of work! This can be compared to the constant 65 flops required by the direct solution implemented in mitsuik.
Furthermore, ikine() is incapable of detecting illegal joint positions, since it knows nothing about the joint limits.
Send a message to roger.toogood@ualberta.ca