Robotics

CSE4316

Assoc. Prof. Dr.Ing.

Mohammed Nour
[email protected]
https://mnourgwad.github.io/CSE4316

Lecture 8: Robot Inverse Kinematics (cont.)

Copyright ©2016 Dr.Ing. Mohammed Nour Abdelgwad Ahmed as part of the course
work and learning material. All Rights Reserved.
Where otherwise noted, this work is licensed under a Creative Commons
Attribution-NonCommercial-ShareAlike 4.0 International License.

Zagazig University | Faculty of Engineering | Computer and Systems Engineering Department | Zagazig, Egypt
 Lecture: 8
Robot Inverse Kinematics (cont.)
Geometric Approach
Algebraic Approach

Mohammed Nour (Assoc. Prof. Dr.Ing.) Robotics 2 / 19

Robot Inverse Kinematics
Algebraic Solution

Algebraic Solution
Find the values of joint parameters that will put the tool frame at a desired position and orientation
(within the workspace)
( )
R3×3 T3×1
Given the transformation matrix: H = , R 2 SO(3)
0 1
Find all solutions to: Tn0 (q1 , · · · , qn ) = H
Noting that: Tn0 (q1 , · · · , qn ) = A1 (q1 ) · · · An (qn )
This gives 12 (nontrivial) equations with n unknowns
with end effector position at T3×1
and its orientation is obtained as:
" # " # " #
R21 R31 sin(ψ) R32
ψ = atan2 , θ = atan2 − , φ = atan2 .
R11 R21 R33

Mohammed Nour (Assoc. Prof. Dr.Ing.) Robotics 3 / 19

Robot Inverse Kinematics
Example: the Stanford manipulator

DH Parameters:
link ai αi di θi
1 0 -90 0 θ1
2 0 90 d2 θ2
3 0 0 d3 0
4 0 -90 0 θ4
5 0 90 0 θ5
6 0 0 d6 θ6

From which we get the transformation matrix (in general form):

2 3
r11 r12 r13 dx
6 r21 r22 r23 dy 7
0
H6 = 4
6 7 = [rij ] , i = 1, · · · , 4,
r31 r32 r33 dz 5 j = 1, · · · , 4

0 0 0 1 d2 = 0.154, d6 = 0.263

Mohammed Nour (Assoc. Prof. Dr.Ing.) Robotics 4 / 19

Robot Inverse Kinematics
We have 12 non-trivial equations:
c1 [c2 (c4 c5 c6 − s4 s6 ) − s2 s5 c6 ] − d2 (s4 c5 c6 + c4 s6 ) = 0
s1 [c2 (c4 c5 c6 − s4 s6 ) − s2 s5 c6 ] + c1 (s4 c5 c6 + c4 s6 ) = 0
For a given: −s2 (c4 c5 c6 − s4 s6 ) − c2 s5 c6 = 1
2 3 c1 [−c2 (c4 c5 s6 + s4 c6 ) + s2 s5 s6 ] − s1 (−s4 c5 s6 + c4 c6 ) = 1
0 1 0 −0.154 −s1 [−c2 (c4 c5 s6 − s4 c6 ) − s2 s5 s6 ] + c1 (−s4 c5 s6 + c4 s6 ) = 0
0 0 1 0.763 7
H60 = 6 s2 (c4 c5 s6 + s4 c6 ) + c2 s5 s6 = 0
6
7
4 1 0 0 0 c1 (c2 c4 s5 + s2 c5 ) − s1 s4 s5 = 0
5
0 0 0 1 s1 (c2 c4 s5 + s2 c5 ) + c1 s4 s5 = 1
T −s2 c4 s5 + c2 c5 = 0
Find q = [θ1 , θ2 , d3, θ4 , θ5 , θ6 ] . c1 s2 d3 − s1 d2 + d6 (c1 c2 c4 s5 + c1 c5 s2 − s1 s4 s5 ) = −0.154
s1 s2 d3 + c1 d2 + d6 (c1 s4 s5 + c2 c4 s1 s5 + c5 s1 s2 ) = 0.763
c2 d3 + d6 (c2 c5 − c4 s2 s5 ) = 0

T
One solution: q = [π/2, π/2, 0.5, π/2, 0, π/2] .
next, we will see how to systematically find such solutions.
Mohammed Nour (Assoc. Prof. Dr.Ing.) Robotics 5 / 19
Robot Inverse Kinematics

previous examples show how difficult it would be to obtain a closed-form solution to the 12
equations, instead,

we develop systematic methods based upon the manipulator configuration

For the forward kinematics there is always a unique solution
◮ Potentially complex nonlinear functions

The inverse kinematics may or may not have a solution

◮ Solutions may or may not be unique
◮ Solutions may violate joint limits
Closed-form solutions are ideal !

Mohammed Nour (Assoc. Prof. Dr.Ing.) Robotics 6 / 19

Robot Inverse Kinematics
Kinematic Decoupling

Appropriate for systems that have an arm with a wrist

◮ Such that the wrist joint axes are aligned at a point (the last 3 joint axes intersecting at a point)
For such systems, we can decouple (split) the inverse kinematics problem into two parts:
1 Inverse position kinematics: position of the wrist center
2 Inverse orientation kinematics: orientation of the wrist
First, assume 6DOF, the last three intersecting at Oc . For given R and O solve 9 rotational and 3
positional equations:

R60 (q0 , · · · , qn ) = R
O60 (q0 , · · · , qn ) = O

Use the position of the wrist center to determine the first three joint angles.

Mohammed Nour (Assoc. Prof. Dr.Ing.) Robotics 7 / 19

Kinematic Decoupling
Spherical wrist as paradigm

Oc is the intersection of the last 3 joint axes (z3 , z4 , and z5 ); link ai αi di θi

origins O4 and O5 will always be at Oc ; 4 0 -90 0 θ4
motion of joints 4, 5 and 6 will not change the position of Oc ; 5 0 90 0 θ5
only motions of joints 1, 2 and 3 can influence position of Oc . 6 0 0 d6 θ6

Mohammed Nour (Assoc. Prof. Dr.Ing.) Robotics 8 / 19

Kinematic Decoupling
Now, origin of tool frame, O6 , is a distance d6 translated along z5 (since z5 and z6 are collinear)
◮ Thus, the third column of R is the direction of z6 (w.r.t base frame) and we can write:
T
O = O60 = Oc0 + d6 R 0 0 1


T
Rearranging: Oc0 = O − d6 R

0 0 1
Recalling:
 T
O= Ox Oy Oz ,
T
Oc0 =

xc yc zc

Then:
   
xc Ox − d6 r13
 yc  =  Oy − d6 r23  ⇒ θ1 , θ2 , θ3
zc Oz − d6 r33

Mohammed Nour (Assoc. Prof. Dr.Ing.) Robotics 9 / 19

Kinematic Decoupling

 T
Since xc yc zc are determined from the first three joint angles,
◮ our forward kinematics expression now allows to solve for the first 3 joint angles decoupled from the
final 3.
◮ Thus we now have R03
Note that: R = R30 R63
To solve for the final three joint angles:

R63 = (R30 )−1 R = (R30 )T R ⇒ θ4 , θ5 , θ6

Since the last three joints for a spherical wrist, we can use a set of Euler angles to solve for them

Mohammed Nour (Assoc. Prof. Dr.Ing.) Robotics 10 / 19

Inverse Position

 T
Now that we have xc yc zc we need to find θ1 , θ2 , θ3
Solve for θi by projecting onto the {xi−1 , yi−1 } plane, solve trig problem
Two examples:
◮ elbow (RRR) and
◮ spherical (RRP) manipulators

Mohammed Nour (Assoc. Prof. Dr.Ing.) Robotics 11 / 19

Inverse Position
Example: RRR Manipulator

to solve for θ1 , project the arm onto the {x0 , y0 } plane: θ1 = atan2(yc , xc )
We can also have: θ1 = π + atan2(yc , xc )
This will of course change the solutions for θ2 and θ3

Mohammed Nour (Assoc. Prof. Dr.Ing.) Robotics 12 / 19

Inverse Position
Singular Configurations, Offsets

If there is an offset, then we will have two solutions for

If xc = yc = 0, θ1 is undefined
θ1 : left arm and right arm
i.e. any value of θ1 will work
However, wrist centers can not intersect z0

Mohammed Nour (Assoc. Prof. Dr.Ing.) Robotics 13 / 19

Inverse Position
Left and Right Arm Solutions

Left Arm Right Arm

θ1 = φ − α θ1 = α + β
φ = atan2(yc , xc ) α = atan2(yc , xc )
 p   p 
α = atan2 d, xc2 + yc2 − d 2 β = π + atan2 d, xc2 + yc2 − d 2
 p 
= atan2 −d, − xc2 + yc2 − d 2

Mohammed Nour (Assoc. Prof. Dr.Ing.) Robotics 14 / 19

Inverse Position
Left and Right Arm Solutions

Therefore there are in general two solutions for θ1

Finding θ2 and θ3 is identical to the planar two-link manipulator we have seen previously:

r 2 + s 2 − L22 − L23
cos θ3 =
2L2 L3 Z0
r 2 = xc2 + yc2 − d 2 x, y
r
s = zc − d1 θ3
2
xc2 + yc2 2
− d + (zc − d1 ) − L22 − L23
cos θ3 = s
2L2 L3 L3
L2
≡D
θ2

Therefore we
 can √
find two solutions for θ3
θ3 = atan2 D, ± 1 − D 2

Mohammed Nour (Assoc. Prof. Dr.Ing.) Robotics 15 / 19

Inverse Position
Left and Right Arm Solutions

The two solutions for θ3 correspond to the elbow-down and elbow-up positions respectively
Now solve for θ2 :

θ2 = atan2 (r , s) − atan2 (L2 + L3 c3 , L3 s3 )

p 
= atan2 xc2 + yc2 − d 2 , zc − d1 − atan2 (L2 + L3 c3 , L3 s3 )

Thus there are two solutions for the pair (θ2 , θ3 )

Mohammed Nour (Assoc. Prof. Dr.Ing.) Robotics 16 / 19

Robot Inverse Kinematics
Inverse position: Example: RRR manipulator

RRR: Four total solutions

In general, there will be a maximum of four solutions to
the inverse position kinematics of an elbow manipulator

The 6R PUMA arm (as an example of the articulated

geometry):

Mohammed Nour (Assoc. Prof. Dr.Ing.) Robotics 17 / 19

Inverse Orientation Problem

link ai αi di θi
1 0 90 d1 θ1
2 L2 0 0 θ2
3 L3 0 0 θ3
4 0 -90 0 θ4
5 0 0 0 θ5
6 0 0 d6 θ6
 
c1 c23 −c1 s23 s1
R30 =  s1 c23 −s1 s23 −c1 
s23 c23 0
 
(c4 c5 c6 − s4 s6 ) (−c4 c5 s6 − s4 c6 ) c4 s5
R63 =  (s4 c5 c6 + c4 s6 ) (−s4 c5 s6 + c4 c6 ) s4 s5 
−s5 c6 s5 c 6 c5

Equation to solve: R63 = (R30 )T R

Mohammed Nour (Assoc. Prof. Dr.Ing.) Robotics 18 / 19

Inverse Orientation Problem
Euler angle solutions can be applied. Taking the third column of (R30 )T R

c4 s5 = c1 c23 r13 + s1 c23 r23 + s23 r33

s4 s5 = −c1 s23 r13 − s1 s23 r23 + c23 r33
c5 = s1 r13 − c1 r23

Again, if θ5 6= 0, we can solve for θ5 :

 q 
2
θ5 = atan2 s1 r13 − c1 r23 , ± 1 − (s1 r13 − c1 r23 )

Finally, we can solve for the two remaining angles as follows:

θ4 = atan2 (c1 c23 r13 + s1 c23 r23 + s23 r33 , −c1 s23 r13 − s1 s23 r23 + c23 r33 )
θ6 = atan2 (−s1 r11 + c1 r21 , s1 r12 − c1 r22 )

For the singular configuration (θ5 = 0), we can only find θ4 + θ6 thus it is common to arbitrarily set
θ4 and solve for θ6
Mohammed Nour (Assoc. Prof. Dr.Ing.) Robotics 19 / 19
Thanks for your attention.
Questions?

Assoc. Prof. Dr.Ing.

Mohammed Nour
[email protected]
https://mnourgwad.github.io/CSE4316 Robotics Research Interest Group (zuR2 IG)
Zagazig University | Faculty of Engineering | Computer
and Systems Engineering Department | Zagazig, Egypt

Copyright ©2016 Dr.Ing. Mohammed Nour Abdelgwad Ahmed as part of the course work and learning material. All Rights Reserved.
Where otherwise noted, this work is licensed under a Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International License.

Mohammed Nour (Assoc. Prof. Dr.Ing.) Robotics 20 / 19