chapter4 lnffiSf fillffmUfilnl(nwnnW

1. lntroduction:

In chapter 3, we did "Direct Kinematics", i.e., given 0 e the joint

space, find the position and orientation of the end-effector in the
Cartesian space.

In this chapter we discuss:

INvERSp KNqPVTATICS:

Given position and orientation of the tool frame with respect to the
base frame, find the joint variables that achieve the specified
position and orientation of the end-effector.
2-3. SolvabiliS:

I. NOU-LINEAR NATIIRE OF THE PROBLEM

Examole:

Given (p,,p,)

find 1e, ,0, )

that satisff

p* = lr cos 0, + l, cos(0, + 0, )
p, = 1, sin o, + 1, sin(O, + o, )

lp,
tl l

LP, J
Solution:
*= (p. - I, coso,)t * (p, - I, sin orY =\'
3 p,' * p"' *lr' - 2p*lrcosg, - 2prlrsin0, = 1r2
2 2 12 12
+ p*cos0, +prsin0, =# =p (* *)
Two approaches to solving (* *)
Approach 1:

, , +Pv , cos0r -$sino,=

p"
+Pv
(* *) {P' {P. (^)
= P"
2
+pv '+pr'

p..

Pr

(A) + cos$cos 0, + sin0sin 0, : k

+ cob(0, -$) :k
+ sin(O, -$) : +,t14
/-\
e1 :0+atan2FJt-t"tJ
-
: atan2(0,,0,)* u tun 2FJi t' ,k) (2 Solutions)
Approaeh2t
t-tan'L 2tan \
Use cosO | -- 2 sin0, : 2 in (* *)
1+ tan' ! l+tao'9
2 2
ll. EnsrettcE oF Sorunorus
f Concept of workspace :
The workspace of a robot is a part of the cartesian space which can
be reached by the robot end-effector

f Two types of workspnce :

O Dextrous worlrspace:
can be reached by the end-effector with any orientations
O Reachable worlcspacei
can be reached by the end-effector in at least one orientation
Obviously, Dextrous workspace ! Reachable worlcspace

Example:
The 2 d.o.f. planar arm
Ir=lz l;Fl2

,t.,a
,.t,,' ...''. ...,.
..,/...," .,a .,a',,.,t'.,..t' ..
..r..' ,.1. ,,.
,..' .,, .. .,,, ...'.,,...'........' ..,/, /.' .,
,',,a ,'t "..' .',rt,,.,'
/,r. , ,,"' ...12
,,..,,..,..,..,.'..,..',....|.......,.,..|..,''. ,,.t,. .r,,.t.

/r';'.',.;t.';",;.r,,
,,lr'/.r',""' , ': ".i"..'
1,,
./ tt 'r\:r
t'
' , ,".r.'
i' ,,r" ,,/' a./n/ , "t .,"
/ l,+1, lrlz

Dextrous workspace Dextrous worlcspace

(One point) Empty
Reachable worlcspace Reachable workspace
(whole disc with v :2h Ring Disc

Conclusion:
If the position of the e.e. is required to be outside of the readable workspace,
no solution exist. If the position is inside dextrous space, there are infinitely
many solutions. If the position e reachable workspace, there at leasf one
solution
I I l. wt u nt p tE s o LUTtows

Examples:
1 PUMA 560: 8 solutions (Text Page P.118)
2. 3 d.o.f. planar robot : 2 solutions
A.
/\
/l
,/ ,/' B
,,,/ ,1,.,

,r"" ,""

,;-_'-_-' d! ,/

Choice of solutions: Based on solutions.

IV.Tvpe oF soLunows

* Numerical solutions (Iterative in nature + Slower)

Algebraic Method

t Closed-form solutions
- I Geometric Method

.i. An important fact on the existence of closed-form solutions

(Pieper, 1968)

For n:6, closed-form solutions exist if there are three neighboring

joint axes that intersect at a point
Example:

PUMA 560

Joints 4.5 .6 intersect at one point.

Assignment #4
Chapter 4: 1,2,3,9
3. Manipulator Subspace:

First we rename some of the frames as follows

{s}

The base frame {B} ( : {0})

The wrist frame {WX : {0})
The tool frame {T}
The station frame {S} ('4 task relevantframe)
The goal frame {G} ffhe location to wltich the robot is to move the
tool)

Deftnition:

The manipulator subspace is defined as the set o reachable wrist

ftll
frames (or reachable tool frames), where there arei n parameters that
can be assigned freely.

Remurk:
Workspace of an n d.o.f. manipulator is a subset of the manipulator
subspace.
Example 1:

The 3d.o.f. planar arm

The manipulator subspace may be defined as all possible wrist
frames with respect to the base frame, i.e., f,r, where all parameters
can be assigned freely.

Note flr=\r=ogff

vl -sl o 0 c2 -s2 0 l1 ca -s3 0 l2

s1 cr0 0 s2c20 0 s3ca0 0

0 01 0 001 0 001 0

0 1 1

c0 -sQ 0 x
sS c$ 0 v
0 0 1 0 (*)

Where

$:0,+0r+0,
x: I1c1*I2Cp
I The 3 variables (parameters ) $, x, y can be
assigned freely
y:
I

11s1*12S12
_J
Where

x, y give the position of the wrist;

$ describes the orientation of the terminal link.

As x, y, $ can take on arbitrary values, the subspace is generated.

Any wrist frame, which does not have the structure of ( * ), lies
outside the subspace ( lies outside of the workspace) of this robot.
=
Link lengths and joint limits restrict the workspace of the
manipulator to be subset of this subspace.
Example 2:

2 d.o.f, non-planar arm

Solution:

The manipulator subspace may be defined by the set of all possible

tool frames.
 11

cl -sl 0 0 100 0

slcl0 0 001 d2
o'p
001 trr= 0 -1
lr -- 0 ; 0 0

0 I

cl -sl 0 0 100 0

s1 c1 0 0 001 d2
orT=o,T.lT
= 0 0 1 0 0 -l 0 0

0 0 1 0 1

cl 0 -sl - Srd,
sl 0 cl crd,
0 -1 0 0

If we define

x=-Srdz , Y=Crdz

then

el- xx :' =1,L

dz= *t +y' : =--=--
d2 \m7' d2=rp4
,l*'*yt
So,

0 .__x
{x- + Y-
-x 0 -_ v v
Where x, y arefree Wamctcrs
ort (variables)
= x" +y' 11x- + y-
0 -1 0 0

0 0 0
 t2

4. Algebraic versus Geometric Approaches

I. Algehraic Apprn*eh:

Inverse kinematics of the 3 d.o.f. planar arn. Given specifications of

the wrist frame with respect to the base frame, i.e.,
{3} {w}
.o--
--- -o- {T}
,,/''/' \
c$ -s{ 0 x r4)
1.1
g
t'"'

s$cQ0 v i
Bm
wl = 001 0
/1\
(r, /

tB) i
l-i t0)
?h {11

Note that

9t2t - srz: 0 lrc, + lrc,,

srz: ctz: 0 l,s, + lrs,,
f,r=o.r=o,r\T'rT = 0 0 I 0 (2)

So the inverse kinematics problem is to find (01,02,0:) such that

(l) - (2)

Equating (l) and (2) gives

cw = c0 (3)
srzr = s0 (4)
X: l,c, +lrcr" (s)
Y: l,s, +lrs' (6)
 IJ

(s)'*(e)'=*'*y' = l,' + l12 + 2111.(c,c,, + s,s,, )

2
=l,r+1, + 2l,l"c.
Hence
w2-
2lJ2
-k : S: =t.7f-kt
0z : atan2(s1, c2)
=
l-\
= :atan2( (tut-tt.t J
(2 solutions for 02)

write (5), (6) as

'-
KI IYz

t-l -t
X: (1, +lrcrl, -1rsrs,
- = k,cr - hr, (7)
:
! lrsrc, (8)
-, + + lrc, F, = k:c, + k,s,
(1,

where
kr=lr+1.c,
k, = I's,

(7) , (8) = [t -k,l[.,-l:[:<.]_[.,[t,

-
-k,l-'[*l
Lkf uf lL',1 Ly:r- L',l|b ul I Lv.l

tI
t, l[*l
k,-il
l-k,
1

!_,,___:, _
k, ll v I
_
k,x + k.y I
,'KzX + ;-
@>- - KrYl
|

cr k,x + kry

9 0, = atan2(- krx + k,y,k,x + kry)

Note: as there are two possible k2, (why?), there are two solutions
for 0r.
To find 03, note 01+02+03 - 0 :atan2(s$, c0)
o: = &tan2(sg,cg)-(e, +er)
=
 1A
la

AtcneRAlc AppRoecFr:

Manipulating the given equations into a form for which a solution is

known.

For many common geometries , there are several forms of

transcendental equations for which solutions are known.

i. sin0 = a
=+ o=atanz("..Jr-*)

2. cos0 = b o = +atan z("t- u-

= u)

3. sinQ=a,cos$=b 0=atan2(a,b)
= ,,

4. acosO+bsin0=0 0=atan2(a,-b)
=
5. acos0+bsinO=c
=
a\
g=ata{b, a)tatan2(Ja: +b: -
"',")
6. The set of equations

acos0-bsin0 = c
asin0+bcss0=d
=) 0 = atan2(ad- bc, ac + M)
 i5

GsoMrrRlc AppnoncH:

Decompose the spatial geometry of the drrn p= into several plane

I ; I

geometry problems and solve the same example.

til
tJ I

/i ;.-fr
o" (.
Particularly when ai : 0 or
fl'0 o. It can be verv- simple
6:
_\,
 16

Recall the LAW OF COSINES

c2 =a2 +b2-2abcoscr

The figure at the top gives

12
*t +yt =ll *lrt -21,1, cos(l80" - o, )

l-
=lj + lr? + 21,1. eos6.

w2- lir tl= k(= s: = {l,-T

xt *y _:r,,-
- k' )
21112
 t/

Another solution: g2': -02 (Check : < l, + t.,) Out of reach?

To find 01, note B : atan 2(y , x)

and qr can be computed using the law of cosines as

x'+y'+l,t -lr'
cos r.y - -k
n,l7 4
r-.
:+ sinV=t{i-k'

= V = atan2(sr.y,cry) 0o<V<1800

= 0, = FtV = atan2(y,x)+atan2(sry,cry)

= 0,=9+,y if 0r.0

0,=F-,y if 0rt0

Finally

0: :0 - (0r+02)
 l8

4.5 ArcngRAlc sorutrox gy Rn,oucrtox to poLvxoMHI_

slnu-_2u
Let Ll = tall
0 l+ur (*)
-2
.^ 1- ut
COSU =
l+u2
(*) is useful geometric substitutionused often in solving Kinematic
equations. Using these substitutions, transcendental equation are
converted to polynomial equations in"Lt" .

Example:

Convert acos0+bsing = c io a polynomial

Solution:

2u
sln U = :------;
-A
Let t+u'
u=tanl
2 = l-u:
cos0 =
i+u2
u- j\
= acos0+bsin0 = u1: *b.
1+u' 1+u' =.
+ a-au'+2bu=e*cu:
) ^,
+ (a+c)u--2bu+(c-a)=0
= u- 61/6'-a'; U
tan- = 1
a 'r9 z.
/---:---:-----:-
t btVb'+a'-c'
2tan-'l
= 0=
L
a+c

NOte: ifa+ c=0 = A =2tan-lco : 1g0o

in computer implementation, this should be checked for ahead of

time.
 19

4.6 PtrprR's soLuttox lvgnN tHRe E rxES lNtrRspct

A general robot with six d.o.f. does not have a closed form solution
except for certain important special cases.
Pieper's solution to the inverse of the kinematics problem of a six
d.o.f. robot where all joints are rotational and three consecutive axes
intersect at one point. Pieper's work applies to the majority of
commercially available industrial robots.
Assume the last t at one
- The origin of frames 4. 5 and 6
We are given a desired position/orientation.

Ip, ]
il,nt
op I in lv.. l

tl
oT- BrI
^ 5oRG
\p,,
t -w --
.........t-
I

1Jl- 000
6

note that

lnp I [',p
'T-"1=L 'i-" I lrp^i-".]=',r"r
I [, r, .]
,.7
tr itli ^1oRG I
(1)
L 1=:.tL L^_t

Where

I
t-l
u. -l

tu" column of 't-'l t,T=l-d,to, (2)

['"t*"]=rnt
i

I d*ca. I
f
From the general formula '-f7-
 20

(1) & (2) +

a3 ll [r,
te,ll
,"{,{ -dosc',
It*t,tl
L'
ire,l; (3)

Lll []-] '-r'l,J Ltti''l

Where

f tre,l1 u, l
[-.oi-"' r a3ca +dosors, +a,
| " doco, l= | , 3cct2s3 - doscrcorc, - dos{rrcu, -U.orl
?l:'l l=:'l
lf,(e,)l
I I
I I la 3scr,2s3 - dosct,rscrrc, + docoreer. - ara"rl
L J L IL
fI

Contain only one unknown 03

cr8r - sr8z
oPoo*o
It' re' 11
srgl + c1g2
I l=qt.ltlfr(o,) (3')
I I | 'lf,(or)l| = 8r
lrj I

gr =czfr -srfr+a,
g, = srccr,f, + crcd.rfr- su,f, - drsc,
g)=srsorf, +crsc,f, +co,f, -drco, =(f,t, +frcrbo, *frco, +drcc,

(contain only two unknowns 0r and 0:)

 2l

Denote the square of the magnitude of o

Poo*o br "r" (known!) then

r = (crgr - )t + (s,g, + crgr)2 + gr'

s,g,

= cr'gr' + sr'gr' - 2crsrgrgr+ srtg,t + cr'gr' + 2srergrg, + gr'

t)7
=Er t9z +8:
-frsr)
'(tc, t-)
k3
4a
kr kt
= (k,c, + krsr)2a, + k, (4a)

p: = gr = (k,s, - krc, )sa, + ko L4h)

where

k, =f,
kz=-fz
k, = f,' +f| +tl +ai +dl+2d2f3
ko=frc0',+drcct
 Recall the useful substitution

t-tan'9 zt*9
2
coso= -----z sin0 = (5)
1+ tan'9 I +tan'9
2 2

Case I
If a, = s (e.g. PUMA 500, Yasukawa motormen etc.)
Then (4a) ::> r= kt (6)
.tl v
lkn"*r] Contains onlv one unknown 0. in s. & c" !

Solve (6) for 0: by using substitution (5)

Case 2

ifsctt = g
p"=k+= fzfar+drcar, (7)
\., I' bxwn

lk""*tl ns onlv on€ unknown &c'!

Solve (7) for Otby using (5)

 z3

Case 3
If a# 0 and scrl * 0 then (4a), (4b) +
[+l'*fo.--uo]=r;*r;
\rar /\sor )
(s)

which contains only one unknown 0:. Solve(S) for 0: by using (5).

Having solved for 0: we may solve (a) for fo and solve (3') for 01
(since gl, 92 depend on 02 and 0: only)

To find 0+ , 0s and 0e , note that f n is given and

in=!n.ln
+ ln=!n-'!n (9)
JJ
known Given known since NR depends on 01, 02, 0: only
Further notice that
j n=lnlnin

[tiltl.o -s4 0 li c5 -s5

0 lT c6 -s6 0
= soca, c4ccr3 - ro, s5ccr4 osccr4 - t", s6ccr,5 c6ccr,5 - sOs
| ll ll
[srsa, c4so3 ca, Jlsrso4 csscr,4 cau J[susu5 c6scr,5 c0s
c4c5c6 - s4s5cc[3cct4

- c4s5s6cc[5 - s4c5s6cc[4cc[5

+ s4s6sc[,4scfs

For many 6 d.o.f. robots , cri: 0 or *90o

For example, PUMA 560 , Cl3: -90o , d4:90o, cr5: -90o (Page
9I af text)
(e)
c6 - s4s6 - c4c5s6 - s+cO - c+SS .l rrr \z tt,
lR= - ssSo c5
: oR{ I
-' v,, v,.l
ln = l| Z,
I

( l0)
- |

Lt t 7r, Trr l
 24

Recall the Z-Y-ZEuler angles

[c*.cB.cy - s*.sy - ro.flP- ro.r, ccr.sFl

tR*(o,F,y) = | so.cB.w+ccr,sy -su.cB.sy+ocr,.cy so.sF (11)

| - rF ry sp.sy oFl
I

- s+so - c4c5s6 - s+cO crs,-lI

[crcrcu
socrcu +c4c6 - s4o5s6 + c4s6 tot'I
= |
L - ssco Ssso c:l
-,r, ftz - -rl
T
rtz t'
11
| fttr
=l-4'-Vr, 7r, = l'rt rzz 'u I (r2)
I

L-vrt -vn vr, ) Lrz, ltz \t I

I

Comparing (12) with (11) we see that (0a, 0s, 0e) e the z-y-z
Euler's angles and therefore, the solution to the inverse Euler's
angles problem can be used to find 0+, 0s, 96 immediately.
That is:
t_ \
0, =F =atanZ(/v3, +y3r.yu)

o, = cr = atan 2fb.frr)
' (.sF sF/

o. =y =atanz[rt.-r")
IsF
sB/

[See Page 49-50 (274)]

Assignment # 5
Chapter 4. 4.6, 4.8, 4.IA
 25

Example 2: IxvsnsE KINEMATIC oF MICRoBoT:

The microbot is a 3 d.o.f non-planar ann as shown inthe figure. The

D-H parameters can be obtained by assigning a set of frames as in
the figure and then using the values of the
D-H convention.

*,
l?/
r

D-H TABLE
 26

cl -s1 0 0 0 0
Q2 -s2
st c1 0 0 0 0 -1 0
0m
tl = 0 0 1 h tT= s2 v2 0 0

0 0

ca *s3 0 e f
s3 ca 0 0 0
2ry
-rr = 0 0 I 0
1T= :
000 1

tr - {1
tf
ecro2 + ft,o,
r
ctczz ctSz:
Srcz: - SiSz: - ttr es1o2 + fs,e*
0m trT.iT.3-T
4l= Sz: czt 0 h+es, +fs,

*g
In the inverse kinematics , specifications (p", pv, p,) in

7tt ttz ttz P*

Tzt fzz fzl py
O7_
Af
-
':.'. ':.'.':.'.
:.".
are grven
f r/q^
0 00 I

F,
-1 \-. CJ
,
0,,O-,63 = 7
 27

Solution:

Note that the robot_has only 3 degree of freedom thus only gripper's
position [p" p, p"]r can be specified

P*=ecrc:+fc,cr, (1)
Pr=esrcr+fsrc* (2)
p, =h+esr +fs* (3)

(1) & (2) + SiP" = CtPv

sr Pn
- -=;
cr P"
+ At = &tan2(pr.p" )

Another solution : 61 =0, +180"

To find 030 note

+
pr - l. r ?J*tf Lf rel
Q)2+e)2+(4)2
p"t f pr'*(p"-h)' =e2c12c22 +f'cr'arr'+2efer2erer.+e'sr'er'+f'sr'err' +2efsrzcrc"r+
e'sr'+f"rr' +2efsrs'
=e2 +f2 +2ef(ercr, +srsr.)
= e2 +f2 +2efc,
p,' I p,' t(p, - h)'-(tI' -{tr
%=T =l(
? 0, = atan2(L/t - kt , k;
(3) p, -h=es2 +fs2c, +fcrs,
=
- (e+ fcr)s, + fsrc,
= krsz +krc,
Whefe kr =e+fc.,k, =fs, (known)
then
oz=dtarr2(kt,k) +atan r(W,p, -h')
Inverse Problem:

Given a rotation matrix !n,findthe equivalent roll, pitch yaw

angles, i.e., given

Tn Y'.1
[v'' yzz
fR =lT, rr, I Solve !n=!n,,r(y,P,o) for u, B, y
fvrr Tzz Yr.-]

Solution i
y^
ify,, * 0, then CB *0 and ranc =#
./'cp
(vrr/ ) ,
r 6s = run-,| f9!l= ,rr'unz(b.Lr)
\cB cB/
[t2tBJ
[See footnote on page 47 of the text book for ATanZ function, i.€.,
ATanZ(2 45o , Atan2(-2, -2) :225o1
,2):
^ti
'"n3_
run}= \'l' t*l
and . rl,.u3 y =ltanz(!:z
(cp'cpl

li?;;; = cF, ranB = -=+-

Tn *Tzt
i , ,\
{Yrr-+Y"- )
z: Tzt: 0, i.e. CF : 0 then F : t90o and

+ Ca.Sy - Sa.Cy + Cg.Cy + So.Svl

+ Su.Sy + Ccr.Cy + Sa.CT - Ca.Sy I

001
guish two cases as follows
Case L:

If Y1 : -1 then F : 90o then

(Yaw angle :0)
I o sin(y - a) cos(y - c) I /
tR"",=| o_ cos(y-a) -sin(y-n)i ChCIoseer-0o
L-l 0 0 J

and find y : ATanZ(yn, yzz)

Thus we reach a set of solution cr:0o, F:90o, y = ATan}(yn,yzz)

Case 2:

If yrt - 1, then F : -90o then

(Yaw angle - 0)
[o - sin(y + o) - cos(y * "l /:
tn"", =l 9 cos(y+a) -sin(y*") lChoose o 0o
Lr00j
and find y : ATan2(yn, yzz)

Thus we have d : 0o, F - -90o, T : -ATanZ(Tn, Tzz)