Velocity Analysis Jacobian University of Bridgeport 1 Introduction to ROBOTICS.

32
Velocity Analysis Jacobian University of Bridgeport 1 Introduction to ROBOTICS

Transcript of Velocity Analysis Jacobian University of Bridgeport 1 Introduction to ROBOTICS.

Velocity AnalysisJacobian

University of Bridgeport

1

Introduction to ROBOTICS

Kinematic relations

6

5

4

3

2

1

z

y

x

X

Joint SpaceTask Space

θ =IK(X)

Location of the tool can be specified using a joint space or a cartesian space description

X=FK(θ)

Velocity relations

Joint Space Task Space

• Relation between joint velocity and cartesian velocity.• JACOBIAN matrix J(θ)

6

5

4

3

2

1

z

y

x

z

y

x

)(JX

XJ )(1

Jacobian• Suppose a position and orientation vector of a

manipulator is a function of 6 joint variables: (from forward kinematics)

X = h(q)

z

y

x

X 16

6

5

4

3

2

1

)(

q

q

q

q

q

q

h

166216

6215

6214

6213

6212

6211

),,,(

),,,(

),,,(

),,,(

),,,(

),,,(

qqqh

qqqh

qqqh

qqqh

qqqh

qqqh

Jacobian MatrixForward kinematics

)( 116 nqhX

qdq

qdh

dt

dq

dq

qdhqh

dt

dX n )()(

)( 116

z

y

x

z

y

x

1

2

1

6

)(

nn

n

q

q

q

dq

qdh

1616 nnqJX

dq

qdhJ

)(

Jacobian Matrix

z

y

x

z

y

x

1

2

1

6

)(

nn

n

q

q

q

dq

qdh

nn

n

n

n

q

h

q

h

q

h

q

h

q

h

q

hq

h

q

h

q

h

dq

qdhJ

6

6

2

6

1

6

2

2

2

1

2

1

2

1

1

1

6

)(

Jacobian is a function of q, it is not a constant!

Jacobian Matrix

Vz

y

x

X

z

y

x

16 nnqJX

z

y

x

V

Linear velocity

z

y

x

Angular velocity

The Jacbian Equation

1

2

1

1

nn

n

q

q

q

q

Example• 2-DOF planar robot arm

– Given l1, l2 , Find: Jacobian2

1

(x , y)

l2

l1

),(

),(

)sin(sin

)cos(cos

212

211

21211

21211

h

h

ll

ll

y

x

)cos()cos(cos

)sin()sin(sin

21221211

21221211

2

2

1

2

2

1

1

1

lll

lllhh

hh

J

2

1

Jy

xY

Singularities• The inverse of the jacobian matrix cannot be

calculated when

det [J(θ)] = 0

• Singular points are such values of θ that cause the determinant of the Jacobian to be zero

• Find the singularity configuration of the 2-DOF planar robot arm

)cos()cos(cos

)sin()sin(sin

21221211

21221211

lll

lllJ

2

1

Jy

xX

2

1

(x , y)

l2

l1

x

Y

=0

V

determinant(J)=0 Not full rank

02 Det(J)=0

Jacobian Matrix• Pseudoinverse

– Let A be an mxn matrix, and let be the pseudoinverse of A. If A is of full rank, then can be computed as:

AA

nmAAA

nmA

nmAAA

ATT

TT

1

1

1

][

][

Jacobian Matrix– Example: Find X s.t.

2

3

011

201x

24

51

41

9

1

21

15

02

10

11

][1

1TT AAAA

16

13

5

9

1bAx

Matlab Command: pinv(A) to calculate A+

Jacobian Matrix• Inverse Jacobian

• Singularity– rank(J)<n : Jacobian Matrix is less than full rank– Jacobian is non-invertable– Boundary Singularities: occur when the tool tip is on the surface

of the work envelop.– Interior Singularities: occur inside the work envelope when two

or more of the axes of the robot form a straight line, i.e., collinear

666261

262221

161211

JJJ

JJJ

JJJ

qJX

6

5

4

3

2

1

q

q

q

q

q

q

XJq 1

5q

1q

Singularity

• At Singularities:– the manipulator end effector cant move in certain

directions.– Bounded End-Effector velocities may correspond

to unbounded joint velocities.– Bounded joint torques may correspond to

unbounded End-Effector forces and torques.

Jacobian Matrix

• If

• Then the cross product

,x x

y y

z z

a b

A a B b

a b

( )y z z y

x y z x z z x

x y z x y y x

i j k a b a b

A B a a a a b a b

b b b a b a b

Remember DH parmeter

• The transformation matrix T

i i i i i i i

i i i i i i i

i i i

c -c s s s a c

s c c -s c a s

0 s c d

0 0 0 1

iA

ii AAAT .....210

Jacobian Matrix

nJJJJ ....21

Jacobian Matrix2-DOF planar robot arm

Given l1, l2 , Find: Jacobian

• Here, n=2,

2

1

(x , y)

l2

l1

19

Where (θ1 + θ2 ) denoted by θ12 and

i i i i i i i

i i i i i i i

i i i

c -c s s s a c

s c c -s c a s

0 s c d

0 0 0 1

iA

0 1

0

0

1

Z Z

1 1 1 1 2 1 2

0 1 1 1 2 1 1 2 1 2

0 cos cos cos( )

0 , sin , sin sin( )

0 0 0

a a a

O O a O a a

Jacobian Matrix2-DOF planar robot arm

Given l1, l2 , Find: Jacobian

• Here, n=2

0 2 0 1 2 11 2

0 1

( ) ( ),

z o o z o oJ J

z z

2

1

(x , y)

l2

l1

Jacobian Matrix

0 2 01

0

( )z o oJ

z

1 1 2 1 2

0 2 0 1 1 2 1 2

1 1 2 1 2 1 1 2 1 2

1 1 2 1 2

1 1 2 1 2

0 cos cos( )

( ) 0 sin sin( )

1 0

0 0 1

cos cos( ) sin sin( ) 0

sin sin( )

cos cos( )

0

a a

Z o o a a

i j k

a a a a

a a

a a

Jacobian Matrix

1 2 12

1

( )z o oJ

z

2 1 2

1 2 1 2 1 2

2 1 2 2 1 2

2 1 2

2 1 2

0 cos( )

( ) 0 sin( )

1 0

0 0 1

cos( ) sin( ) 0

sin( )

cos( )

0

a

Z o o a

i j k

a a

a

a

Jacobian Matrix

2 1 2

2 1 2

2

sin( )

cos( )

0

0

0

1

a

a

J

1 1 2 1 2

1 1 2 1 2

1

sin sin( )

cos cos( )

0

0

0

1

a a

a a

J

1 2J J J

The required Jacobian matrix J

Stanford Manipulator

i i i i i i i

i i i i i i i

i i i

c -c s s s a c

s c c -s c a s

0 s c d

0 0 0 1

iA

The DH parameters are:

Stanford Manipulator

10 1T A

1 2 1 1 2 2 1

1 2 1 1 2 2 120 1 2

2 20 0

0 0 0 1

c c s c s d s

s c c s s d cT A A

s c

0

0

0

1

z

1

1 1

0

s

z c

1 2

2 1 2

2

c s

z s s

c

1 2 1 1 2 3 1 2 2 1

1 2 1 1 2 3 1 2 2 130 1 2 3

2 2 3 20

0 0 0 1

c c s c s d c s d s

s c c s s d s s d cT A A A

s c d c

1 2

3 1 2

2

c s

z s s

c

3 1 2 2 1

3 3 1 2 2 1

3 2

d c s d s

O d s s d c

d c

0 1

0

0

0

O O

2 1

2 2 1

0

d s

O d c

Stanford Manipulator 4

0 1 2 3 4T A A A A

50 1 2 3 4 5T A A A A A

60 1 2 3 4 5 6T A A A A A A

1 2 4 1 4

4 1 2 4 1 4

2 4

c c s s c

z s c s c c

s s

T4 = [ c1c2c4-s1s4, -c1s2, -c1c2s4-s1*c4, c1s2d3-sin1d2][ s1c2c4+c1s4, -s1s2, -s1c2s4+c1c4, s1s2d3+c1*d2][-s2c4, -c2, s2s4, c2*d3][ 0, 0, 0, 1]

3 1 2 2 1

4 3 1 2 2 1

3 2

d c s d s

O d s s d c

d c

Stanford Manipulator T5 = [ (c1c2c4-s1s4)c5-c1s2s5, c1c2s4+s1c4, (c1c2c4-s1s4)s5+c1s2c5, c1s2d3-s1d2][ (s1c2c4+c1s4)c5-s1s2s5, s1c2s4-c1c4, (s1c2c4+c1s4)s5+s1s2c5, s1s2d3+c1d2][ -s2c4c5-c2s5, -s2s4, -s2c4s5+c2c5, c2d3] [ 0, 0, 0, 1]

1 2 4 5 1 4 5 1 2 5

5 1 2 4 5 1 4 5 1 2 5

2 4 5 2 5

c c c s s s s c s c

z s c c s c s s s s c

s c s c c

1 2 4 5 1 4 5 1 2 5

5 1 2 4 5 1 4 5 1 2 5

2 4 5 2 5

c c c s s s s c s c

z s c c s c s s s s c

s c s c c

Stanford Manipulator T5 = [ (c1c2c4-s1s4)c5-c1s2s5, c1c2s4+s1c4, (c1c2c4-s1s4)s5+c1s2c5, c1s2d3-s1d2][ (s1c2c4+c1s4)c5-s1s2s5, s1c2s4-c1c4, (s1c2c4+c1s4)s5+s1s2c5, s1s2d3+c1d2][ -s2c4c5-c2s5, -s2s4, -s2c4s5+c2c5, c2d3] [ 0, 0, 0, 1]

1 2 4 5 1 4 5 1 2 5

5 1 2 4 5 1 4 5 1 2 5

2 4 5 2 5

c c c s s s s c s c

z s c c s c s s s s c

s c s c c

3 1 2 2 1

5 3 1 2 2 1

3 2

d c s d s

O d s s d c

d c

Stanford Manipulator

6

d6s5c1c2c4 d6s5s1s4 d6c1s2c5 c1s2d3 s1d2

d6s5s1c2c4 d6s5c1s4 d6s1s2c5 s1s2d3 c1d2

d6s2c4s5 d6c2c5 c2d3

O

T6 = [ c6c5c1c2c4-c6c5s1s4-c6c1s2s5+s6c1c2s4+s6s1c4, -c5c1c2c4+s6c5s1s4+s6c1s2s5+c6c1c2s4+c6s1c4, s5c1c2c4-s5s1s4+c1s2c5, d6s5c1c2c4-d6s5s1s4+d6c1s2c5+c1s2d3-s1d2][ c6c5s1c2c4+c6c5c1s4-c6s1s2s5+s6s1c2s4-s6c1c4, -s6c5s1c2c4-s6c5c1s4+s6s1s2s5+c6s1c2s4-c6c1c4, s5s1c2c4+s5c1s4+s1s2c5, d6s5s1c2c4+d6s5c1s4+d6s1s2c5+s1s2d3+c1d2][ -c6s2c4c5-c6c2s5-s2s4s6, s6s2c4c5+s6c2s5-s2s4c6, -s2c4s5+c2c5, -d6s2c4s5+d6c2c5+c2d3][ 0, 0, 0, 1]

Stanford Manipulator

0 6 0 1 6 11 2

0 1

( ) ( ),

z o o z o oJ J

z z

Joints 1,2 are revolute

23 0

zJ

Joint 3 is prismatic

3 6 3 5 6 54 6 44 5 6

3 54

( ) ( )( ), ,

z o o z o oz o oJ J J

z zz

The required Jacobian matrix J

1 2 3 4 5 6J J J J J J J

Inverse Velocity– The relation between the joint and end-effector velocities:

where j (m×n). If J is a square matrix (m=n), the joint velocities:

– If m<n, let pseudoinverse J+ where

( )X J q q

1( )q J q X

1[ ]T TJ J JJ

( )q J q X

Acceleration– The relation between the joint and end-effector velocities:

– Differentiating this equation yields an expression for the acceleration:

– Given of the end-effector acceleration, the joint acceleration

( )X J q q

( ) [ ( )]d

X J q q J q qdt

Xq

( ) [ ( )]d

J q q X J q qdt

1( )[ ( )] ]d

q J q X J q qdt