robotics 2011 04 kinematic functions - ladispe.polito.it

Industrial RobotsIndustrial Robots

KinematicsKinematicsKinematicsKinematicsBasilio Bona ROBOTICA 03CFIOR 1

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 p p j pvariables

3. Direct Velocity KF: from joint space velocities to task space y j p pvelocities

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

Basilio Bona 2ROBOTICA 03CFIOR

Kinematic functions

Positions and velocities of what?

It can be the position or velocity of any point of the robotIt can be the position or velocity of any point of the robot, but, usually, is the TCPTCP position and velocity

TCP

PR

BASE 0R?

Wh t i th l ti b t


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 y p (RF origin + three angles of the RF rotation)

A number of conventions were introduced to reduce theA number of conventions were introduced to reduce the number of parameters and to find a common way to describe the relative position of reference framesp

Denavit‐Hartenberg conventions (1955) were the first to be introduced and are widely used in industry (with someintroduced and are widely used in industry (with some minor modifications)


How to define the robot reference frames?

A RF i iti d h li k/ i th ll dA RF is positioned on each link/arm, using the so‐called Denavit‐Hartenberg (DH) conventionsTh i d fi l 4 DOF b iThe convention defines only 4 DOF between two successive RF, instead of the usual 6

b ( ) l ( ) hJoints can be prismatic (P) or rotational (R); the convention is always valid2 parameters are associated to a translation, 2 parameters are associated to a rotationThree out of four parameters depend only on the robot geometry and therefore are constant in timeOne 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 linksThe links may not be necessarily symmetricy y yEach 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 linklink

Upstream joint

Downstream joint


DH convention – 2

Joint 4Link 3

Joint 5

Joint 3

Link 2

Joint 6

Link 2

Wrist

Joint 2Link 1

Shoulder

Joint 1/li kb Joint 1

Link 0 = base

arm/linkib =

jointig =


DH convention – 3

We use the term motion axisWe use the term motion axisto include both revolute and prismatic axes

Motion axis

Motion axis Motion axis

1ib +

1ig +

g

g ib

1i+2i

g +

1ib −

ig

b2i

b +

We have this sequence

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


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

Base TCP

DH convention – 4


DH convention – 5

Motion axis

Motion axis

Motion a isMotion axis Motion axis


DH convention – 6

Motion axis

Motion axis

M ti iMotion axis Motion axis


DH convention – 7

Motion axis

Motion axis



DH convention – 8

Motion axis

Motion axis

M ti iMotion axis Motion axis


DH convention – 9

Motion axis

Motion axis

Motion iMotion axis Motion axis


DH convention – 10

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 jointj j

( )( )i iq t d t≡ ( )( )

i iq t tθ≡Joint variables

, ,i i iaθ α = fixed , ,i i id a α = fissi

Geometrical parameters


Geometrical parameters“calibration”

DH homogeneous roto‐translation matrix

T( , )⎛ ⎞⎟⎜ ⎟⎜= = ⎟⎜R t

T T R t T( , )1 ⎟⎜ ⎟⎜ ⎟⎝ ⎠

T T R t0

⎛ ⎞cos sin cos sin sin cos

sin cos cos cos sin sini i i i i i i

a

a

θ θ α θ α θθ θ α θ α θ

⎛ ⎞− ⎟⎜ ⎟⎜ ⎟⎜ ⎟−⎜ ⎟1sin cos cos cos sin sin

0 sin cosi i i i i i ii

ii i i

a

d

θ θ α θ α θα α

− ⎜ ⎟⎜ ⎟= ⎜ ⎟⎜ ⎟⎟⎜ ⎟⎜ ⎟⎜

T

1 ( ) ( ) ( ) ( )i

0 0 0 1⎜ ⎟⎜ ⎟⎜⎝ ⎠


1, ,

( , ) ( , ) ( , ) ( , )ii k iθ α− =T T I d T R 0 T I a T R 0

From the textbook of Spong ...

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


Exercise – The PUMA robot

2

1 3

4

622

From above

35

332 3

4

11

33

1 45 644 55

66

Lateral view


A procedure to compute the KFs – 1

1. Select and identify links and joints2. Set all RFs using the DH conventions2. Set all RFs using the DH conventions 3. Define the constant geometrical parameters4 D fi th i bl j i t t4. Define the variable joint parameters 5. Compute the homogeneous transformation between the

base RF and the TCP RF6. Extract the direct position KF from the homogeneous

transformation7. Compute the inverse position KF7. Compute the inverse position KF8. Inverse velocity KF: analytical or geometrical approach9 I l i KF ki i i l i bl9. Inverse velocity KF: kinematic singularity problem


A procedure to compute the KFs – 2

1. Select and identify links and joints2. Set all RFs using DH conventions 3 D fi i l3. Define constant geometrical parameters

4( )q t

5( )q t

2( )q t

5( )q

6( )q t

TCPTCP

PR1( )q t

BASE 0R 0PT0 0

0 ( ) ( )( ) q q⎛ ⎞⎟⎜R t


T

0 ( ) ( )( )1

P PP

q qq ⎟⎜= ⎟⎜ ⎟⎜⎝ ⎠R tT0

Joint and cartesian variables

Joint variables Task/cartesian variables/pose

( )⎛ ⎞1

2

( )( )( )

q tq t⎛ ⎞⎟⎜ ⎟⎜ ⎟⎜ ⎟⎜ ⎟⎜ ⎟

1

2

( )( )( )

x tx tt

⎛ ⎞⎟⎜ ⎟⎜ ⎟⎜ ⎟⎜ ⎟⎜ ⎟position

3

4

( )( ) ( )

( )

q tt q t

q t

⎜ ⎟⎟⎜ ⎟⎜= ⎟⎜ ⎟⎜ ⎟⎜ ⎟⎜ ⎟

q3

1

( )( ) ( )

( )

x tt t

tαα

⎜ ⎟⎟⎜ ⎟⎜= ⎟⎜ ⎟⎜ ⎟⎜ ⎟⎜ ⎟⎟⎜

p

orientation5

6

( )( )q tq t⎜ ⎟⎟⎜ ⎟⎜ ⎟⎜ ⎟⎜⎝ ⎠

2

3

( )( )tt

αα

⎟⎜ ⎟⎜ ⎟⎜ ⎟⎜⎝ ⎠

orientation

Direct KF Inverse KF

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


Direct position KF – 1

position1 1( ) ( )( ) ( )q t p tq t p t⎛ ⎞ ⎛ ⎞⎟ ⎟⎜ ⎜⎟ ⎟⎜ ⎜⎟ ⎟⎜ ⎜ ⎛ ⎞

( )2 2

3 3

4 4

( ) ( )( )( ) ( )

( ) ( ( ))( ) ( )

q t p ttq t p t

t tq t p t

⎟ ⎟⎜ ⎜⎟ ⎟ ⎛ ⎞⎜ ⎜⎟ ⎟ ⎟⎜⎜ ⎜⎟ ⎟ ⎟⎜⎟ ⎟⎜ ⎜ ⎟⎜⎟ ⎟⎜ ⎜ ⎟= ⇒ = =⎟ ⎟ ⎜⎜ ⎜ ⎟⎟ ⎟ ⎜ ⎟⎜ ⎜⎟ ⎟

x qq p q

( )4 4

5 5

6 6

( ) ( )( )( ) ( )

( ) ( )

q t p ttq t p t

q t p t

⎜ ⎟⎜ ⎜⎟ ⎟ ⎟⎜⎜ ⎜⎟ ⎟ ⎟⎜⎜ ⎜ ⎝ ⎠⎟ ⎟⎟ ⎟⎜ ⎜⎟ ⎟⎜ ⎜⎟ ⎟⎜ ⎜⎟ ⎟⎜ ⎜⎝ ⎠ ⎝ ⎠

qα

6 6( ) ( )q p⎝ ⎠ ⎝ ⎠orientation

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

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

P PP Pq q q q q q

⎛ ⎞⎟⎜ ⎟= = ⎜ ⎟⎜ ⎟⎜⎝ ⎠

R tT T T T T T T T 0 T

⎝ ⎠

orientation position

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


( )( )P P tR R q ( )( )P P tt t q


0 00

1P P

P

⎛ ⎞⎟⎜ ⎟= ⎜ ⎟⎜ ⎟⎜⎝ ⎠

R tT 0 T

Direct cartesian position KF: easy

1 ⎟⎜⎝ ⎠0

Direct cartesian position KF: easy

11 0txt

⎛ ⎞⎛ ⎞ ⎟⎜⎟⎜ ⎟⎟ ⎜⎜ ⎟⎟ ⎜≡ ⇔ ≡⎜ t02 2

3 3

Px tx t

⎟⎟ ⎜≡ ⇔ ≡⎜ ⎟⎟ ⎜ ⎟⎜ ⎟ ⎜ ⎟⎜ ⎟⎜ ⎟⎜⎝ ⎠ ⎝ ⎠x t

Direct cartesian orientation KF:not so easy to compute but not difficultnot so easy to compute, but not difficult

We will solve the problem in the following slides



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

W l f h i iWe want to compute angles from the rotation matrix.But … it is important to decide which representation to use

?( )( )tqα

Euler anglesRPY lRPY anglesQuaternionsAxis‐angle representationAxis angle representation…


Inverse position KF – 11( )

( )

q t

q t

⎛ ⎞⎟⎜ ⎟⎜ ⎟⎜ ⎟⎜ ⎟⎛ ⎞( ) 2

3

( )( )

( )( ( )) ( )

( )

q tt

q tt t

t

⎟⎜ ⎟⎛ ⎞ ⎜ ⎟⎟⎜ ⎜ ⎟⎟⎜ ⎜ ⎟⎟⎜ ⎟⎜⎟ ⎟⎜= ⇒ = ⎜⎟ ⎟⎜ ⎜⎟

x qp q q

Thi KF i i i l i li d

( )α 4

5

( ( )) ( )( )

( )( )

q tt

q t

⎟⎜ ⎜⎟ ⎟⎜ ⎜⎟ ⎟⎜ ⎟ ⎜ ⎟⎟⎜ ⎜ ⎟⎝ ⎠ ⎜ ⎟⎜ ⎟⎟⎜

p q q

q

This KF is important, since control action are applied to the joint motors, while the user wants to work with

cartesian positions and orientations

6( )q t

⎟⎜ ⎟⎜ ⎟⎜⎝ ⎠cartesian positions and orientations

3( )q t

2( )q t

⎛ ⎞2( )q

( )( )( )

( )

t

t

⎛ ⎞⎟⎜ ⎟⎜ ⎟⎜ ⎟⎜ ⎟⎜ ⎟⎟⎜

x q

qα


1( )q t ( )( )t⎜ ⎟⎟⎜⎝ ⎠qα

Inverse position KF – 2

1. The problem is complex and there is no clear recipe to solve it2. If a spherical wristspherical 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 chainsUse inverse velocity KFUse symbolic manipulation programs (computer algebra systems asUse 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( ) ( )t t−=q f p( )( )

( ){ }1

1

( ) ( )

( ) ( ) 0

( ) ( )

t t

t t−− =

q f p

q f p


( ){ }1min ( ) ( )t t−−q f p

Direct velocity KF – 1

Li d l di t l it KFLinear and angular direct velocity KFNon redundant robot with 6 DOF

Linear velocity

1 1( ) ( )q t p t⎛ ⎞ ⎛ ⎞⎟ ⎟⎜ ⎜⎟ ⎟⎜ ⎜⎟ ⎟

Linear velocity

( )2 2

3 3

( ) ( )( ), ( )

( ) ( )( ) ( )

q t p tt t

q t p t

⎜ ⎜⎟ ⎟⎜ ⎜⎟ ⎟⎜ ⎜⎟ ⎟ ⎛⎜ ⎜⎟ ⎟ ⎜⎜ ⎜⎟ ⎟ ⎜⎜ ⎜⎟ ⎟ ⎜⎟ ⎟⎜ ⎜x q q ( )( ), ( )t t⎞ ⎛ ⎞⎟ ⎟⎜⎟ ⎟⎜⎟ ⎟⎜⎟ ⎟

v q q

( )3 3

4 4

( ) ( )( ) ( )

( ) ( )( ), ( )

( ) ( )

q pt t

q t p tt t

q t p t

⎜⎟ ⎟⎜ ⎜⎟ ⎟= ⇒ = =⎜ ⎜⎟ ⎟⎜ ⎜⎟ ⎟⎜ ⎜⎟ ⎟⎜ ⎜⎟ ⎟⎜ ⎜⎟ ⎟ ⎝⎜ ⎜⎟ ⎟

q p

q qα ( )( ), ( )t t

⎜⎟ ⎟⎜ ⎜=⎟ ⎟⎜ ⎜⎟ ⎟⎜ ⎜⎟ ⎟⎜ ⎜⎟ ⎟⎟ ⎟⎜ ⎜⎠ ⎝ ⎠q qω( )5 5

6 6

( ) ( )

( ) ( )

q t p t

q t p t

⎝⎜ ⎜⎟ ⎟⎜ ⎜⎟ ⎟⎟ ⎟⎜ ⎜⎟ ⎟⎜ ⎜⎟ ⎟⎜ ⎜⎝ ⎠ ⎝ ⎠

( )⎠ ⎝ ⎠

Angular velocity


Direct velocity KF – 2

A b i f i f th ti l t tiA brief review of mathematical notations

( )d( )t

⎛ ⎞⎟⎜ ⎟( )( )dd

( ( ))d

tt

tt

⎜ ⎟⎜ ⎟⎜ ⎟⎜ ⎟⎜ ⎟= ⎜ ⎟⎜ ⎟⎟⎜

x q

p q

( )d

d( )

d

tt

t

⎟⎜ ⎟⎜ ⎟⎜ ⎟⎜ ⎟⎜⎝ ⎠qα

General rule

( ) ( )d d( ) ( ) ( ) ( )f t f q t q t q t=q

General rule

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

d d

( ) ( ) ( )

i i j n

i i i

f t f q t q t q tt t

f f fq t q t q t

=

∂ ∂ ∂= + + + +

q … …

… …1

1

( ) ( ) ( )j n

j n

q t q t q tq q q

+ + + +∂ ∂ ∂

… …


Direct velocity KF – 31( )q t⎛ ⎞⎟⎜ ⎟⎜ ⎟⎜ ⎟⎜

( ) ( )d( )( ) ( ) ( )

di i i

ji fi

f f fq tf t t t

t q q q

⎟⎜ ⎟⎛ ⎞⎜ ⎟∂ ∂ ∂ ⎟⎜⎜ ⎟⎟⎜ ⎟⎜= =⎟ ⎟⎜⎜ ⎟ ⎟⎜∂ ∂ ∂⎜ ⎟⎝ ⎠ ⎟⎜q J q q… … T

1d

( )

j n

n

t q q q

q t

∂ ∂ ∂ ⎟⎝ ⎠ ⎟⎜ ⎟⎜ ⎟⎜ ⎟⎜ ⎟⎜ ⎟⎜⎝ ⎠n⎝ ⎠1 1 1

( )

f f f

q q q

⎛ ⎞∂ ∂ ∂ ⎟⎜ ⎟⎜ ⎟⎜ ⎟∂ ∂ ∂⎜ ⎟ ⎛ ⎞11( )

d

j nq q q

q t

f f f

∂ ∂ ∂⎜ ⎟ ⎛ ⎞⎜ ⎟ ⎟⎜⎜ ⎟ ⎟⎜⎜ ⎟ ⎟⎜⎟⎜ ⎟⎟ ⎜⎜ ⎟⎟ ⎜⎜ ⎟∂ ∂ ∂ ⎟

JACOBIAN

( ) ( )1

d( )( ) ( ) ( )

di i i

jj n

f f fq tt t t

q q qt

⎜⎜ ⎟∂ ∂ ∂ ⎟ ⎜⎜ ⎟⎟ ⎜⎜ ⎟= =⎟ ⎟⎜⎜ ⎟ ⎟∂ ∂ ∂ ⎜⎜ ⎟ ⎟⎜⎜ ⎟ ⎟⎜⎟⎜ ⎟

f q J q q

( )n

m m m

q tf f f

⎜⎟⎜ ⎟⎟ ⎜⎜ ⎟⎟ ⎜⎜ ⎟⎟ ⎜⎜ ⎟⎜⎟ ⎝ ⎠⎜ ⎟∂ ∂ ∂⎜ ⎟⎜ ⎟


1

m m m

j nq q q

⎜ ⎟⎜ ⎟⎜ ⎟∂ ∂ ∂⎜⎝ ⎠

Velocity kinematics is characterized by Jacobians

Further notes on the Jacobians

Velocity kinematics is characterized by Jacobians

Th t t f J bi( )( ) ( ) ( )t t t=p J q q

There are two types of Jacobians:Geometrical Jacobian A l ti l J bi

gJ also called Task Jacobian

Analytical JacobianaJ

⎡ ⎤⎢ ⎥xThe first one is related to

p g⎢ ⎥= =⎢ ⎥⎢ ⎥⎣ ⎦

xv J q

ωThe first one is related to Geometrical VelocitiesGeometrical Velocities

a

⎡ ⎤⎢ ⎥= =⎢ ⎥⎢ ⎥⎣ ⎦

xp J q

αThe second one is related toAnalytical VelocitiesAnalytical Velocities


⎢ ⎥⎣ ⎦αAnalytical VelocitiesAnalytical 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, thaty g ,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 ( )tω

?( ) ( )t t≠∫u ω

( ) ( )( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )θ θ θ S

The exact relation between the two quantities is:∫

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

While it is possible to integrate ( )tpWhile it is possible to integrate ( )tp

( ) ( )t⎡ ⎤ ⎡ ⎤( ) ( )( )d d

( ) ( )

t

t

ττ τ τ

τ

⎡ ⎤ ⎡ ⎤⎢ ⎥ ⎢ ⎥= =⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥⎣ ⎦ ⎣ ⎦

∫ ∫x x

pα α



The geometrical Jacobian is adopted every time a clear physical interpretation of the rotation velocity is neededThe 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

he can use

1( ) ( ) ( )k k kt t t tΔ+ = +q q q

11

( ) ( ) ( ( )) ( )k k g k p kt t t t tΔ−

+ = +q q J q v

he can use

1k k g k p k+

1( ) ( ) ( ( )) ( )t t t t tΔ−+q q J q p

or


1( ) ( ) ( ( )) ( )k k a k kt t t t tΔ+ = +q q J q p

Geometric Jacobian – 1

The geometric Jacobian can be constructed taking into account the following stepsa) Every link has a reference frame defined according to DH

conventionsb) h i i f h i i f i i b

iR

Rb) The position of the origin of is given by:iRig

1ig +

1i−R 1i−r

ib

iR

1i−x1,i i−r

ix


0R

0 11 1 1, 1 1,

ii i i i i i i i

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


Derivation wrt time gives

0 1 0 1i i− −+ + ×x x R r R rω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 rx 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 1i−R R R

10 0 1 0 1

1 1

i i ii i

i i i i i

−− −

− −

=

= +

R R R

R R R R R0 1 0 1

1 1 1 1,0 1 0 0 1

1 1 1 1 1

( ) ( )

( ) ( )

i ii i i i i i i

i ii i i i i i i i

− −− − − −

− −

= +

= +

S R R R S R

S R R S R R R

ω ωω ω

1 1 1 1, 1

0 01 1 1,

( ) ( )

( ) ( )i i i i i i i i

i i i i i

− − − − −

− − −⎡ ⎤= +⎢ ⎥⎣ ⎦S S R Rω ω

Hence: 01 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:

1q⎡ ⎤⎢ ⎥LINEAR JACOBIAN 1

,1 ,2 , 2 3, ,

( ) ,L L L ng L i A i

q

q⎢ ⎥⎢ ⎥⎡ ⎤ ⎡ ⎤⎢ ⎥⎢ ⎥ ⎢ ⎥= = = ∈⎢ ⎥⎢ ⎥ ⎢ ⎥

x J J Jv J q q J J

J J JωR

LINEAR JACOBIAN

, ,,1 ,2 ,

g L i A iA A A n

nq

⎢ ⎥⎢ ⎥ ⎢ ⎥⎢ ⎥⎢ ⎥ ⎢ ⎥⎣ ⎦ ⎣ ⎦ ⎢ ⎥⎢ ⎥⎣ ⎦

J J JωANGULAR JACOBIAN

indicates how contributes to the linear velocity of the TCP,

dL i i

n n

q

∑ ∑

J

x,

1 1

dd i L i i

i ii

q qq= =

= =∑ ∑xx J

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

n n

q

∑ ∑

J


1, ,1 1

i i A i ii i

q−= =

= =∑ ∑Jω ω

Structure of the Jacobian

LINEAR JACOBIANLINEAR JACOBIAN

ANGULAR JACOBIAN


Structure of the Jacobian

If the robot is non‐redundant, the Jacobian matrix is square square

If th b t i d d t thIf the robot is redundant, the Jacobian matrix is rectangularrectangular



If the joint is prismatic

, 1L i i i iq d−=J k0ω

, 1L i i−=J kJ 0

1i−R

R1i−x

TCP1,i−r

If the joint is revolute

1,i i− = 0ω,A i=J 0

R

RTCP1i

xTCP

( ), 1, 1, 1 1,L i i i i i p i i p iq θ− − − −= × = ×J r k rω

0R

1, 1i i i iθ− −= kω

, 1 1,L i i i p− −= ×=

J k rJ k, 1A i i−=J k

All vectors are expresses in 0R


( )TCP TCP is the vector that represents in 0

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


In conclusions, in a geometrical Jacobian, its elements can be computed as follows:

, ,L i A iJ J

PrismaticRevolute

1

1 1 1

i

i i p i

−

− − −×k 0

k r k1 1, 1i i p i− − −


Analytical Jacobian – 1

Analytical Jacobian is computed deriving the expression of the TCP pose

1 1( ( )) ( ( ))

d ( ( ))p t p t

p qt

⎡ ⎤∂ ∂⎢ ⎥⎡ ⎤ ⎡ ⎤⎢ ⎥⎡ ⎤ ∂ ∂⎢ ⎥ ⎢ ⎥

q qx q1 11d ( ( ))d

d ( ( ))

np qt q q

t

⎢ ⎥⎡ ⎤ ∂ ∂⎢ ⎥ ⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥ ⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥ ⎢ ⎥⎢ ⎥= = =⎢ ⎥⎢ ⎥ ⎢ ⎥⎢ ⎥⎢ ⎥

x q

p

6 6 6

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

tp p t p t qt

⎢ ⎥ ⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥ ⎢ ⎥⎢ ⎥⎢ ⎥ ∂ ∂⎢ ⎥ ⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦⎣ ⎦ ⎣ ⎦⎢ ⎥∂ ∂

qq q

α

1 nq q

⎢ ⎥∂ ∂⎢ ⎥⎣ ⎦

( ( ))tJ q( ( ))atJ q


Analytical Jacobian – 2

The first three lines of the analytical Jacobian are equal to the same lines of the geometric JacobianThe last three lines are usually different from the same lines of the geometric JacobianThese can be computed only when the angle representation has been chosen: here we consider only Euler and RPY anglesA transformation matrix relates the analytical and geometric velocities, and the two Jacobians

( )=Tω α α( )

( ) ( )( )

⎡ ⎤⎢ ⎥= ⎢ ⎥I 0

J q J q


( ) ( )( )g a⎢ ⎥

⎢ ⎥⎣ ⎦J q J q

0 T α

Relations between Jacobians

Euler angles { }φ θ ψα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 0y z zθ θ θθ θ θ

⎡ ⎤−⎢ ⎥⎢ ⎥== ⎢ ⎥T α( ) cos sin cos 0

sin 0 1y 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 , g ),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

def


def1( )T T+ −=J J JJ

Inverse velocity KF – 2

If the initial joint vector q(0) is known, inverse velocity can j q( ) ybe 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

Continuous time Discrete time


Singularity – 1

A square matrix is invertible if

( )det ( ) 0t ≠J q( )det ( ) 0t ≠J q

( )When

( )det ( ) 0St =J q

a singularity exists at( )Stq

This is called a singular/singularity configuration


Singularity – 2

For an articulated/anthropomorphic robot threethree singularity conditions exist A. completely extended or folded armB. wrist center on the verticalC. wrist singularity

When joint coordinates approach singularity the joint velocities become very large for small cartesian velocities

1 1 1( )J J J

det1 1 1( )

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

J


Singularity – 3

A. Extended arm

The velocities span a dim‐2 space

The velocities span a dim‐1 space

(the plane) (the tangent line)i.e., singularitysingularity


i.e., singularitysingularity

Singularity – 4

B. Wrist center on the vertical

these velocities cannot beobtained with infinitesimalobtained with infinitesimal

joint rotations


Singularity – 5

C. Wrist singularity

Singularity conditionSingularity conditionTwo axes are aligned


Euler Angles (wrist) singularity

c c s c s c s s c c s sφ ψ φ θ ψ φ ψ φ θ ψ φ θ⎛ ⎞− − − ⎟⎜ ⎟⎜

Let us start from the symbolic matrix3 angles

s c c c s s s c c c c sφ ψ φ θ ψ φ ψ φ θ ψ φ θ

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

⎟⎜ ⎟⎜ ⎟+ − + −⎜ ⎟⎜ ⎟⎟⎜ ⎟⎜ s s c s cψ θ ψ θ θ⎟⎜ ⎟⎜⎝ ⎠

Observe that if 1 0cθ θ= ⇒ =

0⎛ ⎞we have

0

0

c c s s c s s c

s c c s c c s sφ ψ φ ψ φ ψ φ ψ

φ ψ φ ψ φ ψ φ ψ

⎛ ⎞− − − ⎟⎜ ⎟⎜ ⎟⎜ ⎟+ −⎜ ⎟⎜ ⎟cos( )φ ψ+1 angle only

0 0 1φ ψ φ ψ φ ψ φ ψ⎜ ⎟⎜ ⎟⎜ ⎟⎟⎜⎝ ⎠

cos( )φ ψ+

sin( )φ ψ+

only


( )φ ψ+

Singularity

In practice, when the joints are near a singularity configuration, the task space velocity may become excessively large, although the joints are not exactly in singularityNear singularity conditions it is not possible to follow a geometric path and at the same time a given velocity

f lprofile; it is necessary to reduce the cartesian velocity and follow the path, orto follow the velocity profile, but follow an approximated path

l h bIn exact singularity conditions, nothing can be done … so … avoid singularityavoid singularity


Conclusions

Kinematic functions can be computed once the DH conventions are used

Inverse position KF is complexInverse position KF is complex

Direct velocity KF has the problem of angular velocities: analytical vs geometric Jacobians

Inverse velocity can be computed apart from singularityInverse velocity can be computed apart from singularity points

A id i l itAvoid singularity


robotics 2011 04 kinematic functions - ladispe.polito.it

Documents