The Interventional Centre

Ch. 3: Inverse KinematicsCh. 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 center2. Inverse orientation kinematics: orientation of the wrist

• First, assume 6DOF, the last three intersecting at oc

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

( )( ) oqqo

RqqR

=

=

6106

6106

,...,,...,

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:

– Rearranging:

– Calling o = [ox oy oz]T, oc0 = [xc yc zc]T

+==

100

606 Rdooo o

c

−=

100

6Rdoooc

−−−

=

336

236

136

rdordordo

zyx

z

y

x

c

c

c

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:

– To solve for the final three joint angles:

– Since the last three joints for aspherical wrist, we can use a set ofEuler angles to solve for them

36

03RRR =

( ) ( ) RRRRR T03

103

36 ==

−

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:

−+−+−−−

==

10006556565

654546465464654

654546465464654

6543

6 dcccscsdssssccscsscccsdscsccssccssccc

AAAT

The Interventional Centre

• The inverse orientation problem reduces to finding a set of Euler angles (θ4, θ5, θ6) that satisfy:

• to solve this, take two cases:1. Both r13 and r23 are not zero (i.e. θ5 ≠ 0)… nonsingular2. θ5 = 0, thus r13 = r23 = 0… singular

• Nonsingular case– If θ5 ≠ 0, then r33 ≠ ±1 and:

−+−+−−−

=

56565

546465464654

54646546465436

ccscsssccscsscccssccssccssccc

R

−±=

−±==

233335

2335335

1,2

1

rr

rsrc

atan

,

θ

Inverse orientation: spherical wrist

The Interventional Centre

• Thus there are two values for θ5. Using the first (s5 > 0):

• Using the second value for θ5 (s5 < 0):

• Thus for the nonsingular case, there are two solutions for the inverse orientation kinematics

Inverse orientation: spherical wrist

( )( )32316

23134

,2,2

rrrr

−=

=

atanatan

θθ

( )( )32316

23134

,2,2rr

rr−=

−−=

atanatan

θθ

The Interventional Centre

• In the singular case, θ5 = 0 thus s5 = 0 and r13 = r23 = r31 = r32 = 0• Therefore, R6

3 has the form:

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

• Since we can only find the sum, there is an infinite number of solutions (singular configuration)

Inverse orientation: spherical wrist

=

−=

+−+−−−

=10000

10000

10000

2221

1211

4646

4646

64646464

6464646436 rr

rrcssc

ccsssccscsscsscc

R

( ) ( )1211211164 ,2,2 rrrr −==+ atanatanθθ

The Interventional Centre

Inverse Kinematics: general procedure

1. Find q1, q2, q3 such that the position of the wrist center is:

2. Using q1, q2, q3, determine R30

3. Find Euler angles corresponding to the rotation matrix:

−=

100

6Rdoooc

( ) ( ) RRRRR T03

103

36 ==

−

inverse positionkinematics

inverse orientationkinematics

The Interventional Centre

Example: RRR arm with spherical wrist

• For the DH parameters below, we can derive R30 from the forward

kinematics:

• We know that R63 is given as follows:

• To solve the inverse orientation kinematics:

– For a given desired R

link ai αi di θi

1 0 90 d1 θ1

2 a2 0 0 θ2

3 a3 0 0 θ3

−−

−=

02323

1231231

123123103

cscsscs

sscccR

−+−+−−−

=

56565

546465464654

54646546465436

ccscsssccscsscccssccssccssccc

R

( ) RRR T03

36 =

The Interventional Centre

Example: RRR arm with spherical wrist

• Euler angle solutions can be applied. Taking the third column of (R30)TR

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

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

• For the singular configuration (θ5 = 0), we can only find θ4 + θ6 thus it is common to arbitrarily set θ4 and solve for θ6

2311315

3323232311323154

3323232311323154

rcrscrcrssrscss

rsrcsrccsc

−=

+−−=

++=

( )

−−±−= 2

2311312311315 1,2 rcrsrcrsatanθ

( )( )2211212111116

33232323113231332323231132314

,2,2

rcrsrcrsrcrssrscrsrcsrcc

−+−=

+−−++=

atanatan

θθ

The Interventional Centre

Example: elbow manipulator with spherical wrist

• Derive complete inverse kinematics solution

• we are given H = T60 such that:

link

ai αi di θi

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

6 0 0 d6 θ3

=

=

333231

232221

131211

rrrrrrrrr

Rooo

o

z

y

x

,

The Interventional Centre

Example: elbow manipulator with spherical wrist

• First, we find the wrist center:

• Inverse position kinematics:

• Where d is the shoulder offset (if any) and D is given by:

−−−

=

336

236

136

rdordordo

zyx

z

y

x

c

c

c

( )( )

( )23

333321222

2

1

1,2

,2,2

,2

DD

sacaadzdyx

yx

ccc

cc

−±=

+−

−−+=

=

atan

atanatan

atan

θ

θ

θ

( )32

23

22

21

222

2 aaaadzdyxD ccc −−−+−+

=

The Interventional Centre

Example: elbow manipulator with spherical wrist

• Inverse orientation kinematics:– Now that we know θ1, θ2, θ3, we know R3

0. need to find R36:

• Solve for θ4, θ5, θ6, Euler angles:( ) RRR T0

336 =

( )( )

( )2211212111116

22311312311315

33232323113231332323231132314

,2

1,2

,2

rcrsrcrs

rcrsrcrs

rcrssrscrsrcsrcc

−+−=

−−±−=

+−−++=

atan

atan

atan

θ

θ

θ

The Interventional Centre

Example: inverse kinematics of SCARA manipulator

• We are given T40:

−−−+−−−+−+

=

=

1000100

00

10

43

12211412412412412

12211412412412412

04

ddsasassccsccscacasccssscc

oRT

link

ai αi di θi

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:

• Where α is defined as:• To solve for θ1 and θ2 we project the manipulator onto the x0-y0 plane:

• This gives two solutions for θ2:• Once θ2 is known, we can solve for θ1:

• θ4 is now give as:• Finally, it is trivial to see that d3 = oz + d4

−=

10000

αα

αα

cssc

R

( )1211421 ,2 rratan=−+= θθθα

21

22

21

22

2 2 aaaaoo

c yx −−+=

−±= 2

222 1,2 ccatanθ

( ) ( )222211 ,2,2 sacaaoo yx += atan-atanθ

( )1211214 ,2 rratan−+= θθθ

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:

– Therefore the inverse kinematics problem is under-constrained (two equations and three unknowns)

123312211

123312211

sasasadcacacad

y

x

++=

++=

∞ solutions is d is inside the workspace1 solution if d is on the workspace boundary0 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:

– 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:

( )( )dyy

dxx

adwadw

θθ

sincos

3

3

−=

−=

∞ solutions if the wrist center is on the origin2 solutions if wrist center is inside the 2-link workspace1 solution if wrist center is on the 2-link workspace boundary0 solutions else

( )213 θθθθ +−= d

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:

– The velocity of any point on a rigid body due to this angular velocity is:

– 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

k̂θω =

rv ×= ω

The Interventional Centre

Next class…

• Derivation of the Jacobian