Ch.

3: Inverse Kinematics
Ch. 4: Velocity Kinematics

The Interventional Centre

 Recap: kinematic decoupling
• Appropriate for systems that have an arm a wrist
– Such that the wrist joint axes are aligned at a point
• For such systems, we can 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
R60 (q1,..., q6 ) = R
o60 (q1,..., q6 ) = o

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

The Interventional Centre

 Recap: 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/ respect to the base frame)
and we can write: 0
o = o60 = oco + d 6R 0
1
– Rearranging: 0
oco = o − d 6R 0
 1
– Calling o = [ox oy oz]T, oc0 = [xc yc zc]T

 xc  o x − d 6 r13 
 y  = o − d r 
 c  y 6 23 

 zc  oz − d 6 r33 

The Interventional Centre

 Recap: kinematic decoupling
• Since [xc yc zc]T are determined from the first three joint angles, our forward
kinematics expression now allows us to solve for the first three joint angles
decoupled from the final three.
– Thus we now have R30
– Note that:
R = R30R63

– To solve for the final three joint angles:

( )
R63 = R30
−1
( )T
R = R30 R

– Since the last three joints for a

spherical wrist, we can use a set of
Euler angles to solve for them

The Interventional Centre

 Recap: Inverse position kinematics
• Now that we have [xc yc zc]T we need to find q1, q2, q3
– Solve for qi by projecting onto the xi-1, yi-1 plane, solve trig
problem
– Two examples
• elbow (RRR) manipulator: 4 solutions (left-arm elbow-up, left-arm
elbow-down, right-arm elbow-up, right-arm elbow-down)
• spherical (RRP) manipulator: 2 solutions (left-arm, right-arm)

The Interventional Centre

 Inverse orientation kinematics
• Now that we can solve for the position of the wrist center
(given kinematic decoupling), we can use the desired
orientation of the end effector to solve for the last three
joint angles
– Finding a set of Euler angles corresponding to a desired rotation
matrix R
– We want the final three joint angles that give the orientation of the
tool frame with respect to o3 (i.e. R63)

The Interventional Centre

 Inverse orientation: spherical wrist
• Previously, we said that the forward kinematics of the spherical
wrist were identical to a ZYZ Euler angle transformation:
c 4c5c 6 − s4s6 − c 4c 5s6 − s 4c 6 c 4 s5 c 4 s5 d 6 
s c c + c s − s 4 c 5 s6 + c 4 c 6 s 4 s5 s4s5d 6 
T6 = A4 A5 A6 =  4 5 6
3 4 6

 − s5 c 6 s5 c 6 c5 c5d 6 
 
 0 0 0 1 

The Interventional Centre

 Inverse orientation: spherical wrist
• The inverse orientation problem reduces to finding a set of Euler angles (θ4,
θ5, θ6) that satisfy:
c 4c5c 6 − s4s6 − c 4c 5s6 − s 4c 6 c 4 s5 
R = s4c5c 6 + c 4s6
3
6 − s 4c 5s6 + c 4c 6 s4s5 
 − s5c 6 s5 c 6 c5 
• to solve this, take two cases:
1. Both r13 and r23 are not zero (i.e. θ5 ≠ 0)… nonsingular
2. θ5 = 0, thus r13 = r23 = 0… singular
• Nonsingular case
– If θ5 ≠ 0, then r33 ≠ ±1 and:

c5 = r33 , s5 = ± 1 − r33
2

θ 5 = atan2 r33 ,± 1 − r33 

2

 

The Interventional Centre

 Inverse orientation: spherical wrist
• Thus there are two values for θ5. Using the first (s5 > 0):
θ 4 = atan2(r13 , r23 )
θ 6 = atan2(− r31, r32 )
• Using the second value for θ5 (s5 < 0):
θ 4 = atan2(− r13 ,−r23 )
θ 6 = atan2(r31,−r32 )
• Thus for the nonsingular case, there are two solutions for the inverse
orientation kinematics

The Interventional Centre

 Inverse orientation: spherical wrist
• In the singular case, θ5 = 0 thus s5 = 0 and r13 = r23 = r31 = r32 = 0
• Therefore, R63 has the form:

c 4c 6 − s 4s6 − c 4s6 − s 4c 6 0 c 46 − s46 0 r11 r12 0

R = s 4c 6 + c 4s6
3
6 − s 4s6 + c 4c 6 0 = s46 c 46 0 = r21 r22 0
 0 0 1  0 0 1  0 0 1

• So we can find the sum θ4 + θ6 as follows:

θ 4 + θ 6 = atan2(r11, r21 ) = atan2(r11,−r12 )
• Since we can only find the sum, there is an infinite number of solutions
(singular configuration)

The Interventional Centre

 Inverse Kinematics: general
procedure
1. Find q1, q2, q3 such that the position of the wrist center is:
0
oco = o − d 6R 0
inverse position
 1
kinematics
2. Using q1, q2, q3, determine R30
3. Find Euler angles corresponding to the rotation matrix:

( )
R63 = R30
−1
( ) T
R = R30 R
inverse orientation
kinematics

The Interventional Centre

 Example: RRR arm with spherical
wrist
• For the DH parameters below, we can derive R30 from the forward
kinematics: c1c 23 − c1s23 s1 
R30 = s1c 23 − s1s23 − c1 
 s23 c 23 0 
• We know that R63 is given as follows:
c 4c5c 6 − s4s6 − c 4c 5s6 − s 4c 6 c 4 s5 
R63 = s4c5c 6 + c 4s6 − s 4c 5s6 + c 4c 6 s4s5 
 − s5c 6 s5 c 6 c5 

link ai αi di θi
• To solve the inverse orientation kinematics:
1 0 90 d1 θ1
R = R R
3
6 ( )0 T
3
– For a given desired R 2 a2 0 0 θ2
3 a3 0 0 θ3

The Interventional Centre

 Example: RRR arm with spherical
wrist
• Euler angle solutions can be applied. Taking the third column of (R30)TR
c 4s5 = c1c 23 r13 + s1c 23 r23 + s23 r33
s4s5 = −c1s23 r13 − s1s23 r23 + c 23 r33
c5 = s1r13 − c1r23

• Again, if θ5 ≠ 0, we can solve for θ5:

θ 5 = atan2 s1r13 − c1r23 ,± 1 − (s1r13 − c1r23 ) 
2

 
• Finally, we can solve for the two remaining angles as follows:
θ 4 = atan2(c1c 23 r13 + s1c 23 r23 + s23 r33 ,−c1s23 r13 − s1s23 r23 + c 23 r33 )
θ 6 = atan2(− s1r11 + c1r21, s1r12 − c1r22 )
• For the singular configuration (θ5 = 0), we can only find θ4 + θ6 thus it is
common to arbitrarily set θ4 and solve for θ6

The Interventional Centre

 Example: elbow manipulator with
spherical wrist
• Derive complete inverse kinematics solution

lin ai αi di θi
k
1 0 90 d1 θ1
2 a2 0 0 θ2
3 a3 0 0 θ3
4 0 -90 0 θ4
5 0 0 0 θ5

• we are given H = T60 such that: 6 0 0 d6 θ3

o x  r11 r12 r13 
o = oy , R = r21 r22 r23 
oz  r31 r32 r33 

The Interventional Centre

 Example: elbow manipulator with
spherical wrist
• First, we find the wrist center:
 xc  o x − d 6 r13 
 y  = o − d r 
 c  y 6 23 

 zc  oz − d 6 r33 

• Inverse position kinematics:

θ1 = atan2(xc , y c )
θ 2 = atan2 xc + y c − d 2 , zc − d1  − atan2(a2 + a3c3 , a3s3 )
2 2

 
(
θ 3 = atan2 D,± 1 − D 2 )
• Where d is the shoulder offset (if any) and D is given by:
xc + y c − d 2 + (zc − d1 ) − a2 − a3
2 2 2 2 2

D=
2a2a3

The Interventional Centre

 Example: elbow manipulator with
spherical wrist
• Inverse orientation kinematics:
– Now that we know θ1, θ2, θ3, we know R30. need to find R36:
( )
R63 = R30 R
T

• Solve for θ4, θ5, θ6, Euler angles:

θ 4 = atan2(c1c 23 r13 + s1c 23 r23 + s23 r33 ,−c1s23 r13 − s1s23 r23 + c 23 r33 )
θ 5 = atan2 s1r13 − c1r23 ,± 1 − (s1r13 − c1r23 ) 
2

 
θ 6 = atan2(− s1r11 + c1r21, s1r12 − c1r22 )

The Interventional Centre

 Example: inverse kinematics of
SCARA manipulator
R o 
• We are given T40: T40 =  
 0 1
c12c 4 + s12s 4 s12c 4 − c12s 4 0 a1c1 + a2c12 
s c − c s − c12c 4 − s12s4 0 a1s1 + a2s12 
=  12 4 12 4

 0 0 − 1 − d3 − d 4 
 
 0 0 0 1 

lin ai αi di θi
k
1 a1 0 0 θ1
2 a2 180 0 θ2
3 0 0 d3 0
4 0 0 d4 θ4

The Interventional Centre

 Example: inverse kinematics of
SCARA manipulator
• Thus, given the form of T40, R must have the following form:
cα − sα 0
R = sα cα 0
 0 0 1

• Where α is defined as: α = θ1 + θ 2 − θ 4 = atan2(r11, r12 )

• To solve for θ1 and θ2 we project the manipulator onto the x0-y0 plane:
o x + oy − a1 − a2
2 2 2 2

c2 =
2a1a2
• This gives two solutions for θ2: θ 2 = atan2 c 2 ,± 1 − c 2 
2

 
• Once θ2 is known, we can solve for θ1:
θ1 = atan2(o x , oy ) - atan2(a1 + a2c 2 , a2s2 )
• θ4 is now give as: θ 4 = θ1 + θ 2 − atan2(r11, r12 )
• Finally, it is trivial to see that d3 = oz + d4
The Interventional Centre
 Example: number of solutions
• How many solutions to the inverse position kinematics of a planar 3-link
arm?
– given a desired d=[dx dy]T, the forward kinematics can be written as:
d x = a1c1 + a2c12 + a3c123
d y = a1s1 + a2s12 + a3s123
– Therefore the inverse kinematics problem is under-constrained (two equations
and three unknowns)

∞ solutions is d is inside the workspace

1 solution if d is on the workspace boundary
0 solutions else

The Interventional Centre

 Example: number of solutions
• What if now we describe the desired position and orientation of the end
effector?
– given a desired d=[dx dy]T, we can now call the position of o2 the ‘wrist center’.
This position is given as: w x = d x − a3 cos(θ d )
w y = d y − a3 sin(θ d )
– Now we have reduced the problem to finding the joint angles that will give the
desired position of the wrist center (we have done this for a 2D planar
manipulator).
– Finally, θ3 is given as: θ 3 = θ d − (θ1 + θ 2 )

∞ solutions if the wrist center is on the origin

2 solutions if wrist center is inside the 2-link workspace
1 solution if wrist center is on the 2-link workspace boundary
0 solutions else

The Interventional Centre

 Velocity Kinematics
• Now we know how to relate the end-effector position and orientation to the
joint variables
• Now we want to relate end-effector linear and angular velocities with the joint
velocities
• First we will discuss angular velocities about a fixed axis
• Second we discuss angular velocities about arbitrary (moving) axes
• We will then introduce the Jacobian
– Instantaneous transformation between a vector in Rn representing joint velocities to a
vector in R6 representing the linear and angular velocities of the end-effector
• Finally, we use the Jacobian to discuss numerous aspects of manipulators:
– Singular configurations
– Dynamics
– Joint/end-effector forces and torques

The Interventional Centre

 Angular velocity: fixed axis
• When a rigid body rotates about a fixed axis, every point
moves in a circle
– Let k represent the fixed axis of rotation, then the angular velocity is:
ω = θk̂
– The velocity of any point on a rigid body due to this angular velocity
is: v = ω×r

– Where r is the vector from the axis of rotation to the point

• When a rigid body translates, all points attached to the body

have the same velocity

The Interventional Centre

 Next class…
• Derivation of the Jacobian

The Interventional Centre