# Kinematics Analysis of 6-DoF Articulated Robot with Spherical Wrist

Authors: Seemal Asif; Philip Webb
Journal: Mathematical Problems in Engineering
 (2021)

---

## Abstract

The aim of the paper is to study the kinematics of the manipulator. The articulated robot with a spherical wrist has been used for this purpose. The Comau NM45 Manipulator has been chosen for the kinematic model study. The manipulator contains six revolution joints. Pieper’s approach has been employed to study the kinematics (inverse) of the robot manipulator. Using this approach, the inverse kinematic problem is divided into two small less complex problems. This reduces the time of analysing the manipulator kinematically. The forward and inverse kinematics has been performed, and mathematical solutions are detailed based on D-H (Denavit–Hartenberg) parameters. The kinematics solution has been verified by solving the manipulator’s motion. It has been observed that the model is accurate as the motion trajectory was smoothly followed by the manipulator.


---

## Body

## 1. Introduction

Locomotion is the process of causing a rigid body to move. The body needs force to move. Dynamics is the study of the motion of the body in which forces are modelled which helps the body to move, whereas kinematics is the geometrical study of the motion of the body without considering the forces that can affect the motion of the body.Kinematics is the motion description of the rigid body. [1] Links are the connectivity body/member between joints. The kinematic chain is a grouping of links connected by joints, as illustrated in Figure 1. In the kinematic chain, the number of DoF (degree of freedom) is equal to the number of joints.Figure 1
Kinematics of the manipulator: (a) open chain; (b) closed chain [2].
(a)(b)Maintaining a strong connection between the two joints is called the kinematics function of a link. This connection can be described with the following factors:(i)
a: link length(ii)
α: link twistLink length is measured along the line which is mutually perpendicular to both joints/axes. The perpendicularity in joints always exists except when both joints are parallel. Link twist is the angle of projection from the previous joint (i−1) to the next joint (i) onto the axis i−1 (previous joint); the projection line is parallel to the next joint (axis i). The relationship between link length and twist is described in Figure 2.(i)
A joint axis is formed at the connection of two links. This joint will have two parameters (one for each link) connected to it. These parameters are as follows:(ii)
d: distance between links(iii)
ϴ: angle between linksFigure 2
Kinematic function of link.The relative position or distance between the links is called link offset. Figure3 describes these parameters, in which the joint angle is the angle between the links.Figure 3
Link and joint parameters. Joint parameters are highlighted in red.The four parameters demonstrated above are associated with each link. Axes can be aligned using these parameters. The parameters are also known as Denavit–Hartenberg link parameters. These are illustrated in Table1 below:Table 1
Denavit–Hartenberg link parameters.
Link parametersJoint parametersa: link lengthd: distance between linksα: link twistϴ: angle between linksThe link numbering convention follows from the base of the arm till the last moving link. As mentioned in Figure4, the first link is the connection between the base and first joint.Figure 4
A 7-DoF manipulator arm [3].The parameters mentioned above in Table1 are used for kinematic modelling of the robot. In kinematics, modelling the geometry of the robot is represented. Homogenous transformation (of the matrix) is commonly used as the definition of the kinematics model (particularly for chains mechanism). As described below,(1)Tn0=T1T2T3…Ti…Tn,where n is the number of links, Ti is the link transformation from the ith joint, and Tn0 is the final pose for end-effector relative to the base.There are two main types of kinematic models: forward kinematics and inverse kinematics. In forward kinematics, the length of each link and angle of each joint is given, and through that, position of any point (x, y, z) can be found. In inverse kinematics, the length of each link and position of some points (x, y, z) is given, and the angle of each joint is needed to find to obtain that position.Several models are developed for kinematic modelling, but the D-H (Denavit–Hartenberg) model [4] is the most popular model. Limitations of the D-H model are discussed, and CPC (completeness and parametric continuity) model and its mapping with the D-H model were proposed [5]. The parametric continuity of the CPC model was achieved by using singularity free line representation.


## 2. Forward Kinematics of Comau NM45

The Comau NM45 [6] is a medium-scale robot. It has 6 degree of freedom joints. It is an articulated arm with a spherical wrist. The wrist joint intersects at one point. Figure 5 shows the manipulator with its link length and working envelope.Figure 5
Comau NM45 (inline) operative area and link lengths.Forward kinematics is the study of the manipulator to find out its tip or end-effector position and orientation by using joint values of the manipulator. The first step of performing the forward kinematics is to label link lengths. This step has been performed in Figure5. The second step to find the forward kinematics of the manipulator is to assign the frames. The frame assignment is done in Figure 6.Figure 6
NM45 model with coordinates mapping.The D-H parameters can be found based on the frame assignment. The modified DH convention has been used for the frame assignment and DH parameters [7, 8]. These parameters are illustrated in Table 2.Table 2
Comau NM45 DH parameters.
LinkLink lengthαi, mTwist angleαi, degJoint offsetJoint angleθi, deg1L1 = 0.490D1 = 0.75θ12L2 = 0.7500θ23L3 = 0.25900θ340−90D4 = 0.8124θ450900θ5600D6 = 0.175θ6The transformation matrix for a linki is described as follows:(2)Ai=cosθi−sinθicosαisinθisinαiαicosθisinθicosθicosαi−cosθisinαiαisinθi0sinαicosαidi0001.A1 is the transformation matrix T10:(3)T10=cosθ1−sinθ1cosα1sinθ1sinα1α1cosθ1sinθ1cosθ1cosα1−cosθ1sinα1α1sinθ10sinα1cosα1d10001,and α=90, so T10 will be as follows:(4)T10=cosθ10sinθ1α1cosθ1sinθ10−cosθ1α1sinθ1010d10001,whereas a1 = l1 = 0.4 and d1 = 0.75; by placing these values above, the following equation which is the resultant for the transformation between the base and joint 1 is obtained:(5)T10=cosθ10sinθ1α1cosθ1sinθ10−cosθ1α1sinθ10100.750001.ForA2=T21,(6)T21=cosθ2−sinθ20α2cosθ2sinθ2cosθ20α2sinθ2001d20001,whereas a2 = l2 = 0.75 and d2 = 0; by placing these values above, the following equation which is the resultant for the transformation between joint 1 and joint 2 is obtained:(7)T21=cosθ2−sinθ200.75cosθ2sinθ2cosθ200.75sinθ200100001.ForA3=T32,(8)T32=cosθ30sinθ3α3cosθ3sinθ30−cosθ3α3sinθ3010d30001,whereas a3 = l3 = 0.25 and d3 = 0; by placing these values above, the following equation which is the resultant for the transformation between joint 2 and joint 3 is obtained:(9)T32=cosθ30sinθ30.25cosθ3sinθ30−cosθ30.25sinθ301000001.ForA4=T43 and α=90,(10)T43=cosθ40−sinθ4α4cosθ4sinθ40cosθ4α4sinθ40−10d40001,whereas a4 = 0; by placing the value of a4 in the above equation, the following equation which is the resultant for the transformation between joint 3 and joint 4 is obtained:(11)T43=cosθ40−sinθ40sinθ40cosθ400−10d40001.ForA4=T54,(12)T54=cosθ50sinθ5α5cosθ5sinθ50−cosθ5α5sinθ5010d50001,whereas a5 = 0 and d5 = 0; by placing these values in the above equation, the following equation which is the resultant for the transformation between joint 4 and joint 5 is obtained:(13)T54=cosθ50sinθ50sinθ50−cosθ5001000001.ForA5=T65 and α=0,(14)T65=cosθ6−sinθ60α6cosθ6sinθ6cosθ60α6sinθ6001d60001,whereas a6 = 0; by placing the value of a6 in the above equation, the following equation which is the resultant for the transformation between joint 5 and joint 6 is obtained:(15)T65=cosθ6−sinθ600sinθ6cosθ600001d60001.


### 2.1. Transformation

For simplification, the following has been substituted:(16)θ1=u,θ2=v,θ3=w,θ4=a,θ5=b,θ6=m.ForA12=T20=T10×T21, substitute values from equations (5) and (7):(17)T20=T10XT21=cosu0sinu0.4cosusinu0−cosu0.4sinu0100.750001⋅cosv−sinv00.75cosvsinvcosv00.75sinv00100001.The resultant transformation between the base and joint 2 is illustrated as follows:(18)T20=cosucosv−cosusinvsinucosu0.75cosv+0.4cosvsinu−sinusinv−cosu0.75cosv+0.4sinusinvcosv00.75sinv+0.750001,ForA123=T30=T10×T21×T32, for simplicity,(19)m=cosu0.75cosv+0.4,n=0.75cosv+0.4sinu,u=0.75sinv+0.75,T30=T20XT32=cosucosv−cosusinvsinumcosvsinu−sinusinv−cosunsinvcosv0o0001⋅cosw0sinw0.25coswsinw0−cosw0.25sinw01000001.The resultant transformation between the base and joint 3 is illustrated as follows:(20)T30=cosucosv+wsinucosusinv+wm+0.25cosucosv+wcosv+wsinu−cosusinusinv+wn+0.25cosv+wsinusinv+w0−cosv+w0+0.25sinv+w0001.ForA456=T63=T43×T54×T65,(21)A45=T53=T43×T54=cosa0−sina0sina0cosa00−100.81240001⋅cosb0sinb0sinb0−cosb001000001,A45=T53=cosacosb−sinacosasinb0sinacosbcosasinasinb0−sinb0cosb0.81240001,T63=T43×T54×T65=T53×T65=cosacosb−sinacosasinb0sinacosbcosasinasinb0−sinb0cosbd40001⋅cosm−sinm00sinmcosm00001d60001.The resultant transformation between joint 3 and joint 6 is illustrated as follows:(22)T63=cosacosbcosm−sinasinm−cosmsina−cosacosbsinmcosasinbd6cosasinbcosbcosmsina+cosasinmcosacosm−cosbsinasinmsinasinbd6sinasinb−cosmsinbsinbsinmcosbcosbd6+d40001.Spherical wrist position can be extracted using the last column ofT60 transformation matrix, where(23)T60=r11r12r13Pxr21r22r23Pyr31r32r33Pz0001,Which means the T60 the transformation matrix can be represented in terms of R60 and P60 illustrated as follows:(24)T60=R60P6001,and spherical wrist position is illustrated as(25)P60=PxPyPz.Transformation matrix from base to end-effector is(26)T60=T10×T21×T32×T43×T54×T65,T60=r11r12r13Pxr21r22r23Pyr31r32r33Pz0001,T60=R60P6001.


## 2.1. Transformation

For simplification, the following has been substituted:(16)θ1=u,θ2=v,θ3=w,θ4=a,θ5=b,θ6=m.ForA12=T20=T10×T21, substitute values from equations (5) and (7):(17)T20=T10XT21=cosu0sinu0.4cosusinu0−cosu0.4sinu0100.750001⋅cosv−sinv00.75cosvsinvcosv00.75sinv00100001.The resultant transformation between the base and joint 2 is illustrated as follows:(18)T20=cosucosv−cosusinvsinucosu0.75cosv+0.4cosvsinu−sinusinv−cosu0.75cosv+0.4sinusinvcosv00.75sinv+0.750001,ForA123=T30=T10×T21×T32, for simplicity,(19)m=cosu0.75cosv+0.4,n=0.75cosv+0.4sinu,u=0.75sinv+0.75,T30=T20XT32=cosucosv−cosusinvsinumcosvsinu−sinusinv−cosunsinvcosv0o0001⋅cosw0sinw0.25coswsinw0−cosw0.25sinw01000001.The resultant transformation between the base and joint 3 is illustrated as follows:(20)T30=cosucosv+wsinucosusinv+wm+0.25cosucosv+wcosv+wsinu−cosusinusinv+wn+0.25cosv+wsinusinv+w0−cosv+w0+0.25sinv+w0001.ForA456=T63=T43×T54×T65,(21)A45=T53=T43×T54=cosa0−sina0sina0cosa00−100.81240001⋅cosb0sinb0sinb0−cosb001000001,A45=T53=cosacosb−sinacosasinb0sinacosbcosasinasinb0−sinb0cosb0.81240001,T63=T43×T54×T65=T53×T65=cosacosb−sinacosasinb0sinacosbcosasinasinb0−sinb0cosbd40001⋅cosm−sinm00sinmcosm00001d60001.The resultant transformation between joint 3 and joint 6 is illustrated as follows:(22)T63=cosacosbcosm−sinasinm−cosmsina−cosacosbsinmcosasinbd6cosasinbcosbcosmsina+cosasinmcosacosm−cosbsinasinmsinasinbd6sinasinb−cosmsinbsinbsinmcosbcosbd6+d40001.Spherical wrist position can be extracted using the last column ofT60 transformation matrix, where(23)T60=r11r12r13Pxr21r22r23Pyr31r32r33Pz0001,Which means the T60 the transformation matrix can be represented in terms of R60 and P60 illustrated as follows:(24)T60=R60P6001,and spherical wrist position is illustrated as(25)P60=PxPyPz.Transformation matrix from base to end-effector is(26)T60=T10×T21×T32×T43×T54×T65,T60=r11r12r13Pxr21r22r23Pyr31r32r33Pz0001,T60=R60P6001.


## 3. Inverse Kinematics of Comau NM45

Inverse kinematics is finding the joint valuesθ1,θ2,θ3,θ4,θ5,θ6 of the robot arm for the given position (p) and orientation (o). For inverse kinematics, the inverse orientation R and inverse position P are needed.The Comau NM45 is an articulated arm with a spherical wrist. For finding anthropomorphic/articulated arm position and joint valuesθ1,θ2,θ3, the inverse position is needed.LetOc be intersecting the last 3 joints. The motion of joints 4, 5, and 6 will not change the position of Oc, as stated in Figure 7 [19] This is according to Pieper’s approach [7], in which the manipulator is divided to analyse the inverse kinematics [10].Figure 7
Kinematics decoupling (Spong, Hutchinson, and Vidyasagar, 2006).According toT63 matrix from equation (22), the position P63 is always as mentioned in the following equation:(27)P63=00d∂6.So, the positionO will be as follows:(28)O=Oc0+d6R001,where O is the position P60 and R is the orientation which is R. The above equation can be written in terms of Oc0 as follows:(29)Oc0=O−d6R001,The first three joints can be found in the following steps. They will determine the position of the manipulator:(30)O=oxoyoz,Oc0=xcyczc,xcyczc=ox−d6r13ox−d6r23ox−d6r33.For the orientation, the last three joints orientation is needed. The following equation shows the overall rotation of the manipulator in terms ofR30 and R63:(31)R=R30R63.Rearrangement of equation (31) will yield R63 below, through which the last three joint angles can be found:(32)R63=R30−1R⟶θ4,θ5,θ6.For articulated manipulator, the first three joints tell the position, as illustrated in Figure8.Figure 8
Comau NM45 (articulated arm) first three joints (Spong, Hutchinson, and Vidyasagar, 2006).Projection of wrist ontox0,y0 plane has been shown in Figure 9.Figure 9
Projection of wrist centre onto x0, y0 plane (Spong, Hutchinson, and Vidyasagar, 2006).This projection yields the triangle through which the angle value forθ1 can be found as follows:(33)θ1=Atan2xc,yc.If the wrist is rotated, then it will result in the following equation:(34)θ1=Atan2xc,yc+π.Another projection, as mentioned in Figure10, on the plane formed with link 2 and link 3 can help to find the value of joint angle 2 θ2,2 and joint angle 3 θ3,3.Figure 10
Projection on the plane (Link2 l2, Link3 l3) (Spong, Hutchinson, and Vidyasagar, 2006).Law of cosines can be applied to obtain the joint angle 3θ3,3, as follows:(35)cosθ3=r2+s2−a22−a322a2a3.By substituting the values ofr and s, these can be extracted by using Figure 8:(36)cosθ3=xc2+yc2−d2+zc−d12−a22−a322a2a3.As NM45 2.0 is inline, no shoulder is offset and, hence,d = 0.So,(37)cosθ3=xc2+yc2+zc−d12−a22−a322a2a3,where a2=L2 according to DH labelled figure, and(38)cosθ3=xc2+yc2+zc−d12−L22−a322L2a3,sinθ3=1−cosθ32.Also,(39)tanθ3=1−cosθ32cosθ3=1−D2D.The value ofθ3 can be written in terms of atan2 and D as follows:(40)θ3=atan2D,±1−D2,where + is for elbow up and − is for elbow down.The projection in Figure11 has been drawn onto the link 2 and link 3 plane to find θ2:Figure 11
Projection onto l2 and l3 plane to findθ2. θ2 can be written in terms of angles h1 and h2.(41)θ2=h1−h2,h1=atan2r,s=atan2xc2+yc2,Zc−d1,h2=atan2a2+a3cosθ3,a3sinθ3.So, the value ofθ2 can be written as follows:(42)θ2=atan2xc2+yc2,zc−d1−atan2a2+a3cosθ3,a3sinθ3.


### 3.1. Finding Spherical Wrist Joint Values (Rotation) Using Euler Angles

The last three joint variables,θ4,θ5,θ6, are the Euler angles. So,(43)θ4=∅,θ5=θ,θ6=φ.These angles are concerning the coordinate frameo3,x3,y3,z3. Now we need to calculate the transformation from T43 to T65 which is T63:(44)T63=A5XA5XA6=T43×T54XT65,where T63 can be written in terms of R63 and O63 mentioned as follows:(45)T63=R63O6301,where O63 is the position.For the spherical wrist, the ZYZ Euler transformation is needed.R63 is captured from equation (22), so R63 can be written as (46)RZYZ=R63=cos∅cosθcosφ−sin∅sinφ−cosφsin∅−cos∅cosθsinφcos∅sinθcosθcosφsin∅+cos∅sinφcos∅cosφ−cosθsin∅sinφsin∅sinθ−cosφsinθsinθsinφcosθ,R63=r11r12r13r21r22r23r31r32r33,Whereas angles a,b,m are ∅,θ,φ, respectively.Thecosθ can be extracted from r33 from the following equation:(47)cosb=cosθ,=r33,sinb=sinθ1−cos2θ.The abovecosθ and sinθ can be written in form of tan to find the value of θ:(48)θ=tan−11−cos2θr33,θ=tan−11−r332r33.For nonsingular case, if bothr13 and r23 are not zero, then equation (49) will yield the value of θ:(49)θ=atan2r33,±1−r332,or(50)tanθ=r132+r232r33=cos∅sinθ2+sin∅sinθ2cosθ,θ=tan−1r132+r232r33.Equation (51) shows the value of θ in terms of r13,r23andr33:(51)θ=atan2r33,±r132+r232.The last column ofR63 from equation (46) can help to yield the value of ∅:(52)tan∅=r23r13=sin∅sinθcos∅sinθ.∅ can be represented in terms of r13andr23, as follows:(53)∅=atan2r13,r23.Forφ,(54)tanφ=r32−r21=sinθsinφ−−cosφsinθ.Theφ can be represented in terms of r31andr32 mentioned as follows:(55)φ=atan2−r31,r32.For catering singularity, ifsinθ>0, whereas 0<θ<π, then solution will be(56)∅=atan2−r13,−r23,θ=atan2r33,±r132+r232,φ=atan2r31,−r32.Ifsinθ<0, whereas −π<θ<0, then the solution will be(57)∅=atan2−r13,−r23,θ=atan2r33,±r132+r232,φ=atan2r31,−r32.The last three joint values have been resolved by using the above equations of∅,θ, and φ.For the trajectory planning, the program was written in Matlab by using forward and inverse kinematics equations mentioned in the above sections. The planned trajectory required the smooth motion of the end-effector. The joint angles were calculated using the inverse kinematics equations. Figure12 shows the trajectory planning and robot motion along with the trajectory for the Comau NM45. The robot was able to follow the trajectory smoothly.Figure 12
The trajectory of Comau NM45.Figure13 shows the mapping of the values of joints to move on 100 points to follow the trajectory. It shows that the achievement was smooth as there are no sudden spikes in the joint values.Figure 13
Joint trajectory of Comau NM45.


## 3.1. Finding Spherical Wrist Joint Values (Rotation) Using Euler Angles

The last three joint variables,θ4,θ5,θ6, are the Euler angles. So,(43)θ4=∅,θ5=θ,θ6=φ.These angles are concerning the coordinate frameo3,x3,y3,z3. Now we need to calculate the transformation from T43 to T65 which is T63:(44)T63=A5XA5XA6=T43×T54XT65,where T63 can be written in terms of R63 and O63 mentioned as follows:(45)T63=R63O6301,where O63 is the position.For the spherical wrist, the ZYZ Euler transformation is needed.R63 is captured from equation (22), so R63 can be written as (46)RZYZ=R63=cos∅cosθcosφ−sin∅sinφ−cosφsin∅−cos∅cosθsinφcos∅sinθcosθcosφsin∅+cos∅sinφcos∅cosφ−cosθsin∅sinφsin∅sinθ−cosφsinθsinθsinφcosθ,R63=r11r12r13r21r22r23r31r32r33,Whereas angles a,b,m are ∅,θ,φ, respectively.Thecosθ can be extracted from r33 from the following equation:(47)cosb=cosθ,=r33,sinb=sinθ1−cos2θ.The abovecosθ and sinθ can be written in form of tan to find the value of θ:(48)θ=tan−11−cos2θr33,θ=tan−11−r332r33.For nonsingular case, if bothr13 and r23 are not zero, then equation (49) will yield the value of θ:(49)θ=atan2r33,±1−r332,or(50)tanθ=r132+r232r33=cos∅sinθ2+sin∅sinθ2cosθ,θ=tan−1r132+r232r33.Equation (51) shows the value of θ in terms of r13,r23andr33:(51)θ=atan2r33,±r132+r232.The last column ofR63 from equation (46) can help to yield the value of ∅:(52)tan∅=r23r13=sin∅sinθcos∅sinθ.∅ can be represented in terms of r13andr23, as follows:(53)∅=atan2r13,r23.Forφ,(54)tanφ=r32−r21=sinθsinφ−−cosφsinθ.Theφ can be represented in terms of r31andr32 mentioned as follows:(55)φ=atan2−r31,r32.For catering singularity, ifsinθ>0, whereas 0<θ<π, then solution will be(56)∅=atan2−r13,−r23,θ=atan2r33,±r132+r232,φ=atan2r31,−r32.Ifsinθ<0, whereas −π<θ<0, then the solution will be(57)∅=atan2−r13,−r23,θ=atan2r33,±r132+r232,φ=atan2r31,−r32.The last three joint values have been resolved by using the above equations of∅,θ, and φ.For the trajectory planning, the program was written in Matlab by using forward and inverse kinematics equations mentioned in the above sections. The planned trajectory required the smooth motion of the end-effector. The joint angles were calculated using the inverse kinematics equations. Figure12 shows the trajectory planning and robot motion along with the trajectory for the Comau NM45. The robot was able to follow the trajectory smoothly.Figure 12
The trajectory of Comau NM45.Figure13 shows the mapping of the values of joints to move on 100 points to follow the trajectory. It shows that the achievement was smooth as there are no sudden spikes in the joint values.Figure 13
Joint trajectory of Comau NM45.


## 4. Conclusion

The modified DH convention has been used to perform the forward kinematics for the manipulator. The kinematics decoupling has been used to perform the inverse kinematics. The manipulator was divided into two parts to make the inverse kinematics problem simpler. The first 3 joints were resolved by using a geometrical approach, whereas the last three joints were resolved using the algebraic approach. The resultant kinematics solution was applied on the manipulator, and it was able to follow a test trajectory successfully. A similar approach can be used while solving the articulated robot with a spherical wrist. The techniques are applied for the 6-DoF robot.



---

*Source: 6647035-2021-02-02.xml*