Ch. 3: Forward and Inverse Kinematics Recap: The Denavit-Hartenberg - - PDF document

1

• Ch. 3: Forward and Inverse Kinematics

Recap: The Denavit-Hartenberg (DH) Convention

• Representing each individual homogeneous transformation as the

product of four basic transformations: product of four basic transformations:

⎥ ⎥ ⎤ ⎢ ⎢ ⎡ − ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ − ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ − = = 1 1 1 1 1 1 1 1 1 1 1 1

, , , , i i i x a x d z z i

s a s c c c s c a s s c s c c s s c a d c s s c A

i i i i i i i i i i i i i i i i i i

θ α θ α θ θ α α α α θ θ θ θ α θ

Rot Trans Trans Rot ⎥ ⎥ ⎥ ⎦ ⎢ ⎢ ⎢ ⎣ − = 1

i i

d c s s a s c c c s

i i i i i i i i

α α θ α θ α θ θ

2 Recap: the physical basis for DH parameters

• ai: link length, distance between the o0 and o1 (projected along x1)
• α: link twist angle between z and z (measured around x )
• α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)

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

• i 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 z parallel to z

1

• 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

3 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 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 2 3 4 5 6

• 3, o4, o5 are all at

the same point oc

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: can make the following combination:

⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ = = 1

33 32 31 23 22 21 13 12 11 3 6 3 6 z y x

d r r r d r r r d r r r T T T

5 1 5 4 1 13 6 4 6 5 4 32 6 5 1 6 4 1 6 5 4 1 22 6 5 1 6 4 1 6 5 4 1 12 6 4 6 5 4 31 6 5 1 6 4 1 6 5 4 1 21 6 5 1 6 4 1 6 5 4 1 11

r c s s c c r c c c c s r c s c s s s s c c s r c s s c s c s c c c r s c c c s r c s c s s s c c c s r c s s s s c c c c c r − = − = + − − = − − − = − − = − − = + − =

2 1 6 5 4 3 1 6 5 1 6 5 4 1 33 23

d d d s s d d c d c c d s c s d d r r

z y x

+ + − = + + = = = =

4 Example 5: the Stanford manipulator

• 6DOF: need to assign seven coordinate frames:

1 Choose z0 axis (axis of rotation for joint 1 base frame)

• 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 2 3 4 5 6

Example 5: the Stanford manipulator

• Now determine the individual homogeneous transformations:

, 1 1 1 1 1 , 1 1 , 1 1

4 4 4 4 4 3 3 2 2 2 2 2 2 1 1 1 1 1

⎥ ⎥ ⎥ ⎥ ⎤ ⎢ ⎢ ⎢ ⎢ ⎡ − − = ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ = ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ − = ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ − − = c s s c A d A d c s s c A c s s c A 1 ⎥ ⎦ ⎢ ⎣

5 Example 5: the Stanford manipulator

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

kinematics: kinematics:

⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ = ⋅ ⋅ ⋅ = 1

33 32 31 23 22 21 13 12 11 6 1 6 z y x

d r r r d r r r d r r r A A T

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

+ = + + = + − + − − − − = + − − + + − = − − − = + + − − = + − − − = s s s c s s c c c r s s c c s s c c s r s c s c s c s s s c s s c c c s r c c s c s s s s s c s s c c c c r c s c s s c c c s r s c c c s c c s s s s c c c c s r s c c c s d c s s s s c c c c c r

5 4 1 5 2 5 4 2 1 13 6 5 2 6 4 6 5 4 2 32 6 4 6 5 4 1 6 5 2 6 4 6 5 4 2 1 22 6 4 6 5 4 1 6 5 2 6 4 6 5 4 2 1 12 6 5 2 6 4 6 5 4 2 31 6 4 6 5 4 1 6 5 2 6 4 6 5 4 2 1 21 6 4 6 5 4 2 6 5 2 6 4 6 5 4 2 1 11

⎥ ⎦ ⎢ ⎣ 1

( ) ( )

= + + + + = − + + − = = =

z y x

d s s c s s c c s s c d d c d s s d s s s s c c s c c c d d s d s c d r r

2 1 5 5 1 4 2 5 4 1 6 2 1 3 2 1 5 4 1 2 5 1 5 4 2 1 6 2 1 3 2 1 33 23

Example 6: the SCARA manipulator

• 4DOF: need to assign five coordinate frames:

1 Choose z0 axis (axis of rotation for joint 1 base frame)

• 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 2 3 4

6 Example 6: the SCARA manipulator

• Now determine the individual homogeneous transformations:

⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ − = ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ = ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ − − = ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ − = 1 1 1 1 1 1 1 1 1 1

4 4 4 4 4 4 3 3 2 2 2 2 2 2 2 2 2 1 1 1 1 1 1 1 1 1

d c s s c A d A s a c s c a s c A s a c s c a s c A , , , ⎥ ⎥ ⎤ ⎢ ⎢ ⎡ + − − − + + − +

12 2 1 1 4 12 4 12 4 12 4 12 12 2 1 1 4 12 4 12 4 12 4 12

s a s a c c s s s c c s c a c a c s s c s s c c ⎥ ⎥ ⎥ ⎦ ⎢ ⎢ ⎢ ⎣ − − − + = ⋅ ⋅ ⋅ = 1 1

4 3 12 2 1 1 4 12 4 12 4 12 4 12 4 1 4

d d s a s a c c s s s c c s A A T

Forward kinematics of parallel manipulators

• Parallel manipulator: two or more series chains connect the end-

p effector to the base (closed-chain)

• Gruebler’s formula (3D):

( ) ∑

=

+ − =

j

n i i j L

f n n

1

6 DOF #

#DOF for joint i number of links*

*excluding ground

number of joints #DOF for joint i

7 Forward kinematics of parallel manipulators

• Example (2D):

Inverse Kinematics

• Find the values of joint parameters that will put the tool frame at a

j p p desired position and orientation (within the workspace)

– Given H:

( )

3 1 SE

• R

H ∈ ⎥ ⎦ ⎤ ⎢ ⎣ ⎡ =

8 Example: the Stanford manipulator

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

6 4 6 5 4 2 6 5 2 6 4 6 5 4 2 1

+ + = + − − − s c c c s d c s s s s c c c c c

• For a given H:
• Find θ1, θ2, d3, θ4, θ5, θ6:

⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ − = 1 1 763 . 1 154 . 1 H

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

1 1

5 4 1 5 2 5 4 2 1 6 5 2 6 4 6 5 4 2 6 4 6 5 4 1 6 5 2 6 4 6 5 4 2 1 6 4 6 5 4 1 6 5 2 6 4 6 5 4 2 1 6 5 2 6 4 6 5 4 2 6 4 6 5 4 1 6 5 2 6 4 6 5 4 2 1

= − + = + + = + − + − − − − = + − − + + − = − − − = + + − − s s s c s s c c c s s c c s s c c s s c s c s c s s s c s s c c c s c c s c s s s s s c s s c c c c c s c s s c c c s s c c c s c c s s s s c c c c s

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

( ) ( )

763 .

5 2 4 5 2 6 3 2 2 1 5 5 1 4 2 5 4 1 6 2 1 3 2 1

= − + = + + + + s s c c c d d c s s c s s c c s s c d d c d s s

Inverse Kinematics

• For the forward kinematics there is always a unique solution

y q

• The inverse kinematics may or may not have a solution

9 Overview: kinematic decoupling

• Appropriate for systems that have an arm a wrist

pp p y

Overview: kinematic decoupling

• Now, origin of tool frame, o6, is a distance d6 translated along z5 (since

g

6 6

g

5 (

z5 and z6 are collinear)

10 Inverse position

• Now that we have [xc yc zc]T we need to find q1, q2, q3

[ c yc

c]

q1 q2 q3

Background: two argument atan

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

( ) ( ) g angular solutions

– Called ‘four-quadrant’ arctan

( ) ( )

⎪ ⎪ ⎪ ⎪ ⎪ ⎨ ⎧ ≥ ≥ ⎟ ⎠ ⎞ ⎜ ⎝ ⎛ < ≥ ⎟ ⎠ ⎞ ⎜ ⎝ ⎛− − < − − = , , , 2 , 2 x y x y x y x y y x y x y atan atan atan atan π ⎪ ⎪ ⎪ ⎪ ⎩ = = = > ⎠ ⎝ , , 2 x y x y x undefined π

11 Example: RRR manipulator

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

1 p

j

0 y0 p

( )

c c y

x , 2

1

atan = θ

• If there is an offset, then we will
• If xc=yc=0, θ1 is undefined

Caveats: singular configurations, offsets

have two solutions for θ1: left arm and right arm

12

• Left arm:
• Right arm:

Left arm and right arm solutions

• Therefore there are in general two solutions for θ1

Left arm and right arm solutions

g

1

• s for θ3:

13

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

Left arm and right arm solutions

3

p p positions respectively

• In general, there will be a maximum of four solutions to the inverse

RRR: Four total solutions

g position kinematics of an elbow manipulator

– Ex: PUMA

14

• Spherical configuration

Example: RRP manipulator Next class…

• Complete the discussion of inverse kinematics

p

– Inverse orientation – Introduction to other methods

• Introduction to velocity kinematics and the Jacobian