robotics 2012 04 kinematic functions - polito

ROBOTICA

03CFIOR03CFIOR

Basilio Bona

DAUIN – Politecnico di Torino

Basilio Bona ROBOTICA 03CFIOR 1

Kinematic functions

Basilio Bona ROBOTICA 03CFIOR 2

Kinematics and kinematic functions

� Kinematics deals with the study of four functions (called

kinematic functions or KFs) that mathematically transform

joint variables into cartesian variables and vice versa

1. Direct Position KF: from joint space variables to task space

pose

2. Inverse Position KF: from task space pose to joint space 2. Inverse Position KF: from task space pose to joint space

variables

3. Direct Velocity KF: from joint space velocities to task space

velocities

4. Inverse Velocity KF: from task space velocities to joint space

velocities

Basilio Bona 3ROBOTICA 03CFIOR

Kinematic functions

� Positions and velocities of what?

� It can be the position or velocity of any point of the robot,

but, usually, is the TCP position and velocity


BASE

TCP

PR

0R?

What is the relation between these two RF?

Kinematic functions

� The first step for defining KFs is to fix a reference frame on

each robot arm

� In general, to move from a RF to the following one, it is

necessary to define 6 parameters (three translations of the

RF origin + three angles of the RF rotation)

� A number of conventions were introduced to reduce the � A number of conventions were introduced to reduce the

number of parameters and to find a common way to

describe the relative position of reference frames

� Denavit-Hartenberg conventions (1955) were the first to

be introduced and are widely used in industry (with some

minor modifications)


How to define the robot reference frames?

� A RF is positioned on each link/arm, using the so-called

Denavit-Hartenberg (DH) conventions

� The convention defines only 4 DOF between two

successive RF, instead of the usual 6

� Joints can be prismatic (P) or rotational (R); the convention

is always valid

� 2 parameters are associated to a translation, 2 parameters � 2 parameters are associated to a translation, 2 parameters

are associated to a rotation

� Three parameters depend only on the robot geometry and

therefore are constant in time

� One parameter depends on the relative motion between

two successive links, and therefore is a function of time.

� We call this the i-th joint variable


( )iq t

DH convention – 1

� Assume a connected multibody system with n rigid links

� The links may not be necessarily symmetric

� Each link is connected to two joints, one upstream (toward the base), one downstream (toward the TCP)

We want to locate a RF on this arm link


Upstream joint

Downstream joint

link

DH convention – 2

Joint 3

Joint 4

Link 2

Link 3

Joint 5

Joint 6

Wrist


Joint 1

Joint 2

Link 1

Shoulder

Link 0 = base

arm/linkib =

jointig =

DH convention – 3

We use the term motion axis

to include both revolute and prismatic axes

Motion axis

Motion axis

Motion axis

g ib

1ib +

1ig +

2ig +


We have this sequence

0 1 1 2 2 3 Nb g b g b g b− − − − − −

Base TCP

1ib −

ig i

2ib +

DH convention – 4


DH convention – 5

Motion axis

Motion axis

Motion axis


DH convention – 6

Motion axis

Motion axis

Motion axis


DH convention – 7

Motion axis

Motion axis

Motion axis


DH convention – 8

Motion axis

Motion axis

Motion axis


DH convention – 9

Motion axis

Motion axis

Motion axis


DH convention – 10

Motion axis

Motion axis

Motion axis


DH rules – 1


DH rules – 2


DH rules – 3


DH parameters

DH parameters define the transformation

1i−R iR

Depending on the joint type

ig


Prismatic joint Rotation joint

, ,i i iaθ α = fixed

( )( )i iq t d t≡

, ,i i id a α = fissi( )( )

i iq t tθ≡Joint variables

Geometrical parameters“calibration”

DH homogeneous roto-translation matrix

T( , )1

= =

R tT T R t

0

cos sin cos sin sin cosaθ θ α θ α θ −


1

, ,( , ) ( , ) ( , ) ( , )i

i k iθ α

− =T T I d T R 0 T I a T R 0

1

cos sin cos sin sin cos

sin cos cos cos sin sin

0 sin cos

0 0 0 1

i i i i i i i

i i i i i i ii

i

i i i

a

a

d

θ θ α θ α θ

θ θ α θ α θ

α α−

− − =

T

From the textbook of Spong ...

Mark W. Spong, Seth Hutchinson, and M. VidyasagarRobot Modeling and ControlJohn Wiley & Sons, 2006


Exercise – The PUMA robot

From above

1

2

3

4

5

62

3


Lateral view

1

2 3

45 6

1

3

4 5

6

A procedure to compute the KFs – 1

1. Select and identify links and joints

2. Set all RFs using the DH conventions

3. Define the constant geometrical parameters

4. Define the variable joint parameters

5. Compute the homogeneous transformation between the

base RF and the TCP RFbase RF and the TCP RF

6. Extract the direct position KF from the homogeneous

transformation

7. Compute the inverse position KF

8. Inverse velocity KF: analytical or geometrical approach

9. Inverse velocity KF: kinematic singularity problem


A procedure to compute the KFs – 2

1. Select and identify links and joints

2. Set all RFs using DH conventions

3. Define constant geometrical parameters

2( )q t

4( )q t

5( )q t

6( )q t


BASE

TCP

PR

0R 0PT

T

0 00 ( ) ( )( )

1P P

P

q qq = R tT0

1( )q t

6

Joint and cartesian variables

1

2

3

4

( )

( )

( )( )

( )

( )

q t

q t

q tt

q t

q t

=

q

Joint variables Task/cartesian variables/pose

1

2

3

1

2

( )

( )

( )( )

( )

( )

x t

x t

x tt

t

t

α

α

=

p

position

orientation


5

6

( )

( )

q t

q t

Direct KF

( )( ) ( )t t=p f q

2

3

( )

( )

t

t

α

α

Inverse KF

( )1( ) ( )t t−=q f p

orientation

Direct position KF – 1

position

( )

( )

1 1

2 2

3 3

4 4

5 5

6 6

( ) ( )

( ) ( )( )( ) ( )

( ) ( ( ))( ) ( )

( )( ) ( )

( ) ( )

q t p t

q t p ttq t p t

t tq t p t

tq t p t

q t p t

= ⇒ = =

x qq p q

qα

⋯

orientation


( ) ( ) ( ) ( ) ( ) ( )0 0

0 0 1 2 3 4 5 61 1 2 2 3 3 4 4 5 5 6 6 1

P P

P Pq q q q q q = =

R tT T T T T T T T

0 T

6 6( ) ( )q t p t orientation

orientation position

( )0 0 ( )P P t=R R q ( )0 0 ( )P P t=t t q


Direct cartesian position KF: easy

110

txx t

≡ ⇔ ≡ x t

0 0

0

1P P

P

=

R tT

0 T


02 2

3 3

Px tx t

≡ ⇔ ≡ x t

Direct cartesian orientation KF:

not so easy to compute, but not difficult

We will solve the problem in the following slides


( ) ( )( ) ( )t t⇒R q qα

We want to compute angles from the rotation matrix.

But … it is important to decide which representation to use

?( )( )tqα


Euler angles

RPY angles

Quaternions

Axis-angle representation

…

Inverse position KF – 1

This KF is important, since control action are applied

to the joint motors, while the user wants to work with

cartesian positions and orientations

( )

( )α

1

2

3

4

5

6

( )

( )( )

( )( ( )) ( )

( )( )

( )

( )

q t

q tt

q tt t

q tt

q t

q t

= ⇒ =

x q

p q q

q

⋯


cartesian positions and orientations

1( )q t

2( )q t

3( )q t

( )( )( )

( )

t

t

x q

q⋯

α

Inverse position KF – 2

1. The problem is complex and there is no clear recipe to solve it

2. If a spherical wrist is present, then a solution is guaranteed, but

we must find it ...

3. There are several possibilities

� Use brute force or previous solutions found for similar chains

� Use inverse velocity KF

� Use symbolic manipulation programs (computer algebra systems as � Use symbolic manipulation programs (computer algebra systems as

Mathematica, Maple, Maxima, …, Lisp)

� Iteratively compute an approximated numerical expression for the

nonlinear equation (Newton method or others)


( )( )

( ){ }

1

1

1

( ) ( )

( ) ( ) 0

min ( ) ( )

t t

t t

t t

−

−

−

=

− =

−

q f p

q f p

q f p

Direct velocity KF – 1

Linear and angular direct velocity KF

Non redundant robot with 6 DOF

( )1 1

2 2

3 3

( ) ( )

( ) ( )( ), ( )

( ) ( )( ) ( )

q t p t

q t p tt t

q t p tt t

= ⇒ = = x q q

q p

ɺ ɺ

ɺ ɺɺ ɺ

ɺ ɺɺ ɺ ⋯

( )( ), ( )t t =

v q qɺ

⋯

Linear velocity


( )3 3

4 4

5 5

6 6

( ) ( )( ) ( )

( ) ( )( ), ( )

( ) ( )

( ) ( )

q t p tt t

q t p tt t

q t p t

q t p t

= ⇒ = =

q p

q q

ɺ ɺɺ ɺ ⋯

ɺ ɺɺ ɺ

ɺ ɺ

ɺ ɺ

α ( )( ), ( )t t

= q qω

⋯

ɺ

Angular velocity


A brief review of mathematical notations

( )

( )

d( )

dd( ( ))

dd

( )d

tt

tt

tt

=

x q

p q

qα

⋯

General rule


( ) ( )1

1

1

d d( ) ( ), , ( ), , ( )

d d

( ) ( ) ( )

i i j n

i i i

j n

j n

f t f q t q t q tt t

f f fq t q t q tq q q

=

∂ ∂ ∂= + + + +∂ ∂ ∂

q … …

ɺ ɺ ɺ… …

General rule


( ) ( )

1

1

( )

d( )( ) ( ) ( )

d

( )

i i iji fi

j n

n

q t

f f fq tf t t t

t q q q

q t

∂ ∂ ∂ = = ∂ ∂ ∂

q J q q

ɺ

⋮

ɺ ɺ… …

⋮

ɺ

T

1 1 1

1 ( )j n

f f f

q q qq t

∂ ∂ ∂ ∂ ∂ ∂ ⋯ ⋯

ɺ

JACOBIAN


( ) ( )

11

1

1

( )

d( )( ) ( ) ( )

d

( )

j n

i i i

j

j n

n

m m m

j n

q q qq t

f f fq tt t t

q q qt

q tf f f

q q q

∂ ∂ ∂ ∂ ∂ ∂ = = ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂

f q J q q

ɺ⋮ ⋱ ⋮ ⋮

⋮

ɺ ɺ⋯ ⋯

⋮⋮ ⋮ ⋱ ⋮

ɺ

⋯ ⋯

Velocity kinematics is characterized by Jacobians

There are two types of Jacobians:

� Geometrical Jacobian

� Analytical Jacobian

Further notes on the Jacobians

gJ

aJ

xɺ

also called Task Jacobian

The first one is related to

( )( ) ( ) ( )t t t=p J q qɺ ɺ


p g

= =

xv J q

ω

ɺɺ

a

= =

xp J q

ɺɺ ɺ

ɺα

The first one is related to

Geometrical Velocities

The second one is related to

Analytical Velocities

Geometrical and Analytical velocities

What is the difference between these two angular velocities?


On the contrary, linear velocities do not have this problem:

analytical and geometrical velocities are the same vector, that

can be integrated to give the cartesian position


� Moreover it is important to remember that in general no

vector exists that is the integral of ( )tu

( ) ( )( ) ( ) ( ) sin ( ) ( ) 1 cos ( ) ( ) ( )t t t t t t t tθ θ θ= + + −u u S u uωɺ ɺ ɺ

The exact relation between the two quantities is:

( )tω

?( ) ( )t t≠∫u ω


( ) ( )( ) ( ) ( ) sin ( ) ( ) 1 cos ( ) ( ) ( )t t t t t t t tθ θ θ= + + −u u S u uωɺ ɺ ɺ

While it is possible to integrate ( )tpɺ

( ) ( )( )d d

( ) ( )

t

t

ττ τ τ

τ

= =

∫ ∫x x

pɺ

ɺɺα α


� The geometrical Jacobian is adopted every time a clear

physical interpretation of the rotation velocity is needed

� The analytical Jacobian is adopted every time it is

necessary to treat differential quantities in the task space

Then, if one wants to implement recursive formula for the joint

position( ) ( ) ( )t t t t∆= +q q qɺ


1

1( ) ( ) ( ( )) ( )k k g k p kt t t t t∆−+ = +q q J q v

1

1( ) ( ) ( ( )) ( )k k a k kt t t t t∆−+ = +q q J q pɺ

or

he can use

1( ) ( ) ( )k k kt t t t∆+ = +q q qɺ

Geometric Jacobian – 1

� The geometric Jacobian can be constructed taking into

account the following steps

a) Every link has a reference frame defined according to DH

conventions

b) The position of the origin of is given by:

iR

iR

ig

1ig +


1i−R

0R

iR

1i−x

ix

1

1,

i

i i

−−r

0 1

1 1 1, 1 1,

i

i i i i i i i i

−− − − − −= + = +x x R r x r

ib


Derivation wrt time gives

0 1 0 1

1 1 1, 1 1 1,

1 1, 1 1,

i i

i i i i i i i i i

i i i i i i

− −− − − − − −

− − − −

= + + ×= + + ×

x x R r R r

x v r

ω

ω

ɺ ɺ ɺ

ɺ


Linear velocity of wrt 1i i−R R Angular velocity of

1i−R

Remember: ( )= = ×R S R Rω ωɺ


If we derive the composition of two rotations, we have:

0 0 1

1

0 0 1 0 1

1 1

0 1 0 1

1 1 1 1,

0 1 0 0 1

1 1 1 1, 1

( ) ( )

( ) ( )

i

i i i

i i

i i i i i

i i

i i i i i i i

i i

i i i i i i i i

−−

− −− −

− −− − − −

− −− − − − −

=

= +

= +

= +

R R R

R R R R R

S R R R S R

S R R S R R R

ɺ ɺ ɺ

ω ω

ω ω


1 1 1 1, 1

0 0

1 1 1,

( ) ( )

( ) ( )

i i i i i i i i

i i i i i

− − − − −

− − −

= + = +

S R R S R R R

S S R R

ω ω

ω ω

Hence: 0

1 1 1,i i i i i− − −= +Rω ω ω

angular velocity of RFi-1 in RF0

angular velocity of RFi in RF0 angular velocity of RFi wrt RFi-1 in RFi-1


To compute the Geometric Jacobian, one can decompose the

Jacobian matrix as: 1

,1 ,2 , 2 3

, ,,1 ,2 ,

( ) ,L L L n

g L i A i

A A A n

n

q

q

q

= = = ∈

x J J Jv J q q J J

J J Jω

ɺ

ɺ ɺ⋯ɺ

⋯ ⋮

ɺ

R

LINEAR JACOBIAN

ANGULAR JACOBIAN


indicates how contributes to the linear velocity of the TCP

,

,1 1

d

d

L i i

n n

i L i ii ii

q

q qq= =

= =∑ ∑

J

xx J

ɺ

ɺ ɺ ɺ

indicates how contributes to the angular velocity of the TCP,1

1, ,1 1

A i

n n

i i A i ii i

q

q−

= =

= =∑ ∑

J

Jω ω

ɺ

ɺ

Structure of the Jacobian

LINEAR JACOBIAN


ANGULAR JACOBIAN

Structure of the Jacobian

If the robot is non-redundant,

the Jacobian matrix is square


If the robot is redundant, the

Jacobian matrix is rectangular


If the joint is prismatic

If the joint is revolute

, 1

1,

L i i i i

i i

q d−

−

==

J k

0

ɺɺ

ω

, 1

,

L i i

A i

−==

J k

J 0

1i−R

R

RTCP

1i−x

xTCP

TCP1,i−r

( )TCP TCP, 1, 1, 1 1,

1, 1

L i i i i i i i i

i i i i

q θ

θ

− − − −

− −

= × = ×

=

J r k r

k

ω

ω

ɺɺ

ɺ


If the joint is revolute

TCP, 1 1,

, 1

L i i i

A i i

− −

−

= ×=

J k r

J k

0R

( )TCP TCP

All vectors are expresses in

is the vector that represents in

0

1, 1 0i i− −−r x x

R

R


In conclusions, in a geometrical Jacobian, its elements can be

computed as follows:

Prismatic

, ,L i A iJ J

k 0


TCP

Prismatic

Revolute

1

1 1, 1

i

i i i

−

− − −×

k 0

k r k

Analytical Jacobian – 1

Analytical Jacobian is computed deriving the expression of the

TCP pose

1 1

1 11

( ( )) ( ( ))

d ( ( ))

d

d ( ( ))

n

p t p t

p qt q q

t

t

∂ ∂ ∂ ∂ = = =

q q

x q

pqα

⋯ɺ ɺ

ɺ ⋮ ⋮ ⋱ ⋮ ⋮


6 6 6

1

d ( ( ))( ( )) ( ( ))d n

n

tp p t p t qt

q q

∂ ∂ ∂ ∂

pq

q qα

⋮ ⋮ ⋱ ⋮ ⋮

ɺ ɺ⋯

( ( ))atJ q

Analytical Jacobian – 2

� The first three lines of the analytical Jacobian are equal to

the same lines of the geometric Jacobian

� The last three lines are usually different from the same

lines of the geometric Jacobian

� These can be computed only when the angle

representation has been chosen: here we consider only

Euler and RPY anglesEuler and RPY angles

� A transformation matrix relates the analytical and

geometric velocities, and the two Jacobians


( )=Tω α αɺ

( ) ( )( )g a

=

I 0J q J q

0 T α

Relations between Jacobians

� Euler angles

0 cos sin sin

( ) 0 sin cos sin

1 0 cos

φ φ θ

φ φ θ

θ

= −

T α

{ }, ,φ θ ψ=α

� RPY angles { }, ,x y zθ θ θ=α


cos cos sin 0

( ) cos sin cos 0

sin 0 1

y z z

y z z

y

θ θ θ

θ θ θ

θ

− = −

T α

The values of α that zeros the matrix T(α) determinant correspond to a

orientation representation singularity

This means that there are (geometrical) angular velocities that cannot be

expressed by

Inverse velocity KF – 1

� When the Jacobian is a square full-rank matrix, the inverse

velocity kinematic function is simply obtained as:

( )1( ) ( ) ( )t t t−=q J q pɺ ɺ

� When the Jacobian is a rectangular full-rank matrix (i.e.,

when the robotic arm is redundant, but not singular), the


when the robotic arm is redundant, but not singular), the

inverse velocity kinematic function has infinite solutions,

but the (right) pseudo-inverse can be used to compute one

of them

( )( ) ( ) ( )t t t+=q J q pɺ ɺ

T Tdef

1( )+ −=J J JJ

Inverse velocity KF – 2

� If the initial joint vector q(0) is known, inverse velocity can

be used to solve the inverse position kinematics as an

integral

10( ) (0) ( ) d ( ) ( ) ( )

t

k k kt t t t tτ τ

+= + = + ∆∫q q q q q qɺ ɺ


0∫

Continuous time Discrete time

Singularity – 1

� A square matrix is invertible if

( )det ( ) 0t ≠J q

( )det ( ) 0St =J q

� When


a singularity exists at( )Stq

This is called a singular/singularity configuration

Singularity – 2

� For an articulated/anthropomorphic robot three singularity

conditions exist

A. completely extended or folded arm

B. wrist center on the vertical

C. wrist singularity

� When joint coordinates approach singularity the joint

velocities become very large for small cartesian velocities


det

1 1 1( )

ε

−= = → →∞q J q p Jp JpJ

ɺ ɺ ɺ ɺ

Singularity – 3

A. Extended arm


The velocities span

a dim-2 space

(the plane)

The velocities span

a dim-1 space

(the tangent line)

i.e., singularity

Singularity – 4

B. Wrist center on the vertical

these velocities cannot be

obtained with infinitesimal

joint rotations


Singularity – 5

C. Wrist singularity


Singularity condition

Two axes are aligned

Euler Angles (wrist) singularity

c c s c s c s s c c s s

s c c c s s s c c c c s

s s c s c

φ ψ φ θ ψ φ ψ φ θ ψ φ θ

φ ψ φ θ ψ φ ψ φ θ ψ φ θ

ψ θ ψ θ θ

− − − + − + −

Let us start from the symbolic matrix3 angles


Observe that if 1 0cθ

θ= ⇒ =

0

0

0 0 1

c c s s c s s c

s c c s c c s s

φ ψ φ ψ φ ψ φ ψ

φ ψ φ ψ φ ψ φ ψ

− − − + − cos( )φ ψ+

sin( )φ ψ+

1 angle

only

we have

Singularity

� In practice, when the joints are near a singularity

configuration, in order to follow a finite cartesian velocity

the joint velocities may become excessively large.

� Near singularity conditions it is not possible to follow a

geometric path and at the same time a given velocity

profile; it is necessary

� to reduce the cartesian velocity and follow the path, or� to reduce the cartesian velocity and follow the path, or

� to follow the velocity profile, but follow an approximated

path

� In exact singularity conditions, nothing can be done … so …

avoid singularity


Conclusions

� Kinematic functions can be computed once the DH

conventions are used

� Inverse position KF is complex

� Direct velocity KF has the problem of angular velocities:

analytical vs geometric Jacobiansanalytical vs geometric Jacobians

� Inverse velocity can be computed apart from singularity

points

� Avoid singularity


robotics 2012 04 kinematic functions - polito

Documents