Lecture 05Lecture 05

Differential Kinematics

Acknowledgement :Acknowledgement :

Prof. Oussama Khatib, Robotics Laboratory, Stanford University, USA

Prof. Harry Asada, AI Laboratory, MIT, USA

1

Guiding Question

• In robotic applications, not only the position and

orientation, but the velocity of the end-effecter is also to

be monitored and controlled.

• How can the velocity of the end-effector be calculated?• How can the velocity of the end-effector be calculated?

• In order to move the end-effecter in a specified direction

with a specified speed, it is necessary to coordinate the

speeds of the individual joints

• Fundamental methods are to be developed for achieving

such coordinated joint motion in multiple-joint robotic

systems.systems.

• We derive the differential relationship between the joint

displacements and the end-effecter location, and then

solve for the individual joint motions.

2

Differential Relationship

3

Generalized Co-ordinate, q

4

Jacobian: Direct Differentiation

• End-effector position x

• End-effector diflection δx

5

Jacobian: Direct Differentiation

• Due to a “small movements” of individual

joints at the current position δq, the

resultant motion of the end-effecter is δx. Jacobian matrix (matrix of partial q

Jx

qJx

=

=

δδ

δδ

Jacobian matrix (matrix of partial

derivatives) relate δq to δx

• Those small movements are divided by δt

to derive the relationship between joint

and Cartesian velocities

qJx

qJ

x

&& =

=

tt δ

δ

δ

δ

),( ),,( 21 θθ &&&&&& == qx ee yx

6

Example

• Forward kinematics of the

planner manipulator.

21211 )cos(cos θθθ ++= llxe

21

21211

21211

)sin(sin

)cos(cos

θθψ

θθθ

θθθ

+=

++=

++=

lly

llx

e

e

• The differential relationship is

δθδθδ∂

+∂

=xx δθ

θθδ 21

∂∂∂

∂

∂

∂

ee

eyy

xx

x

21

2

2

1

1

2

2

1

1

δθδθδψ

δθθ

δθθ

δ

δθθ

δθθ

δ

+=∂

∂+

∂

∂=

∂

∂+

∂

∂=

eee

eee

yyy

xxx

qJx δδ

δθ

δθ

θθδψ

δ

δ

112

1

21

21

=

∂

∂

∂

∂=

ee

e

eyy

y

x

7

Example

+++

+−+−−

=

11

)cos()cos(cos

)sin()sin(sin

),( 21221211

21221211

21 θθθθθ

θθθθθ

θθ lll

lll

J

+

+−−

=

11

)cos(

)sin(

),(

11

212

212

21 θθ

θθ

θθ lx

ly

e

e

J

8

Jacobian• Jacobian provides the relationship

between the joint velocities and the resultant end-effecter velocity

• Jacobian can be resolved as follows

[ ][ ][ ]

211 θθ &&&2

21

JJx

JJJ

+=

=

• In general, each column vector of

the Jacobian represents the end-

effecter velocity and angular nnqJqJp 1&L&& ++= 1

effecter velocity and angular

velocity generated by the individual

joint velocity while all other joints

are immobilizedT

zyxzyx ),,,,,( φφφ &&&&&&9

Assignment 3: Velocity Profile

• For the planner two-link manipulator

shown, determine the end-effector

velocity profile for the following 10s

motionmotion

θθθθ(0s) = (0°,0°)T

θθθθ(5s) = (45°,90°) T

θθθθ(10s) = (90°,0°) T

Assume L1 = 10cm, L2 = 8cm

L1

L2

• Write Malab m-file, to draw end-

effector speed in x and y directions

10

Singular Arm Configurations

• As long as J1 and J2 are not

aligned, velocities of the two joints

211θθ &&&

2JJx +=

aligned, velocities of the two joints

can be set accordingly to make the

end-effector move in any direction.

• Directions of J1 and J2 are

configuration-dependant, and when

they are aligned, end-effector is they are aligned, end-effector is

only movable along that direction.

• Such arm configurations are known

as singular arm configurations11

• Singularities

occur in planner

two-link arm

when θ2 = 0°,or

Singular Arm Configurations

when θ2 = 0°,or

180°

+++

+−+−−=

)cos()cos(cos

)sin()sin(sin),(

21221211

21221211

21 θθθθθ

θθθθθθθ

lll

lllJ

At singularity configurations

−−

+−=

+

−+−=

=

=

12121

12121

180

12121

12121

0

coscos)(

sinsin)(

coscos)(

sinsin)(

2

2

θθ

θθ

θθ

θθ

θ

θ

lll

lll

lll

lll

o

o

J

J

At singularity configurations

Column vectors

line up each other

12

Determinant of J and Singularity

[ ]

21221211

21221211

21)cos()cos(cos

)sin()sin(sin),(

θθθθθ

θθθθθ

θθθθθθθ

lll

lll

+++−=

+++

+−+−−=J

[ ][ ]

[ ]

221

12112121

21211212

21211212

sin

cos)sin(cos)sin(

)cos(cos)sin(

)sin(sin)cos(||

θ

θθθθθθ

θθθθθ

θθθθθ

ll

ll

lll

lll

=

+−+=

++++

+++−=J

• At singular arm configurations θ =0°. or θ =180°• At singular arm configurations θ2=0°. or θ2=180°

0sin|| 221 == θllJ

13

Inverse Kinematics of Differential Motion

• Resolve end-effector velocity into velocities of individual

joints. Whenever Jacobian is not singular, inverse kinematics

can be solved as follows

xJqxJq δδ 11or

−− == &&

• The solution is unique, unlike the inverse kinematics of end-

effector position, where multiple solutions exist.

• This mapping can be used for robot manipulator control as

proposed (Resolved Motion Rate Control. Daniel Whitney

1969]

xJqxJq δδor == &&

14

Motion Near SingularitiesConsider the two-link

planner articulated robot

arm. We want to move

the endpoint at a

constant speed along a constant speed along a

path staring at point

A(2,0), go close to the

origin through B(ε,0)

and C(0,ε), and reach

the final point D(0,2)

Assume each arm link is

of unit length and obtain

the profiles of the

individual joint

velocities.

Comment on joint velocities in B – C

segment of the motion15

Velocity Inverse Kinematics• By inverting velocity kinematics

2

1

θ

θ

y

x

&

&

&

&

=

−1

J

2121121211

212212

2212

1

1

21221211

21221211

2

1

2

)sin(sin)cos(cos

)sin()cos(

sin

1

)cos()cos(cos

)sin()sin(sin

θθθθθθ

θθθθ

θθ

θ

θθθθθ

θθθθθ

θ

θ

θ

y

x

llll

ll

ll

y

x

lll

lll

y

&

&

&

&

&

&

&

&

&

+−−+−−

++=

+++

+−+−−=

−

221

2121121211

2

21

21211

21211212112212

sin

)]sin(sin[)]cos(cos[

sin

)sin()cos(

)sin(sin)cos(cossin

θ

θθθθθθθ

θ

θθθθθ

θθθθθθθθ

ll

yllxll

l

yx

yllllll

&&&

&&&

&

++−++−=

+++=

+−−+−−

16

Joint Velocities Near Singularities

• Very high joint

velocities are

resulted at points

A and D, which 1

sin

)]sin(sin[)]cos()[cos(

sin

)sin()cos(

2

211211

1

2

2121

1

==

+++++=

+++=

ll

vv

vv

yx

yx

θ

θθθθθθθ

θ

θθθθθ

&

&

are the arm’s

singular

configurations

(θ2=0°)

• Close to the

origin (θ2≈–180°), the velocity of the

121 == ll

2

the velocity of the

first joint

becomes very

large in order to

quickly turn the

arm from B to C, 17

Singularity Analysis

• When the arm is fully extended (θ2=0°, A and D positions)

• For position A

=

=

=−=

=

=

=−=

j

i

j

i 0

)0cos(

)0sin( and ,

2

0

)0cos(2

)0sin(2 1

2

1

1 θ

θ

θ

θJJ

both joints generate endpoint velocity along the y-axis, thus, end-point can move only along y-axis (loss of capability).

• When the arm is flexed (θ2≈-180° B, and C positions)

=

=

=

=

=

=jj )0cos(

and ,2)0cos(2 1

2

1

1 θθJJ

≈−− sin0sin)0)(( θθ lll

First joint does not produce any contribution to endpoint motion

−≈

≈

≈−

≈−−≈

12

12

2

121

121

1cos

sin and ,

0

0

cos)0)((

sin)0)((

θ

θ

θ

θ

l

l

ll

llJJ

18

Jacobian of a 3-Link Arm

• Lock joint 2 and 3, move joint

1 with unit ang rate and find

endpoint velocity → [J1]

• Lock joint 1 and 3, move joint • Lock joint 1 and 3, move joint

2 with unit ang rate and find

endpoint velocity → [J2]

• Lock joint 1 and 2, move joint

3 with unit ang rate and find

endpoint velocity → [J3]3

• Determine Jacobian as

J = [J1 J2 J3]

• Find singularities by |J|=0

19

Singularity and Redundancy

• Sometimes, such singular configurations exist in the middle

of the workspace seriously degrading mobility and

manipulatability of the robot

• To overcome this difficulty, endpoint trajectories can be • To overcome this difficulty, endpoint trajectories can be

planned away from singular configurations. Alternatively,

additional degrees of freedom should be included so that

even when some degrees of freedom are lost at certain

configurations, the robot can still maintain an adequate

number of degrees of freedom (Redundant Manipulators).

• To locate the endpoint at any position with any orientation, a • To locate the endpoint at any position with any orientation, a

planner manipulator needs 3 variables (xe,ye,φe), whereas a

spacial manipulator needs 6 variables (xe,ye,ze,φx ,φy,φz). Same

number of degrees of freedom are required for non-

redundent planner and special arms.20

Jacobian of the SC Arm

12×121

SCA Position Jacobian

0000

000

000

232

2132121321

2132121321

cds

ssdcsdsdsc

scdccdcdss

−

−

−−

By Partial Differentiation

Position JacobianPosition Jacobian

22

SCA Orientation Jacobian

3×1

3×1

−−−

+−−−

+−−−

=

652646542

6465416526465421

6465416526465421

1

1

1

)(

)(])([

)(])([

CSCSSCCCS

SCCCSCCSSSSCCCCS

SCCCSSCSSSSCCCCC

r

r

r

z

y

x

+−−−−−

64654165264654212 )(])([ CCCCSSSSSCSCCCCCr x

9×13×19×1 9×6 6×1

−−

+−+−−−=

652646542

6465416526465421

6465416526465421

2

2

2

)(

)(])([

SSCSSSCCS

CCCCSCCSSSSCCCCS

r

r

z

y

x

+−

++

−+

=

)

)(

)(

52542

541525421

541525421

3

3

3

CCCCS

SSCCSSCCS

SSSCSSCCC

r

r

r

z

y

x

23

Stanford Schinman ArmBy Partial Differentiation

3×1

3×1

3×13×1

24

3×1

3×1

Representations

3×1

3×1

3×1

9×1

4×1

25

Total Jacobian

Jacobian depends on the representation and arm configuration

26