SLIDE SERIES 5:IKS FOR ROBOTS

ME 4135Robotics & ControlR. Lindeke, Ph. D.

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

Joint Space

Cartesian Space

Joint Space

Cartesian Space

So this IKS is Nasty (as we know!)

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

12 equations in 6 unknowns Space is “Under-Specified”

Planer devices with more joints than 2 The Solution set can contain

Redundancies Multiple solutions

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

Uses of IKS

Builds Workspace

Allows “Off-Line Programming” solutions

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

Aids in Workplace design and operation simulations

Performing IKS For (mostly) Industrial Robots:

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

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

This result is as expected by following 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

… From the WRIST Joints Joints 4 to 6 in a full functioning spherical wrist Wrist Joints function as the 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

R Manipulator – a simple and special case which can be found by doing a ‘Pure IKS’ solution

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 a 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 o a d

n o a d

n o a d

Forming The IKS:

Examining these two equation (n, o, a and d are ‘givens’ in an inverse sense!!!):

Term (1, 4) & (2,4) both side allow us to find an equation for as a positioning joint in the arm:

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

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

Form a ratio to build Tan(): S1/C1 = dy/ dx

Tan = dy/dx

= Atan2(dx, dy)

Forming The IKS:

After is found, back substitute and solve for d2:

(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*T0n

given

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 o a d

n o a d

d cl C S n o a d

Simplify RHS means:

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 o C o S a C a S d C d

n o a d

C n S n C o S o 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 (1,4) 0 = -S1*dx + C1*dy

Solving: S1*dx = C1*dy

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

= Atan2(dx, dy)

Selecting and Equating (3,4) -- after back substituting solution

d2 + cl2 = C1*dx + S1*dy

d2 = C1*dx + S1*dy - cl2

Performing IKS For Industrial Robots:

What we saw with the -R IKS solutions will always work for a robot – with or without a spherical wrist

These methods are often tedious – and require some form of “intelligent application” to find a good IKS solution

Hence that is why we like the spherical wrist simplification (for industrial robots) introduced earlier – separating ARM and Wrist Effects and joints

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 a a

a a

2 2 2 2 2 22 3 2 3

sintan 2( )cos

tan 2 , 2 1x y z

A

A P P P a a a a D

One Further Complication:

This can be considered the d2 offset problem

A d2 offset is a design issue that states that the nth frame has a non-zero offset along the Y0 axis as observed in the solution of the T0

n with all joints at home

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

Right solutions

Defining the d2 Offset issue

Here: ‘The ARM’ might be a revolute/prismatic device as in the Stanford Arm or it might be a2 & a3 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'

aQ11

F1

Solving For 1

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

1 1 1

1

.52 2 22 2

1

1 tan 2( , )

tan 2 ,

pc pc

pc pc

A X Y

A X Y d d

2 1 1

.52 2 22 2

1 180

180 tan 2 ,

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 the desiredTarget (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

130

x x x

y y y

z z z given

n o a

R n o a

n o a

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

NOTE: R03 is

Manipulator Arm 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 o R o R o R a R a R a R

n R n R n R o R o R o R a R a R a R

n R n R n R o R o R o R a R a R a R

Finally:

63

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

R S C C C S S C S C C S S

S C S S C

LHS

130

11 12 13

* 21 22 23

31 32 33

U U U

R R U U U

U U U

givenRHS

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 solutions 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

Tan() = U23/U13

= Atan2(U13, U23)

Continuing our Solution:

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

SS = U32

Tan() = U32/-U31

= Atan2(-U31, U32)

Summarizing:

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

= Atan2(U13, U23)

= Atan2(-U31, U32)

Let do a (true) Spherical Wrist:

Z3

X3

Y3

Z4

X4

Y4

Z5

X5

Y5

Z6

X6

Y6

IKSing the Spherical WristFrames Link Var d a

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:

Examination here lets select (3,3) both sides:

0 = C4U23 – S4U13

S4U13 = C4U23

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

4 = Atan2(U13, U23)

With the given desired orientation and 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 modeled earlier

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 o S o C a S a

U U U S n C n S o C n 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(S1ox-C1oz) - S4(C1ox+S1oz)]}