Inverse Kinematics Analysis of 7-DOF Collaborative

Robot by Decoupling Position and Orientation

Nguyen Quang Hoang1,*[0000-0003-2525-2856], Do Tran Thang2, Dinh Van Phong1

1Hanoi University of Science and Technology, Vietnam
2Institute of Mechanics, Vietnam Academy of Science and Technology
(*) Corresponding author: [email protected]

Abstract. Redundant manipulators have some advantages in comparison to

standard ones such as higher flexibility, obstacle, and joint limits avoidance
capability, and much more solutions of inverse kinematics. This paper presents
a combination of Jacobian-based and analytical method for finding solution of
inverse kinematics of a 7-DOF manipulator. In this way, the first four joint
variables are determined with Jacobian-based method, and the last three ones
based on analytical method. Some numerical simulations are carried out to
verify the effectiveness of the proposed approach.

Keywords: redundant manipulator, inverse kinematics, decoupling position and

orientation.

1 Introduction

In the past twenty years, collaborative robots, commonly known as cobots, are
experiencing rapid market growth in this sector of the robotics industry. This type of
robot is able to physically interact with humans in a shared workspace thanks to
sensors, intelligent controls, and other design features such as lightweight materials,
rounded edges.
To operate dexterously like a human arm, the cobot is designed with 7 degrees of
freedom (DOF) with a structure of anthropomorphic arm. As a redundant
manipulator, inverse kinematics of a 7-DOF arm has attracted many researchers.
Solution of this arm can be found in closed form or numerical one. An analytical
solution can be found in case of non-offset arm [1,2]. In this case, the shoulder and
wrist are considered as spherical joints. Hence, the elbow joint can freely rotate
around axis passing through two spherical joints. Based on this feature, the analytical
solution can be found, depending on a freely chosen parameter – free internal motion
of elbow joint around axis through shoulder and wrist that is called swivel angle. In
[2], authors chose the swivel angle of the robot arm so that robot configuration is as
same as human arm as possible. One drawback of this approach is the way to deal
with singularity and joint limit avoidance.
Another approach for inverse kinematics of redundant robot is based on Jacobian
matrix, in which the inverse problem is solved at velocity, acceleration, or jerk level.
A set of linear equations is dealed by pseudo-inverse of Jacobian matrix. The joint
variables are then obtained by integrating joint velocities, acceleration, or jerks. In
this approach, the advantages of redundancy such as trajectory optimization, obstacle,
joint mechanical limits, and singularity avoidance are easily exploited by using null
space of Jacobian matrix [3,5]. This method is simple, however, for 7-DOF
anthropomorphic arm the Jacobian matrix is quite bulky one.
In this paper, the method based on Jacobian matrix and analytical method are
combined for finding solution of inverse kinematics of a 7-DOF anthropomorphic
manipulator without offsets. In this way, the first four joint variables are determined
with Jacobian-based method, and the last three ones based on analytical method.
Some numerical simulations are carried out to verify the effectiveness of the proposed
approach.

2 Kinematic analysis

2.1 Direct kinematics

Let’s consider a 7-DOF manipulator as shown in Fig. 1. The direct kinematics can
be solved systematicly by using Denavit-Hartenberg (DH) method. The link
coordinate systems established with the DH convention and the corresponding DH
parameters table are shown in Fig. 1. and Table 1, respectively. In which
qi , i  1, 2,..., 7 represents the joint variables.

z5 O5,6
q6
z4 z6 d7
x3
x4 q5 q7
z0 d5 x5
x2 q3 O7
q2
q4 z2
O3,4 O5,6
O1,2 q4
d3 z3
n3
n2
d1 x1 C
z1 y0
 h d5
O0 q4
//x1  n1
O3,4
q1 O1,2 d3
x0

Fig. 1. 7-DOF manipulator and link-frames based on DH convention.

 Table 1. DH parameters

link i i di ai i
1 q1 d1=0.28 0 /2
2 q2 d2=0 0 /2
3 q3 d3=0.33 0 -/2
4 q4 d4=0 0 /2
5 q5 d5=0.32 0 -/2
6 q6 d6=0 0 /2
7 q7 d7=0.10 0 0

The relative homogeneous transformation matrices Aii 1(qi ) are calculated by

substituting the DH parameters in Table 1 into the matrix equation for each joint:
cos i  sin i cos i sin i sin i ai cos i 
 
sin i cos i cos i  cos i sin  i ai sin i 
Ai (i )  
i 1
. (1)
0 sin i cos i di 
 
 0 0 0 1 

The position and orientation of the kth link are given by:
 R 0 (q) rOk
(0)
(q)
Tk0 (q)  A10 (q1 )A12 (q2 )  Akk 1(qk )   k  , k  1, 2,..., 7 (2)
0 1 
 
Some results of direct kinematic are given as follow:
0 d sin q cos q 
   3 2 1

rO 1 (q)  rO 2 (q)   0  ,
(0) (0)
rO 3 (q)  rO 4 (q)  d 3 sin q 2 sin q1 
(0) (0)
(3)
d   d  d cos q 
 1  1 3 2 

d cq sq  d [(cq cq cq  sq sq )sq  cq sq cq ]
 3 1 2 5 1 2 3 1 3 4 1 2 4

rO 5 (q)  rO 6 (q)  d 3sq1sq 2  d5 [(sq1cq 2cq 3  cq1sq 3 )sq 4  sq1sq 2cq 4 ]
(0) (0)
(4)
 d1  d 3cq 2  d5 (sq 2cq 3sq 4  cq2cq 4 ) 
 
Remarks: We can see that the position of O3 depends only on q1 and q 2 ; the
position of O5 depends only on q1...q 4 and not on q 5 .

2.2 Singularity analysis

For any robotic arm using rotational joints, there are always singular configurations in
workspace. The kinematic singularities is independent of coordinate frame. The
singularity can be found based on calculating determinant of Jacobian matrix
det(J(q)) = 0 or det(J(q)JT (q)) = 0 [5,8]. However, with the structure of the
manipulator as shown in Fig. 1, geometrically, we can see the singularities in some
cases as shown in Fig. 2:
 When the origin O5,6 is on the z0 axis, the joint variable q1 can have any value.
This is called a shoulder singularity.
 When link 3 and 4 are stretched ( q 4  0 ) out or folded ( q 4   ), axes z2 and z4
are collinear. Rotation of link 3 has no effect on the motion of the end-effector.
This is called an elbow singularity.
 Similarly, when links 5 and 6 are stretched ( q 6  0 ) out or folded ( q 6   ),
axes z4 and z6 are collinear. Rotation of link 6 has no effect on the motion of the
end-effector. This is called a wrist singularity.
 The other cases of singularities are the combination of the mentioned
singularities.
z4
O5 q5
O7
d5
z0 q7

q5
q1

O1,2 q6
q4
q3 d3 z3 z3 z5
z1
q2 q5
q3

a) Shoulder singularity b) Elbow singularity c) Wrist singularity

Fig. 2. Singularities of 7-DOF anthropomorphic manipulator

2.3 Inverse kinematics

Given is a position and orientation of the end-effector r7(0) & R 70 , we need to find
the joint variables qk , k  1,..., 7 .
Because three axes z 4 , z 5 , z 6 are concurrent at one point (O5 = O6), the Pieper’s
method (position and orientation decoupling) can be applied here for inverse
kinematics [4]. The position of the wrist is determined from given position and
orientation of the end-effector as:
r5(0)  r6(0)  r7(0)  d7 R 70 k(7)
7
 [x 5 y5 z 5 ]T , k(7)
7
 [0 0 1]T (5)
Analytical solutions
Consider the triangle O2O3O5, we can find joint angle q 4 :
d32  2d3d5 cos   d52  l22 5

 cos   (d32  d52  l225 ) / (2d3d5 ), sin   1  cos2 

  q 4  (   )  (  atan 2(sin  , cos  )) (6)
where l22 5   r5(0)  r2(0)   (r5(0)  r2(0) )T (r5(0)  r2(0) ) . In formula (6), we can take
positive sign or negative sign depending on upper or lower configuration.
The angle  = (O5O2O3) is determined as:
l 2  d32  d52 d 2  l 2  d52
cos   2  5 ,    arccos 3 2  5
2d3l2  5 2d 3l2  5
From the structure of the manipulator, we can see that if the end-effector is fixed,
the point O3,4 can move a along a circle with radius of h and center C on the axis
through O2 and O5 (see Fig. 1).

Let R  rotn ( ) be a rotation matrix about axis n an angle  :
R  E  sin  n  (1  cos  )nn
 .

Let Cn1n2n 3 be coordinate system as shown in Fig. 1, n2 along the line O2O5,
         
n1  z 0 , n1  n2  z 0 /  n2  z 0  and n 3  n1  n2 .
If we know the center C and the radius h, the position of the point O3,4 is
determined as:

(0)
r3,4  rC(0)  R(n,  )hn1 . (7)
Consider the right triangle O2CO3, we have:
O2C  d 3 cos  , h  d3 sin 
Unit vector of O2O5 is calculated as:
n 2  (r5  r2 ) / l2  5
By freely choosing  , we can find position of point O3 as follows
r3  r2  O2Cn 2  R(n2 ,  )h n1  [x 3 y 3 z 3 ]T (8)
Comparing (8) and (3) yields
d3 sin q 2 cos q1  x 3 , d 3 sin q 2 sin q1  y 3 , d1  d3 cos q 2  z 3 (9)
From (9), the joint variable q 2 can be found as:

d1  d3 cos q2  z 3  cos q 2  (d1  z 3 ) / d3 , sin q 2   1  cos2 q2

 q2  atan 2(sin q2 , cos q2 ) (10)
Formula (10) gives out two solutions of q 2 depending on sign of sin(q2). This sign
will also decide the joint variable q1 :
d3 sin q2 cos q1  x 3  cos q1  x 3 / (d3 sin q2 )
d 3 sin q 2 sin q1  y 3  sin q1  y 3 / (d 3 sin q 2 )
q1  atan 2(y 3 / sin q 2 , x 3 / sin q 2 ) (11)
 We can see that if sin q2  0 (it means q 2  0 or q 2   and the axis z2
coincides with the axis z0), then we can not determine q1 , even though q1  q 3 is
known.
  
If n2  z 0 (it means O5,6 lies on the axis z0), the vector n1 is unidentified.
Therefore, we can not find q1 from (11). That is a singular configuration of this robot
arm.
The joint variable q 3 is determined by comparing r5(0)  r4(0)  r5(0)  r3(0) .
Knowing positions r5(0) & r3(0) of O5 and O3, we can find q 3 as follows:
d [(cq cq cq  sq sq )sq  cq sq cq ] x  x 
 5 1 2 3 1 3 4 1 2 4
  5 3

r (q)  r (q)  d5 [(sq1cq2cq 3  cq1sq 3 )sq 4  sq1sq 2cq 4 ]  y5  y 3 
(0)
O5
(0)
O4
(12)
 d5 (sq 2cq 3sq 4  cq 2cq 4 )  z  z 
   5 3

Solving for sin q 3 and cos q 3 , we get:

cq1cq2z 5  cq1cq 2z 3  d5cq1cq 4  x 5sq2  x 3sq2
sin q 3 
d5sq1sq2sq 4
d5cq 2cq 4  z 5  z 3
cos q 3 
d5sq 2sq 4
 q 3  atan 2(sq 3 , cq 3 ) (13)
So, the first four joint variables q1,..., q 4 are determined by (11), (10), (13) and (6),
respectively. Now, we determine three last joint variables: q 5 , q 6 , q 7 . The relative
orientation of end-effector respect to link 4 is determined by:
R 70  R 04 (q1, q2 , q 3 , q 4 )R 74  R 74  R 04T (q1, q 2 , q 3 , q 4 )R 70
R 54 (q 5 )R 56 (q 6 )R 67 (q 7 )  R 74
From direct kinematics we have:
R 74  R 54 (q 5 )R 56 (q 6 )R 67 (q 7 )
cq cq cq  sq sq cq 5cq 6sq 7  sq 5cq 7 cq 5sq 6  r11 r12 r13 
 5 6 7 5 7
  
 sq 5cq 6cq 7  cq 5sq 7 sq 5cq 6sq 7  cq 5cq 7 sq 5sq 6   r21 r22 r23 
 sq 6cq 7 sq 6sq 7 cq 6  r31 r32 r33 
   
Solving these equations, one gets q 5 , q 6 , q 7 .
Remarks: the free parameter  can be chosen based on additional criteria such as
singularity, obstacle, and joint limit avoidance.

Solution based on Jacobian matrix

Let  T  [ v(0)
E
, 7(0) ]T be the twist of the end-effector, we have:
  J(q)q  q  J† (q) , J† (q)  JT (q)[ J(q)JT (q)]1 (14)
where J(q) and J† (q) are Jacobian matrix and its pseudo-inverse [6,7].
In this paper, equation (14) is not used, since Jacobian of a 7-DOF
anthropomorphic manipulator is quite bulky. Based on the arm structure, we can
see that the redundancy is effective only on the position of O5, when the end-
effector is fixed. Therefore, we exploit the following relation instead of (14):
r5  r5 (q p )  r5  J5 (q p )q p (15)
where q p  [q1,..., q 4 ]T .
Applying pseudo-inverse, the solution of (15) is given as:
q p  J†5 r5 , J†5 (q p )  JT5 [ J5 JT5 ]1 (16)
To avoid singularities, pseudo-inverse of J5 (q p ) is modified by damped least
square inverse as:
J†5 (q p )  JT5 [ J5 JT5  k I3 ]1, k 0 (17)
1
q p  J (J J  k I) (r5  Ke)  (I  J J5 )z0
T
5
T
5 5
†
5
(18)

where K  0 is gain matrix and e  r5 (t )  r (t ) , the error between actual and

d
5

desired position of O5, z0   4 is arbitrary vector. The vector z0 is normally chosen

to exploit the redundancy such as singular, joint limits or obstacle avoidance.
The parameter k depending on w(q)  det(J(q p )JT (q p )) , a manipulability
measure , is chosen as following:
 1  (1  cos( w / w 0 )), w  w 0
k  2 0 (19)
0, w  w0

where  0 and w 0 are small positive numbers.

The block diagram for inverse kinematics based on a combination of Jacobian
matrix and analytical solution is shown in Fig. 3.

Cal. q 5  7
q57

r7 (t ),
r5 (t )
R 70 (t ) Cal. q p 1
r5 (t )
s q p  q1  4

r5(0) Cal. qp(0)

Fig. 3. Blockdiagram for inverse kinematics

3 Numerical experiments

In order to confirm the validity of the algorithms proposed in this paper, some
numerical simulations are carried. Two trajectories including rectilinear and
curvilinear one from A to B are implemented by MATLAB:
rA  [0.5(d 3  d5  d7 ), 0.4(d3  d5 ), d1  0.0d3 ]T
rB  [0.5(d3  d5  d7 ), 0.4(d3  d5 ), d1  0.6d 3 ]T

a) Position of the end-effector vs time; b) Robot configurations

qdot [rad/s]

c) Joint variables vs time; d) Joint velocities vs time

e) Position error of end-effector vs time

Fig. 4. Tracking of end-effector along rectilinear trajectory
 The motion law along trajectory is defined as following:
s  si   t 1 2 t 
s(t )  si  f   sin , 0  t  tf (20)
  t f 2 t f 
Along trajectory, the corresponding orientation is determined by the ZYZ Euler
angles with:
   5 10 20 T
i  [ , , ]T , f  i  ,   [ , , ]
3 3 3 18 18 18
[m]

a) Position of the end-effector vs time; b) Robot configurations

c) Joint variables vs time; d) Joint velocities vs time

10 -6

-2
e e e
x y z
-4
0 0.5 1 1.5 2 2.5 3 3.5
t [s]
e) Position error of end-effector vs time
Fig. 5. Tracking of end-effector along curvilinear trajectory
 The simulation results of two cases are shown in Fig. 4 and Fig. 5, respectively. It
can be seen from the simulation results that the pose of the manipulator changes
uniformly, and the actual trajectory is consistent with the given trajectory, and the
position errors of x, y and z are all within 2.510–6 m. All the joint variables change
smoothly, and no sudden change appears. These results prove that the proposed
inverse kinematics method can be applied to the control of continuous trajectory of
the 7-DOF manipulator.
To verify the ability to avoid singularities, a trajectory is chosen such that the
origin O5 pass through the axis z0. The simulation results in this case is shown in Fig.
6. It can be seen that x 5  y 5  0 at time t = 1.5 s, but time histories of all joint
variables are smooth, no sudden change of 1 occurs at this singularity.

Fig. 6. Position of O5 and joint variables vs time

4 Conclusion

This paper combined successfully Jacobian-based and analytical method for an

inverse kinematics of a redundant anthropomorphic manipulator. With the wrist
equivalent to a spherical joint, we can decouple position and orientation of end-
effector. The Jacobian-based method is applied for inverse kinematics of position, that
is a redundant case. By the proposed approach, the bulkiness of Jacobian matrix J7 is
replaced by a simpler one J5 . Therefore, the computational complexity is reduced.
Additionally, the advantages of the Jacobian method such as singularity avoidance is
retained. The numerical simulations verified the feasibility of the proposed approach.

Acknowledgment
This work was supported in part by the National Program: Support for research,
development, and technology application of industry 4.0 (KC-4.0/19-25), under the grant for
the project: Research, design and manufacture of Cobot applied in industry and some other
fields with human-robot interaction (code: KC-4.0-35/19-35).
References
1. X. Tian, Q. Xu and Q. Zhan (2020), An analytical inverse kinematics solution with joint
limits avoidance of 7-DOF anthropomorphic manipulators without offset, Journal of the
Franklin Institute, https://doi.org/ 10.1016/ j.jfranklin.2020.11.020
2. Wang Y, Artemiadis P (2013) Closed-Form Inverse Kinematic Solution for
Anthropomorphic Motion in Redundant Robot Arms. Adv Robot Autom 2: 110. doi:
10.4172/2168-9695.1000110.
3. Jingguo Wang, Yangmin Li and Xinhua Zhao (2010): Inverse Kinematics and Control of
a 7-DOF Redundant Manipulator Based on the Closed-Loop Algorithm. International
Journal of Advanced Robotic Systems, Vol. 7, No. 4 (2010)
4. John J Craig: Introduction to Robotics: Mechanics and Control. Pearson/Prentice Hall,
2005.
5. Bruno Siciliano, Lorenzo Sciavicco, Luigi Villani, Giuseppe Oriolo: Robotics:
Modelling, Planning and Control. Springer Science & Business Media, 2010.
6. Nakamura Y.: Advanced Robotics/Redundancy and Optimization. Addison-Wesley
Publishing Company, Reading 1991.
7. Rao, C.R.: Generalized Inverse of Matrices and its Applications. New York, Wiley, 1971.
8. Spong M. W.; Hutchinson S. and Vidyasagar M.: Robot Modeling and Control. John
Wiley & Sons, New York, 2006.