Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for...

55
Forward Kinematics 1

Transcript of Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for...

Page 1: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Forward Kinematics

1

Page 2: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Links and Joints

2

n joints, n + 1 links link 0 is fixed (the base) joint i connects link i – 1 to link i link i moves when joint i is actuated

joint 1

joint 2

joint 3 joint 4 joint n-1joint n

link 0

link 1

link

2

link 3

link n-1

link n

.................

prismaticrevolute

i

ii d

q

Page 3: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Forward Kinematics

3

given the joint variables and dimensions of the links what is the position and orientation of the end effector?

2

1

a1

a2

x0

y0

p0 ?

Page 4: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Forward Kinematics

4

because the base frame and frame 1 have the same orientation, we can sum the coordinates to find the position of the end effector in the base frame

2

1

a1

a2

x0

y0

( a1 cos 1 , a1 sin 1 )

1

x1

y1

( a2 cos (1 + 2),a2 sin (1 + 2) )

(a1 cos 1 + a2 cos (1 + 2),a1 sin 1 + a2 sin (1 + 2) )

Page 5: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Forward Kinematics

5

from earlier in the course

2

1

a1

a2

x0

y01

x2 = (cos (1 + 2),sin (1 + 2) )

y2 = (-sin (1 + 2),cos (1 + 2) )

x2y2

p0 = (a1 cos 1 + a2 cos (1 + 2),a1 sin 1 + a2 sin (1 + 2) )

Page 6: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Frames

6

2

1

a1

a2

x0

y0 x1

y1

x2y2

Page 7: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Forward Kinematics

7

using transformation matrices

11 ,,01 axz DRT

22 ,,12 axz DRT

12

01

02 TTT

Page 8: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Links and Joints

8

n joints, n + 1 links link 0 is fixed (the base) joint i connects link i – 1 to link i link i moves when joint i is actuated

joint 1

joint 2

joint 3 joint 4 joint n-1joint n

link 0

link 1

link

2

link 3

link n-1

link n

.................

prismaticrevolute

i

ii d

q

Page 9: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Forward Kinematics

9

attach a frame {i} to link i all points on link i are constant when expressed in {i} if joint i is actuated then frame {i} moves relative to frame {i - 1}

motion is described by the rigid transformation

the state of joint i is a function of its joint variable qi (i.e., is a function of qi)

this makes it easy to find the last frame with respect to the base frame

1iiT

)(11i

ii

ii qTT

123

12

01

0 nnn TTTTT

Page 10: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Forward Kinematics

10

more generally

the forward kinematics problem has been reduced to matrix multiplication

jijiji

TI

TTTT

ij

jj

ij

ii

ij

ififif

1

1121

Page 11: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Forward Kinematics

11

Denavit J and Hartenberg RS, “A kinematic notation for lower-pair mechanisms based on matrices.” Trans ASME J. Appl. Mech,23:215–221, 1955 described a convention for standardizing the attachment of frames

on links of a serial linkage

common convention for attaching reference frames on links of a serial manipulator and computing the transformations between frames

Page 12: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Denavit-Hartenberg

12

iiii xaxdzzii RTTRT ,,,,

1

10000 i

i

i

dcssascccscasscsc

ii

iiiiii

iiiiii

anglejoint offsetlink

link twistlengthlink

i

i

i

i

d

a

Page 13: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Denavit-Hartenberg

13

Page 14: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Denavit-Hartenberg

14

notice the form of the rotation component

this does not look like it can represent arbitrary rotations

can the DH convention actually describe every physically possible link configuration?

ii

iiiii

iiiii

csscccs

sscsc

0

Page 15: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Denavit-Hartenberg

15

yes, but we must choose the orientation and position of the frames in a certain way

(DH1)

(DH2)

claim: if DH1 and DH2 are true then there exists unique numbers

1ˆˆ ii zx

1ˆ intersects ˆ ii zx

,,,,0

1 such that ,,, xaxdzz RDDRTda

Page 16: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Denavit-Hartenberg

16

proof: on blackboard in class

Page 17: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Denavit-Hartenberg

17

Page 18: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

DH Parameters

18

ai : link length distance between zi-1 and zi measured along xi

i : link twist angle between zi-1 and zi measured about xi

di : link offset distance between oi-1 to the intersection of xi and zi-1 measured

along zi-1

i : joint angle angle between xi-1 and xi measured about zi-1

Page 19: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Example with Frames Already Placed

19

Page 20: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Step 5: Find the DH parameters

20

Link ai i di i

1 0 0 d1 1*2 0 -90 d2* 03 0 0 d3* 0

* joint variable

Page 21: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Denavit-Hartenberg Forward Kinematics

21

RPP cylindrical manipulator http://strobotics.com/cylindrical-format-robot.htm

Page 22: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Denavit-Hartenberg Forward Kinematics

22

How do we place the frames?

Page 23: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Step 1: Choose the z-axis for each frame

23

recall the DH transformation matrix

iiii xaxdzzii RTTRT ,,,,

1

10000 i

i

i

dcssascccscasscsc

ii

iiiiii

iiiiii

1ˆ iix 1ˆ i

iy 1ˆ iiz

Page 24: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Step 1: Choose the z-axis for each frame

24

axis of actuation for joint i+1

iz

link i link ilink i+1 link i+1

joint i+1 joint i+1

iz

iz

Page 25: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Step 1: Choose the z-axis for each frame

25

Warning: the picture is deceiving. We do not yet know the origin of the frames; all we know at this point is that each zi points along a joint axis

Page 26: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Step 2: Establish frame {0}

26

place the origin o0 anywhere on z0 often the choice of location is obvious

choose x0 and y0 so that {0} is right-handed often the choice of directions is obvious

Page 27: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Step 2: Establish frame {0}

27

Page 28: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Step 3: Iteratively construct {1}, {2}, ... {n-1}

28

using frame {i-1} construct frame {i} DH1: xi is perpendicular to zi-1

DH2: xi intersects zi-1

3 cases to consider depending on the relationship between zi-1and zi

Page 29: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Step 3: Iteratively construct {1}, {2}, ... {n-1}

29

Case 1 zi-1 and zi are not coplanar (skew)

i angle from zi-1 to zi measured about xi

iz1ˆ iz

(out of page)shortest line betweenand1ˆ iz iz

ix

io point of intersection

ia

Page 30: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Step 3: Iteratively construct {1}, {2}, ... {n-1}

30

Case 2 zi-1 and zi are parallel ( i = 0 )

notice that this choice results in di = 0

iz1ˆ iz

ix

io point of intersection

1ioia

Page 31: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Step 3: Iteratively construct {1}, {2}, ... {n-1}

31

Case 3 zi-1 and zi intersect ( ai = 0 )

iz

1ˆ iz

ixio point of intersection

(out of page)

Page 32: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Step 3: Iteratively construct {1}, {2}, ... {n-1}

32

Page 33: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Step 3: Iteratively construct {1}, {2}, ... {n-1}

33

Page 34: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Step 4: Place the end effector frame

34

“approach”

“sliding”

“normal”

Page 35: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Step 4: Place the end effector frame

35

Page 36: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Step 5: Find the DH parameters

36

ai : distance between zi-1 and zi measured along xi

i : angle between zi-1 and zi measured about xi

di : distance between oi-1 to the intersection of xi and zi-1measured along zi-1

i : angle between xi-1 and xi measured about zi-1

Page 37: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Step 5: Find the DH parameters

37

Link ai i di i

1 0 0 d1 1*2 0 -90 d2* 03 0 0 d3* 0

* joint variable

Page 38: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

More Denavit-Hartenberg Examples

38

Page 39: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Step 5: Find the DH parameters

39

Link ai i di i

1 0 0 d1 1*2 0 -90 d2* 03 0 0 d3* 0

* joint variable

Page 40: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Step 6: Compute the transformation

40

once the DH parameters are known, it is easy to construct the overall transformation

Link ai i di i

1 0 0 d1 1*2 0 -90 d2* 03 0 0 d3* 0

* joint variable

1000100

0000

1

11

11

,,,,01 1111 d

cssc

RTTRT xaxdzz

Page 41: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Step 6: Compute the transformation

41

Link ai i di i

1 0 0 d1 1*2 0 -90 d2* 03 0 0 d3* 0

* joint variable

1000010

01000001

2,,,,

12 2222 d

RTTRT xaxdzz

Page 42: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Step 6: Compute the transformation

42

Link ai i di i

1 0 0 d1 1*2 0 -90 d2* 03 0 0 d3* 0

* joint variable

1000100

00100001

3,,,,

23 3333 d

RTTRT xaxdzz

Page 43: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Step 6: Compute the transformation

43

1000010

00

21

3111

3111

23

12

01

03 dd

dccsdssc

TTTT

Page 44: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Spherical Wrist

44

Page 45: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Spherical Wrist

45

Page 46: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Spherical Wrist: Step 1

46

Page 47: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Spherical Wrist: Step 2

47

Page 48: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Spherical Wrist: Step 2

48

Page 49: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Spherical Wrist: Step 4

49

Page 50: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Step 5: DH Parameters

50

Link ai i di i

4 0 -90 0 4*5 0 90 0 5*6 0 0 d6 6*

* joint variable

Page 51: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Step 6: Compute the transformation

51

10006556565

654546465464654

654546465464654

56

45

34

36 dccsscs

dssssccscsscccsdscsccssccssccc

TTTT

Page 52: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

RPP + Spherical Wrist

52

Page 53: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

RPP + Spherical Wrist

53

1000333231

232221

131211

36

03

06

z

y

x

drrrdrrrdrrr

TTT

21654

651641654111

dddssd

csssscccccr

z

Page 54: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

Stanford Manipulator + Spherical Wrist

54

Link ai i di i

1 0 -90 0 1*2 0 90 d2 2*3 0 0 d3* 04 0 -90 0 4*5 0 90 0 5*6 0 0 d6 6*

* joint variable

Page 55: Forward kinematics...Forward Kinematics 11 Denavit J and Hartenberg RS, “A kinematic notation for lower- pair mechanisms based on matrices.” Trans ASME J. Appl. Mech, 23:215–221,

SCARA + 1DOF Wrist

55

Link ai i di i

1 a1 0 d1 1*2 a2 180 0 2*3 0 0 d3* 04 0 0 d4 4* * joint variable