4 4 9

I E E E TRANSACTI ONS ON SYSTE MS, MAN, AND CYBE RNE TI CS, VOL. SMC-1 1, NO. 6 , JUNE 19 81
a s x k x i a n d y k y " Th e r e f o r e , a s k -, o o ( i .e ., r k 0 ) ,
r
k
k g ( n
x k )
h ( yk ) )
->
,
t h a t i s ,
Pr k ( k ) k k f
Pn ( n
x
) n ( Yn s * )
w h i c h i m p l i e s t h e e x i s t e n c e o f a p o s i t i v e i n t e g e r K 2 s u c h t h a t
Pn ( Yn
,x
) f n ( Yn ,j ) I < ,E C f o r a l l k > K 2 ( 2 5 )
f o r e i n ( 2 3 ) . Be s i d e s , f r o m t h e c o n t i n u i t y o f
f ,
a t a n y (
y ,n x ) ,
w e
h a v e t h e e x i s t e n c e o f a p o s i t i v e i n t e g e r K 3 s u c h t h a t
( f i x ) _g ( y r k ( ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ X ' ) , X k ) ) I < E l f o r a l l k K
( 2 6 )
Se t K =
m a x ( K I ,
K 2 , K 3 ) . Th e n , u s i n g ( 2 5 ) , ( 2 3 ) , a n d ( 2 6 ) , i n
t u r n , w e h a v e t h e f o l l o w i n g r e l a t i o n s f o r a l l k > K .
p k n r
( n k
X k ) < f n ( y n t j )
+
=f n ( n
x E <
f n
(
( x k
x
( 2 7 )
Si n c e
4 > 0 ,
f ( Yn (
X k
x ) < X P ( k ( x k ) ,x ) ( 2 8)
E q u a t i o n s ( 2 7 ) a n d ( 2 8) y i e l d
p r k ( Yk
k
) p r k ( n r k ( X k ) X )
f o r a l l k > K .
Th i s r e l a t i o n a n d ( 2 4 ) c o n t r a d i c t t h a t y n r k ( x k )
i s o p t i m a l f o r ( 3 ) i n
r e s p o n s e t o x k a n d r k . Th e r e f o r e , a n y a c c u m u l a t i o n p o i n t i s
o p t i m a l f o r ( 4 ) w i t h i .
E x i s t e n c e a n d Op t i m a l i t y o f t h e Li m i t Po i n t : Si n c e t h e o p t i m a l
s o l u t i o n y n ( i )
t o ( 4 ) w i t h i i s u n i q u e u n d e r t h e a s s u m p t i o n d ) ,
t h e a c c u m u l a t i o n p o i n t o f y n r k ( x k ) }
i s a l s o u n i q u e . Th e r e f o r e ,
t h e a c c u m u l a t i o n p o i n t b e c o m e s a l i m i t p o i n t o f { n r ( x k ) } ) .
Th u s ,
w e c a n c o n c l u d e t h a t Yn ( x k ) c o n v e r g e s t o Yn ( x ) .
RE FE RE NCE S
[ I ] J. Ab a d i e a n d J. Ca r p e n t e r , " G e n e r a l i z a t i o n o f t h e W o l f e r e d u c e d g r a d i -
e n t m e t h o d t o t h e c a s e o f n o n l i n e a r c o n s t r a i n t s ," i n Op t i m i z a t i o n ,
R. Fl e t c h e r , E d . Ne w Yo r k : Ac a d e m i c , 19 6 9 .
[ 2 ] M. Av r i e l , No n l i n e a r Pr o g r a m m i n g . E n g l e w o o d Cl i f f s , NJ: Pr e n t i c e -
H a l l , 19 7 6 .
[ 3 ] A. V. Fi a c c o a n d G . P. Mc Co r m i k , No n l i n e a r Pr o g r a m m i n g -Se q u e n t i a l
Un c o n s t r a i n e d Mi n i m i z a t i o n Te c h n i q u e s . Ne w Yo r k : W i l e y , 19 6 8.
[ 4 ] A. V. Fi a c c o , " Se n s i t i v i t y a n a l y s i s f o r n o n l i n e a r p r o g r a m m i n g u s i n g
p e n a l t y m e t h o d s ," Ma t h e m a t i c a l Pr o g r a m m i n g , v o l . 10 , p p . 2 87 -3 11,
19 7 6 .
[ 5 ] A. M. G e o f f r i o n a n d W . W . H o g a n , " Co o r d i n a t i o n o f t w o -l e v e l o r g a n i z a -
t i o n w i t h m u l t i p l e o b j e c t i v e s ," i n Te c h n i q u e s o f Op t i m i z a t i o n , A. V.
Ba l a k r i s h n a n , E d . Ne w Yo r k : Ac a d e m i c , 19 7 2 .
[ 6 ] W . W . H o g a n , " Po i n t -t o -s e t m a p s i n m a t h e m a t i c a l p r o g r a m m i i n g ," SI AM
Re v ., v o l . 15 , p p . 5 9 1-6 0 3 , 19 7 3 .
[ 7 ] F. H . Mu r p h y , " A c l a s s o f e x p o n e n t i a l p e n a l t y f u n c t i o n s ," SI AM J.
Co n t r ., v o l . 12 , p p . 6 7 9 -6 87 , 19 7 4 .
[ 8] M. J. D. Po w e l l , " Al g o r i t h m s f o r n o n l i n e a r c o n s t r a i n t s t h a t u s e
La g r a n g i a n f u n c t i o n s ," Ma t h e m a t i c a l Pr o g r a m m i n g , v o l . 4 , p p . 2 2 4 -2 4 8,
19 7 8.
[ 9 ] K . Sh i m i z u , E . Ai y o s h i , a n d T. Ue n o , " De c e n t r a l i z e d o p t i m i z a t i o n s y s -
t e m s a n d t h e i r a p p l i c a t i o n t o a c l a s s o f t r a n s p o r t a t i o n p r o b l e m s ," Tr a n s .
So c . I n s t r u m e n t , Co n t r . E n g ., v o l . 13 , p p . 5 6 1-5 6 8, 19 7 7 ( i n Ja p a n e s e ) .
[ 10 ] K . Sh i m i z u a n d Y. An z a i , " Op t i m i z a t i o n f o r a h i e r a r c h i c a l s y s t e m w i t h
i n d e p e n d e n t l o c a l o b j e c t i v e s ," Tr a n s . So c . I n s t r u m e n t Co n t r . E n g ., v o l .
10 , p p . 6 3 -7 0 , 19 7 4 ( i n Ja p a n e s e ) .
[ I 1] G . Z o u t e n d i j k , Me t h o d s o f Fe a s i b l e Di r e c t i o n s . Am s t e r d a m : E l s e v i e r ,
19 6 0 .
K i n e m a t i c Co n t r o l E q u a t i o n s f o r Si m p l e Ma n i p u l a t o r s
RI CH ARD P. PAUL, SE NI OR ME MBE R, I E E E , BRUCE SH I MANO,
AND G ORDON E . MAYE R
Ab s t r a c t -Th e b a s i s f o r a l l a d v a n c e d m a n i p u l a t o r c o n t r o l i s a r e l a t i o n -
s h i p b e t w e e n t h e Ca r t e s i a n c o o r d i n a t e s o f t h e e n d -e f f e c t o r a n d t h e m a n i p u -
l a t o r j o i n t c o o r d i n a t e s . A d i r e c t m e t h o d f o r a s s i g n i n g l i n k c o o r d i n a t e
s y s t e m s a n d o b t a i n i n g t h e e n d -e f f e c t o r p o s i t i o n i n t e r m s o f j o i n t c o o r d i -
n a t e s i s r e v i e w e d . Te c h n i q u e s f o r o b t a i n i n g t h e s o l u t i o n t o t h e s e e q u a t i o n s
f o r k i n e m a t i c a l l y s i m p l e m a n i p u l a t o r s , w h i c h i n c l u d e s a l l c o m m e r c i a l l y
a v a i l a b l e m a n i p u l a t o r s , a r e p r e s e n t e d .
I NTRODUCTI ON
A s e r i a l l i n k m a n i p u l a t o r c o n s i s t s o f a s e q u e n c e o f m e c h a n i c a l
l i n k s c o n n e c t e d t o g e t h e r b y a c t u a t e d j o i n t s . Su c h a s t r u c t u r e
f o r m s a k i n e m a t i c c h a i n a n d m a y b e a n a l y z e d b y m e t h o d s
d e v e l o p e d b y De n a v i t a n d H a r t e n b e r g [ 10 ] . Th e r e s u l t s o f t h i s
a n a l y s i s a r e t h e m a t r i x e q u a t i o n s e x p r e s s i n g m a n i p u l a t o r e n d -
e f f e c t o r Ca r t e s i a n p o s i t i o n a n d o r i e n t a t i o n i n t e r m s o f t h e j o i n t
c o o r d i n a t e s . Th e s e e q u a t i o n s m a y b e o b t a i n e d f o r a n y m a n i p u l a -
t o r i n d e p e n d e n t o f t h e n u m b e r o f l i n k s o r d e g r e e s o f f r e e d o m .
I n t h i s c o r r e s p o n d e n c e w e f i r s t r e v i e w t h e m e t h o d o f o b t a i n i n g
t h e s e e q u a t i o n s e x t e n d i n g t h e p r o c e d u r e o f a s s i g n i n g c o o r d i n a t e
f r a m e s t o i n c l u d e s i m p l e m a n i p u l a t o r s w h i c h h a v e m a n y z e r o
l e n g t h l i n k s a n d i n t e r s e c t i n g j o i n t a x e s . W h i l e w e m a y o b t a i n
t h e s e k i n e m a t i c e q u a t i o n s f o r a n y m a n i p u l a t o r i t i s t h e i r s o l u t i o n
w h i c h i s o f i n t e r e s t . G i v e n a d e s i r e d Ca r t e s i a n p o s i t i o n a n d
o r i e n t a t i o n o f t h e m a n i p u l a t o r ' s e n d -e f f e c t o r w h a t a r e t h e n e c e s -
s a r y j o i n t c o o r d i n a t e s ? W h i l e t h e r e i s o n l y o n e e n d -e f f e c t o r
p o s i t i o n c o r r e s p o n d i n g t o a g i v e n s e t o f j o i n t c o o r d i n a t e s , t h e r e
a r e a n u m b e r o f c o n f i g u r a t i o n s o f t h e m a n i p u l a t o r ' s l i n k s a l l o f
w h i c h p l a c e t h e e n d -e f f e c t o r i n t h e s a m e p o s i t i o n a n d o r i e n t a t i o n .
No r m a l l y o n l y o n e s o l u t i o n c o r r e s p o n d i n g t o a -g i v e n k i n e m a t i c
c o n f i g u r a t i o n i s d e s i r e d ( e .g ., e l b o w u p o r d o w n , e t c .) , r a t h e r t h a n
t h e e n t i r e s e t o f s o l u t i o n s . Fr e q u e n t l y t h e s o l u t i o n i s t o b e
e m b e d d e d i n a r e a l -t i m e s e r v o l o o p a n d o n l y a v e r y m i n i m u m
n u m b e r o f m a t h e m a t i c a l o p e r a t i o n s m a y b e p e r f o r m e d .
W h e n t h e m a n i p u l a t o r g e o m e t r y i s s i m p l e a n d w e l l u n d e r s t o o d
a t r i g o n o m e t r i c s o l u t i o n m a y o f t e n b e o b t a i n e d
[ I ] -[ 3 ] ,
[ 8] , [ 9 ] .
H o w e v e r , s i x -d e g r e e -o f -f r e e d o m m a n i p u l a t o r s a r e s u f f i c i e n t l y
c o m p l e x t h a t t h e d i r e c t t r i g o n o m e t r i c m e t h o d i s t o o d i f f i c u l t t o
a p p l y . W e p r e s e n t a m e t h o d o f o b t a i n i n g a s o l u t i o n t o t h e
k i n e m a t i c e q u a t i o n s b a s e d o n t h e H a r t e n b e r g -De n a v i t m a t r i c e s
f r o m w h i c h t h e s o l u t i o n i s o b t a i n e d e x p l i c i t l y i n t h e c a s e o f
s i m p l e m a n i p u l a t o r s . Th e e x i s t e n c e o f a n e x p l i c i t s o l u t i o n t o t h e
k i n e m a t i c e q u a t i o n s f o r a n y m a n i p u l a t o r i s o f g r e a t i m p o r t a n c e
i n e v a l u a t i n g t h e m a n i p u l a t o r ' s s u i t a b i l i t y f o r c o m p u t e r c o n t r o l .
I t e r a t i v e s o l u t i o n t e c h n i q u e s c a n i n v o l v e a n o r d e r o f m a g n i t u d e
a n d m o r e c o m p u t a t i o n t h a n a n e x p l i c i t s o l u t i o n . Pi e p e r [ 5 ] i n h i s
t h e s i s c o n s i d e r s a s e r i e s o f s i m p l e m a n i p u l a t o r s f o r w h i c h a
c l o s e d -f o r m s o l u t i o n i s o b t a i n a b l e . I t i s t o t h e s e " s i m p l e " m a n i -
p u l a t o r s t h a t t h e s o l u t i o n m e t h o d p r e s e n t e d i n t h i s c o r r e s p o n -
d e n c e i s a p p l i c a b l e . W e h a v e s o l v e d t h e k i n e m a t i c e q u a t i o n s f o r
a l l c o m m e r c i a l l y a v a i l a b l e m a n i p u l a t o r s a n d f i n d t h a t t h e e q u a -
t i o n s c a n b e r e a d i l y o b t a i n e d i n a m a t t e r o f h o u r s .
Ma n u s c r i p t r e c e i v e d Ma r c h 15 , 19 7 9 ; r e v i s e d Ju l y 2 3 , 19 7 9 ; Ma r c h 3 0 , 19 81.
Th i s m a t e r i a l i s b a s e d u p o n r e s e a r c h s u p p o r t e d b y t h e Na t i o n a l Sc i e n c e
Fo u n d a t i o n u n d e r G r a n t s APR7 7 -14 5 3 3 , APR7 5 -13 0 7 4 , a n d APR7 4 -0 13 9 0 .
An y o p i n i o n s , f i n d i n g s , a n d c o n c l u s i o n s o r r e c o m m e n d a t i o n s e x p r e s s e d i n t h i s
p u b l i c a t i o n a r e t h o s e o f t h e a u t h o r s a n d d o n o t n e c e s s a r i l y r e f l e c t t h e v i e w s o f
t h e Na t i o n a l Sc i e n c e Fo u n d a t i o n .
R. P. Pa u l i s w i t h t h e Ad v a n c e d Te c h n o l o g y La b o r a t o r y , G TE La b o r a t o r i e s
I n c ., 4 0 Sy l v a n Ro a d , W a l t h a m , MA 0 2 2 5 4 , o n l e a v e f r o m t h e De p a r t m e n t o f
E l e c t r i c a l E n g i n e e r i n g , Pu r d u e Un i v e r s i t y , W e s t La f a y e t t e , I N 4 7 9 0 5 .
B. Sh i m a n o i s w i t h t h e W e s t Co a s t Di v i s i o n o f Un i m a t i o n I n c ., 5 84 1A
Up l a n d e r W a y , Cu l v e r Ci t y , CA 9 0 2 3 0 .
G . E . Ma y e r i s w i t h W r i g h t -Pa t t e r s o n AFB, AFW AL/MLTC, OH 4 5 4 3 3 .
0 0 18-9 4 7 2 /81/0 6 0 0 -0 4 4 9 $ 0 0 .7 5
19 81 I E E E
I E E E TRANSACTI ONS ON SYSTE MS, MAN, AND CYBE RNE TI CS, VOL. SMC-I 1, NO. 6 , JUNE 19 81
COORDI NATE FRAm E s
A s e r i a l l i n k m a n i p u l a t o r c o n s i s t s o f a s e q u e n c e o f l i n k s
c o n n e c t e d t o g e t h e r b y a c t u a t e d j o i n t s . Fo r a n n -d e g r e e -o f -f r e e d o m
m a n i p u l a t o r , t h e r e w i l l b e n l i n k s a n d n j o i n t s . Th e b a s e o f t h e
m a n i p u l a t o r i s l i n k 0 a n d i s n o t c o n s i d e r e d o n e o f t h e s i x l i n k s .
Li n k 1 i s c o n n e c t e d t o t h e b a s e l i n k b y j o i n t 1. Th e r e i s n o j o i n t
a t t h e e n d o f t h e f i n a l l i n k . Th e o n l y s i g n i f i c a n c e o f l i n k s i s t h a t
t h e y m a i n t a i n a f i x e d r e l a t i o n s h i p b e t w e e n t h e m a n i p u l a t o r j o i n t s
a t e a c h e n d o f t h e l i n k ( 7 ) . An y l i n k c a n b e c h a r a c t e r i z e d b y t w o
d i m e n s i o n s : t h e c o m m o n n o r m a l d i s t a n c e a , a n d
a n
t h e a n g l e
b e t w e e n t h e a x e s i n a p l a n e p e r p e n d i c u l a r t o
a n .
I t i s c u s t o m a r y
t o c a l l
a n
" t h e l e n g t h " a n d
a ,n
" t h e t w i s t " o f t h e l i n k ( s e e Fi g . 1) .
G e n e r a l l y , t w o l i n k s a r e c o n n e c t e d a t e a c h j o i n t a x i s ( s e e Fi g . 2 ) .
Th e a x i s w i l l h a v e t w o n o r m a l s c o n n e c t e d t o i t , o n e f o r e a c h l i n k .
Th e r e l a t i v e p o s i t i o n o f t w o s u c h c o n n e c t e d l i n k s i s g i v e n b y d ,,
t h e d i s t a n c e b e t w e e n t h e n o r m a l s a l o n g t h e j o i n t n a x i s , a n d
0 On
t h e a n g l e b e t w e e n t h e n o r m a l s m e a s u r e d i n a p l a n e n o r m a l t o t h e
a x i s .
d n
a n d
0 ,n
a r e c a l l e d ' t h e d i s t a n c e " a n d " t h e a n g l e " b e t w e e n
t h e l i n k s , r e s p e c t i v e l y .
I n o r d e r t o d e s c r i b e t h e r e l a t i o n s h i p b e t w e e n l i n k s , w e w i l l
a s s i g n c o o r d i n a t e f r a m e s t o e a c h l i n k . W e w i l l f i r s t c o n s i d e r
r e v o l u t e j o i n t s i n w h i c h 0 ,, i s t h e j o i n t v a r i a b l e . Th e o r i g i n o f t h e
c o o r d i n a t e f r a m e o f l i n k n i s s e t t o b e a t t h e i n t e r s e c t i o n o f t h e
c o m m o n n o r m a l b e t w e e n j o i n t s n a n d n + 1 a n d t h e a x i s o f j o i n t
n + 1. I n t h e c a s e o f i n t e r s e c t i n g j o i n t a x e s , t h e o r i g i n i s a t t h e
p o i n t o f i n t e r s e c t i o n o f t h e j o i n t a x e s . I f t h e a x e s a r e p a r a l l e l , t h e
o r i g i n i s c h o s e n t o m a k e t h e j o i n t d i s t a n c e z e r o f o r t h e n e x t l i n k
w h o s e c o o r d i n a t e o r i g i n i s d e f i n e d . Th e z a x i s f o r l i n k n s h a l l b e
a l i g n e d w i t h t h e a x i s o f j o i n t n + 1. Th e x a x i s w i l l b e a l i g n e d
w i t h a n y c o m m o n n o r m a l w h i c h e x i s t s a n d i s d i r e c t e d a l o n g t h e
n o r m a l f r o m j o i n t n t o j o i n t n + 1. I n t h e c a s e o f i n t e r s e c t i n g
j o i n t s , t h e d i r e c t i o n o f t h e x a x i s i s p a r a l l e l o r a n t i p a r a l l e l t o t h e
v e c t o r c r o s s p r o d u c t Z n -I
X
Z n .
No t i c e t h i s c o n d i t i o n i s a l s o
s a t i s f i e d f o r t h e x a x i s d i r e c t e d a l o n g t h e n o r m a l b e t w e e n j o i n t s n
a n d n + 1. Fo r t h e n t h r e v o l u t e j o i n t w h e n
x n ,,
a n d
x n
a r e
p a r a l l e l a n d h a v e t h e s a m e d i r e c t i o n , 0 ,n
i s a t i t s z e r o p o s i t i o n .
I n t h e c a s e o f a p r i s m a t i c j o i n t t h e d i s t a n c e
d n
i s t h e j o i n t
v a r i a b l e . Th e d i r e c t i o n o f t h e j o i n t a x i s i s t h e d i r e c t i o n i n w h i c h
t h e j o i n t m o v e s . Al t h o u g h t h e d i r e c t i o n o f t h e a x i s i s d e f i n e d ,
u n l i k e a r e v o l u t e j o i n t , i t s p o s i t i o n i n s p a c e i s n o t d e f i n e d ( s e e
Fi g . 3 ) . I n t h e c a s e o f a p r i s m a t i c j o i n t t h e l e n g t h a n
h a s n o
m e a n i n g a n d i s s e t t o z e r o . Th e o r i g i n o f t h e c o o r d i n a t e f r a m e f o r
a p r i s m a t i c j o i n t i s c o i n c i d e n t w i t h t h e n e x t d e f i n e d l i n k o r i g i n .
Th e z a x i s o f t h e p r i s m a t i c l i n k i s a l i g n e d w i t h t h e a x i s o f j o i n t
n + 1. Th e
x n
a x i s i s p a r a l l e l o r a n t i p a r a l l e l t o t h e v e c t o r c r o s s
p r o d u c t o f t h e d i r e c t i o n o f t h e p r i s m a t i c j o i n t a n d
Z n .
Fo r a
p r i s m a t i c j o i n t , w e w i l l d e f i n e i t s z e r o p o s i t i o n , w i t h
d i
=
0 , t o b e
w h e n
x n ,,
a n d
x n
i n t e r s e c t . W i t h t h e m a n i p u l a t o r i n i t s z e r o
p o s i t i o n , t h e p o s i t i v e s e n s e o f r o t a t i o n f o r r e v o l u t e j o i n t s o r
d i s p l a c e m e n t f o r p r i s m a t i c j o i n t s c a n b e d e c i d e d a n d t h e s e n s e o f
t h e d i r e c t i o n o f t h e z a x e s d e t e r m i n e d .
Th e o r i g i n o f t h e b a s e l i n k ( z e r o ) w i l l b e c o i n c i d e n t w i t h t h e
o r i g i n o f l i n k 1. I f i t i s d e s i r e d t o d e f i n e a d i f f e r e n t r e f e r e n c e
c o o r d i n a t e s y s t e m t h e n t h e r e l a t i o n s h i p b e t w e e n t h e r e f e r e n c e
a n d b a s e c o o r d i n a t e s y s t e m s c a n b e d e s c r i b e d b y a f i x e d h o m o g e -
n e o u s t r a n s f o r m a t i o n [ 6 ] . At t h e e n d o f t h e m a n i p u l a t o r t h e f i n a l
d i s p l a c e m e n t d 6 o r r o t a t i o n 0 6 o c c u r s w i t h r e s p e c t t o z 5 . Th e
o r i g i n o f t h e c o o r d i n a t e s y s t e m f o r l i n k 6 i s c h o s e n t o b e
c o i n c i d e n t w i t h t h a t o f t h e l i n k 5 c o o r d i n a t e s y s t e m . I f a t o o l o r
e n d -e f f e c t o r i s u s e d w h o s e o r i g i n a n d a x e s d o n o t c o i n c i d e w i t h
t h e c o o r d i n a t e s y s t e m o f l i n k 6 , t h e t o o l c a n b e r e l a t e d b y a f i x e d
h o m o g e n e o u s t r a n s f o r m a t i o n t o l i n k 6 .
H a v i n g a s s i g n e d c o o r d i n a t e f r a m e s t o a l l l i n k s a c c o r d i n g t o t h e
p r e c e d i n g s c h e m e , w e c a n e s t a b l i s h t h e r e l a t i o n s h i p b e t w e e n
s u c c e s s i v e f r a m e s n -
1, n b y t h e f o l l o w i n g r o t a t i o n s a n d t r a n s l a -
t i o n s .
Jo i n t n i l
Fi g . 1. Le n g t h a , a n d t w i s t a , o f a l i n k .
Tr a n s l a t e a l o n g r o t a t e d
x n _l
=x " , a l e n g t h a ,.
Ro t a t e a b o u t x , t h e t w i s t a n g l e a ,,
Th i s m a y b e e x p r e s s e d a s t h e p r o d u c t o f f o u r h o m o g e n e o u s
t r a n s f o r m a t i o n s r e l a t i n g t h e c o o r d i n a t e f r a m e o f l i n k n t o t h e
c o o r d i n a t e f r a m e o f l i n k n - 1. Th i s r e l a t i o n s h i p i s c a l l e d a n A
m a t r i x :
c o
n
0
-SOCa
COCa
Sa
0
S# Sa
-Co Sa
Ca
0
a CO
a SO
d
1
I
( 1)
w h e r e S a n d C r e f e r t o s i n e a n d c o s i n e , r e s p e c t i v e l y . Fo r a
p r i s m a t i c j o i n t t h e A m a t r i x r e d u c e s t o
[ c o
Lo 0
- So Ca
Sa
0
SOSa
-Co Sa
Ca
0
0
0
d .
1 j
( 2 )
On c e t h e l i n k c o o r d i n a t e f r a m e s h a v e b e e n a s s i g n e d t o t h e
m a n i p u l a t o r t h e v a r i o u s c o n s t a n t l i n k p a r a m e t e r s c a n b e t a b u -
l a t e d : d , a , a n d a f o r a l i n k f o l l o w i n g a r e v o l u t e j o i n t a n d , 0 a n d
a f o r a l i n k f o l l o w i n g a p r i s m a t i c j o i n t . Ba s e d o n t h e s e p a r a m e -
t e r s , t h e c o n s t a n t s i n e a n d c o s i n e v a l u e s o f a m a y b e e v a l u a t e d
a n d t h e v a l u e s f o r t h e s i x
Ai
t r a n s f o r m a t i o n m a t r i c e s d e t e r m i n e d .
K I NE MATI C E Q UATI ONS
H a v i n g a s s i g n e d c o o r d i n a t e f r a m e s t o a m a n i p u l a t o r i t i s
p o s s i b l e t o o b t a i n t h e Ca r t e s i a n p o s i t i o n a n d o r i e n t a t i o n o f t h e
m a n i p u l a t o r e n d -e f f e c t o r w h e n g i v e n t h e j o i n t c o o r d i n a t e s .
Th e d e s c r i p t i o n o f t h e e n d o f t h e m a n i p u l a t o r , l i n k c o o r d i n a t e
f r a m e 6 , w i t h r e s p e c t t o l i n k c o o r d i n a t e f r a m e n - I i s g i v e n b y
Un
w h e r e
U.=A.* A+,*
... * * * A6 . ( 3 )
Th e e n d o f t h e m a n i p u l a t o r w i t h r e s p e c t t o t h e b a s e , k n o w n a s
T6 , i s g i v e n b y U1:
T6 = U,
=
A,* A2 * A3 * A4 * A5 * A6
( 4 )
I f t h e m a n i p u l a t o r i s r e l a t e d t o a r e f e r e n c e c o o r d i n a t e f r a m e b y a
t r a n s f o r m a t i o n Z a n d h a s a t o o l a t t a c h e d t o i t s e n d d e s c r i b e d b y
E , w e h a v e t h e d e s c r i p t i o n o f t h e e n d o f t h e t o o l w i t h r e s p e c t t o
t h e r e f e r e n c e c o o r d i n a t e s y s t e m d e s c r i b e d b y X a s f o l l o w s ( 4 ) :
X =Z * T6 * E . ( 5 )
Ro t a t e a b o u t z ,,-1' a n a n g l e 0 ,,
Tr a n s l a t e a l o n g z ,, 1' a d i s t a n c e d n ,,
I n Fi g . 4 t h e PUMA a r m ( Un i m a t e 6 0 0 Ro b o t ) i s s h o w n w i t h
c o o r d i n a t e f r a m e s a s s i g n e d t o t h e l i n k s . Th e p a r a m e t e r s a r e
s h o w n i n Ta b l e I .
4 5 0
4 5 1
I E E E TRANSACTI ONS ON SYSTE MS, MAN, AND CYBE RNE TI CS, VOL. SMC- 1I , NO. 6 , JUNE 19 81
Jo i n t n
Jo i n t n t l
n +l
n
Jo i n t n -i
0 n -1
x
n
I i n k
Fi g . 2 . Li n k p a r a m e t e r s 0 , d , a , a .
Jo i n t n
Fi g . 3 . Li n k p a r a m e t e r s d , a f o r p r i s m a t i c j o i n t .
Th e A m a t r i c e s f o r t h e PUMA a r m a r e a s f o l l o w s :
[ C1 0 -S1 0
Al
=
SI
0
C,
0
O -1 0 0
0 0 0 1
C2 -S2 0 a 2 C2
A2 =
S2 C2 0 a 2 S2
0 0 1 0
[ 0 0 0 1
( 6 )
( 7 )
[ C3 0 S3 a 3 C3
A3 =
S3 0 -C3 a 3 S3 ( 8)
0 1
0 d 3
L[ 0 0
0
1] -
-C4 0 - S4 0
A4
S4
0 C4 0
j 0 -l 0 d
0 0 0 1
C5 0 S5 0
A5 =
S5 0 -Cs
0 1 0 0
A6 =
S6 C6 0 0
O 0 1 0
O O O 1
( 9 )
( 10 )
( 11)
w h e r e S, r e f e r s t o s i n ( 0 k ) a n d Ci
r e f e r s t o c o s ( 0 ,) . Th e p r o d u c t o f
I E E E TRANSACTI ONS ON SYSTE MS, MAN, AND
CYBE RNE TI CS,
VOL. SMC-1 1, NO.
6 ,
JUNE 19 81
w h e r e S2 3 r e f e r s t o s i n ( 0 2 +0 3 ) a n d C2 3 r e f e r s t o c o s ( 0 2 +0 3 ) ,
Z 5 ) Z 6
~ n x
U,
=
AI U5
=
T6
n y
z
x
Fi g . 4 . PUMA m a n i p u l a t o r .
TABLE I
LI NK PARAME TE RS FOR PUMA ARM
Jo i n t a o
0 d a
Ra n g e
I -9 0 0
0 l
0 0
0 1: +/-16 0 0
2 0 0 2
0
a 2 0 2 : +4 5
-
2 2 5
3 9 0 0
0 3
d 3 a 3 0 3 : 2 2 5 ' -4 5 0
4 -9 0 0 0 4 d 4 0
0 4 : +/-17 0 0
5 9 0 0 0 5 0 0
0 5 : +/-13 5 0
6 0 0 6 0 0 0 6 : +/-17 0 0
a 2 =17 .0 0 0 a 3 = 0 .7 5
d 3
=4 .9 3 7 d 4
= 17 .0 0 0
t h e A m a t r i c e s , s t a r t i n g a t l i n k 6 a n d w o r k i n g b a c k t o t h e
b a s e ,
f o r t h e PUMA a r m a r e
U6
=
A6 ( 12 )
o X a x Px
o y
a
y p y
o z a z p z
O O 1
( 17 )
( 18)
( 19 )
( 2 0 )
( 2 1)
w h e r e
n
x Cl [ C2 3 ( C4 C5 C6 - S4 S6 )
-
S2 3 S5 C6 ]
-SI [ S4 C5 C6 +
C4 S6 ]
n y = Sl [ C2 3 ( C4 C5 C6 - S4 S6 ) -
S2 3 S5 C6 ]
+C1[ S4 C5 C6 + C4 S6 ]
n
z
=
-S2 3 ( C4 C5 C6 - S4 S6
-
C2 3 S5 C6
Ox =
Cl [ -C2 3 ( C4 C5 S6 + S4 C6 )
+
S2 3 S5 S6 ]
-SI [ -S4 C5 S6 +
C4 C6 ]
o y =
SI I -C2 3 ( C4 C5 S6 +
S4 C6 ) +
S2 3 S5 S6 ]
+
Cl [ -S4 C5 S6 +C4 C6 ]
Oz
=
S2 3 ( C4 C5 S6 +S4 C6 ) + C2 3 S5 S6
a x = Cl ( C2 3 C4 S5
+S2 3 C5 )
- SS4 S
a y = Sl ( C2 3 C4 S5
+S2 3 C5 ) +
CI S4 SA
a z
=
-S2 3 C4 S5
+
C2 3 C5
Px = Cj ( d 4 S2 3 +a 3 C2 3 +a 2 C2 ) - Sj d 3
p y
=
SI ( d 4 S2 3
+a 3 C2 3 +a 2 C2 ) +
CI d 3
PZ = -( -d 4 C2 3 +a 3 S2 3 +a 2 S2 ) .
( 2 2 )
( 2 3 )
( 2 4 )
( 2 5 )
( 2 6 )
( 2 7 )
( 2 8)
( 2 9 )
I n o r d e r t o c o m p u t e t h e r i g h t h a n d t h r e e c o l u m n s o f
T6 ,
w e
r e q u i r e 12 t r a n s c e n d e n t a l f u n c t i o n c a l l s , 3 4
m u l t i p l i e s ,
a n d 16
a d d i t i o n s . Th e f i r s t c o l u m n o f T6 c a n b e o b t a i n e d a s t h e v e c t o r
c r o s s p r o d u c t o f t h e s e c o n d a n d t h i r d c o l u m n s .
I f t h e j o i n t c o o r d i n a t e s a r e g i v e n , t h e p o s i t i o n a n d o r i e n t a t i o n
o f t h e h a n d a r e o b t a i n e d b y e v a l u a t i n g t h e s e
e q u a t i o n s
t o o b t a i n
T6 . Th e p o s i t i o n a n d o r i e n t a t i o n o f a t o o l w i t h
r e s p e c t
t o a b a s e
c o o r d i n a t e f r a m e c a n n o w b e o b t a i n e d f r o m
( 5 ) .
Cs 5 C6
U5 =A5 U6
& 5
6
[ 0
C4 C5 C6
-
S4 S6
-
s 5 C6
0
-C5 S6
S5 0
-S5 S6 -C5 0
C6
0 0
0 0 1
-C4 C5 S6 - S4 C6
-S4 C5 S6 + C4 C6
S5 S6
0
SOLUTI ON
( 13 ) I n o r d e r t o c o n t r o l t h e m a n i p u l a t o r , w e a r e i n t e r e s t e d i n t h e
r e v e r s e p r o b l e m , t h a t i s , g i v e n X i n ( 5 ) , w h a t a r e t h e
c o r r e s p o n d -
i n g j o i n t c o o r d i n a t e s ?
n 1
W e m a y f i r s t o b t a i n T6 f r o m ( 5 ) a s
( L4 I ) 5 U
S4 s 5 0
C5 d 4
0 1]
( 14 )
T6 = Z -' * X * E - ( 3 0 )
a n d t h e n t h e t r a d i t i o n a l a p p r o a c h i s t o s o l v e t h e m a t r i x
e q u a t i o n
T6 =A1 * A2 * A3 * A4 * A5 * A6 ( 3 1)
C3 ( C4 C5 C6 - S4 S6 )
-
S3 S5 C6 -C3 ( C4 C5 S6
+
S4 C6 )
+
S3 S5 S6
U3 =A3 U4 = S3 ( C4 c 5 C6 -S4 S6 ) + C3 S5 C6 -S3 ( C4 C5 6
+
S4 C6 ) - C3 S5 6
S4 C5 C6 +C4 S6 -S4 C5 s S6 +
C4 C6
0 0
U2 =A2 U3
=
C2 3 ( C4 C5 C6 - S4 S6 )
-
S2 3 S5 C6
S2 3 ( C4 C5 C6 - S4 S6 ) +C2 3 S5 C6
S4 C5 C6
+
C4 S6
L 0 ~ ~ ~
C3 C4 s 5 +S3 C5
S3 c 4 s 5
-
C3 c 5
0
-
C2 3 ( C4 C5 S6 +
S4 C6 )
+
S2 3 s 5 s 6 C2 3 C4 S5
+
S2 3 C5
-S2 3 ( C4 C5 6 +
S4 c 6 ) C2 3 S5 S6 S2 3 C4 S5 -C2 3 C5
-S4 c 5 s 6
+
C4 C6 S4 s 5
d 4 S3 +a 3 C3
-d 4 C3 +a 3 S3
d 3
I i
d 4 S2 3 +a 3 C2 3 a 2 C2
-d 4 C2 3 +a 3 S2 3 +a 2 S2
U4
=
A4 U5 =
( 15 )
I ( 16 )
. .. ........ . ......... . . . .... . ..... ... .. ..
4 5 2
0 0 I
/" C,
I E E E TRANSACTI ONS ON SYSTE MS, MAN, AND CYBE RNE TI CS, VOL. SMC-1 1, NO. 6 , JUNE 19 81
w h e r e T6 i s g i v e n n u m e r i c v a l u e s . W i t h n u m e r i c v a l u e s a s s i g n e d
t o t h e e l e m e n t s o f T6 , t h e r e q u i r e d v a l u e s o f
0 1, 0 2 , 0 3 , 0 4 , 0 5 , a n d
0 6 c a n b e o b t a i n e d b y s i m u l t a n e o u s l y s o l v i n g ( 18) -( 2 9 ) . Th i s
a p p r o a c h i s d i f f i c u l t f o r t h e f o l l o w i n g r e a s o n s : t h e e q u a t i o n s a r e
t r a n s c e n d e n t a l ; w e w i l l n e e d b o t h t h e s i n e a n d c o s i n e i n o r d e r t o
d e t e r m i n e a n g l e s u n i q u e l y a n d a c c u r a t e l y ; t h e m a n i p u l a t o r e x -
h i b i t s m o r e t h a n o n e s o l u t i o n f o r a g i v e n p o s i t i o n ; a n d w e h a v e
t w e l v e e q u a t i o n s i n s i x u n k n o w n s .
Th e r e a r e , h o w e v e r , s i x o t h e r m a t r i x e q u a t i o n s o b t a i n e d b y
s u c c e s s i v e l y p r e m u l t i p l y i n g ( 3 1) b y t h e A m a t r i x i n v e r s e s :
Al l t h e e l e m e n t s o n t h e r i g h t s i d e o f ( 4 5 ) a r e f u n c t i o n s o f 0 2 , 0 2 3 ,
d 4 , 0 4 , 0 5 , a n d 0 6 e x c e p t f o r e l e m e n t 3 4 . W e m a y e q u a t e t h e 3 4
e l e m e n t s t o o b t a i n
f l 3 ( p )
=
d 3
( 4 6 )
o r
-Sl p ,+ Cl p y =
-d 3 .
( 4 7 )
I n o r d e r t o s o l v e e q u a t i o n s o f t h i s f o r m w e m a k e t h e f o l l o w i n g
t r i g o n o m e t r i c s u b s t i t u t i o n s :
A, * T6 = U2
A2 * A1 ' * T6 = U3
A) -1 * A2 -
* A, l * T6 = U4
A4 - ' * A3 ' * A2 ' * A, * 1T6 = U5
A5 - * A4 l * A3 l * A2 l * A-' * T6 = U6 .
( 3 2 )
( 3 3 )
( 3 4 )
( 3 5 )
( 3 6 )
Th e m a t r i x e l e m e n t s o f t h e l e f t s i d e s o f t h e s e e q u a t i o n s a r e
f u n c t i o n s o f t h e e l e m e n t s o f T6 a n d o f t h e f i r s t n -1 j o i n t
v a r i a b l e s . Th e m a t r i x e l e m e n t s o f t h e r i g h t h a n d s i d e s a r e e i t h e r
z e r o , c o n s t a n t s , o r f u n c t i o n s o f t h e n t h t o 6 t h j o i n t v a r i a b l e s . As
m a t r i x e q u a l i t y i m p l i e s e l e m e n t b y e l e m e n t e q u a l i t y w e o b t a i n 12
e q u a t i o n s f r o m e a c h m a t r i x e q u a t i o n , t h a t i s , o n e e q u a t i o n f o r
e a c h o f t h e c o m p o n e n t s o f t h e f o u r v e c t o r s n , o , a a n d p .
E q u a t i n g e l e m e n t s o f t h e s e m a t r i x e q u a t i o n s f r e q u e n t l y r e s u l t s i n
e q u a t i o n s y i e l d i n g j o i n t v a r i a b l e s e x p l i c i t l y . W e w i l l i l l u s t r a t e t h e
v a r i o u s f o r m s o f t h e s e e q u a t i o n s b y d e v e l o p i n g t h e e q u a t i o n s f o r
t h e PUMA a r m .
I f w e p r e m u l t i p y ( 3 1) b y A1 w e o b t a i n
A *
T6 =
A2 * A3 * A4 * A * A6
AI
1* T6 = U2 .
Th e l e f t s i d e o f ( 3 8) i s g i v e n b y
A C,
A,1* =
-SI
SI
0
Cl
0
o n x
-1 0 n y
o I n z
0 1 J 0
Ox
Oy
OZ
0
a x
a y
a z
0
( 3 7 )
( 3 8)
PI
p y
Dn
Px = r c o s
p y =
r s i n 4
w h e r e
r =+( p 2 p
2 ) /
t - t a n ( P )
( 4 8)
( 4 9 )
( 5 0 )
( 5 1)
As e i t h e r t h e n u m e r a t o r o r d e n o m i n a t o r o f ( 5 1) c a n b e z e r o w e
w i l l u s e t h e a r c t a n g e n t f u n c t i o n o f t w o a r g u m e n t s t o o b t a i n
v a l u e s o f
0 .
Th i s a r c t a n g e n t f u n c t i o n u s e s t h e s i g n o f t h e n u m e r a -
t o r a n d d e n o m i n a t o r t o d e t e r m i n e t h e c o r r e c t q u a d r a n t f o r t h e
r e s u l t i n g a n g l e a n d i s d e f i n e d o v e r t h e r a n g e -i T < s < ' r . Su b -
s t i t u t i n g f o r p , a n d
p y
i n ( 4 7 ) w e o b t a i n
Sb CO,
-
COSO,
= d 3 /r
w i t h
O < d 3 /r - 1.
E q u a t i o n ( 5 2 ) r e d u c e s t o
S( 4 - 0 1) =d 3 /r
w i t h

< 0
-
1
<
f .
W e m a y o b t a i n t h e c o s i n e a s
C( O -0 1 )
=
+A1-r l ( d 3 l r ) 2
( 5 2 )
( 5 3 )
( 5 4 )
w h e r e t h e m i n u s s i g n c o r r e s p o n d s t o a l e f t -h a n d s h o u l d e r c o n f i g -
u r a t i o n o f t h e
m a n i p u l a t o r
a n d t h e
p l u s s i g n c o r r e s p o n d s
t o a
( 3 9 ) r i g h t -h a n d s h o u l d e r c o n f i g u r a t i o n . Fi n a l l y ,
Th e i n v e r s e o f a h o m o g e n e o u s t r a n s f o r m a t i o n i s s i m p l e t o o b t a i n
( s e e Ap p e n d i x I ) a n d t h e p r o d u c t o f t h e s e t w o m a t r i c e s i s
0 1=t a n
-IP-t a n -
3
Px
/I Vr 2 -d 2
f l l ( n )
AI T1
f l 2 ( n )
I
f 13 ( n
f i l ( o ) f l l ( a )
f 12 ( o ) f 12 ( a )
113 ( 0 ) f 13 ( a )
0 0
112 ( P)
( 4 0 )
113 ( P) I
I 11
w h e r e
f i l =Cl x +s l y ( 4 1)
f 12 = -z ( 4 2 )
f 13
=-SI x +
C,y ( 4 3 )
a n d x , y , a n d z r e f e r t o c o m p o n e n t s o f t h e v e c t o r s g i v e n a s
a r g u m e n t s t o
f l ,I 1' i
2 , a n d f 3 ,
f o r e x a m p l e
( 4 4 )
H a v i n g d e t e r m i n e d 0 1 t h e l e f t s i d e o f ( 3 8) i s n o w d e f i n e d .
W h e n e v e r w e h a v e t h e l e f t s i d e o f o n e o f ( 3 2 ) -( 3 6 ) d e f i n e d , w e
e x a m i n e t h e r i g h t s i d e f o r e l e m e n t s w h i c h a r e a f u n c t i o n o f
i n d i v i d u a l j o i n t c o o r d i n a t e s . I n t h e c a s e o f t h e PUMA a r m , a s
w i t h a n y a r m w i t h t w o o r m o r e j o i n t a x e s p a r a l l e l , t h e T6 m a t r i x
i s e x p r e s s e d i n t e r m s o f s u m s o r d i f f e r e n c e s o f t h e a n g l e s r e l a t i n g
t o t h e p a r a l l e l a x e s . I n o r d e r t o s o l v e t h e k i n e m a t i c e q u a t i o n s , t h e
s u m o r d i f f e r e n c e o f t h e a n g l e s m u s t b e d e t e r m i n e d b e f o r e t h e
a n g l e s t h e m s e l v e s c a n b e f o u n d . I n a d d i t i o n t h e s o l u t i o n f o r t h e s e
s u m s o f a n g l e s i n v o l v e s t h e s u m o f t h e s q u a r e s o f t w o e q u a t i o n s .
Su c h i s t h e c a s e i n o r d e r t o s o l v e f o r 0 2 a n d 0 3 . Th e 14 a n d 2 4
e l e m e n t s o f ( 3 8) a r e
d 4 S2 3 +a 3 C2 3 +a 2 C2 =
Cl Px
+
SI Py
-
d 4 C2 3 +a 3 S2 3 +a 2 S2 =-p z
( 5 6 )
( 5 7 )
f Tl ( n )
=
Ci n e
+
Sl n
Th e r i g h t s i d e o f ( 3 8) i s o b t a i n e d f r o m n ( 16 ) a n d i s g i v e n b y
C2 3 ( C4 C5 C6 - S4 S6 )
-
S2 3 S5 C6 -C2 3 ( C4 C5 S6
+
S4 C6 )
+
S2 3 S5 S6 C2 3 C4 S5
+
S2 3 C5 d 4 S2 3
+
a 3 C2 3
+
a 2 C2
S
s 2 3 ( C4 C5 C6 - S4 S6 )
+
C2 3 S5 C6 -S2 3 ( C4 C5 6 +S4 C6 ) - C2 3 S5 6 S2 3 C4 S5 - C2 3 C5 -d 4 C2 3 +a 3 S2 3 +a 2 5 2
4 C0 C6
+
C4 0 S6 -S4 C5 6
+
C4 C6 0 4 5 5 d 3
L~ ~ ~ ~ O 0 1
( 5 5 )
4 5 3
Fz
I
I E E E TRANSACTI ONS ON SYSTE MS, MAN, AND CYBE RNE TI CS, VOL. SMC- 1I , NO. 6 , JUNE 19 81
w h e r e
Cl p x +
S1p y =f i 1p
( 5 8)
-
Pz
=
f i 2 p ( 5 9 )
Sq u a r i n g , a d d i n g , a n d s i m p l i f y i n g :
f 12 ,p
+f 2 2 p - d 4 - a 3 2 - a2 =2 a 2 d 4 5 S3 +
2 a 2 a 3 C3 . ( 6 0 )
Si n c e t h e l e f t s i d e i s k n o w n a n d t h e o n l y v a r i a b l e s a r e S3 a n d C3 ,
t h i s
e q u a t i o n
i s o f t h e f o r m o f
( 4 7 ) .
I t c a n b e s o l v e d t o
y i e l d
O3
=
a r c r c
t d -a n ( 6 1)
w h e r e
d =f l l p 2 p d 4 a 3 - a 2
e =4 a 2 a 2 +4 a 2 d 2 ( c o n s t a n t ) .
E v a l u a t i n g t h e e l e m e n t s o f ( 3 3 ) w e o b t a i n
[ 2 1( n )
f 2 2 ( n )
f 2 3 ( n )
LO
( 6 2 )
( 6 3 )
f 2 l ( o ) f 2 1( a ) f 2 ( P) -a 2
f 2 2 ( o ) f 2 2 ( a ) f 2 2 ( P)
=U
12 3 ( o ) f 2 3 ( a ) f 2 3 ( P) -
0 0 1 1
( 6 4 )
a n d
0 2 = 0 2 3
-
0 3 - ( 7 9 )
W i t h t h e l e f t s i d e o f ( 6 8) n o w d e f i n e d , w e c h e c k t h e r i g h t s i d e f o r
f u n c t i o n s o f s i n g l e v a r i a b l e s . Th e 13 a n d 2 3 e l e m e n t s g i v e u s
e q u a t i o n s f o r t h e s i n e a n d c o s i n e o f 0 4 i f s i n ( 0 5 ) i s n o t z e r o .
W h e n s i n 0 5 =0 , 0 5 = 0 a n d t h e m a n i p u l a t o r b e c o m e s d e g e n e r a t e
w i t h b o t h t h e a x e s o f j o i n t 4 a n d j o i n t 6 a l i g n e d . I n t h i s s t a t e i t i s
o n l y t h e s u m o f 0 4 a n d 0 8 w h i c h i s s i g n i f i c a n t . I f 0 5 i s z e r o w e a r e
f r e e t o c h o o s e a n y v a l u e f o r 0 4 . Th e c u r r e n t v a l u e i s f r e q u e n t l y
a s s i g n e d :
( 80 )
( 81)
C4 5 =
C2 3 ( Ci a ,+ Sl a y ) -2 3 a z
S4 SS
=-Sl a .
+
Cl a y
a n d
0 4 t a n
-Sl a ,+ Cl a y
C2 3 ( Cl a ,

Sl a y )
-
2 a
i f
0 5 > 0
a n d
Q 4 -0 4 +180 0 i f O5 < O.
E v a l u a t i n g t h e e l e m e n t s o f ( 3 5 ) w e o b t a i n
( 82 )
( 83 )
w h e r e
f 2 1
=
C2 ( CI X
+
S1Y) -S2 z
12 2 =-S2 ( CI X +SI y ) - C2 z
f 2 3 = -SI X +
CI y .
Si n c e t h i s y i e l d s n o t h i n g , w e e v a l u a t e ( 3 4 ) a s
f 3 1( o ) f 3 1 ( a ) f 3 l ( p ) -a 2 C3 -a 3
f 3 2 ( 0 ) 13 2 ( a ) f 3 2 ( p ) +d 3
-U4
13 3 ( o ) f 3 3 ( a ) f 3 3 ( P) ) -a 2 S3
0 0 1 1
( 6 8)
f 4 l ( n f ) f 4 1( 0 )
( 6 5 ) 14 2 ( n ) f 4 2 ( o )
( 6 6 ) f 4 3 ( n ) f 4 3 ( o )
( 6 7 ) [ 0
0
f 4 l ( a )
0 C5 C6
f 4 2 ( a ) 0 S5 C6
14 3 ( a ) 0 5 6
0 1 _ O
-C5 S6 S5
-S5 S6 -C5
C6
0
0 0
0
0 1
0 1
11i
( 84 )
w h e r e
f 4 1-C4 [ C2 3 ( C1X +SI y ) -S2 3 Z ] +S4 [ -S1X +C1y ] ( 85 )
( 86 )
14 3
=
S4 [ C2 3 ( CI x
+
SI y )
-
S2 3 z ] +
C4 [ -S1X +
CI y ] .
( 87 )
13 1 = C2 3 ( C1X +
SI Y) - S2 3 z
f 3 2 =-SI X + CI y
f 3 3
=
S2 3 ( C1X
+
SI y )
+
C2 3 z
e q u a t i n g t h e 14 a n d 3 4 t e r m s w e o b t a i n
C2 3 f ,l p -5 2 3 Pz
=
a 2 C3 +a 3
S2 3 f l l p + C2 3 p z
=
d 4 +a 2 S3 .
( 6 9 )
Fr o m t h e
r i g h t
s i d e o f
( 84 ) ,
w e c a n t h e n o b t a i n
e q u a t i o n s
f o r
S5 ,
C5 , 5 6 a n d C6 b y i n s p e c t i o n . W h e n b o t h s i n e a n d c o s i n e a r e
( 7 0 ) d e f i n e d w e o b t a i n a u n i q u e v a l u e f o r t h e j o i n t a n g l e . W e o b t a i n a
( 7 1)
v a l u e f o r
0 5 b y e q u a t i n g
t h e 13 a n d 2 3 e l e m e n t s o f
( 84 ) :
S5
C4 [ C2 3 ( Cl a x + Sl a y )
- S2 3 a z ] +
S4 [ -Sl a x
+
Cl a y ]
( 7 2 )
( 7 3 )
Si n c e C2 3 a n d S2 3 a r e t h e o n l y v a r i a b l e s , w e c a n s o l v e t h e a b o v e
e q u a t i o n s s i m u l t a n e o u s l y t o y i e l d :
S3 =W 2 f l I p
-
W P2
w 1s 2 l -+p w 2
CW l f l l p
+W 2 Pz
-
1
-z
I 3
p 2
w h e r e
w =a 2 C3 +a 3
w 2
=
d 4
+
a 2 S3
( 7 4 )
C5 = S2 3 ( Cl a
+
Sl a y )
+
C2 3 a ,
a n d o b t a i n 0 5 a s
( 88)
( 89 )
0 5
I C4 [ C2 3 ( Cl a x Sl a y ) S2 3 a j ]
+
S4 [ -S1a x +C,a y ]
2 ( Cl a +
S2 3 ( Cl
a + a
)
+
C2 3 4
a
( 9 0 )
W h i l e w e h a v e e q u a t i o n s f o r b o t h S6 a n d C6 , t h e e q u a t i o n f o r 5 6
( 7 5 )
i s i n t e r m s o f e l e m e n t s o f t h e f i r s t c o l u m n w h i c h i n v o l v e s t h e u s e
o f t h e n v e c t o r o f T6 . Th e n v e c t o r o f T6 i s n o t u s u a l l y m a d e
a v a i l a b l e a s i t r e p r e s e n t s r e d u n d a n t i n f o r m a t i o n . I t c a n a l w a y s b e
c o m p u t e d b y t h e v e c t o r c r o s s p r o d u c t o f t h e o a n d a v e c t o r s . By
( 7 6 ) e v a l u a t i n g
t h e e l e m e n t s o f
( 3 6 )
w e c a n o b t a i n
e q u a t i o n s
f o r
S6
a n d C6 a s a f u n c t i o n o f t h e o v e c t o r :
( 7 7 )
t h e r e f o r e
2 3 =a r c t a n f
w l f l l p +W 2 Pz
( 7 8)
f 5 [ ( n ) f 5 l ( o ) 0
-S6
f 5 2 ( n ) 15 2 ( o ) 0
= 5 6 C6 0 0
( 9 1)
5 3 ( n ) 5 3 ( o ) 1 1
L0 0 0
I L0 0
0 1] i
f 3 1( n )
f 3 2 ( n )
f 3 3 ( n )
0
f 4 2 =-S2 3 ( C1X
+
SI Y) - C2 3 Z
4 5 4
I E E E TRANSACTI ONS ON SYSTE MS, MAN, AND CYBE RNE TI CS, VOL. SMC-1 1, NO. 6 , JUNE 19 81
w h e r e
f 5 l C5 { C4 [ C2 3 ( CI X +SI y ) - S2 3 Z ] +S4 [ -SI x + C1y ] }
+S5 { -S2 3 ( CI X
+
SI y )
-
C2 3 Z }
( 9 2 )
f 5 2 -S4 [ C2 3 ( CI X +SI y ) -S2 3 Z ] +C4 [ -SI X + CI y ] ( 9 3 )
f 5 3 = S5 { C4 [ C2 3 ( CI x
+
SI y )
-
S2 3 Z ]
+
5 4 -SI x
+
C1y ] )
+C5 { S2 3 ( CI x
+
SI Y)
+C2 3 Z } ' ( 9 4 )
By e q u a t i n g t h e 12 a n d 2 2 e l e m e n t s w e o b t a i n e x p r e s s i o n s f o r S6
a n d
C6 :
S6 =-C5
{ C4 [ C2 3 ( Cl o x
+S
o y -s 2 3 0 z ]
+S4 [ Sl o x
+Cl o y ]
} +
S5
( S2 3 ( Co I x
+
Sl o y )
+
C2 3 0 Z } ( 9 5 )
C6
=
-S4 [ C2 3 ( Co I x
+
Sl o y )
-
S2 3 o z ]
+
C4 [ -S1o x
+
Cl o y ]
-
( 9 6 )
W e o b t a i n a n e q u a t i o n f o r
0 6
a s :
I t s i n v e r s e i s g i v e n b y
I x
T
-
1=
m x
n x
L 0
I y
m f y
n y
l z _p .1
m z -p .m ,
n z
-p * n
0 1i
( 9 9 )
w h e r e t h e t e r m s o f t h e r i g h t -h a n d c o l u m n a r e o b t a i n e d u s i n g
v e c t o r d o t p r o d u c t . Th a t ( 9 9 ) r e p r e s e n t s t h e i n v e r s e i s e a s i l y
v e r i f i e d b y f o r m i n g t h e m a t r i x p r o d u c t a n d c h e c k i n g t h a t t h e
r e s u l t i s a n i d e n t i t y m a t r i x :
I x
-1 Tm x
n x
L 0
I I -p -l 1x
m y m z -p .m I y
n y n z -p -n
I x
O 0 1
J
O
m x
m y
m z
0
n x
n y
n z
0
p y
p z I
( 10 0 )
-C5 { C4 [ C2 3 ( Co x
+
s l o y ) - S2 3 o Z ] +
S4 [ --So x
+
C1o y ] } +S5
{ S2 3 ( Co x
+
Sl o y )
+
C2 3 o Z )
6 =t a n ) S4 [ C2 3 ( Cl o x + So y ) -S2 3 o z ]
+C4 [ -S1o x + Cl o y ]
E v e n i n t h e c a s e w h e r e 0 4 i s u n d e f i n e d b e c a u s e t h e m a n i p u l a t o r
c o n f i g u r a t i o n i s d e g e n e r a t e , o n c e a v a l u e i s a s s i g n e d t o
0 4
t h e
c o r r e c t v a l u e s f o r
0 5
a n d 0 6 a r e d e t e r m i n e d b y t h e s e e q u a t i o n s .
Th i s s o l u t i o n c o r r e s p o n d s t o 16 t r a n s c e n d e n t a l f u n c t i o n c a l l s , 3 8
m u l t i p l i e s , a n d 2 5 a d d i t i o n s .
E X TE NSI ON TO OTH E R MANI PULATORS
Th i s s o l u t i o n t e c h n i q u e , d e m o n s t r a t e d w i t h t h e PUMA m a n i -
p u l a t o r , i s v a l i d f o r k i n e m a t i c a l l y s i m p l e m a n i p u l a t o r s , i n c l u d i n g
a l l c o m m e r c i a l l y a v a i l a b l e m a n i p u l a t o r s f o r w h i c h s o l u t i o n s h a v e
b e e n o b t a i n e d . Th e r e a r e , h o w e v e r , s o m e m a n i p u l a t o r s w h o s e
c o n f i g u r a t i o n s m a n d a t e a s l i g h t l y d i f f e r e n t a p p r o a c h t o t h e s o l u -
t i o n . I n t h e c a s e o f a m a n i p u l a t o r w i t h a n o f f s e t a t t h e h a n d , t h e
p r o b l e m w a s i n v e r t e d a n d t h e s o l u t i o n t o t h e k i n e m a t i c p r o b l e m
t o p o s i t i o n t h e b a s e a t T6 - w a s s o l v e d .
Th e r e a r e t w o c o m m o n p i t f a l l s i n o b t a i n i n g s o l u t i o n s w h i c h
s h o u l d b e a v o i d e d . On e o f t h e s e i s d i v i s i o n b y t h e s i n e o r c o s i n e
o f a n a n g l e . Th e o t h e r i s n o t m a x i m i z i n g t h e u s e o f c o m m o n
e x p r e s s i o n s . Fo r e x a m p l e , a f t e r s o l v i n g f o r 0 4 f r o m ( 82 ) , a p o s s i -
b l e m e t h o d t o d e t e r m i n e 0 5 w o u l d b e t o e q u a t e t h e 2 , 3 a n d 3 , 3
e l e m e n t s o f ( 6 8) . I n o r d e r t o d o t h i s , t h e 2 ,3 e l e m e n t ( S4 S5 )
w o u l d h a v e t o b e d i v i d e d b y S4 . Th i s l e a d s t o i n a c c u r a c y w h e n S4
i s n e a r o r e q u a l t o z e r o . By e x t e n d i n g t h e m e t h o d o n e m o r e s t e p
a n d p r e m u l t i p l y i n g b y A3 b o t h p r o b l e m s w e r e a v o i d e d .
SUMMARY
W e h a v e r e v i e w e d t h e m e t h o d o f a s s i g n i n g c o o r d i n a t e f r a m e s
t o t h e l i n k s o f a m a n i p u l a t o r . I n t e r m s o f t h e s e c o o r d i n a t e f r a m e s
t h e k i n e m a t i c e q u a t i o n s c a n b e d e v e l o p e d i n a s t r a i g h t f o r w a r d
m a n n e r . Th e s e e q u a t i o n s c a n b e o b t a i n e d f o r a n y m a n i p u l a t o r . I f
t h e m a n i p u l a t o r i s k i n e m a t i c a l l y " s i m p l e ," t h e s o l u t i o n t o t h e
k i n e m a t i c e q u a t i o n s c a n b e o b t a i n e d i n a v e r y s t r a i g h t f o r w a r d ,
e r r o r -f r e e m a n n e r .
APPE NDI X I
G i v e n a h o m o g e n e o u s t r a n s f o r m a t i o n r e p r e s e n t e d b y f o u r v e c -
t o r s 1, m , n , a n d p
I x
T= I y
i z
L
0
( 9 8)
m x n x Px
m y n y p y .
m z n z PZ
O O 1
T.4
T
-
' * T =
m
* l
n -
l -m
m m
n m
0
1 m
m * n
n * n
0
0 1
0 1
0
,
11-
( 10 1)
As t h e t h r e e v e c t o r s 1, m , a n d n a r e o r t h o g o n a l w e h a v e
1-l = m I m
=n -n = 1 ( 10 2 )
a n d
l m =l n =n -m =O ( 10 3 )
a n d t h u s ( 10 1) r e d u c e s t o a n i d e n t i t y m a t r i x .
ACK NOW LE DG ME NT
Mi c k e y K r e b s w a s r e s p o n s i b l e f o r t h e d o c u m e n t p r e p a r a t i o n
a n d Ma r c Re a m f o r p r e p a r i n g t h e d r a w i n g s .
RE FE RE NCE S
[ 1 R. A. Le w i s , " Au t o n o m o u s m a n i p u l a t i o n s o n a r o b o t : Su m m a r y o f
m a n i p u l a t o r s o f t w a r e f u n c t i o n s ," Je t Pr o p u l s i o n La b o r a t o r i e s , Pa s a d e n a ,
CA, TM 3 3 -6 7 9 , 19 7 4 .
[ 2 ] C. Ro s e n e t a l ., " E x p l o r a t o r y r e s e a r c h i n a d v a n c e d a u t o m a t i o n ," 2 n d
Re p ., St a n f o r d Re s e a r c h I n s t i t u t e , St a n f o r d Un i v ., St a n f o r d , CA, Au g .
19 7 4 .
[ 3 ] R. Pa u l , " Mo d e l l i n g , t r a j e c t o r y c a l c u l a t i o n , a n d s e r v o i n g o f a c o m p u t e r
c o n t r o l l e d a r m ," St a n f o r d Ar t i f i c i a l I n t e l l i g e n c e La b ., St a n f o r d Un i v .,
St a n f o r d , CA, Me m o . AI M-7 7 , No v . 19 7 2 .
[ 4 ] -, " Ad v a n c e d i n d u s t r i a l r o b o t c o n t r o l s y s t e m s ," 1s t Re p . Pu r d u e
Un i v ., La f a y e t t e , I N, Me m o . E E 7 8-2 5 , Ma y 19 7 8.
[ 5 ] D. L. Pi e p e r , " Th e k i n e m a t i c s o f m a n i p u l a t o r s u n d e r c o m p u t e r c o n t r o l ,"
St a n f o r d Ar t i f i c i a l I n t e l l i g e n c e Pr o j e c t , St a n f o r d , CA, Me m o . AI M-7 2 ,
Oc t . 19 6 8.
[ 6 ] L. G . Ro b e r t s , " H o m o g e n e o u s m a t r i x r e p r e s e n t a t i o n a n d m a n i p u l a t i o n
o f N-d i m e n s i o n a l c o n s t r u c t s ," M.I .T., Li n c o l n La b s ., Do c u m e n t MS- 10 4 5 ,
Ma y 19 6 5 .
[ 7 ] B. Ro t h , " Pe r f o r m a n c e e v a l u a t i o n o f m a n i p u l a t o r s f r o m a k i n e m a t i c
v i e w p o i n t ," Na t i o n a l Bu r e a u St a n d a r d s , Re p . SP-4 5 9 , 19 7 6 .
[ 8] T. Bi n f o r d e t a l ., " E x p l o r a t o r y s t u d i e s o f c o m p u t e r i n t e g r a t e d a s s e m b l y
s y s t e m s ," Na t i o n a l Sc i e n c e Fo u n d a t i o n Pr o g r e s s Re p ., St a n f o r d Ar t i f i -
c i a l I n t e l l i g e n c e La b ., St a n d f o r d , CA, Me m o . AI M-2 85 , Ju l y 19 7 6 .
[ 9 ] R. H . Ta y l o r , " Pl a n n i n g a n d e x e c u t i o n o f s t r a i g h t -l i n e m a n i p u l a t o r
t r a j e c t o r i e s ," I BM Re s e a r c h Re p . RC 6 6 5 7 , Ju l y 19 7 7 .
[ 10 ] J. De n a v i t a n d R. S. H a r t e n b e r g , " A k i n e m a t i c n o t a t i o n f o r l o w e r -p a i r
m e c h a n i s m s b a s e d o n m a t r i c e s ," J. Ap p l . Me c h ., p p . 2 15 -2 2 1, Ju n e 19 5 5 .
4 5 5
( 9 7 )