Forward Kinematics

“Finding the end effector given the joint angles”

Types of robot joints

• Rotary– Angle θ about z axis

• Prismatic/sliding– Linear displacement d along axis of joint z

• Hooke (2-D Rotary)– 2-D of freedom described by Yaw and Pitch

• Spherical– Described by 3 rotation axes (wrist & shoulder)

Robot configurations

• Cartesian(inspection)– 3 Prismatic joints

• SCARA (pick and place)– Min 3 axes (Z axis and 2+ rotary joints)

• Spherical Wrist– Most common type 6-D freedom – hooke shoulder, rotary elbow and roll-pitch-roll wrist

• …

Setting up the coordinate system:Denavit-Hartenberg Coordinates

• Key basis for DH coordinates– There is only one normal between two lines in

space– Except: Parallel lines and intersecting lines

(common normal has zero length)

– Suppose there are two joint axes zi and zi-1

ii

iiin zz

zz

××=

−

−

1

1

• DH coordinates are numbered relative to the base 0

• Joint i connects link i-1 to link i

• The intersection of ni with zi defines the origin of the link

• We take zi to be parallel to the ni

• The coordinate system for link i is at the distal end

• The coordinate system at joint i is the i-1 system

∀ αI is the skew angle from zi-1 to zi measured about xi

• di is the distance from xi-1 to xi measured along zi-1

∀ θi is the angle from xi-1 to xi measured about zi-1

xi-2

zi-1

xi-1 xi

zi-2

zi

Joint i

Link i

Link i-1

ai

zi

zi-1

xi, ni

Parallel or intersecting axes

• When neighbouring axes intersect ai =0 ⇒ni=0

– Arbitrary choice xi ni or –ni

• For Parallel axes– Chose xi that intersects xi-1 at Oi-1 (prefered)

– Chose xi that intersects xi+1 at Oi+1

• In these cases DH parameters are very sensitive to small changes in alignment – An alternative is the Hayati coordinates

Examples

• 2- Planar Manipulator– Axes at distal ends– Avoid transformation

to find endpoint

– Make x2 the approach vector

θ2

θ1

a1

a2

O2

O1

O0

x1

x0

x2

y1

y2

y0

SCARA

• Note for joint 3 displacement is the joint variable and θ is constant

• Location of z2 is arbtrary

• Location of O3 and x3 are arbitrary, however O3 determines d3

• For simplicity frame 4 is placed at the gripper and z2 , z3 and z4 coincident

z0

x0

z1

x1x2

z2

x3

x4

z3 z4

i ai di αi θi

1 a1 0 0 θ1

2 a2 0 π θ2

3 0 d3 0 04 0 d3 0 θ4

Forward Kinematics

• Link Coordinate Transform– From link i to link i-1of the 2-d planar robot

• Rotate about the zi-1 axis by θi , then about the xi axis by αi.

( ) ( )

−

−=

−

−==−

ii

iiiii

iiiii

ii

iiii

ii

ixzii

cs

scccs

sscsc

cs

sccs

sc

RRR

αααθαθθ

αθαθθ

ααααθθ

θθαθ

00

0

001

100

0

01

Forward Kinematics• Translational component referenced to axes i-1

• Homogeneous transform i-1Ti from frame i to frame i-1

=

+

=+=+= −

−−−

−−

−−

i

ii

ii

i

i

iiiii

iii

iii

iii

iiii

i

d

sa

ca

s

c

adRadad θθ

θθ

01

0

01

111

11

,11 xzxzd

−

−

=

= −

−−−

1000

010,1

111

iii

iiiiiii

iiiiiii

T

iii

ii

ii

dcs

sascccs

casscsc

dRT

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

Relating any two link frames

• By composition of intervening frames we can link any two frames

• For efficiency we normally decompose the frames into separate Rotations and Translation components

ji 11

22

11 <= −

−−

++

+ jj

jj

ii

ii

ji TTTTT

Forward Kinematic computations

• Aim: find the position and orientation of the last frame, n, wrt the base frame 0

• Sometimes additional frames are added at the beginning or end– Vision systems– Tools or object which are picked up

• Once an object is grasped the its kinematics are constant and for practical purposes can be considered part of the last link.

Kinematic Calibration

• Set during Manufacture• Wear, error, etc… • Kinematic callibration schemes

– Measurement– Experimental

• Tool Transform– Most objects geometric and known– Vision system

Inverse Kinmatics (IK)

“Given a goal position find the joint angles for the robot arm”

Inverse Kinematics

• IK generally harder than FK

• Sometimes no analytical solution

• Sometimes multiple solutions– Redundant manipulators

• Sometimes no solution– Outside workspace

• 2-D planar manipulator (again)• Solve for θ2

• Two solutions: elbow up & elbow down

θ2

θ1

a1

a2

O2

O1

O0

x1

x0

x2

y1

y2

y0

φ

ψ

(x,y)

θ2

( ) ( )( ) ( )

( ) ( )( ) ( )22

221

22

22222

211

2

222

21

22

22222

21

22

21

2221

22

21

222122

21

22

21

22

2

22122

21

22

tan2

2

2

cos1

cos1

2tan

accuracygreater for

2cos

)cos(2

aayx

yxaa

aayx

yxaa

aayxaa

aayxaa

aa

aayx

aaaayx

−−++−+±=

−−++−+=

−−++++−−=

+−=

−−+=

−−+=+

−θ

θθθ

θ

θπ

• Given θ2 find θ1

• Note that there are two answers for θ1 based on elbow up or down

• 3-DOF of freedom robot arms– Most robots are made of two interconnected 3-DOF

arms• Elbow joint (position wrist in space)

• Wrist joint (orient object/tool in space)

)cos,sin(2tan

),(2tan

22122

1

θθψφ

ψφθ

aaaa

xya

+==

−=

Workspaces

Workspace limitations depend on•Joint limits•Presence of obstacles

Holes: doughnut shaped WSVoids: empty space in WS

Total or Reachable WorkspacePrimary WS: points reachable in all OrientationsSecondary WS: total -primary

Trajectory Planning

Path Planning

Trajectory planning

• Start to Goal avoiding obstacles along the way• Joint space easiest because no IK

– But end effector pose is not controlled

• Cartesian space planning is easier but IK must be solved

Joint Space Trajectories

• Cubic Trajectories– 4 coefficients– Satisfy position and velocity constraits

– For a joint variable qi

( ) ( )( ) ( )

velocityandposition final theare and

velocityandposition initial theare and

timeend theis start time theis

11

00

0

100

100

qq

qq

tt

qtqqtq

qtqqtq

f

fii

fii

′′

′=′=

==

• Polynomials for joint position and velocity

• The ai coeff. have to be related to the end point constraits

( )( ) 2

321

33

2210

32 tataatq

tatataatq

i

i

++=

+++=

( )( )( ) 1

2321

13

32

210

01000

32

0 ,)0( ,0

qtataatq

qtatataatq

qaqqaqt

fffi

ffffi

ii

′=++=

=+++=

′=====

• This allows us to determine a2 and a3

• Example

( ) ( )

( ) ( )3

10103

2

10012

2

23

f

f

f

f

t

tqqqqa

t

tqqqqa

′+′+−=

′+′−−=

60 ,90 ,0 ,10

1 deg/sec, 0 ,20 ,10

3210

1010

=−===⇒

==′=′−==

aaaa

tqqqq foo

0 100-20

10

Linear segments with parabolic bends

• We want the middle part of the trajectory to have a constant velocity V– Ramp up– Linear segment – Ramp down

Ramp Up

• A quadratic requires 3 constraits – 2 for the start and 1 for const velocity at the end

( )( )

( )( ) tatq

taqtq

aqaqa

taatq

tataatq

i

i

i

i

2

220

20100

21

2210

2

stagenext at found , ,

0 velocity and 0with t

2

=+=

====

+=++=

Linear Section

• Given constant velocity V, which we ramp up to in a unknown time tb, we can find a2 in terms of tb

( )

( )

V

Vtqqt

VtVtqq

tt

Vq

Vtqqt

ttttVtttq

t

VaVtatq

fb

bf

bb

ff

bfbi

bbi

+−=

+−+

=+

+=+=

−≤≤+=+=

===

10

1020b

010

i

010

22

22 tat time

2

22q :symmeteryby

:segmentLinear

2 ,2 :up ramp of end From

α

ααα

• Similarly the end position can be show to be

( )

≤≤+++

≤≤+=⇒==

t-tt t2

tt0 2

1

Let

bfb10

b2

0

VtVtqq

atqtq

t

Vqa

fi

bi

( )( ) ( ) 0 and :where

22

1

221

==

−+−=

fifi

ffi

tqqtq

ta

tatta

qtq