Ch. 3: Inverse Kinematics Ch. 4: Velocity Kinematics Inverse - - PDF document

1

• Ch. 3: Inverse Kinematics
• Ch. 4: Velocity Kinematics

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 end effector to solve for the last three joint angles

2 Inverse orientation: spherical wrist

• Previously, we said that the forward kinematics of the spherical

wrist were identical to a ZYZ Euler angle transformation: wrist were identical to a ZYZ Euler angle transformation:

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

6 5 5 6 5 6 5 6 5 4 5 4 6 4 6 5 4 6 4 6 5 4 6 5 4 5 4 6 4 6 5 4 6 4 6 5 4 6 5 4 3 6

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

• The inverse orientation problem reduces to finding a set of Euler

angles (θ θ θ ) that satisfy:

Inverse orientation: spherical wrist

angles (θ4, θ5, θ6) that satisfy: ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ − + − + − − − =

5 6 5 6 5 5 4 6 4 6 5 4 6 4 6 5 4 5 4 6 4 6 5 4 6 4 6 5 4 3 6

c c s c s s s c c s c s s c c c s s c c s s c c s s c c c R

3

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

Inverse orientation: spherical wrist (Non-singular case)

• Using the second value for θ5 (s5 < 0):
• In the singular case, θ5 = 0

Inverse orientation: spherical wrist (singular case)

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

5 6 5 6 5 5 4 6 4 6 5 4 6 4 6 5 4 5 4 6 4 6 5 4 6 4 6 5 4 3 6

c c s c s s s c c s c s s c c c s s c c s s c c s s c c c R

4 Inverse Kinematics: general procedure

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

⎥ ⎤ ⎢ ⎡0

• 2. Using q1, q2, q3, determine R3
• 3. Find Euler angles corresponding to the rotation matrix:

⎥ ⎥ ⎥ ⎦ ⎢ ⎢ ⎢ ⎣ − = 1

6R

d

• c

inverse position kinematics

( ) ( ) R

R R R R

T 3 1 3 3 6

= =

− inverse orientation kinematics

Example: RRR arm with spherical wrist

• For the DH parameters below, we can derive R3

0 from the

forward kinematics: ⎤ ⎡ s s c c c

• We know that R6

3 is given as follows:

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

23 23 1 23 1 23 1 1 23 1 23 1 3

c s c s s c s s s c c c R

⎥ ⎥ ⎤ ⎢ ⎢ ⎡ + − + − − − =

5 4 6 4 6 5 4 6 4 6 5 4 5 4 6 4 6 5 4 6 4 6 5 4 3 6

s s c c s c s s c c c s s c c s s c c s s c c c R

• To solve the inverse orientation kinematics:

link ai αi di θi 1 90 d1 θ1 2 a2 θ2 3 a3 θ3 ⎥ ⎥ ⎦ ⎢ ⎢ ⎣ −

5 6 5 6 5 5 4 6 4 6 5 4 6 4 6 5 4 6

c c s c s

5 Example: RRR arm with spherical wrist

• Euler angle solutions can be applied. Taking the third column of

(R3

0)TR

33 23 23 23 1 13 23 1 5 4

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

23 1 13 1 5 33 23 23 23 1 13 23 1 5 4

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

Example: elbow manipulator with spherical wrist

• Derive complete inverse kinematics solution

p

link ai αi di θi 1 90 d1 θ1 2 a2 θ2 3 a3 θ3

• we are given H = T6

0 such that:

4

• 90

θ4 5 θ5 6 d6 θ3 ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ =

33 32 31 23 22 21 13 12 11

r r r r r r r r r R

• z

y x

,

6 Example: elbow manipulator with spherical wrist

• First, we find the wrist center:

,

• Inverse position kinematics:

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

33 6 23 6 13 6

r d

• r

d

• r

d

• z

y x

z y x c c c

• Where d is the shoulder offset (if any) and

Example: elbow manipulator with spherical wrist

• Inverse orientation kinematics:

– Now that we know θ1, θ2, θ3, we know R3

• 0. need to find R3

6:

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

( ) R

R R

T 3 3 6 =

( )

33 23 23 23 1 13 23 1 33 23 23 23 1 13 23 1 4

, 2 atan r c r s s r s c r s r c s r c c + − − + + = θ

( ) ( ) ( )

22 1 12 1 21 1 11 1 6 2 23 1 13 1 23 1 13 1 5 33 23 23 23 1 13 23 1 33 23 23 23 1 13 23 1 4

, 2 atan 1 , 2 atan , 2 atan r c r s r c r s r c r s r c r s r c r s s r s c r s r c s r c c − + − = ⎟ ⎠ ⎞ ⎜ ⎝ ⎛ − − ± − = + + + θ θ θ

7 Example: inverse kinematics of SCARA manipulator

• We are given T4

0:

⎤ ⎡

• R

g

4

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

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

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

• R

T link ai αi di θi 1 a1 θ1 2 a2 180 θ2 3 d3 4 d4 θ4

Example: inverse kinematics of SCARA manipulator

• Thus, given the form of T4

0, R must have the following form:

⎥ ⎥ ⎤ ⎢ ⎢ ⎡ − =

α α α α

c s s c R

• Where α is defined as:

⎥ ⎥ ⎦ ⎢ ⎢ ⎣ 1

α α

= − + =

4 2 1

θ θ θ α

8 Example: number of solutions

• How many solutions to the inverse position kinematics of a

planar 3-link arm?

Example: number of solutions

• What if now we describe the desired position and orientation of

the end effector?

9 Velocity Kinematics

• 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

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
• Finally, we use the Jacobian to discuss numerous aspects of

manipulators:

– Singular configurations g g – Dynamics – Joint/end-effector forces and torques

Angular velocity: fixed axis

• When a rigid body rotates about a fixed axis, every

point moves in a circle

10 Angular velocity: arbitrary axis

• Skew-symmetric matrices

– Definition: a matrix S is skew symmetric if: i e S ST – i.e. = + S ST

Angular velocity: arbitrary axis

• Example:

– Let i, j, k be defined as follows: – Then we can define the skew symmetric matrices S(i), S(j), S(k):

⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ = 1 ˆ 1 ˆ 1 ˆ k j i , ,

( ) ( ) ( )

, , , ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ − = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ − = 1 1 ˆ 1 1 ˆ 1 1 ˆ k S j S i S ⎥ ⎦ ⎢ ⎣ ⎥ ⎦ ⎢ ⎣− ⎥ ⎦ ⎢ ⎣ 1 1

11 Angular velocity: arbitrary axis

• Properties of skew-symmetric matrices
• 1. The operator S is linear

( ) ( ) ( )

R R , ∈ ∈ ∀ + = + β α β α β α , , ,

3

b a b S a S b a S

• 2. The operator S is known as the cross product operator

– This can be seen by the definition of the cross product:

( ) ( ) ( )

R R , ∈ ∈ ∀ + + β α β α β α , , ,b a b S a S b a S ( )

3

R p a, , ∈ ∀ × = p a p a S

Angular velocity: arbitrary axis

• Properties of skew-symmetric matrices
• 3. For

Thi b h f R f ll

( )

3

3 R , ∈ ∈ a SO R

– This can be shown for an R as follows:

4 For any

( ) ( )

Ra S R a RS

T =

• 4. For any

( )

n

x n so S R , ∈ ∈

= Sx xT

12 Angular velocity: arbitrary axis

• Derivative of a rotation matrix

( ) ( ) ( )

R d d R R R d d SR

T

θ θ θ θ θ = ⎥ ⎦ ⎤ ⎢ ⎣ ⎡ =

Angular velocity: arbitrary axis

• Example: let R=Rx,θ, then using the previous results we have:

13 Angular velocity: arbitrary axis

• Now consider that we have an angular velocity about an

bit i arbitrary axis

• Further, let R = R(t)

Next class…

• Derivation of the Jacobian