Inverse Kinematics Solutions_Lec_5

Inverse Kinematics –IKS Solutions

ME 4135 – Robotics and ControlsR.R. Lindeke, Ph.D., Fall 2011

FKS vs. IKS

• In FKS we built a tool for finding end frame geometry from Given Joint data:

• In IKS we need Joint models from given End Point POSE Geometry:

Joint Space

Cartesian Space

Joint Space

Cartesian Space

So this IKS is ‘Nasty’ – it is a so-call Hard Mathematical Problem

• It a more difficult problem because:– The Equation set is “Over-Specified”

• 12 equations in 6 unknowns

– Space may be “Under-Specified”• for example ‘Planer devices’ with more joints than 2

– The Solution set can contain Redundancies • Thus they exhibit Multiple solutions

– The Solution Sets may be un-defined• Unreachable in 1 or many joints

But the IKS in Critical in Robotics

• Builds Workspace limits and controller maps

• Allows “Off-Line Programming” solutions

• Thus, compares Workspace capabilities with Programming desirability to assure that execution is feasible

• Aids in Workplace design and operational simulations

Doing a Pure IKS solution: the R Manipulator

X0

Y0

Z0

Z1

X1

Y1 Y2

X2

Z2

R Frame Skeleton

LP Table and Ai’s

1

1 0 1 0

1 0 1 0

0 1 0 0

0 0 0 1

S C

C SA

Frames Link Var d l S C S C

0 → 1 1 R + 90 0 0 90 1 0 C1 -S1

1 → 2 2 P 0 d2 + cl2 0 0 1 0 1 0

22 2

1 0 0 0

0 1 0 0

0 0 1

0 0 0 1

Ad cl

FKS is A1*A2:

2 2

1 0 1 0 1 0 0 0

1 0 1 0 0 1 0 0

0 1 0 0 0 0 1

0 0 0 1 0 0 0 1

S C

C S

d cl

2 2

2 2

1 0 1 1 ( )

1 0 1 1 ( )

0 1 0 0

0 0 0 1

S C C d cl

C S S d cl

Forming The IKS:

2 2

2 2

1 0 1 1 ( )

1 0 1 1 ( )

0 1 0 0

0 0 0 1

S C C d cl

C S S d cl

0 0 0 1

x x x x

y y y y

z z z z

n s a d

n s a d

n s a d

Forming The IKS:• Examining the two sides of this FKS equation

– n, s, a and d are given in an inverse sense – since we desire to put the manipulator on a particular target

– But we will build the general solution only• Term (1, 4) & (2,4) both side allow us to find an equation for :

• Select (1,4): C1*(d2+cl2) = dx

• Select (2,4): S1*(d2+cl2) = dy

• Form a ratio of Term2,4 by Term1,4 to build an equation for Tan() and then :

• S1/C1 = dy/ dx

• Tan = dy/dx

• = Atan2(dx, dy)

Forming The IKS:

• After is found, back substitute and solve for d2:

• Choose term(1,4): C1*(d2+cl2) = dx

• Isolating d2: d2 = [dx/C1] - cl2

Alternative Method – doing a pure inverse approach

• Form [A1]-1 then pre-multiply both side by this ‘inverse’

• Leads to: A2 = A1-1*T0

ngiven

2 2

1 0 0 0 1 1 0 0

0 1 0 0 0 0 1 0

0 0 1 1 1 0 0

0 0 0 1 0 0 0 1 0 0 0 1

x x x x

y y y y

z z z z

S C n s a d

n s a d

d cl C S n s a d

After Simplifying the RHS:

1 1 1 1 1 1 1 1

1 1 1 1 1 1 1 1

0 0 0 1

x y x y x y x y

z z z z

x y x y x y x y

S n C n S s C s S a C a S d C d

n s a d

C n S n C s S s C a S a C d S d

2 2

1 0 0 0

0 1 0 0

0 0 1

0 0 0 1

d cl

Solving:

• Selecting and Equating terms (1,4)• 0 = -S1*dx + C1*dy

• Solving: S1*dx = C1*dy

• Tan() = (S1/C1) = (dy/dx)

• = Atan2(dx, dy)

• Selecting and Equating terms (3,4) -- after back substituting solution – and note, after the above step, is known as an angle

• d2 + cl2 = C1*dx + S1*dy

• d2 = C1*dx + S1*dy - cl2

d2 = Cos[Atan2(dx, dy)]*dx + Sin[Atan2(dx, dy)]*dy –cl2

Performing IKS For Industrial Robots:

• First lets consider the previously defined Spherical Wrist simplification– All Wrist joint Z’s intersect at a point– The n Frame is offset from this intersection as a

distance dn along the a vector of the desired solution (3rd column of desired orientation sub-matrix)

– This is as expected by the DH Algorithm

Performing IKS

• We can now separate the effects of the ARM joints – Joints 1 to 3 in a full function manipulator (without

redundant joints)– They function to position the spherical wrist at a target

POSITION related to the desired target POSE

• Arm Joints are separated from the WRIST Joints– Joints 4 to 6 in a full functioning spherical wrist– Wrist Joints function as a primary tool to ORIENT the end

frame as required by the desired target POSE

Performing IKS: Focus on Positioning

• We will define a point (the WRIST CENTER) as:– Pc = [Px, Py, Pz]

– Here we define Pc = dtarget - dn*a• Px = dtarget,x - dn*ax

• Py = dtarget,y - dn*ay

• Pz = dtarget,z - dn*az

Focusing on the ARM Manipulators in terms of Pc:

• Prismatic:• q1 = Pz (its along Z0!) – cl1

• q2 = Px or Py - cl2

• q3 = Py or Px - cl3

• Cylindrical:• 1 = Atan2(Px, Py)

• d2 = Pz – cl2

• d3 = Px*C1 – cl3 or +(Px2 + Py

2).5 – cl3

Focusing on the ARM Manipulators in terms of Pc:

• Spherical:• 1 = Atan2(Px, Py)• 2 = Atan2( (Px

2 + Py2).5 , Pz)

• D3 = (Px2 + Py

2 + Pz2).5 – cl3

Focusing on the ARM Manipulators in terms of Pc:

• Articulating:• 1 = Atan2(Px, Py)• 3 = Atan2(D, (1 – D2).5)

– Where D =

• 2 = - – is: Atan2((Px2 + Py2).5, Pz)

– is:

2 2 2 2 22 3

2 32x y zP P P l l

l l

2 2 2 2 2 22 3 2 3

tan 2(cos ,sin )

tan 2 , 2 1x y z

A

A P P P l l l l D

One Further Complication:

• This is called the d2 offset problem

• A d2 offset is a problem that states that the n frame has a non-zero offset along the Y0 axis as observed with all joints at home, in the solution of the 0Tn

• This leads to two solutions for 1 the So-Called Shoulder Left and Shoulder Right solutions

Defining the d2 Offset issue

X0, X1

Y0, Z1

Z0 d2

The ARMThe Wrist

Ypc

Xpc

Zpc

Here: ‘The ARM’ might be a prismatic joint as in the Stanford Arm or it might be l2 & l3 links in an Articulating Arm and rotates out of plane

A d2 offset means that there are two places where 1 can be placed to touch a given point (and note, when 1 is at Home, the wrist center is not on the X0 axis!)

Lets look at this Device “From the Top”

Pc'(Px, Py)

X0

Y0

Z1

Z1'

X1

d2

d2

a2'

a3'

R'

X1'

L2’

L3’

11

X1

Solving For 1

• We will have a Choice (of two) poses for :

1 1 1

1

.52 2 22 2

1

1 tan2( , )

tan 2 ,

pc pc

pc pc

A X Y

A X Y d d

2 1 1

.52 2 22 2

1 180

180 tan2 ,

tan 2 ,

pc pc

pc pc

A X Y d d

A X Y

In this so-called “Hard Arm”

• We have two 1’s

• These lead to two 2’s (Spherical)

• Or to four 2’s and 3’s in the Articulating Arm • Shoulder Right Elbow Up & Down• Shoulder Left Elbow Up & Down

The Orientation Model

• Evolves from:

• Separates Arm Joint and Wrist Joint Contribution to Target (given) orientation

3 60 3 givenR R R

Focusing on Orientation Issues

• Lets begin by considering Euler Angles (they are a model that is almost identical to a full functioning Spherical Wrist):

• Form Product:– Rz1*Ry2*Rz3

– This becomes R36

1

2

3

cos sin 0

sin cos 0

0 0 1

cos 0 sin

0 1 0

sin 0 cos

cos sin 0

sin cos 0

0 0 1

z

y

z

R

R

R

Euler Wrist Simplified:C C C S S C C S S C C S

S C C C S S C S C C S S

S C S S C

And this matrix is equal to a U matrix prepared by multiplying the inverse of the ARM joint orientation matrices inverse and the Desired (given) target orientation

130

x x x

y y y

z z z given

n s a

R n s a

n s a

NOTE: R0

3 is Manipulator dependent!

Simplifying the RHS: (our so-called U Matrix)

30

130

11 12 13

21 22 23

31 32 33

11 21 31

12 22 32

13 23 33

(this is a transpose!)

R R R

R R R R

R R R

R R R

R R R R

R R R

Continuing:

11 12 13

21 22 23

31 32 33

11 21 31 11 21 31 11 21 31

12 22 32 12 22 32 12 22 32

13 23 33 13 23 33 13 23 33

x y z x y z x y z

x y z x y z x y z

x y z x y z x y z

U U U

U U U

U U U

n R n R n R s R s R s R a R a R a R

n R n R n R s R s R s R a R a R a R

n R n R n R s R s R s R a R a R a R

Finally:C C C S S C C S S C C S

S C C C S S C S C C S S

S C S S C

11 12 13

21 22 23

31 32 33

U U U

U U U

U U U

Solving for Individual Orientation Angles (1st ):

• Selecting (3,3)→ C = U33

• With C we “know” S = (1-C2).5

• Hence: = Atan2(U33, (1-U332).5

• NOTE: 2 solution for !

Re-examining the Matrices:

• To solve for : Select terms: (1,3) & (2,3) CS = U13

SS = U23

Dividing the 2nd by the 1st: S /C = U23/U13

so Tan() = U23/U13

leading to: = Atan2(U13, U23)

Continuing our Solution:

• To solve for : Select terms: (3,1) & (3,2) -SC = U31

SS = U32

so Tan() = U32/-U31 (note the ‘minus’ sign track the appropriate term!)

And Thus = Atan2(-U31, U32)

Summarizing:

• = Atan2(U33, (1-U332).5

• = Atan2(U13, U23)

• = Atan2(-U31, U32)

Let’s examine a Spherical Wrist:

Z3

X3

Y3

Z4

X4

Y4

Z5

X5

Y5

Z6

X6

Y6

IKSing the Spherical WristFrames Link Var d l

3 → 4 4 R 4 0 0 -90

4 → 5 5 R 5 0 0 +90

5 → 6 6 R 6 d6 0 0

4

4 0 4 5 0 5 6 6 0

4 0 4 ; 5 5 0 5 ; 6 6 6 0

0 1 0 0 1 0 0 0 1

C S C S C S

R S C R S C R S C

Writing The Solution:

11 12 13

21 22 23

31 32 33

4 0 4 5 0 5 6 6 0

4 0 4 5 0 5 6 6 0

0 1 0 0 1 0 0 0 1

C S C S C S

S C S C S C

U U U

U U U

U U U

Lets See (By Pure Inverse) Technique:

11 12 13

21 22 23

31 32 33

5 0 5 6 6 0

5 0 5 6 6 0

0 1 0 0 0 1

4 4 0

0 0 1

4 4 0

C S C S

S C S C

C S U U U

U U U

S C U U U

Simplifying

11 21 12 22 13 23

31 32 33

21 11 22 12 23 13

5 6 5 6 5

5 6 5 6 5

6 6 0

4 4 4 4 4 4

4 4 4 4 4 4

C C C S S

S C S S C

S C

C U S U C U S U C U S U

U U U

C U S U C U S U C U S U

Solving:

• After Examination here let’s select (3,3) both sides:

• 0 = C4U23 – S4U13

• S4U13 = C4U23

• Tan(4) = S4/C4 = U23/U13

• 4 = Atan2(U13, U23)

• With the given and back-substituted values (from the arm joints) we have a value for 4

the RHS is completely known

Solving for 5 & 6

• For 5: Select (1,3) & (2,3) terms• S5 = C4U13 + S4U23

• C5 = U33

• Tan(5) = S5/C5 = (C4U13 + S4U23)/U33

• 5 = Atan2(U33, C4U13 + S4U23)

• For 6: Select (3,1) & (3, 2)• S6 = C4U21 – S4U11

• C6 = C4U22 – S4U12

• Tan(6) = S6/C6 = ([C4U21 – S4U11],[C4U22 – S4U12])

• 6 = Atan2 ([C4U21 – S4U11], [C4U22 – S4U12])

Summarizing:

• 4 = Atan2(U13, U23)

• 5 = Atan2(U33, C4U13 + S4U23)

• 6 = Atan2 ([C4U21 – S4U11], [C4U22 – S4U12])

Lets Try One:

• Cylindrical Robot w/ Spherical Wrist• Given a Target matrix (it’s an IKS after all!)• The d3 “constant” is 400mm; the d6 offset (call

it the ‘Hand Span’) is 150 mm.• 1 = Atan2((dx – ax*150),(dy-ay*150))

• d2 = (dz – az*150)

• d3 = ((dx – ax*150)2,(dy-ay*150)2).5 - 400

The Frame Skeleton:

X

ZF0

F1 Z

X

X

F2 Z

Z

X

F6

F2.5

Z

X

F4

XF3

Z

F5

Z

X

X

Z

Note “Dummy” Frame to account for Orientation problem with Spherical Wrist

Solving for U:

1 1 0 0 0 1 0 0 1 0 1 0

1 1 0 1 0 0 1 0 0 1 0 0

0 0 1 0 1 0 0 1 0 0 0 1

x x x

y y y

z z z

C S n o a

U S C n o a

n o a

NOTE: We needed a “Dummy Frame” to account for the Orientation issue at the end of the Arm

Simplifying:

11 12 13

21 22 23

31 32 33

1 1 1 1 1 1

1 1 1 1 1 1x z x z x z

x z x z x z

y y y

U U U C n S n C s S s C a S a

U U U S n C n S s C s S a C a

U U U n o a

Subbing Uij’s Into Spherical Wrist Joint Models:

• 4 = Atan2(U13, U23)= Atan2((C1ax + S1az), (S1ax-C1az))

• 5 = Atan2(U33, C4U13 + S4U23)= Atan2{ay, [C4(C1ax+S1az) + S4 (S1ax-C1az)]}

• 6 = Atan2 ([C4U21 - S4U11], [C4U22 - S4U12]) = Atan2{[C4(S1nx-C1nz) - S4(C1nx+S1nz)],

[C4(S1sx-C1sz) - S4(C1sx+S1sz)]}