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

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

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

DH convention – 2

Joint 3

Joint 4

Link 2

Link 3

Joint 5

Joint 6

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

We have this sequence

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

Base TCP

1ib −

DH convention – 4

DH convention – 5

Motion axis

DH convention – 6

Motion axis

DH convention – 7

Motion axis

DH convention – 8

Motion axis

DH convention – 9

Motion axis

DH convention – 10

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

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

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

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

i k iθ α

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

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

θ θ α θ α θ

α α−

− − =

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

Lateral view

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

0R 0PT

0 00 ( ) ( )( )

q qq = R tT0

1( )q t

Joint and cartesian variables

( )( )

Joint variables Task/cartesian variables/pose

( )( )

position

orientation

Direct KF

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

Inverse KF

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

orientation

Direct position KF – 1

position

( ) ( )

( ) ( )( )( ) ( )

( ) ( ( ))( ) ( )

( )( ) ( )

( ) ( )

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

orientation

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

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

P Pq q q q q q = =

R tT T T T T T T 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

≡ ⇔ ≡ x t

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

( )( )

( )( ( )) ( )

( )( )

q tt t

= ⇒ =

cartesian positions and orientations

1( )q t

2( )q t

3( )q t

( )( )( )

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)

( )( )

( ){ }

( ) ( )

( ) ( ) 0

min ( ) ( )

Direct velocity KF – 1

Linear and angular direct velocity KF

Non redundant robot with 6 DOF

( )1 1

( ) ( )

( ) ( )( ), ( )

( ) ( )( ) ( )

q t p t

q t p tt t

= ⇒ = = x q q

ɺ ɺɺ ɺ

ɺ ɺɺ ɺ ⋯

( )( ), ( )t t =

v q qɺ

Linear velocity

( )3 3

( ) ( )( ) ( )

( ) ( )( ), ( )

( ) ( )

q t p tt t

q t p t

= ⇒ = =

ɺ ɺɺ ɺ ⋯

ɺ ɺɺ ɺ

α ( )( ), ( )t t

= q qω

Angular velocity

A brief review of mathematical notations

dd( ( ))

General rule

( ) ( )1

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

( ) ( ) ( )

i i 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

( ) ( )

d( )( ) ( ) ( )

i i iji fi

f f fq tf t t t

t q q q

∂ ∂ ∂ = = ∂ ∂ ∂

q J q q

ɺ ɺ… …

1 ( )j n

q q qq t

∂ ∂ ∂ ∂ ∂ ∂ ⋯ ⋯

JACOBIAN

( ) ( )

d( )( ) ( ) ( )

q q qq t

f f fq tt t t

q q qt

q tf f f

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

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

also called Task Jacobian

The first one is related to

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

xv J q

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

( ) ( )

ττ τ τ

∫ ∫x x

ɺɺα α

� 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( ) ( ) ( ( )) ( )k k g k p kt t t t t∆−+ = +q q J q v

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

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:

1i−R

1i−x

−−r

1 1 1, 1 1,

i i i i i i i i

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

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

− −− − − − − −

− − − −

= + + ×= + + ×

x x R r R 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 0 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

−−

− −− −

− −− − − −

− −− − − − −

R R R R R

S R R R S R

S R R S R R R

ɺ ɺ ɺ

1 1 1 1, 1

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

= = = ∈

x J J Jv J q q J J

J J Jω

ɺ ɺ⋯ɺ

⋯ ⋮

LINEAR JACOBIAN

ANGULAR JACOBIAN

indicates how contributes to the linear velocity of the TCP

i L i ii ii

q qq= =

= =∑ ∑

ɺ ɺ ɺ

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

1, ,1 1

i i A i ii i

= =∑ ∑

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

L i i i i

q d−

1i−R

1i−x

TCP1,i−r

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

L i i i i i i i i

i i i i

− − − −

− −

= × = ×

J r k r

If the joint is revolute

TCP, 1 1,

L i i i

− −

( )TCP TCP

All vectors are expresses in

is the vector that represents in

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

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

computed as follows:

Prismatic

, ,L i A iJ J

Prismatic

Revolute

1 1, 1

− − −×

Analytical Jacobian – 1

Analytical Jacobian is computed deriving the expression of the

TCP pose

( ( )) ( ( ))

d ( ( ))

p t p t

p qt q q

∂ ∂ ∂ ∂ = = =

⋯ɺ ɺ

ɺ ⋮ ⋮ ⋱ ⋮ ⋮

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

tp p t p t qt

∂ ∂ ∂ ∂

⋮ ⋮ ⋱ ⋮ ⋮

ɺ ɺ⋯

( ( ))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

φ φ θ

{ }, ,φ θ ψ=α

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

cos cos sin 0

( ) cos sin cos 0

sin 0 1

θ θ θ

− = −

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

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 ( ) ( ) ( )

k k kt t t t tτ τ

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

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

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θ

θ= ⇒ =

c c s s c s s c

s c c s c c s s

φ ψ φ ψ φ ψ φ ψ

− − − + − cos( )φ ψ+

sin( )φ ψ+

1 angle

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

� 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

robotics 2017 06 trajectory planning 1 - polito.it ·...

well testing, polito

sensor robotics 2015 02 - polito

robotics & automation serial kinematic chain, forward

applying differential geometry to kinematic modeling in...

acnowledgements - polito

introduction to robotics...kinematic equations -...

gps/rtk vs. robotics. some gps lingo. rtk: real time...

introduction and kinematic modeling - · pdf...

sun slice team polito

telecommunication electronics - polito

marketing summary - polito

robotics 2011 04 kinematic functions - ladispe.polito.it

kinematic modelling in robotics

internet routing architecture - polito

welcome back to robotics · 2016. 8. 3. · • kinematic...

polomode mag # polito pieres

robotics 1deluca/rob1_en/rob1_en_2014...kinematic kalman...

(modern) html - polito

centro interdipartimentale smartdata@polito ·...