Inverse Kinematics of Robot Manipulators with Offset Wrist

Min-Kuang Wu, Ying-Shieh Kung, Feng-Chi Lee and Wen-Chuan Chen

Ahstract- The inverse kinematics for a robot manipulator with wrist singularities [3,4,5]. These robots are commonly used
offset wrist is formulated as a minimization problem in this in welding, painting and material handling. The equations in
paper to reduce its complexity and accelerate the computation inverse kinematics become highly nonlinear and variables
speed. A specific type of robot such as Yaskawa MA1400, which are coupled, which cannot be solved by analytical approach.
has an offset between the last two axes, is chosen for analysis. Algorithms based on iterations are usually taken to solve the
Given an initial guess angle of last joint, all the 6 joint angles
inverse kinematics problem. The classical way is to compute
can be easily found through analytical solution. The output
Jacobian matrix and use Newton-Raphson method to search
value is compared with the initial guess. An error function with
for the correct solution [6]. Raghavan and Roth transform
only one variable is then defined and Newton-Raphson method
the kinematics equations into a 16 degree polynomial using
is applied to search for the solution where the error goes to zero.
the half-angle tangent of joint variables and calculate the
To check the validity of the algorithm, forward kinematics is
first computed and the position and orientation of end-effector
solutions [7]. Z. Fu, W. Yang and Z. Yang apply the theory
are then applied in the inverse kinematics to solve the joint
of Geometric Algebra to the kinematic modeling of 6R robot
angles back. The computation converges within 5 iterations in
manipulators and generate closed-form kinematics equations,
cases when the angle 85 is far away from zero and the accuracy
reformulate as a generalized eigenvalue problem, and then
is within 10.
10
degree. When 85 approaches zero, searching
yield 16 solutions [8]. The above approaches involve
direction is slightly modified to achieve convergence for the complicated computations and may not be suitable in real
algorithm. The number of iterations increases for the same time applications. To increase computation efficiency, some
accuracy. The results show that the approach presented in this methods convert the inverse kinematics problem into an
paper greatly reduces the computation time and is suitable for equivalent minimization problem without using inverse
real time application. Jacobian matrix [9, 10]. Offset modification assumes zero
offset first such that closed-form solution can be obtained
Key words: inverse kinematics, offset wrist, minimization
and then apply iterative method to minimize the error [II].
I. INTRODUCTION In this paper, similar approach like offset modification is
proposed, but the unknown variable to minimize the error is
Inverse kinematics of robot manipulator is to find the further reduced to one dimension. It reduces the complexity
joint variables when the position and orientation of the end and convergence is achieved even when the robot
effector are given. It is essential to make the transformations configuration is close to singularity.
from the Cartesian space to joint space for trajectory
planning and motion control. For a general 6-00F robot, the II. KINEMATIC AND INVERSE KrNEMATIC MODELLING
equations of 6 variables are highly coupled such that the
The Yaskawa robot MA1400, as shown in figure I, is
problem to solve the inverse kinematics is very complicated
chosen for this research. This serial manipulator has six
and time-consuming to be implemented in the real
revolute joints with offset wrist between last two axes. A
applications. So most industrial robots are built such that
coordinate frame is assigned to each link based on
three adjacent joint axes are intersecting at a common point
Denavit-Hartenberg notation. The transformation matrix
or parallel to one another. This greatly reduces the
from frame i to i-I is given by
complexity by decoupling the system into 2 sets of equations
with 3 joint variables each. The position of wrist center is sin8i sinai
used to get the first three joint angles and the orientation of
the tool is taken to solve for the other three joint angles. It -cos8i sin ai
cosai
ensures that closed-form solutions can be obtained based on
Pieper's criterion [I]. o

The special wrist configuration described above is not Table I lists the O-H parameters for the robot in Figure I.
adequate for industrial applications. Some robots are built
with offset wrist to handle high pay loads and to get long TABLE I O-H PARAMETERS FOR ROBOT IN FIGURE I
horizontal reach and appropriate angle [2]. The double
Joint i ai ai di (Ji
universal joint (OUJ) wrist has been proposed to eliminate
Research supported by ITRI Foundation.
-n/2 a1 d1 fJ1
M.K. Wu is with the Southern Taiwan University of Science and 2 0 a2 0 fJ2
Technology, Tainan, Taiwan (corresponding author to provide phone 3 -n/2 a3 0 fJ3
886·6·2533131 ext. 3520; e·mai1:w2m@mai1.stust.edu.tw).
Y.S. Kung is with the Southern Taiwan University of Science and
4 n/2 0 d4 fJ4
Technology,Tainan,Taiwan (e·mail: kung@mai1.stust.edu.tw) 5 -n/2 as 0 fJs
Feng·Chi Lee and Wen·Chuan Chen are with Mechanical and System 6 0 0 d6 fJ6
Research Laboratories, Industrial Technology Research Institute,
Hsinchu,Taiwan

978-1-4799-1851-5/15/$31.00 ©2015 IEEE

 is known as the inverse kinematics problem. For the robot
with wrist offset in Fig. 1, the rotations of link 4 and 5
would not change the position of wrist center C. This
suggests that if an initial value of 86 is assumed, the
kinematics equations can be decoupled from calculating the
4 S l
\ \ X6
position C. Multiply both sides of (1) by ( As A6 r :
4 1 I 2
\
\ ( A/A6)- (oA6) = °A1 A2 A/A4 (3)
\

\
\
\
Equating elements (1,4), (2,4) and (3,4) of (3) yields
x, C1 (a1 +a2s2+a3s23 +d4C23) = qx-d6wx-aSC6Ux+aSs6Vx=Cx (4)
sl(a1 +a2s2+a3 s23 +d4C23) = qrd6wraSC6Uy+aSS6Vy=Cy (5)
f
X, d1 +a2c2+a3c23-d4s23 = qcd6wz-aSc6uz+aSs6vz=Cz (6)
I

/ Let 86=86 g, where 86 g is the guess value of 86 .Substitute into

\ (4) and (5).81 * can be found by
J
I
!
I Xo I
81 * = atan2 (cy, cx) when al+a2c2+a3c23-d4S23 > 0 (7a)
81 * = atan2(cy, cx)+n when al+a2c2+a3c23-d4S23 < 0 (7b)
Figure 1. Robot manipulator where 81 * is the solution corresponding to the guess value
86 g. From (4), (5) and (6), summing the squares of
In this paper, fh is defined to be zero when link 2 stands
(CxCI+cySI-al) and (cz-dl) gives
vertically. Thus, 82 is replaced by 8r90° in matrix A2 . The
2 2 2 2 2
solution of forward kinematics is obtained by multiplying 2a2(a3 Crd4S3) = (CxCI+CySI-al) +(cz-dl) -a2 -a3 -d4 (8)
the transformation matrices as follows:
The angle of joint 3 corresponding to 86 g can be solved as:
( 1) 1 1
83 * = -a+ cos- (K) or -a- cos- (K) (9)
l

Let °A6 = l � :� :� :�j ,

�U
o
Vz Wz qz
z
0 0 1
substitute D-H parameters of
where a=atan- (dJa3) and
2 2
K=[(CXC1+CySI-al) +(ccdl) -al-a/-dl]j( 2a2 �a32 + d/ ).
Table 1 into (1) and expand the matrices, the elements ofo A6 The two solutions of 83 * are corresponding to the robot
are obtained as follows: elbow up or down. After 81 * and � * are obtained, substitute
into equations (4), (5) and (6) to compute 82 *,
Ux = CI[S23(C4CSC6-S4S6)+C23 SSC6]+SI(S4CSC6+C4S6)
uy = Sl[S23(C4CSC6-S4S6)+C23SSC6]-C1 (S4CSC6+C4S6) Once 81 *, 82 * and � * are found, all elements of matrices
l 2
°Al A2 A3 are known. Multiply both sides of (1) by
Uz = C23 (C4CSC6-S4S6)-S23SSC6 l 2 I
(0Al A2 A3 )- to solve for the remaining angles:
Vx = -c1 [S23(C4CSS6+S4C6)+C23SSS6]+S1 (-S4CSS6+C4C6) 1 2 1 3 4 S
(OAI A2 A3)- 0A6 = A4 As A6 (10)
Vy = -s1 [S23(C4CSS6+S4C6)+C23 SSS6]-CI(-S4CSS6+C4C6)
Vz = --c23(C4CSS6+S4C6)+S23SSS6 From element (3, 3), joint angle 85* is given by
Wx = -C1 (S23C4SS-C23CS)-SlS4SS 1
U11
s * = cos- (C1 C23 Wx+S1 C23 WrS23 Wz) or (IIa)
Wy = -Sl(S23C4SS--c23CS)+CIS4SS I
85* = -cos- (CIC23 Wx+SIC23Wy-S23Wz) (11b)
Wz = -C23C4SS - S23 CS
Angle 8/ is solved from element (1, 3) and (2, 3) of the
qx = CI[al+a2s2+a3s23+d4C23 -d6(S23 C4SS-C23 CS)]-d6S1 S4SS matrix in (10) as follows:
+as[CI(s23 C4CS + C23SS) + SIS4CS]
C4 = --(C1 S23Wx+S1 S23Wy+C23Wz) / Ss (I2a)
qy = Sl[al+a2s2+a3 s23+d4C23-d6(S23C4SS-C23CS)]+d6CIS4SS
S4 = -(SlWx--c1Wy) / Ss (I2b)
+as [SI(S23 C4CS+C23SS)-CIS4CS]
Finally, 86 * is obtained from element (3,1) and (3, 2):
qz = d1 +a2C2+a3C23 -d4S23-d6(C23C4SS+S23CS)
C6 = (C1 C23 Ux+S1 C23UrS23Uz) / Ss (13a)
+as (C23 C4CS-S23 SS)
S6 = -(C1 C23 Vx+S1 C23VrC23Vz) / Ss (l3b)
Above results are compared with those for the robot
which has no offset at wrist, i.e. as=O. The orientation When the above solutions 81 * to 86 * are substituted into
vectors of u, v, ware exactly the same. The offset does not kinematics equations, the orientation of end-effector would

[ ]
change the orientation. The position is shifted by be exactly the same as required, but the position of the wrist
center will deviate from the correct one by some distance. If
C1 (s 23 c4c S + c 23 sS) + S1S4CS the guess value 86 g can be made to be equal to 86 *, then 81 *
Offset = sl (s 23 c4cS + c 23 sS) -cls4c5 x as (2) to 86 * would be the solution for inverse kinematics.
c 23 c4cS -s 23 sS 111. ALGORITHM FOR INVERSE KINEMATICS

When last frame °A6 is given, solving the angles of joints Define two functions:
 *
Inv_g(86g) = 86 (14a)
*
Error(86g) = Inv_g(86g) - 86g = 86 - 86g (14b)
The inverse kinematics problem is equivalent to find the
solution for the equation: Error(86g) = o. The advantage of
(14) is to convert a six to six function into an one to one
function. The computation would be greatly simplified and
convergence for iteration could be better controlled. To
check the properties of the function, Error(86g) is plotted in
Figure 2 to 11 for the following 10 cases:
(1) Arm and wrist are bending downwards, 8] =45.1 0,
Figure 6. Case 5 Figure 7. Case 6
82=23.6°, �=12.7°, 84�56.7°, 85=34.2°, 86=78.9°
:s
(2) Arm and wrist are bending upwards, 8]�7.8°, 82=78.3°,
� �::;v�I
83=-95.6°, 84=47.8°, 8s�27.8°, 86�123.4° -,
(3) Arm is downwards and wrist is upward, 8)=45.1 0,
3
82=23.6°, 83=12.7°, 84�56.7°,85=-27.8°,86=78.9°
(4) Arm is upwards and wrist is downward, 8]=-67.8°, §
,�
..,.
L-
82=78.3°, ��95.6°, 84=47.8°, 85=34.2°, 86�123.4°
(5) Arm and wrist are bending downwards but 85 is small, "�
,,..
..., • .;.;., lWoD J4t '" ,;,
...
�
aai;l, m

8]=45.1°, 82=23.6°, �=12.r, 84�56.7°,85=0.2°,

86=78.9° Figure 8. Case 7 Figure 9. Case 8
(6) Arm and wrist are bending upwards but 85 is small, :=J
8)=-67.8°,82=78.3°, ��95.6°, 84=47.8°,8s�0.1°, ::1
86=-123.4° :!1
(7) Arm is downwards and wrist is upward, but 85 is small, �
8)=45.1°, 82=23.6°, �=12.7°, 84�56.7°, 8s�.2°,
86=78.9° � --�
(8) Arm is upwards and wrist is downward, but 85 is small, .S
�
::1V
I
8)=-67.8°,82=78.3°, ��95.6°, 84=47.8°, 85=0.1°, ·mo-�II ,.. '" ,.. 1101 IIIQ.O ,0.. Of to. IoN noo
... ....
86=-123.4°
(9) Arm is downwards and 85=0°, 8]=45.1°,82=23.6°, Figure 10. Case 9 Figure II. Case 10
�=12.7°, 84�56.7°, 85=0°, 86=78.9° From figure 2 to 5, it can be seen that Error(86g) is near a
(10) Arm is upwards and 85=0°, 8]�7.8°, 82=78.3°, straight line when 85 is far away from zero. It is easy to
��95.6°, 84=47.8°, 85=0°, 86=-123.4° solve for Error(86g)=0 by searching along the gradient
direction. Fast convergence can also be expected. When 85 is
'::':1
,-
getting smaller, some curves show discontinuities at the
point where Inv_g(86g)=180° or -180° and the gap is equal
to 360°. It is clearly caused by the definition that the value
of function atan2 is constrained within the range (-180°,
180°]. This may cause divergence in iterations. To solve the
,..., problem, the range of function parameter 86g can be
":::l extended from (-180°, 180°] to (-540°, 540°] and function
Error(86g) keeps the same outputs in the following three
Figure 2. Case 1 Figure 3. Case 2 regions: (-540°, -180°], (-180°, 180°] and (180°, 540°].
""..
Further, the function value of Error(86g) is limited within
- (-180°,180°]. Thus, the next iteration point 86g(i+)) should be
::J
."., modified as follows:
....,
..,.,
...., If Error(86g(i) >180° => 86g0+]) = Inv_g(86g(i)) -360°
,.., ----"..,---/
- If Error (86g(i)) :::;-180° => 86g(i+]) = Inv_g(86g0)) +360°
".,
.:i Once 86g is defined in the region (-540°, 540°], there exist
:::J three solutions for Error(86g)=0. No matter what solution is
, ...,
-,
'­
""'.,
found, it can be transformed back to a value within (-180°,
..,.
NOO 11001 )IIQ.O • llQoD
.." ,0.. No *
...
I� 180°] since three solutions represent the same angle.
Figure 4. Case 3 Figure 5. Case 4 When 85 is small in case 5 to 8, a sharp drop exists
around the angle where 86g is the right solution and Error(86g)
function goes across zero point. When 85 is zero, the drop
becomes vertical and discontinuity occurs. The iteration may
blow out if searching is following the direction of slope. end-effector. Results in Cartesian space are then applied in
Thus, the algorithm presented in this paper uses two guess inverse kinematics to convert back to joint space to obtain
points 86g1 and 86g2. Next point 86g3 is found by linear angles ( 8i ,82 ,83,84 ,85,86 ). The error is defined as
interpolation as below:
e6g2 -e6g1
e6g3 =
e6g1 - x Error(e 6gd (15)
Error(e 6g2) - Error(e 6g1) i=l
(16)
6
If the signs of e6g1 and e6g2 are different, discard the point 10
which has the same sign as 86g3 and replace it by 86g3. Allowed tolerance of error is set to be 10- degree. Variables
Otherwise, replace the one which has larger error. Repeat in programs are running at double precision. The following
iteration until error is smaller than the allowed tolerance. cases are tested and results are shown in the following
The algorithm for inverse kinematics can be summarized as figures. Figure (a) shows the error defined in (16) and figure
follows: (b) shows the number of iterations needed for inverse
kinematics. The error in (a) is expressed in logarithmic scale.
Step I : Guess 86g1
(I) Arm and wrist are bending downwards, 81=45.1°,
Step 2 : Compute Error(86gl) 82=23.6°, 83=12.7°, 84�56.7°, 8s=34.2°, 86=78.9°.
Initial guess of 86g is obtained by assuming as=Omm.
If Error(86gl) < 0 => 86g2 =86g1 + 0.1
(1.1) 81 is replaced by a rotation from -179° to 179°.
Else => 86g2 =e6g1- 0.1
Compute Error( e6g2)
Step 3 :
e6g2 -e6g1
e6g3 =
e6g1 - x Error(e6g1)
Error( 6g2) -Error(e6g1)
e

Step 4 : If I Error(86g3)1 < Tolerance => End

Step 5 : If Error(86gl) x Error(86g2) > 0 .,...." -
0-

If I Error(86g2)1 < I Error(86g1) 1 => Figure 12(a). Errors Figure 12(b). No. of iterations
e6g1 - 86g2 ' Error(86g1) - Error(e6g2) (l.2) 84 is replaced by a rotation from -179° to 179°.
Else
If Error(86g3) x Error(86g2) < 0 =>

86g1 - 86g2 ' Error(86gl) - Error(86g2)

"

86g2 - 86g3 ' Error(86g2) - Error(86g3) ill

,.
�

�:.�
Step 6 : Go to Step 3 ,

"
It can be seen from case 6 and 7 that there are two
solutions in either case. Initial guess must be carefully .I,;.,.a '[WI --.it..!! g.e ..aa ltiul 1VJ..a �
-
chosen to prevent from falling into a solution which is not
expected. Since wrist offset is usually small, one way for Figure l3(a). Errors Figure l3(b). No. of iterations
initial guess is to assume as=Omm and solve inverse (l.3) 86 is replaced by a rotation from -179° to 179°.
kinematics analytically. Take 86 of this solution for initial Ill,
guess. The other way is to choose a point which is known to
be near the correct solution. For example, the end-effector of
robot manipulator is planned to move from point PO to Pn
following a trajectory. The joint angles of PO and Pn are
known values since they must be specified by the user first.
If the trajectory is divided into n sections, PI, P2 ... Pi ... Pn-I
are intermediate points sequentially. To find the joint angles
for Pi, initial guess can be obtained from Pi-I.
IV . SlMULAT10NS AND RESULTS
Figure 14(a). Errors Figure 14(b). No. of iterations
Forward and inverse kinematics described above are
verified for Yaskawa robot MA1400. Its O-H parameters are (2) Arm and wrist are bending upwards, 81=-67.8°, �=78.3°,
listed below: al = 150mm ' d1 = 555mm ' a2 = 760mm ' ��95.6°, 84=47.8°, 85=-27.8°, 86�123.4°. Initial
a3 = 220mm ' d4 = 870mm ' as = 45mm ' d6 = 185mm. Joint guess of 86g is obtained by assuming as=Omm.
angles (8j, 8b 83, 84, 85, 86) are first substituted in forward (2.1) 81 is replaced by a rotation from -179° to 179°.
kinematics to compute the position and orientation of
 converges to an unexpected solution when ()s is about
"J
" between _3° and 0°. This indicates that selection of initial
condition is important. When there is more than one solution,
it may lead to the wrong one. Now assume that initial guess
is close to the correct solution. Let ()6g=()6+1° and run the
program again. Results are shown in figure 19.

. ..;
�
-
"'
''''
n,
Figure 15(a). Errors Figure 15(b). No. of iterations �
""
(2.2) ()4 is replaced by a rotation from -179° to 179°. �1
.."
£JrQftll09� '"
'"
�"

...
S----iP.'
,.�-
�J,.r,lmo . 'U - !IIIJI .:n.,o ::. lU � >u tm.o UUI

Figure 19(a). Errors Figure 19(b). No. of iterations

The results show that there is a unique solution when ()s =0°.
:� The error is within 0.01 The maximum error occurs when
0.

"i ()s is in the range between _2° to _3°. To further examine

.� _.� .IIUI -"u :.. ... lena .� ';au.
this situation, let ()1=45.lo, �=23.6°, ()3=12.7°, ()4�56.7°,
Figure 16(a). Errors Figure 16(b). No. of iterations ()s=-2.5°, ()6=78.9°, and plot function Error(()6g) in figure
(2.3) ()6 is replaced by a rotation from -179° to 179°. 20(a). Figure 20(b) shows the portion in detail where ()6g is
between 60° and 90°.
:1 ::.
..
----,,---
.,
..
..
c•
..
"'i
'"

::r-- - --"-_£.
4>'
...
..
.,.;
..
... ,
..
..
1'IOtI -JOU 100 0.0 \00 1000 I""
....
Figure 17(a). Errors Figure 17(b). No. of iterations
Figure 20(a). Error(()6g) (b). Error(()6g) in detail
Above results indicate that no matter when the arm or
wrist are bending upwards or downwards,computations end It shows that there are four solutions in this situation. Two of
10 them are very close to each other. They are only 3° apart.
within 5 iterations with accuracies better than 10. degree.
The algorithm is fast and accurate enough to be implemented Maximum error in figure 19 is about 0.6°. It indicates that
in real time. inverse kinematics converges to the correct solution but it is
The following case will show the condition when wrist is not accurate enough. Therefore,initial guess of ()6g should be
bending from upwards to downwards, which goes across correctly taken and solution from inverse kinematics must be
wrist singularity which occurs when wrist offset does not carefully checked when ()s is small.
exist. Let ()j=45.1°, ()2=23.6°, ()3=12.7°, ()4�56.7°, ()6=78.9°,
and ()s rotates from -120° to 120°. Initial guess of ()6g is V. CONCLUSION
obtained by assuming as=Omm. Results are shown in figure An algorithm to solve the inverse kinematics is presented
18. in this paper for a 6-axes robot with wrist offset. It converts
a set of equations with 6 degrees of freedom to a nonlinear
function with only one variable. The algorithm does not use
Jacobian matrix. So the computation speed can be greatly
improved and convergence can be better controlled. When
10
wrist angle is away from 0°, accuracy within 10. degree
'"
can be achieved in 5 iterations. When wrist angle is close to
."
�,
0°, there might be multiple solutions. The numbers of
...
" iterations increase for the allowed tolerance. Correct initial
:! .r----r<L-......_.r-- guess should be taken and solution must be carefully
0.0 . . , . ,
l�_LOOmDG�tlCIDCJ:) ut.e-IDQ.tIJ$.O
-1O.D.J'!o.D 0.0 lU � n.DIDGOtlU
.... lO.Cioo:Ilr:nCo:Dl ....
checked for inverse kinematics.
Figure 18(a). Errors Figure 18(b). No. of iterations
It can be found in the figure that the inverse kinematics
 ACKNOWLEDGMENT

The research is supported by Industrial Technology

Research Institute.
REFERENCES

[I] D. L. Pieper,The kinematics of manipulators under computer

control," PhD dissertation,Comput. Sci. Dept Stanford Univ ,Oct.
1968
[2] Kucuk,S ,Bingul,Z. Inverse Kinematics Solutions of Fundamental
Robot Manipulatorswith Otlset Wrist. Mechatronics. pp. 197-201,
2005
[3] Trevelyan,.l. P.,Kovesi,P.O. ,Ong,M. ,and Elford,D.,"ET: A Wrist
Mechanism Without SingularPositions", The International Journal of
Robotics Research, Vol. 4,No. 4,pp. 71-85,1986
[4] Milenkovic,V. ,"New Nonsingular Robot Wrist Design",Robots II
Coriference Proceedings RIISME, Chicago,IL,pp. 13.29-13.42. ,
1987
[5] Rosheim,ME,"Singularity-Free Hollow Spray Painting Wrists",
Robots II Coriference Proceedings RIISME, Chicago,IL,pp.
13.7-13.28. ,1987
[6] L. W Tsai,Robot Analysis-The Mechanics of Serial and Parallel
Manipulators,John Wiley & Sons, Inc,1999
[7 ] M. Raghavan,B. Roth,Inverse kinematics of the general 6R
manipulator and the related linkages,Trans. ASME I Mech. Des. 115
502 - 508., 1993
[8] Z Fu,W. Yang and Z. yang,"Solution of Inverse Kinematics for 6R
Robot Manipulators with Offset Wrist Based on Geometric Algebra",
Journal of Mechanisms and Robotics,Vol. 5,August 2013
[9] AA Goldenberg,IA Apkarian,H. W. Smith,A new approach to
kinematic control of robot manipulators, ASME.I. Dyn. Syst. Meas.
Control 109,97-103 ,1987
[10] AS. Hall Jr. ,R. R Root,E. Sandgren,"A dependable method for
solving matrix loop equations for general three-dimensional
mechanism",ASME.I. Eng. Ind. 547-550.,1997
[11] H. Pan,B. Fu,L. Chen,and.l. Feng,"The Inverse Kinematics
Solutions of Robot Manipulators with Onset Wrist Using the Onset
Modification Method",Advances in Automation and Robotics,Vol. 1,
LNEE 122,pp. 655-663,2011