ES159/259

Ch. 3: Forward and Inverse Kinematics

ES159/259

Updates

• Document clarifying the Denavit-Hartenberg convention is posted • Labs and section times announced

– If you haven’t already, please forward your availability to Shelten & Ben • Matlab review session Tuesday 2/13, 6:00 MD 221

ES159/259

Recap: The Denavit-Hartenberg (DH) Convention

• Representing each individual homogeneous transformation as the product of four basic transformations:

−

−

=

−

−

=

=

10000

100000000001

100001000010

001

1000100

00100001

100001000000

,,,,

i

i

i

i

i

xaxdzzi

dcssascccscasscsc

cssc

a

dcssc

A

ii

iiiiii

iiiiii

ii

iiii

ii

iiii

αα

θαθαθθ

θαθαθθ

αα

ααθθ

θθ

αθ RotTransTransRot

ES159/259

Recap: the physical basis for DH parameters

• ai: link length, distance between the o0 and o1 (projected along x1) • αi: link twist, angle between z0 and z1 (measured around x1) • di: link offset, distance between o0 and o1 (projected along z0) • θi: joint angle, angle between x0 and x1 (measured around z0)

ES159/259

General procedure for determining forward kinematics

1. Label joint axes as z0, …, zn-1 (axis zi is joint axis for joint i+1) 2. Choose base frame: set o0 on z0 and choose x0 and y0 using right-

handed convention 3. For i=1:n-1,

i. Place oi where the normal to zi and zi-1 intersects zi. If zi intersects zi-1, put oi at intersection. If zi and zi-1 are parallel, place oi along zi such that di=0

ii. xi is the common normal through oi, or normal to the plane formed by zi-1 and zi if the two intersect

iii. Determine yi using right-handed convention 4. Place the tool frame: set zn parallel to zn-1 5. For i=1:n, fill in the table of DH parameters 6. Form homogeneous transformation matrices, Ai 7. Create Tn

0 that gives the position and orientation of the end-effector in the inertial frame

ES159/259

Example 2: three-link cylindrical robot

• 3DOF: need to assign four coordinate frames 1. Choose z0 axis (axis of rotation for joint 1, base frame) 2. Choose z1 axis (axis of translation for joint 2) 3. Choose z2 axis (axis of translation for joint 3) 4. Choose z3 axis (tool frame)

• This is again arbitrary for this case since we have described no wrist/gripper • Instead, define z3 as parallel to z2

ES159/259

Example 2: three-link cylindrical robot

• Now define DH parameters – First, define the constant parameters ai, αi – Second, define the variable parameters θi, di

link ai αi di θi

1 0 0 d1 θ1

2 0 -90 d2 0

3 0 0 d3 0

=

−=

−

=

1000100

00100001

1000010

01000001

1000100

0000

33

22

1

11

11

1 dA

dA

dcssc

A , ,

+−

−−

==

1000010

00

21

3111

3111

3210

3 dddccsdssc

AAAT

ES159/259

Example 3: spherical wrist

• 3DOF: need to assign four coordinate frames – yaw, pitch, roll (θ4, θ5, θ6) all intersecting at one point o (wrist center) 1. Choose z3 axis (axis of rotation for joint 4) 2. Choose z4 axis (axis of rotation for joint 5) 3. Choose z5 axis (axis of rotation for joint 6) 4. Choose tool frame:

• z6 (a) is collinear with z5

• y6 (s) is in the direction the gripper closes • x6 (n) is chosen with a right-handed convention

ES159/259

Example 3: spherical wrist

link ai αi di θi

4 0 -90 0 θ4

5 0 90 0 θ5

6 0 0 d6 θ6

• Now define DH parameters – First, define the constant parameters ai, αi – Second, define the variable parameters θi, di

−

=

−

−

=

−

−

=

1000100

0000

100000100000

100000100000

6

66

66

655

55

544

44

4 dcssc

Acssc

Acssc

A , ,

−+−+−−−

==

10006556565

654546465464654

654546465464654

6543

6 dcccscsdssssccscsscccsdscsccssccssccc

AAAT

ES159/259

Example 4: cylindrical robot with spherical wrist

• 6DOF: need to assign seven coordinate frames – But we already did this for the previous two examples, so we can fill in the

table of DH parameters:

link ai αi di θi

1 0 0 d1 θ1

2 0 -90 d2 0

3 0 0 d3 0

4 0 -90 0 θ4

5 0 90 0 θ5

6 0 0 d6 θ6

o3, o4, o5 are all at the same point oc

ES159/259

Example 4: cylindrical robot with spherical wrist

• Note that z3 (axis for joint 4) is collinear with z2 (axis for joint 3), thus we can make the following combination:

==

1000333231

232221

131211

36

03

06

z

y

x

drrrdrrrdrrr

TTT

21654

316516541

316516541

5433

5154123

5154113

6465432

651641654122

651641654112

6465431

651641654121

651641654111

dddssddcdccdscsddsdcsdsccd

ssrccscsrcssccrccccsr

cscssssccsrcsscscscccr

scccsrcscssscccsrcsssscccccr

z

y

x

++−=

++=

−−=

−=

+=

−=

−=

+−−=

−−−=

−−=

−−=

+−=

ES159/259

Example 5: the Stanford manipulator

• 6DOF: need to assign seven coordinate frames: 1. Choose z0 axis (axis of rotation for joint 1, base frame) 2. Choose z1-z5 axes (axes of rotation/translation for joints 2-6) 3. Choose xi axes 4. Choose tool frame 5. Fill in table of 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

ES159/259

Example 5: the Stanford manipulator

• Now determine the individual homogeneous transformations:

−

=

−−

=

−

−

=

=

−

=

−

−

=

1000100

0000

100000100000

100000100000

1000100

00100001

1000010

0000

100000100000

6

66

66

655

55

544

44

4

33

2

22

22

211

11

1

dcssc

Acs

sc

Acssc

A

dA

dcs

sc

Acssc

A

, ,

, ,

ES159/259

Example 5: the Stanford manipulator

• Finally, combine to give the complete description of the forward kinematics:

=⋅⋅⋅=

1000333231

232221

131211

610

6z

y

x

drrrdrrrdrrr

AAT

( )[ ] ( )( )[ ] ( )

( )( )[ ] ( )

( )[ ] ( )( )( )( )

( )( )

( )52452632

2155142541621321

5412515421621321

5254233

54152542123

54152542113

65264654232

646541652646542122

646541652646542112

65264654231

646541652646542121

646542652646542111

sscccddcdsscssccsscddcdssdssssccscccddsdscd

ccscsrssccssccsrssscsscccrssccssccsr

scscscssscsscccsrccscssssscssccccr

cscsscccsrscccsccssssccccsrscccsdcsssscccccr

z

y

x

−+=

++++=

−++−=

+−=

++=

−+=

++=

+−+−−−−=

+−−++−=

−−−=

++−−=

+−−−=

ES159/259

Example 6: the SCARA manipulator

• 4DOF: need to assign five coordinate frames: 1. Choose z0 axis (axis of rotation for joint 1, base frame) 2. Choose z1-z3 axes (axes of rotation/translation for joints 2-4) 3. Choose xi axes 4. Choose tool frame 5. Fill in table of DH parameters:

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

ES159/259

Example 6: the SCARA manipulator

• Now determine the individual homogeneous transformations:

−

=

=

−−

=

−

=

1000100

0000

1000100

00100001

10000100

00

10000100

00

4

44

44

43

32222

2222

21111

1111

1 dcssc

Ad

Asacscasc

Asacscasc

A , , ,

−−−+−−−++−+

=⋅⋅⋅=

1000100

00

43

12211412412412412

12211412412412412

410

4 ddsasaccsssccscacacsscsscc

AAT

ES159/259

Forward kinematics of parallel manipulators

• Parallel manipulator: two or more series chains connect the end-effector to the base (closed-chain)

• # of DOF for a parallel manipulator determined by taking the total DOFs for all links and subtracting the number of constraints imposed by the closed-chain configuration

• Gruebler’s formula (3D):

( ) ∑=

+−=jn

iijL fnn

16DOF#

number of links*

*excluding ground

number of joints

#DOF for joint i

ES159/259

Forward kinematics of parallel manipulators

• Gruebler’s formula (2D):

• Example (2D): – Planar four-bar, nL = 3, nj = 4, fi = 1(for all joints)

• 3(3-4)+4 = 1DOF – Forward kinematics:

( ) ∑=

+−=jn

iijL fnn

13DOF#

( ) 2tan

2

22cos1

21

22

21

22

21 π

δδ

δδθ −

−

+

+−

+−= −−

LL

LL

L

ES159/259

Inverse Kinematics

• Find the values of joint parameters that will put the tool frame at a desired position and orientation (within the workspace)

– Given H:

– Find all solutions to:

– Noting that:

– This gives 12 (nontrivial) equations with n unknowns

( )310

SEoR

H ∈

=

( ) HqqT nn =,...,10

( ) ( ) ( )nnnn qAqAqqT ⋅⋅⋅= 1110 ,...,

ES159/259

• For a given H:

• Find θ1, θ2, d3, θ4, θ5, θ6:

• One solution: θ1 = π/2, θ2 = π/2, d3 = 0.5, θ4 = π/2, θ5 = 0, θ6 = π/2

−

=

10000001763.0100154.0010

H

Example: the Stanford manipulator

( )[ ] ( )( )[ ] ( )

( )( )[ ] ( )( )[ ] ( )

( )( )( )

( )( )

( ) 0763.0

154.0010001100

52452632

2155142541621321

5412515421621321

52542

541525421

541525421

652646542

6465416526465421

6465416526465421

652646542

6465416526465421

6465426526465421

=−+

=++++

−=−++−

=+−

=++

=−+

=++

=+−+−−−−

=+−−++−

=−−−

=++−−

=+−−−

sscccddcsscssccsscddcdssssssccscccddsdscccscsssccssccsssscsscccssccssccs

scscscssscsscccsccscssssscsscccccscsscccsscccsccssssccccsscccsdcssssccccc

ES159/259

Inverse Kinematics

• The previous example shows 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!

ES159/259

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

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

( )( ) oqqo

RqqR

=

=

6106

6106

,...,,...,

ES159/259

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

ES159/259

Overview: 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 a spherical wrist, we can use a set of Euler angles to solve for them

36

03RRR =

( ) ( ) RRRRR T03

103

36 ==

−

ES159/259

Inverse position

• 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) and spherical (RRP) manipulators – For example, for an elbow manipulator, to solve for θ1, project the arm onto

the x0, y0 plane

ES159/259

Background: two argument atan

• We use atan2(·) instead of atan(·) to account for the full range of angular solutions

– Called ‘four-quadrant’ arctan

( )

( )

==

=>

≥≥

<≥

−−

<−−

=

0,0

0,02

0,0

0,0

0,2

,2

xy

xy

xyxy

xyxy

yxy

xy

undefined

atan

atan

atan

atan

π

π

ES159/259

Example: RRR manipulator

1. To solve for θ1, project the arm onto the x0, y0 plane

– Can also have: • This will of course change the solutions for θ2 and θ3

( )cc yx ,21 atan=θ

( )cc yx ,21 atan+= πθ

ES159/259

• If there is an offset, then we will have two solutions for θ1: left arm and right arm

– However, wrist centers cannot intersect z0

• If xc=yc=0, θ1 is undefined – i.e. any value of θ1 will work

Caveats: singular configurations, offsets

ES159/259

• Left arm: • Right arm:

Left arm and right arm solutions

( )

−+=

==

ddyx

yx

cc

cc

,2

,2222

1

atan

atan-

α

φαφθ ( )

−−+−=

−++=

=+=

ddyx

ddyx

yx

cc

cc

cc

,2

,2

,2

222

222

1

atan

atan

atan

πβ

αβαθ

ES159/259

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

• Therefore we can find two solutions for θ3:

Left arm and right arm solutions

( ) Daa

aadzdyx

dzsdyxr

aaaasr

ccc

c

cc

≡−−−+−+

=⇒

−=

−+=

−−+=

32

23

22

21

222

3

1

2222

32

23

22

22

3

2cos

2cos

θ

θ

( )23 1,2 DD −±= atanθ

ES159/259

• The two solutions for θ3 correspond to the elbow-down and elbow-up positions respectively

• Now solve for θ2:

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

Left arm and right arm solutions

( ) ( )( )333321

222

333322

,2,2

,2,2

sacaadzdyx

sacaasr

ccc +−

−−+=

+−=

atanatan

atanatanθ

ES159/259

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

– Ex: PUMA

RRR: Four total solutions

ES159/259

• Spherical configuration – Solve for θ1 using same method as with RRR

– Again, if there is an offset, there will be left-arm and right-arm solutions – Solve for θ2:

– Solve for d3:

Example: RRP manipulator

( )cc yx ,21 atan=θ

( )

1

222

2 ,2

dzsyxr

rs

c

cc

−=

+=

= atanθ

( )21

22

223

dzyx

srd

ccc −++=

+=

ES159/259

Next class…

• Complete the discussion of inverse kinematics – Inverse orientation – Introduction to other methods

• Introduction to velocity kinematics and the Jacobian