Lec04 Forward InverseKinematicsII

7/31/2019 Lec04 Forward InverseKinematicsII

1/33

ES159/259

Ch. 3: Forward and Inverse Kinematics


2/33

ES159/259

Recap: The Denavit-Hartenberg (DH) Convention

Representing each individual homogeneous transformation as theproduct of four basic transformations:

1000

0

1000

00

00

0001

1000

0100

0010

001

1000

100

0010

0001

1000

0100

00

00

,,,,

i

i

i

i

i

xaxdzzi

dcs

sascccs

casscsc

cs

sc

a

d

cs

sc

A

ii

iiiiii

iiiiii

ii

iiii

ii

iiii

RotTransTransRot


3/33

ES159/259

Recap: the physical basis for DH parameters

ai: link length, distance between the o0and o1 (projected along x1)

i: link twist, angle between z0and z1 (measured around x1)

di: link offset, distance between o0and o1 (projected along z0)

i: joint angle, angle between x0and x1 (measured around z0)


4/33

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 o0on z0and choose x0and y0using right-handed convention

3. For i=1:n-1,

i. Place oiwhere the normal to zi and zi-1 intersects zi. If zi intersects zi-1, putoiat intersection. If zi and zi-1 are parallel, place oialong zi such that di=0

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

iii. Determine yiusing right-handed convention

4. Place the tool frame: set znparallel to zn-1

5. For i=1:n, fill in the table of DH parameters6. Form homogeneous transformation matrices, Ai7. Create Tn

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


5/33

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 z3as parallel to z2


6/33

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

1000

100

0010

0001

1000

010

0100

0001

1000

100

00

00

3

3

2

2

1

11

11

1d

Ad

Ad

cs

sc

A ,,

1000

010

0

0

21

3111

3111

321

0

3dd

dccs

dssc

AAAT


7/33

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


8/33

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

1000

100

00

00

1000

0010

00

00

1000

0010

00

00

6

66

66

6

55

55

5

44

44

4d

cs

sc

Acs

sc

Acs

sc

A ,,

1000

6556565

654546465464654

654546465464654

65436

dcccscsdssssccscsscccs

dscsccssccssccc

AAAT


9/33

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 thetable 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, o5are all atthe same point oc


10/33

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

1000

333231

232221

131211

3

6

0

3

0

6

z

y

x

drrr

drrr

drrr

TTT

21654

316516541

316516541

5433

5154123

5154113

6465432

651641654122

651641654112

6465431

651641654121

651641654111

dddssd

dcdccdscsd

dsdcsdsccd

ssr

ccscsr

cssccr

ccccsr

cscssssccsr

csscscscccrscccsr

cscssscccsr

csssscccccr

z

y

x


11/33

ES159/259

Example 5: the Stanford manipulator

6DOF: need to assign seven coordinate frames:


2. Choose z1-z5 axes (axes of rotation/translation for joints 2-6)

3. Choose xiaxes

4. Choose tool frame5. 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

Suggested insertion:photo of the Stanfordmanipulator


12/33

ES159/259


Now determine the individual homogeneous transformations:

1000

100

00

00

1000

0010

00

00

1000

0010

00

00

1000

100

0010

0001

1000

010

00

00

1000

0010

00

00

6

66

66

6

55

55

5

44

44

4

33

2

22

22

2

11

11

1

d

cs

sc

Acs

sc

Acs

sc

A

dAd

cs

sc

A

cs

sc

A

,,

,,


13/33

ES159/259


Finally, combine to give the complete description of the forwardkinematics:

1000

333231

232221

131211

61

0

6

z

y

x

drrr

drrr

drrr

AAT

52452632

2155142541621321

5412515421621321

5254233

54152542123

54152542113

65264654232

646541652646542122

646541652646542112

65264654231

646541652646542121

646542652646542111

sscccddcd

sscssccsscddcdssd

ssssccscccddsdscd

ccscsr

ssccssccsr

ssscsscccr

ssccssccsr

scscscssscsscccsr

ccscssssscssccccrcscsscccsr

scccsccssssccccsr

scccsdcsssscccccr

z

y

x


14/33

ES159/259

Example 6: the SCARA manipulator

4DOF: need to assign five coordinate frames:


2. Choose z1-z3 axes (axes of rotation/translation for joints 2-4)

3. Choose xiaxes

4. Choose tool frame5. 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

Suggestedinsertion: photoof SCARAmanipulator


15/33

ES159/259

Example 6: the SCARA manipulator

Now determine the individual homogeneous transformations:

1000

100

00

00

1000

100

0010

0001

1000

0100

0

0

1000

0100

0

0

4

44

44

4

3

3

2222

2222

2

1111

1111

1

d

cs

sc

A

d

Asacs

casc

Asacs

casc

A ,,,

1000100

0

0

43

12211412412412412

12211412412412412

41

0

4

dd

sasaccsssccs

cacacsscsscc

AAT


16/33

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 DOFsfor all links and subtracting the number of constraints imposed by the

closed-chain configuration

Grueblers formula (3D):

jn

iijL fnn

1

6DOF#

number of links*

*excluding ground

number of joints

#DOF for joint i


17/33

ES159/259

Forward kinematics of parallel manipulators

Grueblers formula (2D):

Example (2D):

Planar four-bar, nL = 3, nj= 4, fi= 1(for all joints)

3(3-4)+4 = 1DOF

Forward kinematics:

jn

i

ijL fnn1

3DOF#

2tan

2

22cos

1

21

2

2

2

1

2

2

21

L

L

LL

L


18/33

ES159/259

Inverse Kinematics

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

Given H:

Find allsolutions to:

Noting that:

This gives 12 (nontrivial) equations with nunknowns

310

SEoR

H

HqqT nn ,...,10

nnnn qAqAqqT 1110

,...,


19/33

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

1000

0001

763.0100

154.0010

H

Example: the Stanford manipulator

0

763.0

154.0

0

1

0

0

0

1

1

0

0

52452632

2155142541621321

5412515421621321

52542

541525421

541525421

652646542

6465416526465421

6465416526465421

652646542

6465416526465421

6465426526465421

sscccddcsscssccsscddcdss

ssssccscccddsdsc

ccscs

ssccssccs

ssscssccc

ssccssccs

scscscssscsscccs

ccscssssscsscccc

cscsscccs

scccsccssssccccs

scccsdcssssccccc


20/33

ES159/259

Inverse Kinematics

The previous example shows how difficult it would be to obtain aclosed-form solution to the 12 equations

Instead, we develop systematic methods based upon the manipulatorconfiguration

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!


21/33

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

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 jointangles

oqqo

RqqR

61

0

6

61

0

6

,...,

,...,


22/33

ES159/259


Now, origin of tool frame, o6, is a distance d6 translated along z5 (sincez5and z6are collinear)

Thus, the third column of Ris the direction of z6 (w/ respect to the baseframe) and we can write:

Rearranging:

Calling o= [oxoyoz]

T

, oc0

= [xcyczc]

T

1

0

0

606 Rdooo

oc

1

0

0

6Rdoo

o

c

336

236

136

rdo

rdo

rdo

z

y

x

z

y

x

c

c

c


23/33

ES159/259


Since [xcyczc]Tare determined from the first three joint angles, our

forward kinematics expression now allows us to solve for the first threejoint 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 aspherical wrist, we can use a set of

Euler angles to solve for them

3

6

0

3RRR

RRRRR T0310

3

3

6


24/33

ES159/259

Inverse position

Now that we have [xcyczc]T we need to find q1, q2, q3

Solve for qiby 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, y0plane


25/33

ES159/259

Background: two argument atan

We use atan2() instead of atan() to account for the full range ofangular solutions

Called four-quadrant arctan

0,0

0,02

0,0

0,0

0,2

,2

xy

xy

xyx

y

xyx

y

yxy

xy

undefined

atan

atan

atan

atan


26/33

ES159/259

Example: RRR manipulator

1. To solve for 1, project the arm onto the x0, y0plane

Can also have:

This will of course change the solutions for 2and 3

cc yx ,21 atan

cc yx ,21 atan


27/33

ES159/259

If there is an offset, then we willhave two solutions for 1: left armand right arm

However, wrist centers cannotintersect z0

If xc=yc=0, 1 is undefined

i.e. any value of 1 will work

Caveats: singular configurations, offsets


28/33

ES159/259

Left arm: Right arm:

Left arm and right arm solutions

ddyx

yx

cc

cc

,2

,2

222

1

atan

atan

-

ddyx

ddyx

yx

cc

cc

cc

,2

,2

,2

222

222

1

atan

atan

atan


29/33

ES159/259

Therefore there are in general two solutions for 1 Finding 2and 3 is identical to the planar two-link manipulator we have

seen previously:

Therefore we can find two solutions for 3:


D

aa

aadzdyx

dzs

dyxr

aa

aasr

ccc

c

cc

32

2

3

2

2

2

1

222

3

1

2222

32

2

3

2

2

22

3

2cos

2cos

2

3 1,2 DD atan


30/33

ES159/259

The two solutions for 3correspond to the elbow-down and elbow-uppositions respectively

Now solve for 2:

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


333321222

333322

,2,2

,2,2

sacaadzdyx

sacaasr

ccc

atanatan

atanatan


31/33

ES159/259

In general, there will be a maximum of four solutions to the inversepositionkinematics of an elbow manipulator

Ex: PUMA

RRR: Four total solutions


32/33

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

dzs

yxr

rs

c

cc

atan

21

22

22

3

dzyx

srd

ccc


33/33

ES159/259

Next class

Complete the discussion of inverse kinematics

Inverse orientation

Introduction to other methods

Introduction to velocity kinematics and the Jacobian

Lec04 Forward InverseKinematicsII

Documents

Transcript of Lec04 Forward InverseKinematicsII