BEHA VIOURAL ANALYSIS OF BIPEDAL ROBOT A dissertation submitted to the Department of Electrical Engineering, University of Moratuwa in partial fulfilment of the requirements for the Degree of Master of Science by SAMAN MANJULA WELIHINDA Supervised by: Prof. Lanka Udawatta Department of Electrical Engineering University of Moratuwa, Sri Lanka 2010 94542 Abstract In this thesis a 7 OoF bipedal robot has been simulated, and analyzed its behavior by varying torso angle to achieve dynamic stability while walking on sloping surfaces. The revolutionary dynamic stability criteria introduced by Prof. Miomir Vukobratovic in 1969 has been used throughout the thesis. The dynamically stable human walking on slopes are very complex and this thesis address this problem by starting from lower body and in the middle by adding a torso with the intention of gaining dynamic stability and finishes showing the effectiveness of the variation of torso angle. The ZMP formula presented in the paper of Prof. Miomir Vukobratovic is involved with huge computations and due to that most of the researches in this field have chosen alternatives such as GA and NN to find the ZMP. Although it is convenient to use GA and NN to avoid mathematical calculation the accuracy of resultant ZMP is questionable. The original ZMP equation has been used extensively in all cases to calculate ZMP in this thesis and rather verifies the accuracy of this concept. The simulations have been performed to verify the accuracy of the kinematics models that has been created before moving to the ZMP calculations. The effectiveness of the variation of torso angle on the dynamic stability of the bipedal robot has been analyzed by varying slope angle, step length and step time. It is indicated using real ZMP calculations the bipedal robot can maintain its dynamic stability while walking on slopes at any circumstances. D E C L A R A T I O N T h e w o r k s u b m i t t e d i n t h i s d i s s e r t a t i o n i s t h e r e s u l t o f m y o w n i n v e s t i g a t i o n , e x c e p t w h e r e o t h e r w i s e s t a t e d . I t h a s n o t a l r e a d y b e e n a c c e p t e d f o r a n y d e g r e e , a n d i s a l s o n o t b e i n g U ) J l l ' u r r e n t l ) s u b m i t t e d f o r a n y o t h e r d e g r e e . : ~­ ~·· S a m a n M a n j u l a W e l i h i n d a D a t e - 0 8 - 0 2 - 2 0 I 0 1 e n d o r s e t h e d e c l a r a t i o n b y t h e c a n d i d a t e . : j l . I , / ' t i ' P r o f L a n k a U d a w a t t a l ) c c l a r u t i o n \ b s t r a c t \ c k n m \ l e d g c m e n t I i s t o i ' • \ b b r e v i a t i o n s L i s t o l · I i g u r e s I J s l C l l . I ~1bles C h a p t e r s I . I n t r o d u c t i o n I . I B i p e d a l R o b o t C o n t e n t s I . 2 B a s i c s o f b i p e d a l r o b o t w a l k i n g 1 . 2 . 1 C a i t p h a s e s 1 . 2 . 2 S t a t i c a n d d y n a m i c w a l k i n g I . 2 . 3 Z e r o m o m e n t p o i n t I . 3 R e v i e w a n d r e s e a r c h I A M o t i v a t i o n 1 . 5 R e s e a r c h o b j e c t i v e I . 6 O v e l ' \ i e \ v 2 . B i p e d a l R o b o t D e s c r i p t i o n a n d T r a j e c t o r y P l a n n i n g 2 . 1 B i p e d a l R o b o t D e s c r i p t i o n 1 1 I r a j e c t o r y P l a n n i n g 2 . 2 . 1 T r a j e c t o r ) 2 . 2 . 1 . 1 J o i n t s p a c e t e c h n i q u e s 2 . 2 . 1 . 2 C a r t e s i a n s p a c e t e c h n i q u e s , . . ~ 2 . 2 . 2 S e l e c t i o n o f T r a j e c t o r i e s f o r t h e b i p e d a l r o b o t 2 . 2 . 2 . 1 T r a j e c t o r y f o r s w i n g l e g 2 . 2 . 2 . 2 T r a j e c t o r y f o r h i p 2 . 2 . 2 . 3 R e l a t i o n s h i p b e t w e e n s w i n g l e g m o v e m e n t a n d h i p m o v e m e n t 2 . 2 . 3 S e l e c t i o n o f j o i n t a n g l e t r a j e c t o r y f o r t h e b i p e d a l r o b o t J . 1 \ : . i n c m a t i c s o f t h e b i p e d a l r o b o t ) I K i n e m a t i c s o f t h e b i p e d a l r o b o t 3 . 1 . 1 K i n e m a t i c s o f t h e s w i n g l e g 3 . 1 . 2 C o o r d i n a t e c o n v e r s i o n 3 . 2 K i n e m a t i c s o f t h c s t a n c e l e g 3 . 2 . 1 J o i n t a n g l e s o f s t a n c e l e g 3 . 2 . 2 C o o r d i n a t e c o n v e r s i o n > . 3 C a l c u l a t i o n o f j o i n t a n g l e s 3 . 3 . 1 C a l c u l a t i o n o f j o i n t a n g l e s o f s w i n g l e g 3 . 3 . 2 C a l c u l a t i o n o f j o i n t a n g l e s o f s t a n c e l e g v V I V I I V I I I X 1 2 4 5 8 1 3 1 3 1 4 1 5 1 5 1 6 1 6 1 6 1 6 1 7 1 7 1 8 1 9 1 9 2 2 2 2 2 2 2 4 2 4 2 4 2 5 2 6 2 6 2 6 I I 3 . 3 . 3 M o d i f i c a t i o n o f j o i n t a n g l e s o f s w i n g l e g t o c o m p e n s a t e h i p 2 6 m o v e m e n t - L S i m u l a t i o n o f t h e b i p e d a l r o b o t 4 . 1 S i m u l a t i o n s o f t w a r e 4 . 2 S i m u l a t i o n o f s w i n g l e g 4 . 3 S i m u l a t i o n o f s t a n c e l e g 4 . 4 S i m u l a t i o n o f b o t h l e g s 4 . 5 S i m u l a t i o n o r l o w e r b o d y a n d t o r s o , • 2 8 2 8 2 8 3 2 " " _ ) - ' 3 6 5 . C a l c u l a t i o n o f Z M P a n d D y n a m i c s t a b i l i t y 3 9 5 . 1 G e n e r a l Z M P e q u a t i o n f o r t h e b i p e d a l r o b o t 3 9 5 . 2 D y n a m i c S t a b i l i t y M a r g i n ( D S M ) o f b i p e d a l r o b o t 3 9 5 . 3 A p p l i c a t i o n o f Z M P e q u a t i o n f o r t h e b i p e d a l r o b o t 4 0 5 . 3 . 1 M a s s c e n t r e c o o r d i n a t e s o f l i n k s 4 1 5 . 3 . 2 A n g u l a r a c c e l e r a t i o n o f l i n k s 4 1 5 . 3 . 3 A c c e l e r a t i o n o f l i n k s 4 2 5 . 3 . 4 I n e r t i a o f l i n k s 4 2 5 . 3 . 5 B u i l d i n g M A T L A B p r o g r a m t o c a l c u l a t e Z M P 4 3 5 . 4 C a l c u l a t i o n m e t h o d o f Z M P 4 3 5 . 5 / M P o f t h e l o w e r b o d y o r b i p e d a l r o b o t 4 4 5 . 6 Z M P v a r i a t i o n o f t h e l o w e r b o d y o f t h e b i p e d a l r o b o t 4 4 5 . 7 A d d i n g T o r s o 4 5 5 . 7 . 1 M a s s c e n t r e c o o r d i n a t e s o f t o r s o 4 5 5 . 7 . 2 A n g u l a r a c c e l e r a t i o n o f t o r s o 4 6 5 . 7 . 3 A c c e l e r a t i o n o f t o r s o 4 6 5 . 8 D e f i n i n g t o r s o a n g l e 4 6 ) C ) I M P o f t h e b i p e d a l r o b o t v v i t h t o r s o 4 7 " . 1 0 Z M P v a r i a t i o n o r t h e b i p e d a l r o b o t w i t h t o r s o 4 7 : ' . I I / M P o l ' t h c b i p e d a l r o b o t w i t h d i f f e r e n t t o r s o a n g l e s 4 8 5 . 1 2 Z M P v a r i a t i o n o f t h e b i p e d a l r o b o t w i t h d i f f e r e n t t o r s o a n g l e s 4 9 6 . B e h a v i o r a l a n a l y s i s o f b i p e d a l r o b o t 5 0 6 . 1 E f f e c t o f t o r s o a n g l e o n d y n a m i c s t a b i l i t y o n v a r y i n g s l o p e s 5 0 6 . 1 . 1 B i p e d a l r o b o t v . a l k i n g o n 5 d e g r e e s l o p e 5 1 6 . 1 . 2 B i p e d a l r o b o t w a l k i n g o n I 0 d e g r e e s l o p e 5 2 6 . 1 . 3 B i p e d a l r o b o t w a l k i n g o n 1 5 d e g r e e s l o p e 5 2 6 . 2 E f f ' c c t o f t o r s o a n g l e o n d y n a m i c s t a b i l i t y a t d i f f e r e n t s t e p s 5 3 6 . 2 . 1 B i p e d a l r o b o t v v a l k i n g o n f u l l s t e p 5 3 6 . 2 . 2 B i p e d a l r o b o t w a l k i n g o n h a l f s t e p 5 4 6 . 2 . 3 B i p e d a l r o b o t w a l k i n g o n q u a r t e r s t e p 5 4 ( J . 3 r ! T e e t o f t o r s o a n g l e o n d y n a m i c s t a b i l i t y a t d i f f e r e n t s t e p d u r a t i o n s 5 5 6 . 4 U l e c t o f t o r s o m a s s o n d y n a m i c s t a b i l i t y o f r o b o t 5 6 6 . 4 . 1 D y n a m i c s t a b i l i t y w i t h d i f f e r e n t m a s s e s o n t o r s o a t t o r s o a n g l e o f 5 6 ] ( ) ( ) I l l 6 . 4 . 2 D v n a m i c s t a b i l i t v w i t h d i f i C r c n t m a s s e s o n t o r s o a t t o r s o a m . ; l c o f 5 7 1 . : : ( ) . ~ ) 6 . 5 E f f e c t o n l e n g t h o f ' t o r s o o n d y n a m i c s t a b i l i t y o f r o b o t 5 7 6 . 6 E J T e c t o n w e i g h t s o f l o w e r b o d y o n d y n a m i c s t a b i l i t y o f r o b o t 5 8 6 . 7 B e h a v i o r a l a n a l y s i s o f j o i n t a n g l e s 6 0 7 . C o n c l u s i o n s a n d F u t u r e w o r k 6 2 7 . 1 C o n c l u s i o n s 6 2 7 . 2 F u t u r e W o r k 6 3 R e f e r e n c e s A p p e n d i c e s A p p e n d i x A M J \ T L A B p r o g r a m s A p p e n d i x B J o i n t a n g l e t r a j e c t o r i e s A p p e n d i x C : S i m u l a t i o n p r o g r a m s ~· 6 4 6 6 6 6 8 2 8 6 I V A c k n o w l e d g e m e n t III~lllb , t r c d u e p r i m a r i l ) t o 1 1 1 ) s u p e r v i s o r . P r o f e s s o r L a n k a U d m \ a t t a , f o r t h e c o n t r i b u t i o n ,1thi~ g t c < t t i i h i g h t s . p e r s p c c t i \ e s . c o r r e c t - g u i d a n c e a n d s e n s e o f h u m o r . I n a l l . I l l ) s i n c e r e t h a n k s g o t o t h e o f f i c e r s i n t h e P o s t G r a d u a t e O f f i c e . F a c u l t y o f l . n g i n e e r i n g . a n d t o t h e U n i v e r s i t y o f M o r a t u w a o f S r i L a n k a f o r t h e c o r p o r a t i o n e x t e n d e d t o m e i n n u m e r o u s \ \ a y s f o r t h e c l a r i f i c a t i o n o f m a t t e r s r e l a t e d t o m y a c a d e m i c s t u d i e s o n t i m e t i m H i g h o u t t h e c o u r s e . - ' .\l~o I l l ) s i n c e r e g r a t i t u d e e x t e m l e d t o t h e s t a f f w h o s e r v e s i n t h e D e 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 o f f i c e . I a l s o w i s h t o t h a n k m y f e l l o w e n g i n e e r s i n o f f i c e f o r t h e i r g e n e r o u s s u p p o r t g i v e n f r o m t h e b e g i n n i n g t o t h e e n d o f t h e c o u r s e . I a l s o c : - . . : t e n d m y s i n c e r e t h a n k s t o M r . A r u n a A b e y r a t n e f o r d e r i v i n g t h e e q u a t i o n s I l e e e s s < t i . \ f o r t h e s i m u l a t i o n s a n d c a l c u l a t i o n s . I s h o u l d t h a n k m a n : i n d i v i d u a l s . f r i e n d s a n d c o l l e a g u e s w h o h a v e n o t b e e n m e n t i o n e d h e r e p e r s o n a l ! ) i n m a k i n g t h i s e d u c a t i o n a l p r o c e s s a s u c c e s s . l~tst h u t n u t l e a s t I l l ) g r a t i t u d e g o e s t o m y w i f e f o r t h e l o v e , m o r a l s u p p o r t a n d u n d e r s t a n d i n g t i · o m s t a r t t o e n d o l ' t h i s c o u r s e . V I L i s t o f A b b r e v i a t i o n s a D o F g \ I T D - H r Z M P F R I F 7 r v i P I . I l l ( } C J . \ \ 1 \ C O M N P C M c , s , i f ( I ) S l o p e A n g l e D e g r e e s o f f r e e d o m A c c e l e r a t i o n d u e t o g r a v i t y S t e p l e n g t h S t e p T i m e D e n a v i t - H a r t e n b e r g n o t a t i o n L e n g t h o r l i n k i Z e r o m o m e n t p o i n t F o o t r o t a t i o n i n d e x F i c t i o n a l z e r o m o m e n t p o i n t L e n g t h o r T o r s o M a s s o i ' T o r s o T o r s o a n g l e C i e n e t i c a l g o r i t h m l \ e u r a l n e t \ \ o r k C e n t r e o r M a s s N o r m a l P r o j e c t i o n o f t h e C e n t r e o f M a s s C o s i n e o r j o i n t a n g l e i S i n e o f j o i n t a n g l e i J o i n t a n g l e a t t i m e t ~· V I I L i s t o f F i g u r e s F i g u r e 1 . 1 A n a t o m i c a l p l a n e s 1 . 2 L e g p o s i t i o n s d u r i n g o n e h a l f c y c l e 1 . 3 T h e c y c l e p h a s e r o t a t i o n o f b i p e d a l w a l k i n g 1 . 4 A b i p e d a l r o b o t s ' g a i t c y c l e 1 . 5 F o r c e s a n d t o r q u e a c t i n g o n a r o b o t f o o t 1 . 6 S i m p l i f i e d f o o t d y n a m i c s " ' 1 . 7 T h e b i p e d a l r o b o t s E O - E 3 i n t r o d u c e d b y H o n d a f r o m 1 9 8 6 - 1 9 9 1 1 . 8 i . 9 ! . I ( ) 1 . 1 1 1 . 1 2 2 . 1 I I I ~ - · · ' T h e b i p e d a l r o b o t s E 4 - E 6 i n t r o d u c e d b y H o n d a f r o m 1 9 9 1 - 1 9 9 3 I h e h u m a n o i d r o b o t s P I - P 3 p r e s e n t e d b y H o n d a f r o m 1 9 9 3 - 1 9 9 7 I h e h u m a n o i d r o b o t A S I M O l l u m a n o i d " ' Q R I O " i n t r o d u c e d b y S o n y c o m p a n y M I T h u m a n o i d r o b o t s : ( a ) S p r i n g F l a m i n g o . ( b ) S p r i n g T u r k e y . ( c ) ( l e e k . B o t 7 l i n k . 7 D o l - b i p e d a l r o b o t s , , i n g l e g a n d H i p t r a j e c t o r i e s A n g u l a r v e l o c i t y v a r i a t i o n o f t h e s w i n g l e g a n d s t a n c e l e g j o i n t a n g l e s f o r h a l f C \ C l e d u r a t i o n 3 . 1 S \ \ i n g l e g m o t i o n o f b i p e d a l r o b o t 3 . 2 S t a n c e l e g m o t i o n o f b i p e d a l r o b o t 3 . 3 T h e m o t i o n s o f s v · , i n g l e g a n d s t a n c e l e g - 1 . I F o o t t r a j e c t o r y a t 5 d e g r e e s l o p e - 1 . 2 I n i t i a l p o s e o f t h e S \ \ i n g l e g s i m u l a t i o n I ' s . I . I . 4'~1ll 1 j . - t . . ' . ' ' 1 1 1 g e g s 1 m u a t I o n a t · _ c a t a p o i n t - 1 - 1 s , , i n g l e g s i m u l a t i o n a t 1 6 3 ' d d a t a p o i n t - f . : ' I i n a ! p o s e o f t h e S \ \ i n g l e g s i m u l a t i o n - 1 . 6 I h e s w i n g l e g f o l l O \ v i n g p r e d e f i n e d t r a j e c t o r y - 1 . 7 T h e i n i t i a l a n d f i n a l p o s i t i o n s o f t h e s t a n c e l e g - 1 . 8 T h e s t a n c e l e g f o l l O \ v i n g t h e p r e d e f i n e d t r a j e c t o r y - f l ) I n i t i a l p o s i t i o n o f l o \ \ e r b o d y - I . I U L m \ e r b o d y s i m u l a t i o n a t 3 3 ' 0 d a t a p o i n t - 1 . 1 1 I 0 \ \ e r b o d y s i m u l a t i o n a t 1 5 4 1 1 1 d a t a p o i n t - 1 . 1 2 l i n a ! p o s i t i o n o f t h e ] ( m e r b o d y 4 . 1 3 L O \ \ C r b o d y f o l l o w i n g f o o t a n d h i p t r a j e c t o r y 4 . 1 4 I n i t i a l p o s i t i o n o f t h e b i p e d a l r o b o t 4 . 1 5 S i m u l a t i o n a t 4 i 1 1 d a t a p o i n t 4 . 1 6 S i m u l a t i o n a t 1 4 2 1 1 0 d a t a p o i n t 4 . 1 7 T h e f i n a l p o s i t i o n o f m o t i o n 4 . 1 8 T h e S \ v i n g l e g . h i p a n d t o r s o m o v e m e n t ' 1 . 1 D ) n a m i c s t a b i l i t y s a f e m a r g i n 5 . 2 L O \ w r b o d y o f b i p e d a l r o b o t 5 . 3 / M J > v a r i a t i o n o f L c m e r b o d y o f t h e b i p e d a l r o b o t P a g e 2 ' " > _ ) 3 4 5 6 8 8 9 1 0 I I 1 2 1 5 1 7 2 0 2 2 2 4 2 7 2 9 2 9 3 0 3 0 3 1 3 1 " ' I .)~ 3 2 ' " > ' " > . ) . ) ' " ' ' " ' . 1 . 1 3 4 3 4 3 5 3 6 3 6 3 7 3 7 3 8 4 0 4 0 4 4 V I I I ~-+ 1 3 i p c d a l r o b o t w i t h T o r s o 4 5 5 . 5 I l l u s t r a t i o n o f t o r s o a n g l e 4 6 5 . 6 / ' v 1 P o f b i p e d a l r o b o t w i t h T o r s o 4 8 5 . 7 I M P v a r i a t i o n o ! ' b i p e d a l r o b o t w i t h d i ! ' l ' e r e n t t o r s o a n g l e s 4 9 6 . 1 I M P v a r i a t i o n w i t h t o r s o a n g l e a t 5 d e g . s l o p e w i t h f u l l s t e p l e n g t h 5 1 6 . 2 1 ! \ 1 P v a r i a t i o n \ \ i t h t o r s o a n g l e a t I 0 d e g . s l o p e w i t h f u l l s t e p l e n g t h 5 2 6 . 3 / M P v a r i a t i o n w i t h t o r s o a n g l e a t 1 5 d e g . s l o p e w i t h f u l l s t e p l e n g t h 5 2 6 . 4 Z M P v a r i a t i o n w i t h t o r s o a n g l e a t 5 d e g . s l o p e w i t h f u l l s t e p l e n g t h 5 3 6 . 5 Z M P v a r i a t i o n w i t h t o r s o a n g l e a t 5 d e g . s l o p e w i t h h a l f s t e p l e n g t h 5 4 6 . 6 Z M P v a r i a t i o n w i t h t o r s o a n g l e a t 5 d e g . s l o p e w i t h g u a r t e r s t e p l e n g t h 5 4 6 . 7 Z M P v a r i a t i o n w i t h t o r s o a n g l e a t 5 d e g . s l o p e w i t h f u l l s t e p l e n g t h v a r y i n g 5 5 s t e p t i m e 6 . 8 I M P v a r i a t i o n k e e p i n g t o r s o a n g l e a t I 0 d e g . a n d v a r y i n g t o r s o m a s s 5 6 6 . 9 Z M P v a r i a t i o n k e e p i n g t o r s o a n g l e a t 1 5 d e g . a n d v a r y i n g t o r s o m a s s 5 7 6 . 1 0 Z M P v a r i a t i o n k e e p i n g t o r s o a n g l e a t I 0 d e g . a n d v a r y i n g t o r s o l e n g t h 5 8 6 . 1 1 Z M P v a r i a t i o n k e e p i n g t o r s o a n g l e a t I 0 d e g . a n d v a r y i n g t h e w e i g h t s o f l o w e r 5 9 b o d y 6 . 1 2 A n g u l a r p o s i t i o n . A n g u l a r v e l o c i t y a n d A n g u l a r a c c e l e r a t i o n o f j o i n t a n g l e 8 1 6 0 6 . 1 3 A n g u l a r p o s i t i o n . A n g u l a r v e l o c i t y a n d A n g u l a r a c c e l e r a t i o n o f j o i n t a n g l e 0 2 6 0 6 . 1 4 A n g u l a r p o s i t i o n , A n g u l a r v e l o c i t y a n d A n g u l a r a c c e l e r a t i o n o f j o i n t a n g l e 0 , 6 1 6 . 1 5 A n g u l a r p o s i t i o n . A n g u l a r v e l o c i t y a n d A n g u l a r a c c e l e r a t i o n o ! " j o i n t a n g l e 8 ( , 6 1 I X L i s t o f T a b l e s I a I l k : ; i l l - 1 1 pdrcunetcr~ o f S \ \ i n g k g : 1 . 2 D - 1 1 p a r a m e t e r s o f s t a n c e l e g - 1 - . 1 P a r a m e t e r s f o r t h e s w i n g l e g s i m u l a t i o n 5 . 1 P a r a m e t e r s f ( ) r t h e c a l c u l a t i o n o f Z M P o f l m v e r b o d y 5 . 2 P a r a m e t e r s f o r t h e c a l c u l a t i o n o f Z V I P o f b i p e d a l r ( 1 b o t w i t h t o r s o \ 3 P a r a m e t e r s f o r t h e c a l c u l a t i o n o f Z M P o f b i p e d a l r o b o t w i t h d i f f e r e n t t o r s o a n g l e s 6 . 1 P a r a m e t e r s f o r t h e c a l c u l a t i o n o f Z M P o f b i p e d a l r o b o t a t d i f f e r e n t s l o p e s 6 . 2 P a r a m e t e r s f o r t h e c a l c u l a t i o n o f Z M P o f b i p e d a l r o b o t a t d i f f e r e n t s t e p s 6 . 3 P a r a m e t e r s f o r t h e c a l c u l a t i o n o f Z M P o f b i p e d a l r o b o t a t d i f f e r e n t s t e p t i m e s 6 . 4 P a r a m e t e r s f o r t h e c a l c u l a t i o n o f Z M P o f b i p e d a l r o b o t w i t h d i f f e r e n t m a s s e s o f l o r s o 6 . 5 P a r a m e t e r s f o r t h e c a l c u l a t i o n o f Z M P o f b i p e d a l r o b o t w i t h d i t T e r e n t l e n g t h s o f I o r s u P a g e I " . c _ _ ) 2 5 2 8 4 4 4 7 4 8 5 0 5 3 5 5 5 6 5 8 ( J . ( J l ) a r a m e t e r s f o r t h e c a l c u l a t i o n o f / M P o f b i p e d a l r o b o t w i t h d i f f e r e n t w e i g h t s o f 5 9 ] ( m e r b o d v X C h a p t e r 1 I n t r o d u c t i o n . ; r I n rcc~_'tll : e a r s t h e r e h a s b e e n m u c h i n t e r e s t s i m u l a t e d i n b i p e d a l r o b o t i c s a n d l e g g e d l o c r l m o t i o n i n g e n e r a l . P a r t o f t h e r e a s o n f o r t h i s i n t e r e s t i s t h e n e e d f ( ) r r o b o t s w h i c h c a n o p e r a t e i n h u m a n o r i e n t e d e n v i r o n m e n t s . H m v c v c L t h e b i g g e s t c h a l l e n g e i s b i p e d w a l k i n g l i k e t h a t o r a h u m a n b e i n g . w h i c h \ \ a s i m p r o v e d o v e r m a n y c e n t u r i e s o f e v o l u t i o n . S i n c e a p p r O \ i m a t c l y 1 9 8 0 . t h e r e s e a r c h h a s b e e n m o r e f o c u s s e d o n b i p e d w a l k i n g . 1 . 1 B i p e d a l r o b o t T h e h u m a n b e i n g s a n d a l m o s t a l l o n l a n d l i v i n g a n i m a l s u s e l e g s f o r l o c o m o t i o n . H o w e v e r n u t m a n y m a c h i n e s w e r e b u i l t u s i n g l e g s f o r m o v e m e n t . T h e r e a s o n s t h e r e f o r e a r e t h e c o m p l e x d e s i g n a n d c o n t r o l . N e v e r t h e l e s s . t h e m a i n a d v a n t a g e o f w a k i n g m a c h i n e s i s t h a t i n c o n t r a s t t o \ \ h e e l e d r o b o t s . t h e y d o n o t n e e d a c u s t o m i z e d e n v i r o n m e n t . T h e y c o u l d b e : 1 h l c t u n w \ e i n a n e n v i r o n m e n t t h a t i s o n l y a c c e s s i b l e b y h u m a n b e i n g s . I n t h e o r y n o t o n l y 1 \ C d k i t l g b u t a l s o r u n n i n g . j u m p i n g . c l i m b i n g o r e v e n s w i m m i n g c o u l d b e i m p l e m e n t e d . I n u l t l t l ' a s t . \ \ h e e l e d m a c h i n e s n e e d a r e l a t i v e p l a n a r t e r r a i n a n d e n o u g h s p a c e t o a v o i d oh~tacle:-,. B i p e d s u s c d i f f e r e n t s u p p o r t a r e a s f o r c a r r y i n g t h e i r w e i g h t a n d g e t t i n g g r i p a n d a r e i n t h e i d e a l c a s e a s f~lst a n d t l e x i b l e a s a h u m a n . U s i n g t h i s t l e x i b l e s u p p o r t o n t h e g r o u n d . a l a r g e a d a p t a b i l i t y i s a c h i e v e d . T h e l e g s c a n a l s o b e c o n s i d e r e d a s a n i n d i v i d u a l \ U S p c n s i o n S ) s t e m w h e r e b ) t h e u p p e r p a r t o f t h e b o d y m o v e s f o r w a r d o n a n o t h e r t r a j e c t o r y a s t h e k e t . D c c o u p l i n g t h e l e g s f ' r o m t h e r e s t o f t h e b o d y a l l o w s c a r r y i n g p a y l o a d s m o o t h t h r o u g h a r o u g h t e r r a i n . B o t h t ) p e s o r r o b o t s a r c d e s i g n e d f o r a s p e c i f i e d e n v i r o n m e n t : T h e 1 1 h e e l e d r o b o t s a r c m o r e e f f i c i e n t o n a p l a n a r s u r f a c e w h e r e a s w a l k i n g m a c h i n e s h a v e a n a d v a n t a g e o n a l l o t h e r t e r r a i n s . T h e o p e r a t i o n a l a r e a o f r o b o t s . e s p e c i a l l y w i t h t w o l e g s . i s t h e n a t u r a l s e t t i n g o f h u m a n s . T h e h u m a n b o d y h a s , b e c a u s e o f h i s a n a t o m y . a n e x c e p t i o n a l l y m a n e u v e r a b i l i t y w h i c h i s p e r f e c t e x p l o i t e d f o r h i s l o c o m o t i o n . T h u s , h e c a n a d a p t t o a n e w e n v i r o n m e n t w i t h m i n i m a l e f f o r t . 1 . 2 B a s i c s o f b i p e d a l w a l k i n g T o u n d e r s t a n d t h e t o p i c o f t h e b i p e d w a l k i n g a n o v e r v i e w o f a h u m a n m o d e l w i l l b e s h o w n . l m t h e r e a s o n t h a t m o s t o f t h e h u m a n o i d r o b o t s u s e t h e h u m a n b o d y a s p a r a g o n . i t i s \ u g g e s t i \ e t o u s e t h e s a m e t e r m i n o l o g y a s f o r t h e h u m a n a n a t o m y . T h e r e a r e t h r e e b a s i c p l a n e s r e f e r r e d t o a s f r o n t a l ( o r c o r o n a l ) . s a g i t t a l a n d t r a n s v e r s a l a s s h o w n i n F i g u r e 1 . 1 . " T r a n s v e r s e F r o n t a l S a g i t t a l F i g u r e 1 . 1 : A n a t o m i c a l p l a n e s 1 . 2 . 1 G a i t P h a s e s \ \ a \ k i n g i s a C ) c l i c m o v e m e n t c o n s i s t i n g o f t w o m a i n p h a s e s , w h i c h a l t e r n a t e s o n b o t h l e g s a n d i l l u s t r a t l ' d i n F i g u r e 1 . 2 a n d 1 . 3 . D o u b l e s u p p o r t p h a s e i s t h e p h a s e d u r i n g w h i c h b o t h f e e t a r e i n t o u c h w i t h g r o u n d . I n s i n g l e s u p p o r t p h a s e o n l y o n e f o o t i s i n t o u c h w i t h g r o u n d w h i l e o t h e r C o o t i s i n s w i n g i n g . D u r i n g t h e d o u b l e s u p p o r t p h a s e ( 1 ) . b o t h f e e t a r c i n c o n t a c t w i t h t h e g r o u n d . I n t h i s p h a s e . t h e b o d : h a s a s t a b l e p o s i t i o n b e c a u s e o r t h e w i d e s u p p o r t a r e a o n t h e g r o u n d . T h e s y s t e m e n t e r s t h i s s t a t e w i t h t h e h e e l s t r i k e ( I V ) a n d e x i t s i t w i t h t h e t o e o f f ( I I ) m o v e m e n t . D u r i n g t h e s i n g l e s u p p o r t p h a s e , o n l y o n e f o o t i s i n c o n t a c t w i t h t h e f l o o r . I n t h i s s t a t e . t h e c e n t r e o f m a s s ( C O M ) o f t h e s y s t e m r o t a t e s l i k e a n i n v e r t e d p e n d u l u m a b o v e t h e c o n t a c t p o i n t . M e a n w h i l e t h e s w i n g l e g m o v e s f o r w a r d ( I l l ) t o t o u c h t h e g r o u n d a g a i n a n d e n t e r t h e o t h e r p h a s e . 2 F i g u r e 1 . 2 : L e g p o s i t i o n s d u r i n g o n e h a l f c y c l e T h e F i g u r e 1 . 2 s h o w t h e f o u r l e g p o s i t i o n s d u r i n g o n e h a l f c y c l e w h i l e w a l k i n g . T h e c o m p l e t e w a l k i n g c y c l e i l l u s t r a t e d i n F i g u r e 1 . 3 . T h e r i g h t l e g a n d l e f t l e g i s g o i n g t h r o u g h t h e s a m e s e q u e n c e a l t e r n a t e l y a n d t h i s r e p e a t s w h i l e w a l k i n g . I I R i 2 h t i L e f t = I l e o 1 l e o b I b I l l I V F i g u r e 1 . 3 : T h e c y c l i c p h a s e r o t a t i o n o f b i p e d a l w a l k i n g ' " ' . ) 1 . 2 . 2 S t a t i c a n d . D y n a m i c W a l k i n g W a l k i n g c a n b e d i v i d e d i n t o t w o m a i n g r o u p s : w a l k i n g w i t h s t a t i c b a l a n c e a n d w a l k i n g ' ' i t h d ) n a m i c b a l a n c e . T h e F i g u r e 1 . 4 s h o w s t h e g a i t c y c l e o f a b i p e d a l r o b o t . D u r i n g a s t a t i c w a l k r 1 J t h e n o r m a l p r o j e c t i o n o f t h e c e n t r e o f m a s s ( N P C M ) a l w a y s s t a y s i n b c t \ \ c c n t h e b o u n d a r i e s d e l i n e d b y t h e f e e t . I f b o t h f e e t a r e o n t h e g r o u n d . t h e N P C M h a s t o b e w i t h i n t h e p o l y g o n d e t e r m i n e d b y t h e o u t e r c o r n e r s o f t h e b i p e d f e e t . I f o n l y o n e f o o t i s i n c o n t a c t w i t h t h e g r o u n d . t h e N P C M h a s t o b e w i t h i n t h e a r e a o f t h i s f o o t . W h i l e t h e m o v e m e n t i s s l o w e n o u g h . t h e S ) ' S t c m d v n a m i c s c a n b e i r m o r e d . S t a t i c w a l k i n g a s s u m e s . _ . , J ' - ' ' . . . . . . _ _ t h a t i f t h e s y s t e m ' s m o t i o n i s s t o p p e d a t a n y t i m e . i t w i l l s t a y i n a s t a b l e p o s i t i o n i n d c l i n i t c l y . I l m v c v e r . t h e s p e e d a c h i e v e d u s i n g s t a t i c w a l k i s n o t t h a t h i g h a n d t h e e f f i c i e n c y i s t~u· a w a y f r o m t h e h u m a n w a l k i n g s p e e d . I n c o n t r a s t . a d y n a m i c a l l y o r a c t i v e l y b a L m c e d w a l k [ 2 ] i s n o t c o n s t r a i n e d i n s u c h a m a n n e r . T h e C O M m a y l e a v e t h e s u p p o r t a r e a f o r m e d b y t h e f e e t f o r p e r i o d s o f t i m e . T h i s a l l o v • s t h e s y s t e m t o e x p e r i e n c e t i p p i n g m o m e n t s , w h i c h g i v e r i s e t o h o r i z o n t a l a c c e l e r a t i o n . l l o w e v c r . s u c h p e r i o d s o f t i m e a r e k e p t b r i e f a n d s t r i c t l y c o n t r o l l e d s o t h a t t h e s y s t e m d o e s n o t b e c o m e u n s t a b l e . T h u s o n e m a y t h i n k o f a d y n a m i c a l l y b a l a n c e d s y s t e m a s o n e w h e r e ~mall a m o u n t s o f c o n t r o l l e d i n s t a b i l i t y a r e i n t r o d u c e d i n s u c h a w a y a s t o m a i n t a i n t h e o \ e r a l l e q u i l i b r i u m . T i p p i n g m o m e n t s i n o n e d i r e c t i o n a r e n e g a t e d b y t i p p i n g m o m e n t s i n t h e o p p o s i t e d i r e c t i o n . 0 ~ 0 0 € ) 0 0 0 0 0 0 0 E ) F i g u r e 1 . 4 : A b i p e d a l r o b o t ' s g a i t c y c l e ; t h e r e d c i r c l e s i n d i c a t e t h e f o o t p r i n t s ; t h e s h a d e d a r e a i s t h e s u p p o r t i n g a r e a : t h e l e g m o v e m e n t i s e x p r e s s e d b y t h e a r r o w s \ \ h e n c o m p a r i n g t h e t w o m e t h o d s o f b a l a n c e , i t c a n b e s e e n t h a t t h e s t a t i c m e t h o d i s h i g h l y r e s t r i c t i v e a n d r e s u l t s i n m o v e m e n t w h i c h i s s l o w . V e r y r a r e l y d o a n i m a l s a n d h u m a n s e x h i b i t s u c h b e h a v i o r f o r t h i s r e a s o n t h e v e l o c i t y a c h i e v a b l e i s v e r y l o w a n d t h e m o t i o n i s n o t e f f i c i e n t . l l o w c v e r . i t c a n s e c t h a t b y r e m o v i n g t h e c o n s t r a i n i n g n a t u r e o f t h e r u l e l o r s t a t i c b a l a n c e t h a t t h e m o b i l i t y o f t h e s y s t e m i s i n c r e a s e d . T h i s i s d u e t o t h e i n c r e a s e d e d i b i l i t y o f t h e m o v e m e n t o f t h e l e g s a n d p l a c e m e n t o f t h e f e e t . T h e a c c e l e r a t i n g t i p p i n g m o m e n t s c a n b e u s e d t o a c h i e v e h i g h e r s p e e d s . m o v e a l l l e g s a t o n c e o r t o u t i l i z e f o o t h o l d s \ \ h i c h a r e f~u· a p a r t . r h c r e f o r e i t c a n b e s e e n t h a t i n o r d e r t o r a b i p e d a l r o b o t t o g a i n e f f i c i e n c y a n d s p e e d , i t w i l l r e q u i r e d y n a m i c b a l a n c e . 4 1 . 2 . 3 Z e r o m o m e n t P o i n t P e r h a p s t h e m o s t w i d e l y u s e d c l a s s i c a l a p p r o a c h t o d y n a m i c b i p e d w a l k i n g a r c t h o s e c o n t r o l s y s t e m s b a s e d o n t h e m e a s u r e m e n t o f z e r o m o m e n t p o i n t ( Z M P ) [ 3 ] [ 4 ] . T h e Z M P i \ a t o o l u s e d t o m e a s u r e t h e d y n a m i c s t a b i l i t y o f a w a l k i n g s y s t e m . I n a n d o f i t s e l f , t h e / \ 1 P i s n o t a c o m p l e t e c o n t r o l s y s t e m . R a t h e r . t h e Z M P m e a s u r e m e n t i s u s e d b y a c o n t r o l S ) s t e m i n t h e s a m e w a y t h a t t h e c e n t r e o f m a s s p r o j e c t e d o n t o t h e g r o u n d ( G C o M ) c a n b e u s e d f o r s t a t i c w a l k i n g s y s t e m s . I t i s u s e d i n t h e o f f l i n e g e n e r a t i o n o f d y n a m i c a l l y s t a b l e 1 1 a l k i n g g a i t s . a n d o n l i n e t o p r e d i c t i f a s y s t e m n e e d s t o t a k e s o m e k i n d o f c o r r e c t i v e a c t i o n t o p r e v e n t t h e l o s s o f i t s d y n a m i c s t a b i l i t y . _ , D : n a m i c s t a b i l i t y . a s d e ! i n e d b y r e s e a r c h e r s u s i n g Z M P w a l k i n g s t r a t e g i e s . h a s a s p e c i f i c m e a n i n g : I n o r d e r f o r a b i p e d w a l k e r t o b e d y n a m i c a l l y s t a b l e a t a p a r t i c u l a r i n t i m e . i t s I C e t m u s t r e m a i n m o t i o n l e s s 1 1 i t h r e s p e c t t o t h e g r o u n d l o r t h e e n t i r e t i m e t h a t t h e y s u p p o r t t h e r o b o t " s 1 1 c i g h t . I f a r o b o t b e g i n s t o • · t i p o v e r ' · t h e e d g e o f i t s s u p p o r t f o o t . t h e n i t i s n o l o n g e r c o n s i d e r e d d y n a m i c a l l y s t a b l e . ! h e Z M P i s d e f i n e d a s t h e p o i n t w i t h i n t h e s u p p o r t i n g p o l y g o n o f a d y n a m i c a l l y s t a b l e s t r u c t u r e a t w h i c h a s i n g l e g r o u n d r e a c t i o n f o r c e a c t s i n o r d e r t o c a u s e t h e s u m o f a l l m o m e n t s o f a c t i v e f o r c e s o n t h e r o b o t i c s y s t e m t o e q u a l z e r o . T h e d i a g r a m b e l o w ( F i g u r e 1 . 5 ) s h o w s a r e p r e s e n t a t i o n o f a t y p i c a l r o b o t f o o t . F . " \ . . t , _ G , . _ - . - . _ - - - . _ _ _ · - - _ _ t _ _ A T · . - ~· t l ~:,· - ' -{··-~:~~- : : _ : _ _ ' - - . " - - , , - - '·"~ ~-. . . . . . ,,,,_~ F - "~- / _ , . . . . · Z M P / . . - - _ - - - - - - · / " - · / f p F i g u r e 1 . 5 : F o r c e s a n d t o r q u e a c t i n g o n a r o b o t f o o t F , I n t e r a c t i o n b e t w e e n t h e f o o t a n d t h e r e s t o f t h e r o b o t ' s s t r u c t u r e c a n b e r e p r e s e n t e d b y a s i n g l e f o r c e F 1 1 a n d t o r q u e r 1 a c t i n g o n t h e f o o t ' s a n k l e j o i n t . A d d i t i o n a l f o r c e s w o r k i n g o n t h e s v s t e m i n c l u d e : 5 • h , . t h e g r a v i t a t i o n a l f o r c e a c t i n g o n t h e f o o t , t h r o u g h t h e f o o f s c e n t r e o f m a s s • I 1 a n d t 1 t h e f r i c t i o n a l f o r c e a n d t o r q u e p r e v e n t i n g t h e f o o t f r o m s l i d i n g o r t w i s t i n g a l o n g t h e g r o u n d . a n d • I , _ t h e g r o u n d r e a c t i o n f o r c e p r e v e n t i n g t h e f o o t f r o m p e n e t r a t i n g t h e g r o u n d sud~tce. T h e Z M P d e f i n i t i o n o f a d y n a m i c a l l y s t a b l e r o b o t r e q u i r e s t h e f o o t d o e s n o t r o l l o r t i l t w i t h r e s p e c t t o t h e g r o u n d d u r i n g g a i t e x e c u t i o n . T h i s m e a n s t h a t t o b e s t a b l e , t h e r e s u l t a n t o f a l l t h e p i t c h a n d r o l l t o r q u e s a c t i n g o n t h e f o o t m u s t b e z e r o . I t i s l e f t t o t h e g r o u n d r e a c t i o n f o r c e F J { t o c a n c e l o u t a n y h o r i z o n t a l c o m p o n e n t s o f t o r q u e r e s u l t i n g f r o m r ~ a n d h o r i / o n t a l t o r q u e c o n t r i b u t i o n s o r a l l o t h e r f o r c e s . S i n c e t i l e g r o u n d r e a c t i o n f o r c e i s f i x e d i n m a g n i t u d e . t h e o n l y \ \ a y i t c a n c o m p e n s a t e f o r v a r y i n g h o r i z o n t a l t o r q u e i s t o b e a p p l i e d a t d i l l c r c n t p o i n t s o n t h e r o h u t · s s u p p o r t i n g p o l y g o n . N o t e t h a t i t i s p o s s i b l e f o r a r o b o t " s ! ( J o t t o s l i d e w h i l e s t i l l s a t i s f y i n g t h e Z M P s t a b i l i t y c r i t e r i a . r . } - ' , : . . . . - - - - d · . , . - - - - S u p p o r t i n g p o t y g o n - - - - - - . . · F i g u r e 1 . 6 : S i m p l i t i e d f o o t d y n a m i c s I n t h e e x a m p l e d e p i c t e d b y F i g u r e 1 . 6 a b o v e . a s t h e d i s t a n c e d i n c r e a s e s . t h e r e a c t i v e t o r q u e p r o d u c e d b y F R a l s o i n c r e a s e s . s u c h t h a t : r = d * F R ( 1 . 1 ) I r a p o i n t c a n b e f o u n d w i t h i n t h e s u p p o r t i n g p o l y g o n s u c h t h a t t h e r e a c t i v e t o r q u e r 1 1 a p p l i e d a t t h i s p o i n t e x a c t l y c a n c e l s o u t a l l o t h e r t o r q u e s a c t i n g o n t h e C o o t , t h e n t h i s p o i n t i s k n m v n a s t h e z e r o m o m e n t p o i n t . a n d t h e r o b o t i s d y n a m i c a l l y s t a b l e . I n t h e e x a m p l e a b o v e . w h e n d c o i n c i d e s w i t h t h e Z M P , t h e r e a c t i v e f o r c e F R w i l l c a n c e l o u t a l l o t h e r t o r q u e s . s o i f a l l o t h e r f o r c e s a r e k n o w n , t h e l o c a t i o n o f t h e Z M P c a n b e c a l c u l a t e d : r 0 ( 1 . 2 ) 6 • h , . t h e g r a v i t a t i o n a l f o r c e a c t i n g o n t h e f o o L t h r o u g h t h e f o o t ' s c e n t r e o f m a s s • 1 1 a n d t 1 t h e f i · i c t i o n a l f o r c e a n d t o r q u e p r e v e n t i n g t h e f o o t f r o m s l i d i n g o r t \ v i s t i n g a l o n g t h e g r o u n d . a n d • l 1 , . t h e g r o u n d r e a c t i o n f o r c e p r e v e n t i n g t h e f o o t f r o m p e n e t r a t i n g t h e g r o u n d surl~tce. T h e Z M P d e f i n i t i o n o f a d y n a m i c a l l y s t a b l e r o b o t r e q u i r e s t h e f o o t d o e s n o t r o l l o r t i l t w i t h r e s p e c t t o t h e g r o u n d d u r i n g g a i t e x e c u t i o n . T h i s m e a n s t h a t t o b e s t a b l e . t h e r e s u l t a n t o f a l l t h e p i t c h a n d r o l l t o r q u e s a c t i n g o n t h e f o o t m u s t b e z e r o . I t i s l e f t t o t h e g r o u n d r e a c t i o n f o r c e F R t o c a n c e l o u t a n y h o r i z o n t a l c o m p o n e n t s o f t o r q u e r e s u l t i n g f r o m r 4 a n d h o r i / ( l f l t a l t o r q u e c o n t r i b u t i o n s o f a l l o t h e r f o r c e s . S i n c e t i l e g r o u n d r e a c t i o n f o r c e i s f i x e d i n m a g n i t u d e . t h e o n i . J w a y i t c a n c o m p e n s a t e f o r v a r y i n g h o r i z o n t a l t o r q u e i s t o h e a p p l i e d ~11 d i l k r c n t p o i n t s o n t h e r o b o t ' s s u p p o r t i n g p o l y g o n . N o t e t h a t i t i s p o s s i b l e f o r a r o b o t ' s f o o t t o s l i d e w h i l e s t i l l s a t i s l ' y i n g t h e Z M P s t a b i l i t y c r i t e r i a . r . } - ' , ~- - - - d · . . , . - - - - S u p p o r t i n g p o t y g o n - - - - - - . . . · ! - i g u r e 1 . 6 : S i m p l i J i e d l ' o o t d y n a m i c s I n t h e e x a m p l e d e p i c t e d b y I i g u r e 1 . 6 a b o v e . a s t h e d i s t a n c e d i n c r e a s e s , t h e r e a c t i v e t o r q u e p r o d u c e d b y F R a l s o i n c r e a s e s . s u c h t h a t : r = d * F 1 , ( I . l ) I f a p o i n t c a n b e f o u n d w i t h i n t h e s u p p o r t i n g p o l y g o n s u c h t h a t t h e r e a c t i v e t o r q u e r 1 1 a p p l i e d a t t h i s p o i n t e x a c t l y c a n c e l s o u t a l l o t h e r t o r q u e s a c t i n g o n t h e f o o t , t h e n t h i s p o i n t i s k n O \ v n a s t h e z e r o m o m e n t p o i n t , a n d t h e r o b o t i s d y n a m i c a l l y s t a b l e . I n t h e e x a m p l e a b o v e . w h e n d c o i n c i d e s w i t h t h e Z M P . t h e r e a c t i v e f o r c e F R w i l l c a n c e l o u t a l l o t h e r t o r q u e s . s o i f a l l o t h e r f o r c e s a r e k n o w n , t h e l o c a t i o n o f t h e Z M P c a n b e c a l c u l a t e d : r r l ) ( 1 . 2 ) 6 r " ' h~ ~ ( ) ( 1 . 3 ) d / \ 1 1 ' - r , / ~R ( I . 4 ) A s t o r q u e t r a n s m i t t e d t o t h e f o o t b y t h e r e s t o f t h e r o b o t s y s t e m i n c r e a s e s . t h e Z M P m u s t m o \ c c l o s e r t o t h e e d g e o f t h e f ( ) o t i n o r d e r t o g e n e r a t e e n o u g h r e a c t i v e t o r q u e t o m a i n t a i n d y n a m i c s t a b i l i t y . O n c e t h e Z M P r e a c h e s t h e f o o t ' s e d g e , t h e s y s t e m i s o n l y m a r g i n a l l y s t a b l e a n d a n y f u r t h e r i n c r e a s e i n a p p l i e d t o r q u e w i l l c a u s e t h e s y s t e m t o t i p o v e r . S o i n d~namically b a l a n c e d s y s t e m s . t h e Z M P h a s t h e s a m e r e l a t i o n s h i p t o t h e r o b o t ' s s u p p o r t i n g p o l y g o n a s t h e C J C o M d o c s i n s t a t i c a l l y b a l a n c e d s y s t e m s . . . . A s t h e Z M P m o v e s t o w a r d s t h e b o r d e r o r t h e r o b o t ' s s u p p o r t i n g p o l y g o n . t h e r o b o t ' s d y n a m i c s t a b i l i t y b e c o m e s m o r e m a r g i n a l . 8~ d e f i n i t i o n . t h e Z M P c a n n e v e r e x i s t o u t s i d e o f t h e r o b o t s s u p p o r t i n g p o l y g o n . I f t h e m b o t i s n o t d y n a m i c a l l y s t a b l e . t h e n t h e r e i s n o z e r o m o m e n t p o i n t . S o m e t i m e s i t i s u s e f u l t u b e a b l e t o c o m p a r e t h e r e l a t i v e s t a b i l i t y o f v a r i o u s u n s t a b l e s t a t e s . T o t h i s e n d . r e s e a r c h e r s h a v e e x t e n d e d t h e Z M P c o n c e p t t o i n c l u d e a n i m a g i n a r y p o i n t o u t s i d e o f t h e r o b o t ' s s u p p o r t i n g p o l y g o n . w h e r e t h e g r o u n d r e a c t i o n f o r c e w o u l d h a v e t o a c t i f i t w e r e t o m a i n t a i n d y n a m i c s t a b i l i t y . T h i s i m a g i n a r y f o r c e i s c a l c u l a t e d i n t h e s a m e w a y a s t h e Z M P . \ \ i t h t h e c o n s t r a i n t t o r e m a i n w i t h i n t h e s u p p o r t i n g p o l y g o n r e m o v e d . I n o r d e r t o h i g h l i g h t t h e d i f l e r e n c e b e t w e e n t h i s i m a g i n a r y p o i n t . a n d a r e a l Z M P . t h e t e r m s f o o t r o t a t i o n i n d e x ( F R I ) o r f i c t i o n a l z e r o m o m e n t p o i n t ( F Z M P ) h a v e b e e n s u g g e s t e d [ 5 ] . S i n c e i t i s i m p o s s i b l e t o a p p l y a f o r c e t o a n o b j e c t w i t h o u t c o n t a c t i n g i t , w h i l e i t i s o u t s i d e t h e r o b o t ' s s u p p o r t i n g p o l y g o n . t h e r R I d o c s n o t e x i s t a n d s o c a n n o t b e m e a s u r e d . W h i l e t h e F R I l i e s w i t h i n t h e r o b o t ' s s u p p o r t i n g p o l y g o n . i t i s e q u i v a l e n t t o Z M P a n d t h e r o b o t i s d y n a m i c a l l y s t a b l e . S i n c e t h e F R I i s n o t a l w a y s m e a s u r a b l e . a n d m a i n l y a p p l i e s t o s y s t e m s t h a t h a v e a l r e a d y l o s t d y n a m i c s t a b i l i t y . i t i s n o t v e r y u s e f u l f o r o n - l i n e Z M P b a s e d c o n t r o l s y s t e m s . F R I / F Z M P c a l c u l a t i o n s a r c m o r e u s e f u l l y a p p l i e d t o o t T - l i n e g a i t s y n t h e s i s t a s k s . 7 1 . 3 R e v i e w a n d R e s e a r c h T h e r e h a s b e e n m u c h r e s e a r c h p e r f o r m e d b y b o t h u n i v e r s i t i e s a n d p r i v a t e o r g a n i z a t i o n s i n t h e a r e a o r b i p e d a l w a l k i n g i n t h e p a s t 4 0 y e a r s . I t i s n o t p o s s i b l e t o o u t l i n e a l l t h e d e v e l o p m e n t s t h a t h a v e b e e n m a d e i n a p r o j e c t o f t h i s s i z e . H e n c e b r i e f o v e r v i e w o f b i p e d a l r o b o t s w i l l b e g i v e n . T h e c u r r e n t l y b e s t k n o w n e x a m p l e s o f t h e s t a t e - o f - t h e - a r t i n h u m a n o i d r o b o t s a r e t h e H o n d a A S I M O l 6 ] a n d t h e S o n y Q R I O [ 7 J a n d b o t h u s c Z M P b a s e d c o n t r o l a p p r o a c h e s . T h e l l o n d a C o m p a n y i n J a p a n h a s p r e s e n t e d i t s f i r s t t w o l e g g e d r o b o t E O i n 1 9 8 6 . T h e 1 : : 0 \ v a l k e d s l o w l y i n a s t r a i g h t l i n e . A f t e r t h e i n t r o d u c t i o n J V f E O o t h e r t w o l e g g e d r o b o t s E I - E 2 - E 3 a s s h o w n i n F i g u r e 1 . 7 h a s b e e n d e v e l o p e d f r o m 1 9 8 7 - 1 9 9 1 t o a c h i e v e t~1st w a l k i n g . T h e F 2 r o b o t a c h i e v e d f a s t w a l k i n g a t a s p e e d o f 1 . 2 K m / h o n f l a t s u r f a c e s . -IL~ - l • . • ~ F • • . • : p • . : ' ";t~' ~ ! ! O l ; . :~ • E O E 1 E 2 F i g u r e 1 . 7 : T h e b i p e d a l r o b o t s E O - E 3 i n t r o d u c e d b y H o n d a f r o m 1 9 8 6 - 1 9 9 1 T h e r o b o t s E 4 - E 5 - E 6 s h o w i n g i n F i g u r e 1 . 8 p r e s e n t e d b e t w e e n 1 9 9 1 - 1 9 9 3 e s t a b l i s h i n g t e c h n i q u e s f o r s t a b l e w a l k i n g a n d d e v e l o p e d t h r e e c o n t r o l t e c h n i q u e s . E 4 E 5 E 6 F i g u r e 1 . 8 : T h e b i p e d a l r o b o t s E 4 - E 6 i n t r o d u c e d b y H o n d a f r o m 1 9 9 1 - 1 9 9 3 A t l c r t h e s e v e n b a s i c m o d e l s t h e H O N D A C o m p a n y c h a n g e d t h e r e s e a r c h d i r e c t i o n t o w a r d s h u m a n o i d r o b o t . T h e h u m a n o i d r o b o t s P I , P 2 , P 3 s h o w i n g i n F i g u r e 1 . 9 w e r e 8 p r e s e n t e d f r o m 1 9 9 3 - 1 9 9 7 . P I i s t h e f i r s t H o n d a p r o t o t y p e o f a h u m a n o i d r o b o t . T h i s r o b o t c a n t u r n e l e c t r i c a l a n d c o m p u t e r s w i t c h e s , g r a b d o o r k n o b s . p i c k u p a n d c a r r y t h i n g s . T h e h e i g h t o f P I i s 1 . 9 1 5 m a n d 1 7 5 K g . P 2 i s a s e l f r e g u l a t i n g b a t t e r y - o p e r a t e d a n d r o i d w h i c h c a n w a l k i n d e p e n d e n t l y . w a l k u p a n d d o w n t h e s t a i r s , a n d p u s h c a r t s . W i r e l e s s t e c h n i q u e s \ \ e r e u s e d w i t h a c o m p u t e r t o r s o , m o t o r s , b a t t e r y , w i r e l e s s r a d i o a n d o t h e r b u i l t - i n d e v i c e s . P 2 i s 1 . 8 2 m t a l l a n d 2 1 O K g . P 3 c h a n g e s i n c o m p o n e n t m a t e r i a l s a n d a d e c e n t r a l i z e d c o n t r o l s ; . s t e m w h i c h r e s u l t e d i n d e c r e a s e i n h e i g h t , 1 . 6 m a n d w e i g h t 1 3 0 K g . I t h a s 1 6 j o i n t s i n t o t a l . I t h a s 3 0 d e g r e e s o f f r e e d o m ; 1 2 f o r l e g s . 1 4 f o r a r m s a n d 4 f o r h a n d s . T h e m a x i m u m w a l k i n g s p e e d o f P 3 i s 2 K m / h r . " } . . P l P 2 l l j . , " , ,"\.~.;. l - ' ! O l i o " · 1 • , . _ - · - - P 3 F i g u r e 1 . 9 : T h e h u m a n o i d r o b o t s P 1 - P 3 p r e s e n t e d b y H o n d a f r o m 1 9 9 3 - 1 9 9 7 , \ S I M O ( A d v a n c e d S t e p i n I n n o v a t i v e M o b i l i t y ) w a s p r e s e n t e d i n 2 0 0 I t o t h e w o r l d . ( s e e F i g u r e 1 . 1 0 ) . W i t h a w e i g h t o f 4 3 K g . h e i s m u c h l i g h t e r t h a n h i s p r e d e c e s s o r s a r e a n d t h a t i s o n e o l ' t h e m o s t i m p o r t a n t a d v a n t a g e s . T h i s r o b o t i s t h e r e s u l t o f a b o u t 1 4 y e a r s o f r e s e a r c h a n d a p p r o x i m a t e l y 1 0 p r o t o t y p e s . i \ S I M O c a n w a l k c o n t i n u o u s l y w h i l e c h a n g i n g d i r e c t i o n s , t a k i n g e v e r y s t e p s m o o t h a n d n a t u r a l . H e a l s o h a s t h e a b i l i t y t o c l i m b u p a n d d o w n a f 1 i g h t o f s t a i r s . 9 t ( ' ; ' \ -~~l J ~c - " - F i g u r e 1 . 1 0 : T h e h u m a n o i d r o b o t A S I M O I n D e c e m b e r 2 0 0 2 H o n d a a d d e d i n t e l l i g e n c e t e c h n o l o g y t o A S I M O w h i c h i s c a p a b l e o f i n t e r p r e t i n g t h e p o s t u r e s a n d g e s t u r e s o f h u m a n s a n d m o v i n g i n d e p e n d e n t l y i n r e s p o n s e . A S I M O ' s a b i l i t y t o i n t e r a c t w i t h h u m a n s h a s a d v a n c e d s i g n i f i c a n t l y ; i t c a n g r e e t a p p r o a c h i n g p e o p l e , f o l l o w t h e m , a n d m o v e i n t h e d i r e c t i o n t h e y i n d i c a t e , a n d e v e n r e c o g n i z e t h e i r f a c e s a n d a d d r e s s t h e m b y n a m e . F u r t h e r , u t i l i z i n g n e t w o r k s u c h a s I n t e r n e t A S I M O c a n p r o v i d e i n f o r m a t i o n w h i l e e x e c u t i n g t a s k s s u c h a s r e c e p t i o n d u t i e s . l l o n d a d e b u t e d a n e w A S I M O h u m a n o i d r o b o t w h i c h f e a t u r e s t h e a b i l i t y t o p u r s u e k e y t a s k s i n a r e a l - l i f e e n v i r o n m e n t s u c h a s a n o f f i c e a n d a n a d v a n c e d l e v e l o f p h y s i c a l c a p a b i l i t i e s . C o m p a r e d t o t h e p r e v i o u s m o d e l , t h e n e w A S I M O a c h i e v e s t h e e n h a n c e d a b i l i t y t o a c t i n s y n c w i t h p e o p l e . I n a d d i t i o n . t h e r u n n i n g c a p a b i l i t y i s d r a m a t i c a l l y i m p r o v e d . w i t h A S I M O n o w c a p a b l e o f r u n n i n g a t a s p e e d o f 6 K m / h a n d o f r u n n i n g i n a c i r c u l a r p a t t e r n . N e w A S I M O w e i g h t s 5 4 K g a n d t o t a l h e i g h t i s 1 3 0 c m a n d n o r m a l w a l k i n g s p e e d i s 2 . 7 K m / h a n d c o n s i s t s o v e r a l l 3 4 d e g r e e s o f f r e e d o m . 1 0 ( ) R I O ( . . Q u e s t f o r c u R I O s i t y " . o r i g i n a l l y n a m e d S o n y D r e a m R o b o t o r S D R ) i n F i g u r e 1 . 1 I \ \ a s a b i p e d a l h u m a n o i d r o b o t d e v e l o p e d a n d m a r k e t e d ( b u t n e v e r s o l d ) b ) ' l u n ; . ( u r p t l r a t i o n t o f o l l o w u p o n t h e s u c c e s s o f i t s A I B O t o y . Q R I O s t o o d a p p r o x i m a t e ! ) I J ( J I l l t~lil ~llld \ \ e i g h e d 7 . ] k g . . . - - . F i g u r e 1 . 1 1 : H u m a n o i d r o b o t " Q R I O ' ' i n t r o d u c e d b y S o n y Q R I O i s c a p a b l e o r v o i c e a n d f a c e r e c o g n i t i o n . m a k i n g i t a b l e t o r e m e m b e r p e o p l e a s w e l l a s t h e i r l i k e s a n d d i s l i k e s . Q R I O c a n r u n a t 2 3 c m / s , a n d i s c r e d i t e d i n G u i n n c s s W o r l d R e c o r d s ( 2 0 0 5 e d i t i o n ) a s b e i n g t h e f i r s t b i p e d a l r o b o t c a p a b l e o f r u n n i n g ( w h i c h i t d e f i n e s a \ t l l l l \ i n g ' ' h i l e b o t h l e g s a r e o f T t h e g r o u n d a t t h e s a m e t i m e ) . T h e 4 t h g e n e r a t i o n Q R I O ' s i m e r n a l b a t t e r : - · l a s t s a b o u t I h o u r . J \ s p e c i a l f e a t u r e o f Q R I O i s t h a t i t r e a c t s t o p r o t e c t i t s e l ! a g a i n s t a n i m p a c t . I n t h e e v e n t o f f a l l i n g , i t g e t s b a c k u p b y i t s e l f a f t e r c h e c k i n g i n a l l d i r e c t i o n s . r h e M a s s a c h u s e t t s I n s t i t u t e o f T e c h n o l o g y ( M I T ) [ 8 ] w a s o n e o f t h e t i r s t i n d e v e l o p i n g a \ a r i e t ) o ! \ \ a l k i n g . r u n n i n g o r j u m p i n g m a c h i n e s . T h e M I T h a s c o m e u p w i t h s e v e r a l r o b o t s a s s h o \ \ n i n F i g u r e 1 . 1 2 . T h e w a l k i n g r o b o t G c c k b o t u s e d t o s t u d y t h e s m o o t h t r a n s f e r o f s u p p o r t t i · o m o n e f o o t t o t h e o t h e r . T h e r o b o t i s d e s i g n e d C o r m o t i o n i n t h e c o r o n a l p l a n e o n l y . T h e C i e e k B o t c o n s i s t s o f t w o c u r v e d f e e t , a c t u a t e d a n k l e s a n d h i p s . a n d a p e l v i s . T h e b a s i c b e h a v i o r i s c o n t r o l l e d r o c k i n g f r o m o n e f o o t t o t h e o t h e r . T h i s r e s e a r c h h a s c a r r i e d o u t f r o m 1 9 9 4 t o 1 9 9 5 . S p r i n g T u r k e y w a s d e s i g n e d a n d b u i l t b y P e t e r D i l w o r t h a n d J e r r y P r a t t i n 1 9 9 4 . I t h a s a n a c t u a t e d h i p a n d k n e e o n e a c h l e g . A n u n a c t u a t e d b o o m c o n s t r a i n s S p r i n g T u r k e y ' s r o i L I I y a w , a n d l a t e r a l m o t i o n t h e r e b y r e d u c i n g i t t o a t w o d i m e n s i o n a l r o b o t . S p r i n g T u r k e y 1 1 e i g h s i n a t a p p r o x i m a t e l y 2 2 l b s ( I 0 k g ) a n d s t a n d s 2 f t ( 6 0 e m ) t a l l . \ t l C \ \ r o b o t . S p r i n g F l a m i n g o . w a s s t a r t e d a f t e r t h e S p r i n g T u r k e y . S p r i n g F l a m i n g o W e i g h s i n a p p r o x i m a t e l y 3 0 l b s ( 1 3 . 5 k g ) a n d s t a n d s 3 f t ( 9 0 e m ) t a l l . T h e r o b o t h a s a n a c t u a t e d h i p . k n e e . a n d a n k l e o n e a c h l e g . A n u n a c t u a t e d b o o m c o n s t r a i n s S p r i n g F l a m i n g o · s r o l l . y a \ \ , a n d l a t e r a l m o t i o n t h e r e b y r e d u c i n g i t t o a p l a n a r r o b o t . t : ( . . . . . . . - ( a ) ( b ) F i g u r e 1 . 1 2 : M I T h u m a n o i d r o b o t s : ( a ) S p r i n g F l a m i n g o , ( b ) S p r i n g T u r k e y , ( c ) G e e k B o t ( c ) M a n y o t h e r n o t a b l e , s u c c e s s f u l b i p e d s w i t h s t i f f l e g s a n d l a r g e f e e t e x i s t . ( e . g . H R P - 2 [ 9 J , 1 - 1 6 [ 1 0 ] , H O A P - 2 [ 1 1 ] , J o h n n i e [ 1 2 ] , K H R - 1 [ 1 3 ] ) a n d s o m e a r e e v e n a v a i l a b l e c o m m e r c i a l l y . 1 2 1 . 4 M o t i v a t i o n L e g g e d l o c o m o t i o n i s t h e m o s t c o n v e n i e n t f o r m o f l o c o m o t i o n w h e n c o m p a r e d t o t h e o t h e r f o r m s o f l o c o m o t i o n d u e t o t h e a b i l i t y o f n e g o t i a t i n g c o m p l e x t e r r a i n s . L e g g e d r o b o t s c a n b e u s e d f o r l o t · o f p r a c t i c a l a p p l i c a t i o n s s u c h a s e m e r g e n c y f a s t r e s p o n s e [ 1 4 ] . m o b i l e m o n i t o r i n g . i n s p e c t i o n a n d s u r v e i l l a n c e i n h a z a r d o u s o r h o s t i l e e n v i r o n m e n t s l l 5 . 1 6 ] . p l a n e t a r y e x p l o r a t i o n [ 1 7 ] . a n d p e r s o n a l a p p l i c a t i o n s , t o n a m e a f e w . B i p e d a l r o b o t s a r e e s p e c i a l l y i n t e r e s t i n g b e c a u s e h u m a n s a r e t w o - l e g g e d a n d p l a c e s m e a n t t o b e o c c u p i e d b y h u m a n s ( s u c h a s h o m e s a n d o f f i c e s ) a r e d e s i g n e d f o r t w o l e g g e d l o c o m o t i o n . " T h e H O N D A C o m p a n y h a s c o m e u p w i t h t h e s t a t e o f t h e a r t h u m a n o i d r o b o t A S I M O a f t e r 1 4 y e a r s r e s e a r c h w i t h b i l l i o n s o f d o l l a r s i n v e s t m e n t . B u t s t i l l t h e r e a r e l o t s o f t m p r o v e m e n t s a r e n e e d e d t o m a k e b i p e d a l r o b o t s c o m m e r c i a l l y v i a b l e . I n t h e f u t u r e t h e h u m a n l a b o r w i l l b e c o m e s o e x p e n s i v e a n d t h e r e w i l l b e l o t o f d e m a n d f o r t h e a u t o n o m o u s l a b o r . A s m o b i l i t y i s a k e y e l e m e n t i n d e v e l o p i n g a u t o n o m o u s v e h i c l e s , l e g g e d r o b o t s w i l l b e o f i n c r e a s i n g s i g n i f i c a n c e a s t h e r e i s a t r e n d t o w a r d s m o r e c a p a b l e a u t o n o m o u s r o b o t c o n t i n u e s . S t i l l t h e r e a r e n o p r a c t i c a l a p p l i c a t i o n s o f b i p e d a l r o b o t s o u t s i d e t h e l a b o r a t o r y a p a r t f r o m n u m b e r o f e n t e r t a i n m e n t r o b o t s . A l t h o u g h t h e r e w e r e h u n d r e d s o f r e s e a r c h e s c a r r i e d o u t o n b i p e d a l r o b o t s s t i l l t h e r e i s l o t m o r e t o l e a r n a b o u t b i p e d a l w a l k i n g . W h e n g o i n g t h r o u g h t h e l i t e r a t u r e i t i s n o t e d t h a t t h e r e a r c v e r y f e w r e s e a r c h e s a r e c a r r i e d o u t o n b i p e d a l r o b o t s w h i c h c a n w a l k o n s l o p i n g s u r f a c e s . A u t h o r a l s o n o t e d t h a t t h e u s e o f o r i g i n a l Z M P e q u a t i o n p r e s e n t e d b y P r o f . M i o m i r V u k o b r a t o v i c s o m u c h r a r e a n d m o s t o f t h e t i m e r e s e a r c h e s h a s u s e d a r t i f i c i a l i n t e l l i g e n c e t e c h n i q u e s t o t e s t d y n a m i c s t a b i l i t y b u t t h i s s u r e l y d o e s n o t g i v e e x a c t r e s u l t b u t a p p r o x i m a t e d v a l u e . T h e r e f o r e d u e t o t h e a b o v e r e a s o n s t h e a u t h o r h a s m o t i v a t e d t o s t u d y a b o u t a b i p e d a l \ \ a l k i n g o n s l o p i n g s u r f a c e s a n d t e s t i n g o f i t s d y n a m i c s t a b i l i t y d u r i n g d i f f e r e n t c i r c u m s t a n c e s b y c a l c u l a t i n g i t s d y n a m i c s t a b i l i t y u s i n g o r i g i n a l Z M P e q u a t i o n . 1 . 5 R e s e a r c h O b j e c t i v e s T h e a i m o f t h i s r e s e a r c h i s t h e s i m u l a t i o n a n d a n a l y z i n g o f t h e b i p e d a l r o b o t w a l k i n g o n s l o p i n g s u r l " a c e s a n d t o v e r i f y t h e a b i l i t y o f m a i n t a i n i n g d y n a m i c s t a b i l i t y b y v a r y i n g t o r s o a n g l e . T o a c h i e v e t h i s o b j e c t i v e a b i p e d a l r o b o t c o n s i s t s o f s e v e n l i n k s h a v i n g t w o l e g s a n d a t o r s o h a s b e e r ) m o d e l e d . T h e d y n a m i c s t a b i l i t y h a s b e e n t e s t e d f o r t h e p e r i o d o f s w i n g p h a s e o f t h e l e g u s i n g p r e d e f i n e d t r a j e c t o r i e s u s i n g t h e Z M P c o n c e p t i n t r o d u c e d b y t h e P r o f . \ 1 i o m i r V u k o b r a t o v i c . T h e o r i g i n a l Z M P e q u a t i o n h a s b e e n u s e d f o r t h e c a l c u l a t i o n o f 7 M P a t e a c h t r a j e c t o r y p o i n t . 1 3 1 . 6 O v e r v i e w T h e s t r u c t u r e o f t h i s t h e s i s i s d i v i d e d i n t o s e v e n c h a p t e r s . C h a p t e r I r e v i e w s p a s t l i t e r a t u r e a n d t h e c u r r e n t s t a t e o f r e s e a r c h i n b i p e d a l r o b o t i c s . I n C h a p t e r 2 t h e b i p e d a l r o b o t d e s c r i p t i o n a n d t r a j e c t o r y p l a n n i n g h a s b e e n d i s c u s s e d . T h e c h a p t e r 3 d i s c u s s e s t h e a p p l i c a t i o n o f k i n e m a t i c s t o t h e b i p e d a l r o b o t a n d C h a p t e r 4 d e m o n s t r a t e s t h e a c c u r a c y o f k i n e m a t i c s b y s i m u l a t i o n . C h a p t e r 5 show~ t h e c a l c u l a t i o n m e t h o d o f Z M P a n d t h e n a p p l i e s i t t o t h e l o w e r b o d y o f t h e b i p e d a l r o b o t t o v e r i f y t h e d y n a m i c s t a b i l i t y w h i l e w a l k i n g . A l s o t h e e f f e c t o f a d d i n g t o r s o o n d y n a m i c s t a b i l i t y i s a l s o d i s c u s s e d . " C h a p t e r 6 d i s c u s s e s t h e e f T e c t o f v a r i a t i o n o f t o r s o a n g l e w h i l e b i p e d a l r o b o t w a l k i n g u n d e r d i f f e r e n t c i r c u m s t a n c e s . H e r e s e v e r a l i n d i v i d u a l c a s e s h a v e b e e n a n a l y z e d t h o r o u g h l y \ ( 1 1 ' ) i n g e a c h p a r a m e t e r a t a t i m e . C h a p t e r 7 d i s c u s s e s t h e c o n c l u s i o n s r e g a r d i n g t h e e f f e c t i v e n e s s o f t h e t o r s o a n g l e v a r i a t i o n i n d i f f e r e n t c i r c u m s t a n c e s w h i l e v v a l k i n g i n o r d e r t o m a i n t a i n d y n a m i c s t a b i l i t y a n d f u t u r e w o r k . 1 4 C h a p t e r 2 B i p e d a l r o b o t d e s c r i p t i o n a n d t r a j e c t o r y p l a n n i n g 2 . 1 B i p e d a l r o b o t d e s c r i p t i o n Y I I H m 3 e r ''~, - - - I J e ' l m " ~,' \::==-~-~=~\-=~~:_~--~~ ~~----------- 1 " D i r e c t i o n o f M o t i o n - - - - - - - - - - X F i g u r e 2 . 1 : 7 l i n k s , 7 D o F b i p e d a l r o b o t ! h e F i g u r e 2 . 1 s h o w s t h e s c h e m a t i c o f a p l a n e r t w o - l e g g e d r o b o t h a v i n g 7 D o F ( t w o a t t h e a n k l e s . t w o a t t h e k n e e s a n d t h r e e a t t h e h i p ) . w h i c h i s m o v i n g u p a l o n g a s l o p i n g s u r f a c e . T h e r o b o t c o n s i s t s o f t w o a n k l e s , t w o l o w e r l e g s , t w o u p p e r l e g s a n d a t o r s o . A l l t h e l i m b s a r e a s s u m e d t o h a v e t h e i r l u m p e d m a s s e s a t t a c h e d a t s o m e s p e c i f i c p o i n t s o n t h e m . T h e j o i n t s o f t h e r o b o t a r e a s s u m e d t o b e c o n s i s t i n g o f r o t a r y j o i n t s . T h e r o b o t m o v e m e n t h a s h c c n c o n s i d e r e d o n l y i n s a g i t t a l X - Y p l a n e . 1 5 2 . 2 T r a j e c t o r y P l a n n i n g 2 . 2 . 1 T r a j e c t o r y \ lt'aJ~CtUr) j : , ~~ p a t h \ \ i t h s p e c i f i e d q u a n t i t i e s o f m o t i o n . ; \ t r a j e c t o r y i s b o t h a s p a t i a l a n d t e m p o r a l r e p r e s e n t a t i o n o f m o t i o n . I t c a n b e s p e c i f i e d e i t h e r i n j o i n t s p a c e o r i n C a r t e s i a n s p a c e [ I 8 J , T h e v a r i o u s t r a j e c t o r y p l a n n i n g t e c h n i q u e s t~dl i n t o o n e o f t h e t w o c a t e g o r i e s . I . . I o i n t s p a c e t e c h n i q u e s ; ' C a r t e s i a n s p a c e t e c h n i q u e s 2 . 2 . 1 . 1 J o i n t s p a c e T e c h n i q u e s f h i s i s f o r t h e r o b o t i c a p p l i c a t i o n s i n v o l v i n g p o i n t t o p o i n t m o t i o n i n w h i c h m o t i o n p l a n n i n g i s d o n e a t t h e j o i n t l e v e l . T h e j o i n t - s p a c e p l a n n i n g s c h e m e s g e n e r a t e t i m e d e p e n d e n t f u n c t i o n s ( t i m e h i s t o r y ) o f a l l j o i n t v a r i a b l e s a n d t h e i r t i r s t t w o d e r i v a t i v e s t o d e s c r i b e t h e d e s i r e d m o t i o n o f t h e m a n i p u l a t o r . I h e i m p o r t a n t l c a t u r e s o f j o i n t s p a c e t r a j e c t o r y p l a n n i n g a r e , • J o i n t a n g l e l o c a t i o n s , v e l o c i t i e s a n d a c c e l e r a t i o n s s h o u l d b e c o n t i n u o u s f u n c t i o n s o f t i m e s o a s t o e n s u r e s m o o t h m o t i o n . • T h e j o i n t p o l y n o m i a l f u n c t i o n s h o u l d n o t b e c o m p u t a t i o n a l l y i n t e n s i v e • ! \ o n - s m o o t h t r a j e c t o r i e s a n d o t h e r s i m i l a r u n d e s i r a b l e e f f e c t s a r e m i n i m i / e J . 2 . 2 . 1 . 2 C a r t e s i a n s p a c e T e c h n i q u e s ! h i s i s f o r t h e r o b o t i c a p p l i c a t i o n s t h a t r e q u i r e c o n t i n u o u s p a t h m o t i o n . T h e C a r t e s i a n ~pace-planning t e c h n i q u e s p r o v i d e t i m e h i s t o r y o f t h e l o c a t i o n , v e l o c i t y , a n d a c c e l e r a t i o n o f t h e e n d - c i T e c t o r w i t h r e s p e c t t o t h e b a s e . T h e c o r r e s p o n d i n g j o i n t v a r i a b l e s a n d t h e i r d e r i v a t i v e s a r c c o m p u t e d , u s i n g i n v e r s e k i n e m a t i c s . 1 6 2 . 2 . 2 S e l e c t i o n o f T r a j e c t o r i e s f o r t h e B i p e d a l R o b o t 2 . 2 . 2 . 1 T r a j e c t o r y f o r s w i n g l e g ~-,:9r _ _ _ _ _ _ " " ' _ _ _ _ _ _ _ . . ----\----~ \ = - - - r · - - - - - · \ / 6 j HipTr;j~~toty 6 , y I I H L i~~~~~~-:_---- 11~:~~~~- ~~~-~--~---~ k--~ I F i g u r e 2 . 2 : S w i n g l e g a n d H i p t r a j e c t o r i e s . ; X l o t · t h e s 1 1 i n g l e g o f b i p e d a l r o b o t t h e C a r t e s i a n s p a c e i s p r e f e r r e d b e c a u s e t h e f o o t tr~ljcctor;, i s a p r e d e t e r m i n e d p a t h . I h e F i g u r e 2 . 2 s h o w s b i p e d a l r o b o t w i t h t o r s o . T h e s w i n g l e g j o i n t a n g l e s 0 1 J ) 2 f)~ a n d ~lance l e g j o i n t a n g l e s 0 , . 0 ( , a r e c l e a r l y s h o w n i n t h e d i a g r a m . A l l l i n k l e n g t h s a n d ma~ses a r e m a r k e d a g a i n s t t h e r e s p e c t i v e l i n k s . T h e o r i g i n o f c o o r d i n a t e f r a m e w a s p l a c e d a t t h e b e g i n n i n g o l " t h e s l o p e a n d d i s t a n c e s x l . x 2 i n d i c a t e s t h e i n i t i a l p o s i t i o n s o r ' \ \ i n g l ( l u l c l ! 1 d s t a n c e r o o t \ \ h i l c .\"~indicates t h e f i n a l p o s i t i o n o r t h e S \ \ i n g f o o t . T h e L t l ' J I C I b o d : c o n s i s t s o l " a t o r s o a n d m a k e s a n a n g l e 0 1 v v i t h t h e h o r i z o n t a l a x i s . T h e s w i n g l o o t t r a j e c t o r y a n d h i p t r a j e c t o r ) a r e a l s o i n d i c a t e d . 1 7 H e r e . f o o t t r a j e c t o r y i s a s s u m e d t o f o l l o w c u b i c p o l y n o m i a l t r a j e c t o r y a s s h o w n i n f-igure~-~ a n d d e l i n e d a s . r = c 1 1 + c , x + c , x · + c , x ( 2 . 1 ) \ \ h e r e ~ r e p r e s e n t s t h e h e i g h t o f t h e s w i n g f o o t ( a n k l e j o i n t ) a t a d i s t a n c e x f r o m t h e s t a r t i n g p o i n t a n d c : 1 1 • c 1 • c . , . c , a r e t h e c o e f f i c i e n t s . w h o s e v a l u e s a r e d e t e r m i n e d u s i n g t h e f o l k m i n g b o u n d a r y c o n d i t i o n s . 1 a t x = x , c o s a . y = x 1 s i n a . 1 a t x = ( x : c o s a + x 2 c o s a ) / 2 . y = x 2 s i n a + L , / 2 . 1 a t x = ( x , c o s a + x , c o s a ) I 2 . y = x , s i n a + L , I 2 . • a t x = x , c o s a . y = x , s i n a " A p p l y i n g t h e b o u n d a r y c o n d i t i o n s ( 2 . 2 ) . ( 2 . 3 ) . ( 2 . 4 ) a n d ( 2 . 5 ) i n ( 2 . 1 ) . ( 2 . 2 ) ( 2 . 3 ) ( 2 . 4 ) ( 2 . 5 ) . " ) 1 ; ' x : s m a = c , , + c : 1 x 1 c o s a + c , x 1 c o s · a + c , x ; c o s a ( 2 . 6 ) x . s i n u - , - U , / 2 ) = c : 0 + c 1 ( x 1 c o s a + x 2 c o s a ) / 2 + c : 2 ( x 1 c o s a + x 2 c o s a ) 2 / 4 ( 2 . 7 ) ~c,(x 1 c o s a + x 2 c o s a ) ' / 8 x , s i n a + ( L , / 2 ) = c 0 + c 1 ( x 2 c o s a + x 3 c o s a ) / 2 + c 2 ( x 2 c o s a + x , c o s a ) 2 / 4 C 2 · 8 l + c , ( x : c o s a + x , c o s a ) ' / 8 x ' - I l l ! I c , - 1 c 1 x , c o s u - t - c . < c o s 2 a + c , x ; c o s ' a ( 2 . 9 ) T h e e q u a t i o n s ( 2 . 6 ) . ( 2 . 7 ) . ( 2 . 8 ) a n d ( 2 . 9 ) h a s b e e n s o l v e d w r i t i n g a M i \ T L A B p r o g r a m A . I a n d i n d i c a t e d i n A p p e n d i x A . U s i n g t h i s p r o g r a m a n o t h e r M A T L J \ B p r o g r a m A . 2 i n d i c a t e d i n A p p e n d i x A h a s b e e n w r i t t e n t o c a l c u l a t e t h e r e q u i r e d f o o t t r a j e c t o r y c o e t 1 i c i c n t s c . c 1 • c , a n d c , n u m e r i c a l l y b y i n p u t t i n g v a l u e s f o r . \ · 1 • x , . x , . r t a n d L , . 2 . 2 . 2 . 2 T r a j e c t o r y f o r h i p T h e h i p t r a j e c t o r y h a s c h o s e n a s a s t r a i g h t l i n e p a r a l l e l t o t h e s l o p e a s s h o w n i n F i g u r e ~.~and d e t i n e d a s . _ 1 ~ l t c t l l U ) . r ( 2 . 1 0 ) 1 8 • 2 . 2 . 2 . 3 R e l a t i o n s h i p b e t w e e n s w i n g l e g m o v e m e n t a n d h i p m o v e m e n t I h e r e s h o u l d b e a r e l a t i o n s h i p b e t v \ c e n s w i n g l e g m o v e m e n t a n d h i p m o v e m e n t f o r c o n t i n u o u s \ \ a ! k i n g . I n o r d e r t o m a i n t a i n c y c l i c g a i t t h e f i n a l p o s i t i o n o f s w i n g l e g s h o u l d b e s a m e a s t h e i n i t i a l p o s i t i o n o r t h e s t a n c e l e g . A f t e r s t u d y i n g t h e h u m a n v v a l k i n g p a t t e r n t h e t ( J ! i o \ \ i n g f o r m u l a h a s b e e n r o n n u l a t e d . H i p m o v e m e n t f o r S i n g l e S t e p ( x ) = 0 . 5 * S t e p L e n g t h ( 2 . 1 1 ) 2 . 2 . 3 S e l e c t i o n o f J o i n t a n g l e T r a j e c t o r y f o r t h e Bi~dal R o b o t ! ' h e s e l e c t i o n o r a s i n g l e p o l y n o m i a l f o r t h e e n t i r e j o i n t p a t h d e p e n d s o n t h e n u m b e r o f c o n s t r a i n s i m p o s e d a n d t h e t y p e o f m o t i o n d e s i r e d . T h e m i n i m u m n u m b e r o f c o n s t r a i n s f o r a s m o o t h m o t i o n b e t w e e n t w o p o i n t s a r e ( i ) I n i t i a l p o s i t i o n , ( i i ) I n i t i a l v e l o c i t y . ( i i i ) F i n a l p o s i t i o n a n d ( i \ J I i n a ! v e l o c i t y . \ \ i t h f o u r c o n s t r a i n s , a t h i r d - d e g r e e p o l y n o m i a l w i t h f o u r c o e f f i c i e n t s c a n b e u s e d . I f i n a d d i t i o n . w i t h t w o a c c e l e r a t i o n c o n s t r a i n s f i f t h - d e g r e e p o l y n o m i a l c a n b e u s e d . H o w e v e r , i f t h e d e g r e e o f p o l y n o m i a l i s h i g h ( > 5 ) , i t b e c o m e s c o m p u t a t i o n a l l y i n t e n s i v e a n d i t t e n d s t o c a u s e e x t r a n e o u s m o t i o n . f o e n s u r e s m o o t h f u n c t i o n i n g , t h e v a r i a t i o n o f j o i n t a n g l e s o r t h e b i p e d a l r o b o t i s a s s u m e d t o f o l l o w f i f t h - o r d e r p o l y n o m i a l a s s h o w n b e l o w : q , ( l ) = a , 1 1 + a ) + a , , / 2 + a j ' + a , c / 1 + a j 5 ( 2 . I 2 ) W h e r e a , 1 1 , a ! [ , a , , , a , , . a , . . , a n d a , , a r c t h e c o e f f i c i e n t s , w h o s e v a l u e s a r e d e t e r m i n e d u s m g d i f f e r e n t v a l u e s o f ( j , a t d i f f e r e n t i n t e r v a l s o r t i m e i n a c y c l e a n d i = 1 , 2 , . . . . n j o i n t s . 1 9 945~2 T h e a s s u m e d v e l o c i t y v a r i a t i o n w i t h t i m e i s s h o w n i n f i g u r e 2 . 3 . ~~~gular i l,r·aij/~.e~=.! I - - - - - ,-~~~~~~~~~~~~~~~~-----.. 1 " \ . ; I \ , I ~\ ' \ . . . . . 0 t l t 2 \ . \ . . . . \ t l T i m e ( s e C : I f i g u r e 2 . 3 - A n g u l a r v e l o c i t y v a r i a t i o n o f t h e s w i n g l e g a n d s t a n c e l e g j o i n t a n g l e s f o r h a l f c y c l e d u r a t i o n l h e c o e f f i c i e n t s a , 1 1 . u , 1 . a , , . a , , . a , 1 a n d a , , a r e c a l c u l a t e d b y r e f e r r i n g t o t h e v e l o c i t y p r o t i l c ' > h l l \ \ n i s F i g u r e 2 . 3 . " \ p p l ) i n g r o l l o w i n g c o n d i t i o n s t o t h e e q u a t i o n s 2 . 1 2 A t t = O i n i t i a l a n g u l a r p o s i t i o n A t t = t ; f i n a l a n g u l a r p o s i t i o n - \ t t = O I n i t i a l a n g u l a r v e l o c i t y ~~ t = t ; f i n a l a n g u l a r v e l o c i t y \ t t - t a n g u l a r a c c e l e r a t i o n A t t = t " a n g u l a r a c c e l e r a t i o n C f \ 1 " ) = o ' " + u , ) t ~~ + u ' ,I~~ + a i l I~~ + a ' 5 I~~ I _ , - + ' ) 1 / 1 1 , ) = o , , 1 + a , 1 t ~ + u , J , + a , _ . J , + a , J , i f ( I , J = 2 u , 1 / 1 1 + 3a,J 1 ~ + 4 a j , ; + Sa,,/ 1 ~ _ ) - . 2 4 1 5 4 1 ; ( / . ) - _ o , / 1 + _ ) a , , t , + a , 1 1 , + a , J , ( 2 . 1 3 ) ( 2 . 1 4 ) ( 2 . 1 5 ) ( 2 . 1 6 ) 2 0 ( j 1 ! , ) = ~u 1 + 6 o , , t 1 + 1 2 a , 1 1 , = + 2 0 o , , t i ( 2 . 1 7 ) ( j ( / i = c 2 u , + ( n t J = + 1 2 o , / +20a,J~ ( 2 . 1 8 ) S o l \ l l l g t h e s i : - ; e q u a t i o n s m e n t i o n e d a b o v e t h e c o e f f i c i e n t s o f t h e 5 t 1 1 d e g r e e p o l y n o m i a l o f t h e rc~pcctivc j o i n t a n g l e c a n h e f o u n d . I h e \ L \ T L , \ B p r o g r a m i \ . 7 i n d i c a t e d i n a p p e n d i x A h a s b e e n " w r i t t e n t o l i n d t h e c o t l . l i c i c n h s \ m b o l i c a l l \ . . . T h e n u s i n g t h e o u t p u t o f p r o g r a m A . 7 a n o t h e r M A T L A B p r o g r a m i \ . 8 h a s b e e n w r i t t e n t o c a l c u l a t e c o e r t i c i e n t s o w o , 1 . u , , . a , , . a , . J a n d a , , a s i n d i c a t e d i n a p p e n d i x A . 2 1 l C h a p t e r 3 K i n e m a t i c s o f t h e b i p e d a l R o b o t 3 . 1 K i n e m a t i c s o f t h e S w i n g L e g ! h e k i n e m a t i c s o f t h e l o v ' v e r b o d y h a s b e e n s t u d i e d i n o r d e r t o f j . n d 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 Cartc~ian m o v e m e n t s a n d t h e r o b o t j o i n t a n g l e s i n o r d e r t o s t u d y r o b o t w a l k i n g . 3 . 1 . 1 J o i n t a n g l e s o f S w i n g L e g T h e l l g u r e 3 . 1 s h m v s t h e s v v i n g l e g o l " t h e b i p e d a l r o b o t . l ' I I r i I H L • . ~) ( ) . .~( ~-.. · · . . . . . . . L 4 · . . . . . . · . . • . \ . . . . • . . . . . . . . . . . . . . . . . . . . . . . . I s --~----!--- F o o t t r a . j e c t o r y . L 6 / ··---~~~\---------- - - - - _ _ _ . . . . . . . . . . . . . . . - - - F i g u r e 3 . ! : S v v i n g l e g m o t i o n o f b i p e d a l r o b o t X 2 2 T h e S \ \ i n g l e g w i t h l i n k l e n g t h s o f L 1 . L 2 a n d / " , a n d j o i n t a n g l e s 0 1 . 0 2 a n d 0 , m o v i n g 1 1 1 h i g h l i g h t e d t r a j e c t o r y i s i l l u s t r a t e d i n t h e F i g u r e 3 . 1 . T h e s t a n c e l e g w i t h l i n k l e n g t h s o f L I , a n d !~" i s i n d i c a t e d i n d o t t e d l i n e s . T h e d i s t a n c e f r o m t h e i n i t i a l p o s i t i o n o f s w i n g f o o t t o t h e h i p i s t a k e n a s H . T h e o r i g i n o f x - y c o o r d i n a t e f r a m e i s a t t h e s t a r t o f ' t h e s l o p e . T h e x . . Y ; d i s t a n c e s i n d i c a t e s s w i n g l e g i n i t i a l a n d l~nal p o s i t i o n f r o m t h e o r i g i n a n d x 2 i s t h e i n i t i a l p o s i t i o n o f t h e s t a n c e l e g . T h e o r i g i n o f X - Y c o o r d i n a t e s y s t e m i s p l a c e d a t t h e h i p a n d u s e d a s t h e r e f e r e n c e f o r t h e s w i n g l e g k i n e m a t i c s . . . T h e D e n a v i t - H a r t e n b e r g ( 0 - H ) [ 1 8 ] p a r a m e t e r s o f t h e s w i n g l e g i s s h o w n i n T a b l e 3 . 1 . T h e a , . 1 a,~ 1 d,~ 1 a n d 8,.~ 1 h a v e u s u a l D e n a v i t - H a r t e n b e r g n o t a t i o n . T h e i n v e r s e k i n e m a t i c s c a n b e a p p l i e d t o t h e s w i n g l e g c o n s i d e r i n g h i p a s b a s e . T h e n t h e j o i n t a n g l e s r 1 1 • f J , a n d { 1 , c a n b e e x p r e s s e d a s [ 1 9 ] . f i = . - 1 t a n 2 [ S , . C , ] \ \ h e r e . c . = I \ • + r 2 - I~~ - L ~ I 2 r J " , I c I C . , , ) \ ' ' ) - 1 · , [ } X J 1 · ; I . K . I I - • l d l l - . - - - . t c l n - ( \ ' . I ) L r r \ \ h e r e . f . : 1 = 1~ 1 + L C ' 2 , K 2 = L , S , a n d r = \ 1 \ - r - K } B : u s i n g g e o m e t r y o f t h e r o b o t . 0 = ( } + 0 , + a ( 3 . 1 ) ( 3 . 2 ) ( 3 . 3 ) ' Y ' ~_) 3 . 1 . 2 C o o r d i n a t e c o n v e r s i o n I h e c u o r d i n a t e s o l ' t h e o f t h e s w i n g C o o t h a s b e e n c a l c u l a t e d r e l a t i v e t o t h e H i p c o o r d i n a t e ' ) ' t c m a n d t h e c o o r d i n a t e c o n v e r s i o n f r o m t h e o r i g i n a l c o o r d i n a t e s y s t e m a r e a s f o l l o w s . \ - - . \ - - . Y , C O S U J = I I . . , . \ 1 - , i n a - _ 1 • 3 . 2 K i n e m a t i c s o f t h e s t a n c e L e g J ( 3 . 4 ) ( 3 . 5 ) K i n e m a t i c s o f t h e s t a n c e l e g h a s b e e n s t u d i e d s e p a r a t e l y . T h e F i g u r e 3 . 2 s h o w s t h e I 1 1 0 \ C I 1 1 C I 1 t o r r o b o t S t a n c e l e g . 3 . 2 . 1 J o i n t a n g l e s o f S t a n c e L e g T h e F i g u r e 3 . 2 s h o w s t h e s t a n c e l e g o f t h e b i p e d a l r o b o t T h e s t a n c e l e g i s h i g h l i g h t e d i n d i c a t i n g t h e s t a n c e l e g j o i n t a n g l e s 6 1 5 a n d e r , . T h e v a r i a t i o n o f j o i n t a n g l e s 6 1 5 a n d e r , w i l l r c - , u l t t h e t l H l \ e m e n t o r t h e h i r a l o n g t h e t r a j e c t o r y \ \ h i c h i s p a r a l l e l t o t h e s l o p e . H y r _ _ _ _ . . x _ - - - - ) ' - - - -~: _ _ _ - · - - - = - . . _ - -6'~--- H i p T r a . j e c t o r y ' ' · . . . \ L l · . . . . . . . . . . . . ~ . . . . . : · L . . . . : : ( 3 5 I I _ I F " I L ( ) · - . . . _ 6 . . . . . . . - - - - - - - - - , . L _ _ _ ~":,------ , . . . . : r I _ T l - · ' ~----·~ ~~--~-~~='-- - - - - - - * : r \ F i g u r e 3 . 2 : S t a n c e l e g m o t i o n o f b i p e d a l r o b o t 2 4 T h e s e c o n d l e g h a s b e e n c o n s i d e r e d s e p a r a t e l y a s a t h r e e l i n k s e r i a l m a n i p u l a t o r . ! \ r c l ' c r c n c e c o o r d i n a t e f r a m e l o c a t e d a t t h e f o o t o f t h e s t a n c e f o o t i s c o n s i d e r e d i n d e r i v i n g k i n e m a t i c s . T h e D - H p a r a m e t e r s o f t h e s t a n c e l e g i s s h o w n i n T a b l e 3 . 2 . T h e s w i n g l e g a n d 'l~lllct· lc~ a r c n e e d t o h e i d e n t i c a l i n o r d e r t o w a l k a n d t h e l i n k d i s t a n c e s I : m d I . , = ! , . ! ! a b l e 3 . 2 : D - H p a r a m e t e r s o f s t a n c e l e g L i n k a , _ , a , _ , E 4 - l n u m b e r ---+--------~--------~------+-------~ 0 0 0 ( 1 . . . - - I . - - - - - - - - - - + - - 2 0 : ! . , ( ! . , ) 0 e , ' ) I ( ) I / . , ( / . . ) 0 0 _ ) I i A p p l ) i n g i n v e r s e k i n e m a t i c s t h e j o i n t a n g l e s 8 ) a n d 8 6 a r e f o u n d . T h e r e s p e c t i v e e q u a t i o n f o r e a c h _ j o i n t a n g l e i s i n d i c a t e d b e l m v . l i , = . 1 t a n 2 [ . ' ) 2 • C ' 2 ] \ \ h e r e . - , - C = I \ - + l : , - - I ] - L~ I : ? . ! J . I \ I - C - ' . \ ' f ! = . J t a n 2 ( } 1 1 . X , ) - A t a n 2 ( K 1 , K , ) \ \ h e r e . f . . = ~~ + l o , C C f . . . = - : L~·. 3 . 2 . 2 C o o r d i n a t e c o n v e r s i o n ( 3 . 6 ) ( 3 . 7 ) T h e c o o r d i n a t e s o r t h e e n d o f l i n k lo.~, ( H i p ) h a s b e e n c a l c u l a t e d r e l a t i v e t o t h e r o o t o f s t a n c e l e g c o o r d i n a t e s y s t e m a n d t h e c o o r d i n a t e c o n v e r s i o n f r o m t h e o r i g i n a l c o o r d i n a t e s y s t e m a r c a s f o l i o \ \ s . \ , c _ y - ( x , - x l ) c o s c x ( 3 . 8 ) l = l f - ( x , - x 1 ) s i n a + 6 . y ( 3 . 9 ) 2 5 l h l ' s e c o n d l e g h a s b e e n c o n s i d e r e d s e p a r a t e l y a s a t h r e e l i n k s e r i a l m a n i p u l a t o r . 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 l o c a t e d a t t h e f o o t o f t h e s t a n c e f o o t i s c o n s i d e r e d i n d e r i v i n g k i n e m a t i c s . T h e D - 1 - 1 p a r a m e t e r s o f t h e s t a n c e l e g i s s h o w n i n T a b l e 3 . 2 . T h e s w i n g l e g a n d s t a n c c i l ' t ' - a r c n e e d t o h e i d e n t i c a l i n o r d e r t o w a l k a n d t h e l i n k d i s t a n c e s r a n d / . = l T a b l e 3 . 2 : D - 1 - 1 p a r a m e t e r s o r s t a n c e l e g L i n k a , 1 a , _ 1 I n u m b e r . . I I ~~ ! 0 I 0 i - - - - -~- ~- ~--~J------- t . 2 • ( ) I ! . , ( L , ) ~---------+---- I l _ _ _ _ _ _ 3 I ( ) I I ( 1 , 1 i d , . j ~I 0 f j ( , 0 f 1 , 0 0 \ p r J : . i n g i n v e r s e k i n e m a t i c s t h e j o i n t a n g l e s ( ) ) a n d ( ) 6 a r e f o u n d . T h e r e s p e c t i v e e q u a t i o n ! c 1 r e a c h j o i n t a n g l e i s i n d i c a t e d b e l m v . 1 ! , = . 1 t a n 2 l s , _ . c , _ ] \ \ h e r e . . · - I \ ' ~ } ' 2 I ' L 2 I ' ) I l I ( - . + ' ' - " I - ' ' - - ' 1 . , \ I C . . . \ - _ . ( ! = . 1 t a n 2 ( } u . X 1 1 ) - A t a n 2 ( K 1 . K 2 ) \ \ h e r e . 1 \ = r , + 1 / - ' , _ 1 \ . ' " I , S . 3 . 2 . 2 C o o r d i n a t e c o n v e r s i o n ( 3 . 6 ) ( 3 . 7 ) T h e c o o r d i n a t e s o f t h e e n d o f l i n k ! , _ . , ( H i p ) h a s b e e n c a l c u l a t e d r e l a t i v e t o t h e C o o t o f s t a n c e k g c o o r d i n a t e s y s t e m a n d t h e c o o r d i n a t e c o n v e r s i o n f r o m t h e o r i g i n a l c o o r d i n a t e s y s t e m a r c a s f ' o l l m \ s . \ , = . Y - ( x , - x 1 ) c o s a ( 3 . 8 ) l = I I - ( x~ - x 1 ) s i n a + L \ y ( 3 . 9 ) 2 5 - " r = I t a n u ) : i r ( 3 . 1 ( ) ) \ - \ c o : , ( , ( ( 3 . 1 l ) 3 . 3 C a l c u l a t i o n o f j o i n t a n g l e s 3 . 3 . 1 C a l c u l a t i o n o f j o i n t a n g l e s o f s w i n g l e g " ' I h e t ( 1 I I u \ \ i n g s t e p s a r c f o l l o v . e d t o c a l c u l a t e t h e j o i n t a n g l e s f 3 1 • f ) c . 1 i l C o o r d i n a t e s ( . Y . y ) o f t h e t r a j e c t o r y p a t h p o i n t s w h i c h t h e s w i n g l e g m o v e s a r c e x t r a c t e d b y f i n d i n g t r a j e c t o r y c o e f f i c i e n t s a n d p l o t t i n g t h e t r a j e c t o r y i n V I A T L A B . 1 i i ) I h e o r i g i n a l c o o r d i n a t e s h a v e c o n v e r t e d r e l a t i v e t o t h e h i p c o o r d i n a t e f r a m e a c c o r d i n g t o t h e e q u a t i o n s ( 3 . 4 ) a n d ( 3 . 5 ) . ( i i i ) T h e e q u a t i o n s ( 3 . 1 ) a n d ( 3 . 2 ) a r e u s e d t o c a l c u l a t e t h e a n g l e s r e l a t e d t o e a c h t r a j e c t o r y p a t h p o i n t u s i n g M A T L A B p r o g r a m s A . 3 a n d A . 4 i n d i c a t e d i n A p p e n d i x A . 3 . 3 . 2 C a l c u l a t i o n o f j o i n t a n g l e s o f s t a n c e l e g I h e ! ' o l l m \ i n g s t e p s a r e f o ! l o w e d t o c a l c u l a t e j o i n t a n g l e s f J , . f 3 6 . 1 i l T h e h i p m o v e m e n t d i s t a n c e h a s b e e n c a l c u l a t e d w i t h e q u a t i o n ( 2 . 1 I ) a n d d i v i d e d e q u a l l y t o t h e n u m b e r o f s w i n g l e g t r a j e c t o r y p a t h p o i n t s . 1 i i ) T h e o r i g i n a l c o o r d i n a t e s h a v e c o n v e r t e d r e l a t i v e t o t h e f o o t c o o r d i n a t e t l · a m e a c c o r d i n g t o t h e e q u a t i o n s ( 3 . 8 ) . ( 3 . 9 ) . ( 3 . 1 0 ) a n d ( 3 . 1 1 ) . 1 i i i ) T h e e q u a t i o n s ( 3 . 6 ) a n d ( 3 . 7 ) a r e u s e d t o c a l c u l a t e t h e a n g l e s r e l a t e d t o e a c h t r a j e c t o r y p a t h p o i n t u s i n g M A T L A B p r o g r a m s A . S a n d A . 6 i n d i c a t e d i n A p p e n d i x A . 3 . 3 . 3 M o d i f i c a t i o n o f j o i n t a n g l e s o f s w i n g l e g t o c o m p e n s a t e h i p m o v e m e n t T h e j o i n t a n g l e s o f t h e s w i n g l e g h a s b e e n c a l c u l a t e d i n s e c t i o n 3 . 1 . 1 a s s u m i n g t h e h i p i s - ; t a t i o n a r ) b u t w i t h t h e m o v e m e n t o f t h e h i p a l o n g w i t h s t a n c e l e g i n o r d e r t o m o v e t h e ' \ \ i n g ! ( l O t i n t h e s a m e t r a j e c t o r y t h e h i p m o v e m e n t n e e d t o b e c o n s i d e r e d i n j o i n t a n g l e L d k u l c l l i o n : - ; . T h e r e f o r e t h e j o i n t a n g l e s c a l c u l a t e d c o n s i d e r i n g o n l y t h e m o t i o n o f t h e s w i n g k g i n s e c t i o n 3 . 1 . 2 n e e d t o b e r e c a l c u l a t e d t o c o m p e n s a t e f o r t h e h i p m o v e m e n t . 2 6 ! h e h g u r c : 1 . 3 i n d i c a t e s t h e h i p m o v e m e n t a l o n g a t r a j e c t o r y p a r a l l e l t o t h e s l o p e . T h e l l l l r i / ( l l l t a l d i s t a n c e t h e h i p m o v e d a l o n g x - a x i s i s t a k e n a s . 6 . x a n d t h e v e r t i c a l m o v e m e n t a l o n g . 1 - a : - . i s i s t a k e n a s L \ y . T h e c o o r d i n a t e s ( , \ : · . Y ) u s e d t o c a l c u l a t e s w i n g l e g j o i n t a n g b n e e d t o r e c a l c u l a t e c o n s i d e r i n g t h e m o v e m e n t o f h i p m a k i n g m o d i f i c a t i o n s t o t h e e q u a t i o n s ( 3 . 4 ) a n d ( 3 . 5 ) . I ' r t - - H / , · + · . • r L , • , . _ , , . . . . - --~~---r~-~:c:tu~j~~tw; 1....~. : ---f-~_+ _ _ - . e . , - - - - \ {.~~ \ t \ _ _ : _ : _ _ . , . : Y x . _ . j · I r - -----~~-,~-~ ~ . 1*-----~_::c--~--· " ' - - - - . Y F i g u r e 3 . 3 : T h e m o t i o n s o f s w i n g l e g a n d s t a n c e l e g ! h e m o d i f i e d e q u a t i o n s ( 3 . 4 ) a n d ( 3 . 5 ) t o f i n d t h e ( X , Y ) c o o r d i n a t e s o f t h e t r a j e c t o r y p a t h l l l l l \ i d c t · i n g h i p m o v e m e n t a r c a s f o l l o w s . . \ = ( . \ ' - x 1 c o s a ) - , 6 ) , ( 3 . 1 2 ) · ( / / 1 Y 1 s i n a - y ) + L \ y ( 3 . 1 3 ) 2 7 C h a p t e r 4 S i m u l a t i o n o f t h e b i p e d a l R o b o t S i m u l a t i o n o f t h e b i p e d a l r o b o t h a s b e e n c a r r i e d o u t w i t h t h e r e s u l t s f r o m t h e p r e v i o u s c h a p t e r . T h e b i p e d a l r o b o t v v a l k i n g o n 5 d e g r e e s l o p e h a s b e e n s i m u l a t e d a f t e r c r e a t i n g j o i n t a n g l e d a t a . M A T L A B s o l t \ \ a r e h a s b e e n u s e d e x t e n s i v e l y ' f o r t h e j o i n t a n g l e c a l c u l a t i o n s . E a c h t r a j e c t o r y h a s b e e n p l o t t e d a n d e x t r a c t e d d a t a p o i n t s s u c h t h a t t h e d i f f e r e n t b e t w e e n t w o c o n s e c u t i v e d a t a p o i n t s t o b e m i n i m u m i n o r d e r t o g e t v e r y a c c u r a t e r e s u l t . L a c h s i m u l a t i o n i n v o l v e s m o r e t h a n h u n d r e d d a t a p o i n t s f o r e a c h j o i n t a n g l e \ a r i a t i o n . 4 . 1 S i m u l a t i o n S o f t w a r e T h e s i m u l a t i o n s o f t w a r e u s e d i n t h i s s i m u l a t i o n i s ' · R o b o W o r k s · ' i n t r o d u c e d b y c o m p a n y c a l l e d ~e\\tonium. T h i s s o f t w a r e i s u s e r f r i e n d l y a n d h a s c a p a b i l i t y t o s i m u l a t e u s i n g d a t a t i l t ' - ; [ 2 0 [ . 4 . 2 S i m u l a t i o n o f S w i n g L e g T h e \ 1 / \ T l ~AB p r o g r a m s A . l a n d A . 2 i n A p p e n d i x A h a s b e e n u s e d f o r t h e c a l c u l a t i o n o f t h e c o e f f i c i e n t s o r t h e e q u a t i o n 2 . 1 u s i n g t h e p a r a m e t e r s m e n t i o n e d i n t a b l e 4 . 1 . T h e b o d y J x u · a m e t e r : - , o f t h e b i p e d a l r o b o t a r c t a k e n a p p r o x i m a t e l y e q u a l t o t h e h u m a n s k e l e t o n . T a b l e 4 . 1 - P a r a m e t e r s f o r t h e s w i n g l e g s i m u l a t i o n r , . '~~ 4 6 0 m m x , l O O m m I ! . , . r , 4 8 0 m m X , 4 5 0 m m L , . !~~, 1 8 0 m m x , 8 0 0 m m a i s u I I 7 0 0 m m - - - - - - I h l · l l l U l t r a j c c t o r ) a f t e r t h e c a l c u l a t i o n \ \ a s . 1 l ) J . 6 4 8 2 + 1 . 1 2 2 5 x - 0 . 0 0 1 2 x ' ( 4 . 1 ) I h e I i g u r c 4 . 1 s h o w s t h e p l o t o f t h e c a l c u l a t e d t r a j e c t o r y a l o n g w i t h t h e s l o p e . 2 8 5 0 0 I I 4 5 0 i - I I 400~ I I 3 5 0 1 - I I - 3 0 0 r 250~ I I f 2 o o 1 " " I 1 0 0 I 50~ I o L 8 1 0 0 2 0 0 3 0 0 F o o t t r a j r l ' t o r y o n S d e g . s l o p e y = - 9 1 . 6 . t 8 2 + 1 . 1 2 2 S x - . O O 1 2 x 2 4 0 0 I 5 0 0 D i s t a n t ' e ( 1 1 1 1 1 1 ) . . 6 0 0 7 0 0 F i g u r e 4 . 1 : F o o t t r a j e c t o r y a t 5 d e g r e e s l o p e T B O O 9 0 0 1 0 0 0 T h e s i m u l a t i o n p r o g r a m E . l w r i t t e n f o r t h e s i m u l a t i o n o f s w i n g l e g i s i n d i c a t e d i n A p p e n d i x E . T h e d a t a f i l e h a s b e e n c r e a t e d i n . d a t f o r m a t u s i n g t h e a n g l e s 8 1 J ) 2 a n d ( ) c a l c u l a t e d a n d u s e d i n t h e s i m u l a t i o n . T h e t l g u r e 4 . 2 . 4 . 3 . 4 . 4 . 4 . 5 a n d 4 . 6 s h o w s t h e s i m u l a t i o n s t e p s o f s w i n g l e g s i m u l a t i o n . T h e t i g u r e 4 . 2 s h o w s t h e i n i t i a l p o s i t i o n o f t h e s w i n g l e g a n d s t a n c e l e g . T h e c u r r e n t d a t a p o i n t s h o w n i s z e r o . T h e m o n i t o r w i n d o w s h o w s t h e i n i t i a l v a l u e s o f t h e s w i n g l e g j o i n t a n g l e s ( - J 1 • f ) , a n d 0 1 • r \ ' l c n 1 t c r ~ ~::.,: I h g l l " ' " r i - c · T i ; J < : ; l h e l a 1 - 4 5 ~~~ ) < : < . ' " ' . ~ . . , . , l h d : J . : ' t h e t 0 l 3 1 ·:~ s F i l e P l a · . e r F ' o h o ' . • ' . ' m h 1 C ' \ 8 : s s : _L1U~~JJJ__QJ • l . ! . l F'ro;a~1'1e o r · w r · · , t e r • : • I S e t f - ' , : , w d : = 1 : ' . 4 C u r r e n t P o w r l = 0 ~-pee·~ F : ~·e T 1 r n e [ 1 e l a y l m - : : 1 ~ _ c j ~ F i g u r e 4 . 2 : I n i t i a l p o s e o f t h e s w i n g l e g s i m u l a t i o n 2 9 I h e I i i : ' l l l l ' ~.3 s h o w s t h e S v \ i n g l e g a t 4 2 " d d a t a p o i n t a n d t h e s w i n g l e g j o i n t a n g l e s a r e \ I ' I h k ' I l l l l l l l l l l t o r \ \ i n d 0 \ \ . ~,11r; 1 1 r t c r 8 ) " · · · 'f;,~j :,:~,t" , . , . , , , . . _ , , , . . ~.rrllt:~"";,...£·~";:'~;. tf-,et:~3 . . F i l e P l a y e r : R o b o W o r k ; ; 1 : - - ; ; - 1 8 ~ ~~- ~!-<4111 ~~Ill p l • l • l --~_____j~ - r lurrrt,~· u l - · , _ r r r l 1 : : . \ F - - ' o r n l " 4 : ' ·~ p e c : : d P 3 t e : T 1 1 1 , e D e ' , a y l r r r : l ] 1 : : ' : : : ! ~ F i g u r e 4 . 3 : S w i n g l e g s i m u l a t i o n a t 4 2 " d d a t a p o i n t ! h e f i~ure 4 . 4 s h o w s a n o t h e r p o s e o f t h e s w i n g l e g s i m u l a t i o n a t ! 6 3 t h d a t a p o i n t . F i l e P l a y e r : R o b o W o r k s l ~ F i g u r e 4 . 4 : S w i n g l e g s i m u l a t i o n a t 1 6 3 r d d a t a p o i n t 3 0 I h l · I i g u r c - 1 - . 5 s h o V \ s t h e S V \ i n g l e g a t t h e f i n a l d a t a p o i n t . M c n 1 t c r ~ ~he~.01J 1:.~ 4'~ . . ----=-~4'-'-- F i l e P l a y e r . R o b o W o r k s l F ' r o p e r t l f : : : ' I·~ u m b e r o~ ~,et P o 1 n l ' 1 : = : : . . 1 S p e e d R a l e r - : 3 Q ; F i g u r e - 1 - . 5 : F i n a l p o s e o f t h e s w i n g l e g s i m u l a t i o n l h l · I i g u r e 4 . 6 s h o w s t h e a c c u r a c y o f t h e s w i n g l e g s i m u l a t i o n b y s h o w i n g a l l t h e l l l i l \ c m e n t s i n a s i n g l e f r a m e . I t c a n b e s e e n t h a t t h e f o o t o f t h e s w i n g l e g i s f o l l o w i n g t h e tr~1jcclm) s h o w n i n F i g u r e 4 . 1 . T h i s p r o v e s t h a t t h e a c c u r a c y o f t h e j o i n t a n g l e e q u a t i o n s . F i g u r e 4 . 6 : T h e s w i n g l e g f o l l o w i n g p r e d e f i n e d t r a j e c t o r y 3 1 4 . 3 S i m u l a t i o n o f S t a n c e L e g T h e c , t a n c c l e g s i m u l a t i o n h a s b e e n c a r r i e d o u t u s i n g t h e c a l c u l a t e d j o i n t a n g l e s ( } , a n d B . . . \ J a t a f i l e i n . d a t f o r m a t h a s b e e n c r e a t e d u s i n g j o i n t a n g l e d a t a i n o r d e r t o s i m u l a t e t h e s t a n c l ' \c~ i n R o h o W o r k s s o f t w a r e . l h c lt~urc - 1 - . 7 s h o w s t h e i n i t i a l a n d t i n a ! p o s i t i o n s o f t h e s t a n c e l e g . T h e F i g u r e 4 . 8 s h o w s t h e c o m p l e t e m o t i o n a n d s h o w s c o r r e c t n e s s o f t h e j o i n t a n g l e c a l c u l a t i o n b e c a u s e t h e h i p m o \ e o i n t h e h i p t r a j e c t o r y p l a n n e d . P r o p e r t i e - s t·~ u r n b e r o~ ~, e t F'uu-ot~ = I 9 4 S p e e d R a t e ~ I ' . - e J . . C u r r ' = ' n t F ' , _ q r o t = 1 9 3 T t m e [ J e i . O ! y [ m : 1 F i g u r e 4 . 7 : T h e i n i t i a l a n d t i n a ! p o s i t i o n s o f t h e s t a n c e l e g F ' r o p e r t t e - , t · J u m b e r o f ~.et F ' o t n t : o : = 1 ' 3 4 [ t _ m e r - , t F ' r _ , , r - , t - 1 ' ) 3 ':·~·eed F : . o . t e [ ~ _ _ , _ j F i g u r e 4 . 8 : T h e s t a n c e l e g f o l l o w i n g t h e p r e d e f i n e d t r a j e c t o r y 3 2 4 A S i m u l a t i o n o f b o t h l e g s ! h e ~imulation o f b o t h l e g s h a s b e e n c a r r i e d o u t a f t e r m o d i f i c a t i o n o f a n g l e s f J 1 a n d 0 2 o f s 1 1 i n g l e g a s i n d i c a t e d i n s e c t i o n 3 . 3 . 3 a n d j o i n t a n g l e d a t a o f b o t h l e g s w e r e u s e d t o c r e a t e date~ I l k i n . d a t f o r m a t T h e F i g u r e s f r o m 4 . 9 t o 4 . 1 3 s h o w s t h e d i f f e r e n t p o s e s o f t h e s i m u l a t i o n f r o m s t a r t t o e n d . I _ " ' I @ l ~ I lil-t~ l ! J _ : ' ! J J ! J _ ! ! L 2 . l • I . . . _ I P r o p e r t 1 e : : . ! ' l u m b e r o~ S e t F'o1nt~"' 1 ' 3 4 C u r r e n t F ' o t n t = 1 T 1 r f 1 8 [ 1 e l 0 1 y lm~J F i g u r e 4 . 9 : I n i t i a l p o s i t i o n o f l o w e r b o d y I " 1 , ~-:~tliJJJJ_£] • I . . . _ I F'roper~ t · J u m b e r o f S e t P o t n t . : , " ' 1 ' 3 4 C u r r e n t F ' o 1 n t = D F i g u r e 4 . 1 0 : L o w e r b o d y s i m u l a t i o n a t 3 3 r d d a t a p o i n t - , - , _ ) . ) •·l~ 1!J~J!Jl!L2J • I ~I F ' r o p e r t 1 e s : t·~ u m b e r o f ~. e t F'oint~· = 1 ' 3 4 ~.peed F : . : J t e I , _ : j C u r r e n t P o i n t = 1 5 4 T 1 m e [lela~~ l m : = : . l F i g u r e 4 . 1 1 : L o w e r b o d y s i m u l a t i o n a t ! 5 4 t h d a t a p o i n t ~t~~J!JJlJ_2j • I ~I F ' r o p e r t 1 e s H t _ m t b e r o~ S e t F ' o 1 n t : . 1 3 4 S p e e d F : a t e ~ I , _ : j C u r r e n t F ' o 1 n t = 1 3 3 T 1 m e D e l . ; : , y ( r n s ) F i g u r e 4 . 1 2 : F i n a l p o s i t i o n o f t h e l o w e r b o d y 3 4 i 1 " l p c = J _ I G 1~1 ~_!U~~!!J~ • I ~I P r o p e r t 1 e s N u m b e r o i ' = · e t F ' o w i l c = 1 9 4 S p e e d R a t e r : ; - - : : ; J I ' ~ C u r r e n t F ' o i n t = 1 ~3:3 T i m e D e l a y [ m : : . : } L : 1 1 2 F i g u r e 4 . 1 3 : L o w e r b o d y f o l l o w i n g f o o t a n d h i p t r a j e c t o r y I h e - ; i m u l a t i o n o f l o w e r b o d y h a s b e e n c a r r i e d o u t v v i t h 1 9 3 d a t a p o i n t s w i t h e a c h d a t a p \ l i n t c o n s i s t i n g v a l u e s o f a n g l e s ( ) 1 , ( ) 2 , ( ) 3 , ( ) 5 a n d ( ) 6 w h i c h a r e c a l c u l a t e d u s i n g t h e j o i n t a n g l e e q u a t i o n s d e r i v e d u s i n g i n v e r s e k i n e m a t i c s . T h e F i g u r e 4 . 9 s h o w s t h e i n i t i a l p o s e o f b u t h l e g s . T h e F i g u r e 4 . 1 0 a n d F i g u r e 4 . 1 1 s h o w s t h e s i m u l a t i o n p o s e s a t 3 3 r d a n d 1 5 4 1 1 1 d a t a p o i n t s . T h e F i g u r e 4 . 1 2 i n d i c a t e s t h e f i n a l p o s e o f t h e s i m u l a t i o n . I n o r d e r t o m a i n t a i n c y c l i c g a i t t h e f i n a l p o s i t i o n o f s t a n c e l e g s h o u l d b e e q u a l t o t h e i n i t i a l p o s i t i o n o f t h e s w i n g l e g a n d t h i s i s c l e a r l y v i s i b l e w h e n c o m p a r i n g t h e F i g u r e 4 . 9 a n d I i g u r e - 1 - . 1 2 . T h e F i g u r e 4 . 1 3 s h o w s t h e s w i n g f o o t t r a j e c t o r y a n d h i p t r a j e c t o r y b o t h f o l l o w i n g t h e p r e d e f i n e d t r a j e c t o r i e s a s p l a n n e d i n t h e s e c t i o n 2 . 2 . 2 . 3 5 4 . 5 S i m u l a t i o n o f L o w e r b o d y a n d T o r s o I h e ' l l l l l l l a t i o n h a s b e e n c a r r i e d o u t a f t e r a d d i n g t o r s o . T h e h i p c o n s i s t s o f 3 r o t a r y j o i n t s 1 1 1 1 h I I \ ( ) f o r l e g s a n d o n e C o r t h e h i p . T h e a n g l e t h e t o r s o m a k e s w i t h t h e h o r i z o n t a l a x i s i s c o n : - . l d c ! · c d a s t o r s o a n g l e . T h c , c : - . i m u l a t i o p w i n d o w s s h o w h o w t h e b i p e d a l w i l l n e g o t i a t e a s c e n d i n g s l o p e o f 5 d c g r l ' l ' " · T h e F i g u r e s 4 . 1 4 t o 4 . 1 7 s h o w s t h e d i f f e r e n t s i m u l a t i o n p h a s e s w h i c h s h o v v s t h e b e g i n n i n g . i n t e r m e d i a t e a n d f i n i s h i n g s t a g e s . i ~llJ~__!l!!LQJ • I ~I F ' r o p e r t r e - , H r l m b e r o f ~.et F',Jrtrt~ = 1 7 5 C u r r e n t P ( 1 1 n l = ( I F i g u r e 4 . 1 4 : I n i t i a l p o s i t i o n o f t h e b i p e d a l r o b o t ~ llJ~__!j!!J_QJ • I ~I[ P r . : r p e r t r e ; r · J u r n b e r o t ' 3 e l i = ' c n r d s = l _ r : r r f : : ' n r ' - ' _ q r r 1 = 4 7 S p e e d R a t > : " T m · e [ " e l a v l r r " ] ~ ~ F i g u r e 4 . 1 5 : S i m u l a t i o n a t 4 i h d a t a p o i n t 3 6 1 T h e l i g u r c 4 . 1 8 s h o w s a l l t h e m o t i o n s o f s w i n g l e g , s t a n c e l e g a n d t o r s o i n a s i n g l e v v i n d o w t o i l l u s t r a t e t r a j e c t o r i e s o f m o t i o n . T h i s s i m u l a t i o n r e s u l t v e r i f i e s t h e k i n e m a t i c s o f t h e b i p e d a l m b o t a n d v a l i d a t e s t h i s a p p l i c a t i o n i n r e a l e n v i r o n m e n t . T h i s s i m u l a t i o n h a s b e e n c a r r y o u t \ \ i t h l a r g e d a t a p o i n t s a n d h e n c e t h e s w i n g f o o t a n d h i p m o v e s i n t h e p r e d e f i n e d t r a i c c t o r : s m o o t h l y . F i g u r e 4 . 1 8 : T h e s w i n g l e g , h i p a n d t o r s o m o v e m e n t 3 8 C h a p t e r 5 C a l c u l a t i o n o f Z M P a n d D y n a m i c S t a b i l i t y 5 . 1 ( ; c n e r a l Z M P E q u a t i o n f o r t h e b i p e d a l r o b o t ; I i l , , i ' > l d n c c o f t h e Z M P f r o m t h e a n k l e j o i n t o f t h e s u p p o r t i n g l e g o f t h e b i p e d a l r o b o t i n t l h : . I ' - ' d i o n o f m o t i o n c a n b e d e t e r m i n e d f r o m t h e f o l l o w i n g e q u a t i o n [ 2 1 J . . \ ~ ' 1 ( 1 , r o , + m , X , ( Y , - K ) - m , X,~) IL:~~ 1 m, ( } ; - g ) f . -Mom~nt o f I n e r t i a o f i t h l i n k ( k g l m 2 ) ( ' ' - / \ n g u l a r a c c e l e r a t i o n o f l i n k I ( r a d l s 2 ) I J / - \ ! l a s s o r i 1 1 1 l i n k ( K g ) ( ' d ' t ' h · t h 1 d x . r . - o o r m a t e o t e 1 u m p e m a s s ~ - i \ c c e l e r a t i o n d u e t o g r a v i t y ( m l s 2 ) ) - / \ c c e l e r a t i o n o f l i n k i i n y - d i r e c t i o n ( m l s 2 ) \ - A c c e l e r a t i o n o f l i n k i i n x - d i r e c t i o n ( m l s 2 ) 5 . 2 D y n a m i c S t a b i l i t y M a r g i n ( D S M ) o f b i p e d a l r o b o t ( 5 . 1 ) I I , , ti~urc 5 . 1 s h o w s t h e f o o t o f t h e s t a n c e f o o t a n d m a r k e d w i t h d i s t a n c e X n v i P T h e \ \ , 1 : \ . 111~ b e c o m e s d y n a m i c s t a b l e i f t h e Z M P f a l l s b e t w e e n t h e d y n a m i c s t a b i l i t y m a r g i n o f ' t l l l c c ! ' o o t . I I ' . ' J ) n a m i c s t a b i l i t y r e g i o n ( D S M ) = ( x 1 - L 6 I 2 ) t o ( x 1 + L r , I 2 ) ( 5 . 2 ) 3 9 \ \ ' - - - - " t ; - - - - x / M I ' D y n a m i c S t a b i l i t y M a r g i n F i g u r e 5 . 1 - D y n a m i c s t a b i l i t y s a f e m a r g i n ; S . J A p p l i c a t i o n o f Z M P E q u a t i o n f o r t h e b i p e d a l r o b o t Th~· ~·quation 5 . 1 c a n b e a p p l i e d t o f i n d t h e Z M P o f b i p e d a l r o b o t i n s w i n g i n g p h a s e a t e a c h tra1v~tory p o i n t i n o r d e r t o f i n d d y n a m i c s t a b i l i t y w h i l e w a l k i n g . ---~ ~~ : : x . . y I ( X : ; , J ; ; ) t _ _ _ : : s _ _ _ - ' 1 ' : X~ . : ' l----------------~ r x . \ ~------"" . . . . . . . . - - - - - - - - - - - - - - - - - - - 1 X F i g u r e 5 . 2 - L o w e r b o d y o f b i p e d a l r o b o t 4 0 I ~~·I i ! l u r c 5 . . 2 s h O \ v s t h e m a s s e s . l e n g t h s o f e a c h l i n k a n d c o o r d i n a t e s o f t h e c e n t r e o f m a s s , , , · : t c h l i n k . ! !,,~ t ( l h m c i d e n t i c a l leg~. 1 1 : 1 1 1 1 • m , = m , . L 1 = / " 1 • / " = = L , . 5 . 3 . 1 M a s s c e n t r e c o o r d i n a t e s o f l i n k s I k l c l l l < m i n g e q u a t i o n s w e r e f o r m u l a t e d c o n s i d e r i n g t h e m o t i o n o H h e b i p e d a l r o b o t . \ ' c o s ( X + 6 . x + 0 . 5 r ! c o s e l ( 5 . 3 ) \ + 0 . 5 / . 1 s i n 0 1 + L s i n ( . i r - ( 8 1 + 8 2 ) ) ( 5 . 4 ) . \ _ ; _ O . S L , C O S ( i r - ( 0 1 + 0 , ) ) ( 5 . 5 ) • \ + o . s r , s i n ( i r - ( 0 1 + 0 , ) ) ( 5 . 6 ) - . r . c o s a + 0 . 5 L , c o s 8 1 , ( 5 . 7 ) r , s i n a + 0 . 5 L 2 s i n f } r . ( 5 . 8 ) L \ x + 0 . 5 L 1 c o s ( ; y - ( 8 5 + 8 1 , ) ) + x 1 c o s a ( 5 . 9 ) \ , ' . i n a + L , s i n f J 1 , + ( ) . S L 1 s i n ( ; y - ( 0 5 + 0 6 ) ) ( 5 . 1 0 ) 5 . . \ . 2 A n g u l a r a c c e l e r a t i o n s o f l i n k s 1 1 1 , · c o e f f i c i e n t s o f t h e f i f t h - o r d e r j o i n t a n g l e p o l y n o m i a l f o u n d b y w n t m g a M A T L A B l ' t ' ' ! l t · a m s i n d i c a t e d i n A p p e n d i x A . 7 a n d A . 8 i n d i c a t e d i n A p p e n d i x A t o s o l v e s i x l'L!~t~ltions i n d i c a t e d i n s e c t i o n 2 . 2 . 3 . T h i s p r o g r a m h a s b e e n u s e d t o f i n d e a c h j o i n t a n g l e 1 ' • ' 1 \ t w m i a l s e p a r a t e l y . I h · : m g u l a r a c c e l e r a t i o n o f j o i n t i i s g i v e n b y : : ) = 2 a 1 1 + 6 a , J 2 + 1 2 a , 4 t ~ + 2 0 a , 5 t ; ( 5 . 1 1 ) 4 1 5 . . ' . J A c c e l e r a t i o n s o f l i n k s l t \ . : c u r s i v c N c v \ t o n - E u l e r f o r m u l a t i o n h a s b e e n c a r r i e d o u t t o f i n d t h e l i n k a c c e l e r a t i o n s u ' l , t e l l l i n k o r b i p e d a l r o b o t a n d m e n t i o n e d b e l o w . f 2 2 l · · - 1 · , ( ·~ + w , - g ) S 1 - o . 5 L 1 o i ' = r · , . \ 1 + ( V , - g ) C 1 - 0 . 5 L 1 f J 1 . 1 . ; \ c 2 + v I l s 2 1 - 0 . 5 L , _ ( f ) I + f ) 2 ) 2 f I : . \ ' , + f ' t : C + 0 . 5 [ , ( { - J J + O o ) g . \ , - 0 . 5 L , 0 , : - g ( , \ - 0 . 5 I , ' ( ) ' I , , = L , 0 , S , - L 2 f ) , C 4 - / _ 1 f ) , - L 1 0 _ , - 2 L 1 f J , f J 1 - gS'~ 4 c 1 · , , = L , o , C 4 - r 1 e , + L 1 e 1 + L 2 f h S 4 - g C , 4 1 · 1 : + 0 . 5 L 1 ( f J , + f J 1 ) -I~-0.5L 1 (fJ,+fJ1) 5 . . t - l I n e r t i a o f l i n k s I i , · r \ l h u t i s c o n s i d e r e d t o b e m a d e u p o f s l e n d e r b a r s . II,~· m o m e n t o r i n e r t i a o f i t h l i n k I , = I I 1 2 m , L~ h e r e 1 1 1 - m a s s o f l i n k i I I , , - l e n g t h o r l i n k i ( 5 . 1 2 ) j ( 5 . 1 3 ) ( 5 . 1 4 ) ( 5 . 1 5 ) ( 5 . 1 6 ) ( 5 . 1 7 ) ( 5 . 1 8 ) ( 5 . 1 9 ) ( 5 . 2 0 ) ( 5 . 2 1 ) ( 5 . 2 2 ) 4 2 l 5 . 3 . 5 B u i l d i n g M A T L A B p r o g r a m t o c a l c u l a t e Z M P \ \ I A T L A B p r o g r a m A . 9 h a s b e e n f o r m u l a t e d t o c a l c u l a t e Z M P i n X d i r e c t i o n c o m b i n i n g [ h e ' - ' l J U a t i o n s i n d i c a t e d i n s e c t i o n s 5 . 3 . 1 . 5 . 3 . 2 . 5 . 3 . 3 a n d 5 . 3 . 4 T h e s e M A T L A B p r o g r a m s : c l t c c t c h c a s e i s i n d i c a t e d i n a p p e n d i x A . I h i \ \ l A I L A B f u n c t i o n c a n b e u s e d t o c a l c u l a t e Z M P i n X d i r e c t i o n a t a n y i n s t a n t . 5 . 4 C a l c u l a t i o n m e t h o d o f Z M P I h e s t e p s o f c a l c u l a t i o n o f Z M P a r e i n d i c a t e d b e l o w . l o r a l l t h e Z M P c a l c u l a t i o n s t h e t ( l l l ( l \ \ i n g m e t h o d h a s b e e n f o l l o w e d . ! h e f o o t h a s n o t b e e n c o n s i d e r e d f o r t h e Z M P c a l c u l a t i o n d u e t o t h e s m a l l e r m a s s c o m p a r e d t o o t h e r l i n k s h e n c e i t s e f f e c t o n t h e d y n a m i c s t a b i l i t y o f r o b o t i s a s s u m e d t o b e n c g l i g i h l e . ( i ) D e c i d i n g v a l u e s f o r L 1 . L , _ , L 3 , L 4 . L 5 , L 6 . a , H , x 1 , x c . x 3 • ( i i ) C a l c u l a t i o n o f t r a j e c t o r y c o e f f i c i e n t s u s i n g t h e M a t l a b p r o g r a m J \ . 2 . ( i i i ) P l o t t i n g o f t r a j e c t o r ) a n d e x t r a c t i n g t r a j e c t o r y c o o r d i n a t e s x . y . ( i i i ) C a l c u l a t i o n o f c o o r d i n a t e s X . Y u s i n g e q u a t i o n s 3 . 4 a n d 3 . 5 . ( i \ ) C a l c u l a t i o n o f h i p t r a j e c t o r y c o o r d i n a t e s A \ . L ' l y u s i n g e q u a t i o n s 3 . 1 0 a n d 3 . 1 1 . ( \ ) M o d i f i c a t i o n o f X . Y u s i n g e q u a t i o n s 3 . 1 2 a n d 3 . 1 3 . ( v i ) C a l c u l a t i o n o f j o i n t a n g l e 8 2 u s i n g M A T L A B p r o g r a m A . 3 . ( ' i i ) C a l c u l a t i o n o f j o i n t a n g l e 8 1 u s i n g M A T L A B p r o g r a m A . 4 . ( \ i i i ) C a l c u l a t i o n o f j o i n t a n g l e ( ) r , u s i n g M A T L A B p r o g r a m A . 6 . ( i x ) C a l c u l a t i o n o f j o i n t a n g l e 0 , u s i n g M A T L A B p r o g r a m A . 5 . ( x ) D e c i d i n g v a l u e s f o r m 1 . m , . m , . m 1 . J ' . t 0 . 1 1 . t , l 1 . } . . - · . ( x i ) C a l c u l a t i o n o r c o e f f i c i e n t s o f e a c h j o i n t a n g l e t r a j e c t o r y a , 0 , a , 1 • a , 2 • a , , . a , , a n d o , 5 u s i n g M A T L A B p r o g r a m J \ . 8 . ( x i i ) C a l c u l a t i o n o f Z M P u s i n g r e l e v a n t M A T L A B p r o g r a m i n d i c a t e d i n A p p e n d i x ; \ u s i n g t h e d a t a c a l c u l a t e d i n p r e v i o u s s t e p s . 4 3 . 5 . 5 Z M P o i · t h e L o w e r b o d y o f b i p e d a l r o b o t / \ l f > \ a l u e s f o r s i n g l e s t e p h a v e b e e n c a l c u l a t e d u s i n g t h e M A T L A B p r o g r a m A . 9 u s i n g :r~tjcctory d a t a u s i n g p a r a m e t e r s i n T a b l e 5 . 1 . ' l t c p L e n g t h ( S L ) = X 1 - x 1 ' i t c p I i m e ( T ) = ' l 1 - / 1 1 T a b l e 5 . 1 - P a r a m e t e r s f o r t h e c a l c u l a t i o n o f Z M P o f l o w e r b o d y ; I I I . I --~ --~)0 l l l l 1 1 . - - - - - - - + - - - · - - - - + - - - - - + - - -~~_x,__ 1 ~- --l~-- I 0 0 1 1 1 1 1 1 ~ 0 _ _ . . ' : _ _ _ ± · _ _ _ - 1 - 8 0 m m \ , 4 s o 1 1 1 1 1 1 1 I~__ _ _ _ _ 1 8 0 m m I x , 8 0 0 m m I ' S I T I . - - - - - - - - - 1 u _ _ L _ _ _ _ _ _ _ --~ - - - · - - - - - + - - + - - - ~--_!::I_ _ _ - + - - _ l { ) _ O m m _ 1 I 9 . 8 1 m / s - m , , m 1 5 . 0 K g - r ( ) ~ x I 3 5 0 m m m , , m , 5 . 2 K g - - l T - - l 5 s e c - - - - - - + - - ~- - -~ - - - 0 s e c - - I 5 . 6 Z M P v a r i a t i o n o f t h e L o w e r b o d y o f b i p e d a l r o b o t I h e / Y I P v a l u e s o b t a i n e d h a v e b e e n p o t t e d a n d s h o w n i n F i g u r e 5 . 3 . T h e d y n a m i c s t a b i l i t y m a r g i n h a s b e e n c a l c u l a t e d a c c o r d i n g t o t h e e q u a t i o n 5 . 2 . D ' i M = 3 6 0 m m t o 5 4 0 m m " E E 5 0 0 4 0 0 C l . . : § : : 3 0 0 N 2 0 0 Z M P v a r i a t i o n o f L o w e r b o d y ~fDynar m a r g i n - Z M P - - - - - - - - - - - - - c - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - _ t - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - I O O L _ _ _ l _ _ _ _ L _ _ ~--~---L---~---L--~---L--- 0 0 . 5 1 1 . 5 2 2 . 5 3 3 . 5 4 4 . 5 5 T i m e ( s e c ) F i g u r e 5 . 3 - Z M P v a r i a t i o n o f L o w e r b o d y o f t h e b i p e d a l r o b o t 4 4 \ c , · u r d i n g t o t h e F i g u r e 5 . 3 l o v \ e r b o d y m o t i o n i s h i g h l y u n s t a b l e d u r i n g m o s t o f t h e s t e p l t t l l l ' t h c r c i ' o r c i t i s e v i d e n t t h a t l o w e r b o d y a l o n e c a n n o t a c h i e v e d y n a m i c s t a b i l i t y . 5 . 7 A d d i n g T o r s o \ , h i g h l i g h t e d i n s e c t i o n 5 . 6 i t i s i m p o s s i b l e t o a c h i e v e d y n a m i c s t a b i l i t y a l o n e w i t h l o w e r b u d : - . T h e r e f o r e i n h o p i n g t o g a i n s t a b i l i t y a t o r s o h a s b e e n a d d e d a s a n u p p e r b o d y . T h e ! i c > u r c 5 . . + i n d i c a t e s t h e b i p e d a l r o b o t w i t h t o r s o . J ..l.,~· :):~4: . 1 ; 4 - V : : . x : : : . , , . . l ' I : : : · x . . , . \ • . . > ; ; • ~ _ : , _ : , / . \ ) : . , . . . -~~ \ 1-~ ~. - - - - - . -~· \ - - - - - - - : r : -f·-~-- t - - - - - ~--~' ~---_..,......,---- \ , X F i g u r e 5 . 4 - B i p e d a l r o b o t w i t h T o r s o = ' · 7 . 1 M a s s c e n t r e c o o r d i n a t e s o f T o r s o ' h e e q u a t i o n d e r i v e d c o n s i d e r i n g t h e g e o m e t r y o f t h e b i p e d a l r o b o t t o c a l c u l a t e t h e m a s s , c · n t r c c o o r d i n a t e s o f t h e t o r s o a n d a s f o l l o w s . \ . = Y 1 c o s a + A x + 0 . 5 L , c o s ( 8 1 ) ( 5 . 2 3 ) 4 5 . I I T . _ \ \ T O . ' : i r , s i n ( { I ; ) ( 5 . 2 4 ) 5 . 7 . 2 A n g u l a r a c c e l e r a t i o n o f T o r s o \ 1 1 cclll~lr a c c e l e r a t i o n o f t o r s o i s a s s u m e d t o h e v e r y s m a l l c o m p a r e d t o t h e a c c e l e r a t i o n s o f , q i i L T l i n k s h e n c e i t i s c o n s i d e r e d a s z e r o f o r t h e Z M P c o m p u t a t i o n s . 5 . 7 . 1 A c c e l e r a t i o n o f T o r s o ; I i L ' r e c u r s i v e N e w t o n - E u l e r f o r m u l a t i o n h a s h e e n c a r r i e d o u t t o f i n d t h e T o r s o a c c e l e r a t i o n J I L l m e n t i o n e d b e l o w . [ 2 2 ] I , c o : , ( } _ - 1 · 1 , s i n O , - O . ' : i L 1 ( 0 , + B r , + O ; ) ( 5 . 2 5 ) c I , , s i n O , + r·~, cosO~ + 0 . 5 L 1 ( B , + { ) 6 + 0 7 ) ( 5 . 2 6 ) 5 . S D e f i n i n g t o r s o a n g l e H . . , . X f i g u r e 5 . 5 - l l l u s t r a t i o n o f t o r s o a n g l e 4 6 \ : - , s h O \ \ n i n F i g u r e 5 . 5 t h e a n g l e 0 1 h a s b e e n i n t r o d u c e d a s T o r s o a n g l e . I u r - . , o a n g l e ( f J 1 ) - ' 0 , + 0 1 , - f ) _ ( 5 . 2 7 ) I t t o r d e r t o k e e p f J 1 a t l i x e d v a l u e w h i l e w a l k i n g i t i s n e c e s s a r y t o a d j u s t t h e a n g l e 1 1 ~1ccordingl_\ t o s u p p r e s s t h e v a r i a t i o n o f f J 1 • I 1 1 L T e l ( J r c t h e a n g l e f ) _ c a n b e c a l c u l a t e d u s i n g e q u a t i o n 5 . 2 7 a t e a c h d a t a p o i n t t o c a l c u l a t e t i l l ' r l ' S p e c t i v e / M P v a l u e a t t h a t t o r s o a n g l e . ( } _ = 0 , + 0 ( , - f ) ! ; 5 . 9 Z M P o f t h e b i p e d a l r o b o t w i t h t o r s o / \ l P v a l u e s f o r s i n g l e s t e p h a v e b e e n c a l c u l a t e d u s i n g t h e m a t l a b p r o g r a m A . l 0 1 1 1 \ p p e n d i x A u s i n g t r a j e c t o r y d a t a u s i n g d a t a i n T a b l e 5 . 2 I k r c t o r s o a n g l e w i t h r e s p e c t i v e t o t h e h o r i z o n t a l a x i s . f J 1 i s t a k e n a s I 0 ° . T a b l e 5 . 2 - P a r a m e t e r s f o r t h e c a l c u l a t i o n o f Z M P o f b i p e d a l r o b o t w i t h t o r s o -~-- - · - · - - - - - r , . r , ~60 1 1 1 1 1 1 x , 1 0 0 m m / . ( I ~RO m m ' I 4 5 0 m m x , - - - - - - - - _ _ _ . _ _ 1 , . 1 , 1 8 0 m m ' · 8 0 0 m m - - ( j _ ! 5 1 1 ! H 7 0 0 m m _ _ _ _ _ _ _ , _ g ' 9.8lm/s~ I m l , m 4 5 . 0 K g . \ " 3 5 0 m l 1 1 i m 2 , m , 5 . 2 K g T 5 s e c / o 0 s e c / t 1 . 2 5 s e c 1 - - , 3 . 7 5 s e c / , - - + 5 . 0 0 s e c f ) r 1 0 ° _ _ _ _ _ _ _ _ ' _ _ SI~ i 7 0 0 m m L J 8 0 0 m m I l l ! 3 5 K g ! - - - - - - - - - - - - - + · - - - - - ~- - - 5 . 1 0 Z M P v a r i a t i o n o f t h e b i p e d a l r o b o t w i t h t o r s o I i l l · / M P v a l u e s o b t a i n e d h a v e b e e n p l o t t e d a n d s h o w n b e l o w . T h e d y n a m i c s t a b i l i t y % t r g i n h a s b e e n c a l c u l a t e d a c c o r d i n g t o t h e e q u a t i o n 5 . 2 . I ) S i \ 1 ~ 3 5 9 m m t o 5 3 8 m m I r o m t h e F i g u r e 5 . 6 i t i s e v i d e n t t h a t b y a d d i n g a t o r s o t h e b i p e d a l r o b o t h a s g a i n e d ~t~tbilit) t ( J r a l m o s t 7 0 % o f t h e s t e p t i m e . 4 7 ' I Z M P w i t h T o r s o 5 5 0 1 . ----,-----,---~----~----.-----,---~~---,----,---~ I 500~ E ' i E I ~450 c . . : 2 : N 4 0 0 l D y n a m i c s t a b i l i t y M a r g i n ; - + - - 1 0 d e g 350~--~~----L---~L-----L---~L_--~L_--~~--~L_--~L_ _ _ _ _ 0 Q 5 1 1 . 5 2 ~5 3 1 5 4 ~5 5 T i m e ( s e c ) F i g u r e 5 . 6 - Z M P o f b i p e d a l r o b o t w i t h T o r s o 5 . 1 1 Z M P o f t h e b i p e d a l r o b o t w i t h d i f f e r e n t t o r s o a n g l e s Z M P v a l u e s f o r s i n g l e s t e p h a v e b e e n c a l c u l a t e d u s i n g t h e M A ' r L I \ 8 p r o g r a m ! \ . I 0 u s i n g t r a j e c t o r y d a t a a n d u s i n g d a t a i n T a b l e 5 . 3 . I ! e r e s i x d i ! T e r e n t t o r s o a n g l e s ( 0 1 ) a r e c o n s i d e r e d . T a b l e 5 . 3 - P a r a m e t e r s f o r t h e c a l c u l a t i o n o f Z M P o f b i p e d a l r o b o t w i t h d i f f e r e n t t o r s o a n g l e s ! . ! . ! . I t 4 6 0 m m . . _ _ , -~---+--- . . ! . , I 4 8 0 m m - · - - · · - - - - " - - - - . ' · i 1 8 0 1 1 1 1 1 1 - - - - · - - - ; - - - - - ( J . , s ' ) ( ) 9.81m/s~ t o X 3 5 0 m m x , 1 0 0 m m X , 4 5 0 m m I x , 8 0 0 m m H 7 0 0 m m m 1 , m _ 1 5 . 0 K g 1 1 1 7 . , 1 1 1 3 5 . 2 K g 1 - - - - · T 5 s e c t ( ) 0 s e c - - · - - - · - - / I 1 . 2 5 s e c + I ' 3 . 7 5 s e c - - - - - - - - + - - - - · · - - - - - - - · - - - - · - - - - I 0 11 .15 11 .20(1,25 11 ,30 11 ,3S~ I I 5 . 0 0 s e c 0 1 I I I I ) L - - - - - + - - - - - - 7 0 0 m m + L ; 8 0 0 m m - - - - · - - - - - - · - - - - - - + - 1 _ 1 1 _ _ _ i 3 5 K g 4 8 5 . 1 2 Z M P v a r i a t i o n o f t h e b i p e d a l r o b o t w i t h d i f f e r e n t t o r s o a n g l e s l h c / I V I P v a l u e s o b t a i n e d h a v e p l o t t e d a s s h o w n i n F i g u r e 5 . 7 . T h e d y n a m i c s t a b i l i t y m a r g i n h a s b e e n c a l c u l a t e d a c c o r d i n g t o t h e e q u a t i o n 5 . 2 . I ) S f v l = J 5 9 m m t o 5 J 8 m m Z M P v a r i a t i o n w i t h T o r s o an~e 5 5 0 . 1~-------, I - 1 0 d e g 500 ~ - 1 5 d e g I . - 2 0 d e g E r~-25 d e g I ~450 j - 3 0 d e g : a : 1 - 3 5 d e g N 1 D y n a m i c s t a b i l i t y m a r g i n 2 3 T i m e ( s e c ) 4 h g u r e 5 . 7 : Z M P v a r i a t i o n o f b i p e d a l r o b o t w i t h d i f f e r e n t t o r s o a n g l e s 5 l h e f i g u r e 5 . 7 s h o w s s i x l i n e s r e p r e s e n t i n g Z r v ! P f o r d i f f e r e n t t o r s o a n g l e s . I t i s c l e a r , d ! s e r \ i n g g r a p h t h a t t h e b i p e d a l r o b o t c a n k e e p d y n a m i c s t a b i l i t y b y v a r y i n g t o r s o a n g l e l n l l l l 1 0 ' 1 - . 2 0 1 1 • 4 9 C h a p t e r 6 B e h a v i o r a l a n a l y s i s o f b i p e d a l r o b o t I l l p r e v i o u s c h a p t e r s h o w s t h e c a p a b i l i t y o f g a i n i n g d y n a m i c s t a b i l i t y o f b i p e d a l r o b o t b y ,~clcling a t o r s o a s u p p e r b o d y a n d v a r y i n g t o r s o a n g l e . i r 1 thi~ c h a p t e r t h e e f f e c t i v e n e s s o f g a i n i n g d y n a m i c s t a b i l i t y b y < ' a r y i n g t o r s o i s f u r t h e r , t r l , d ; . / e d b y s t u d y i n g d i f f e r e n t c a s e s v a r y i n g s i n g l e p a r a m e t e r i n e a c h c a s e s u c h a s s l o p e ~trlc_~k. s t e p l e n g t h . s t e p t i m e , e t c . \ " ' t h e j o i n t a n g l e v a r i a t i o n i s a n a l y z e d a n d p r e s e n t e d b e c a u s e s m o o t h j o i n t a n g l e \ , , , r , t l t \ l l l i : - , i m p o r t a n t t o m i n i m i z e j e r k s w h i l e w a l k i n g . t h e c~liculations i n t h i s c h a p t e r h a v e b e e n d o n e a c c o r d i n g t o t h e m e t h o d i n d i c a t e d i n < I i 1 l l l . : ; _ - f . I i c l u l l s t e p l e n g t h i s c o n s i d e r e d a s 7 0 0 1 1 1 1 1 1 w h i c h i s a p p r o x i m a t e l y e q u a l t o t h e h u m a n ' i l l ' l e n g t h w h e n \ V a l k i n g . 6 . 1 E f f e c t o f T o r s o a n g l e o n d y n a m i c s t a b i l i t y o n v a r i o u s s l o p e s I , , t l ' b i p e d a l r o b o t w a l k i n g o n t h r e e s l o p e s h a v i n g s l o p e a n g l e s 5 ° , I O u a n d 1 5 ° a r e , r ' · e c . t i g a t e d . T h e d a t a r e l e v a n t t o t h e Z M P c a l c u l a t i o n o f a l l s l o p e a n g l e s a r e i n d i c a t e d i n i , r i l l e 6 . 1 . 1 , r t 1 l e 6 . 1 - P a r a m e t e r s f o r t h e c a l c u l a t i o n o f Z M P o f b i p e d a l r o b o t a t d i f f e r e n t s l o p e s 1 . , . I ' I . ! , I , . I S L ( ) t : ' X / r m , = F 4 6 0 m m 4 8 0 - - - - - - - 1 8 0 1 1 1 1 1 1 - - - - . . . I 7 0 0 m 1 1 1 - - + - 9 . 8 1 m / s - 3 5 0 1 1 1 1 1 1 - - - - - f - - - - - - - - - - 5 s e c - - - - - - - - - - - - - - I . 2 5 s e c 5 . 0 0 s e c - - - - - - - - I 3 5 K g x , 1 0 0 m m X , 4 5 0 1 1 1 m X 8 0 0 1 1 1 1 1 1 H 7 0 0 m l 1 1 m 1 , m 4 5 . 0 K g m 2 , m , 5 . 2 K g t o 0 s e c - · - - r - - - - - - - - - - - - - - - - - - - - - - / 2 3 . 7 5 s e c L / 8 0 0 1 1 1 1 1 1 \ - t - - - - - - - - - - - - - - - 5 0 6 . 1 . 1 B i p e d a l r o b o t w a l k i n g o n 5 ° s l o p e I I , ' 1 1 p c d a l m b o t \ \ a l k i n g o n 5 ' ' s l o p e i s c o n s i d e r e d . T h e Z M P c a l c u l a t i o n h a s b e e n c a r r i e d 1 n , ' I l l d i l f e r e n t t o r s o a n g l e s u s i n g t h e M A T L A B p r o g r a m A . l 0 i n a p p e n d i x A a n d t h e / \ 1 1 ' \~!lues a r e p l o t t e d a n d s h o v v n i n F i g u r e 6 . 1 . \ ) " - - ' 1 c , . : ; , c h Y \ \ \ 1 \ . o S 3 ' 6 Y \ I \ \ \ Z M P v a r i a t i o n w i t h T o r s o a n g l e 550F=======================r===~~==~======~ 1~-------, 1 1 - 1 0 d e g soo~J-15 d e g , 1 - 2 0 d e g ~ \ - 2 5 d e g ~ 450~-30 d e g 2 - 3 5 d e g N 400~ i ~/ 3 5 0 0 D y n a m i c s t a b i l i t y m a r g i n 1 2 3 T i m e ( s e c ) 4 I i g u r c 6 . 1 - Z M P v a r i a t i o n w i t h t o r s o a n g l e a t 5 d e g . s l o p e w i t h f u l l s t e p l e n g t h 5 \ , l \ l r d i n g t o t h e P i g . 6 . 1 t o r s o a n g l e o n l y n e e d t o b e v a r y f r o m I 0 - 3 5 d e g r e e s i n o r d e r t o ' ' l t l n t a i n t h e d y n a m i c s t a b i l i t y d u r i n g i t s s w i n g i n g t i m e . T h e r e i s a g r e a t e r c h a n g e i n Z M P , . t : l l l ' ! ' o r t o r s o a n g l e . I t i s a l s o o b s e r v e d w h e n i n c r e a s i n g T o r s o b y 5 ° s t e p Z M P i m p r o v e d · c l l i l i c a n t l y . T h e r e f o r e i t i s e v i d e n t t h a t b y a d d i n g a t o r s o i t i s p o s s i b l e t o m a i n t a i n t h e , l \ t E l m i c s t a b i l i t y b y a d j u s t i n g t o r s o a n g l e a c c o r d i n g l y . I t i s o b s e r v e d f o r a c e r t a i n a n g l e t h e / \ I P s t a y s i n d y n a m i c s t a b i l i t y r e g i o n m o r e t h a n h a l f o f t h e s t e p t i m e t h e r e f o r e t o m a i n t a i n , I , r J a m i c s t a b i l i t y t o r s o d o e s n o t n e e d t o v a r y q u i c k l y . t i l t h r e e c a s e s o f s l o p e a n g l e 5 ° . 1 0 1 1 a n d 1 5 ° t h e r o b o t c a n m a i n t a i n d y n a m i c s t a b i l i t y b y \ 1 1 1 g t h e t o r s o a n g l e t h e r e f o r e t h i s r o b o t c a n n e g o t i a t e s t e e p s l o p e s w i t h o u t f a l l i n g . 5 1 o . l . 2 B i p e d a l r o b o t w a l k i n g o n 1 0 ° s l o p e I l l b i p e d a l r o b o t w a l k i n g o n I 0 ° s l o p e i s c o n s i d e r e d . T h e Z M P c a l c u l a t i o n h a s b e e n l J t t i c d o u t f o r d i f f e r e n t t o r s o a n g l e s u s i n g t h e M A T L A B p r o g r a m A . I I i n a p p e n d i x A a n d t h l · I M P v a l u e s a r e p l o t t e d a n d s h o w n i n F i g u r e 6 . 2 . A c c o r d i n g t o t h e F i g u r e 6 . 2 t h e t o r s o J t l : > k c~m h e v a r i e d t l · o m 7 - 3 5 d e g r e e s i n 1 0 d e g r e e s a s c e n d i n g s l o p e . Z M P V a r i a t i o n w i t h T o r s o a n g l e G o o 1 I ' ~~ , 1 - 7 d e g . I t 5 5 0 1 1 - 1 0 d e g - 1 5 d e g l I - 500~-20 d e g l ~ 1 1 - 2 5 d e g ' - 4 5 0 - 3 0 d e g 0 . . 2 : : - 3 5 d e g N 4 0 0 . y - 1 3 0 0 0 1 m a r g i n 2 3 4 T i m e ( s e c ) 5 I i g u r c 6 . 2 - Z M P v a r i a t i o n ' v v i t h t o r s o a n g l e a t I 0 d e g . s l o p e w i t h f u l l s t e p l e n g t h 6 . 1 . 3 B i p e d a l r o b o t w a l k i n g o n 1 5 ° s l o p e I h e / \ 1 P c a l c u l a t i o n h a s b e e n c a r r i e d o u t f o r d i f f e r e n t t o r s o a n g l e s u s i n g t h e M A T L A B p r u g t · c l t l l . \ . 1 2 i n a p p e n d i x ! \ a n d t h e Z M P v a l u e s a r e p l o t t e d a n d s h o w n i n F i g u r e 6 . 3 . \ L c o r d i n g t o t h e F i g u r e 6 . 3 t h e t o r s o a n g l e c a n b e v a r i e d t l · o m I 0 - 3 5 d e g r e e s i n 1 5 d e g r e e s a - , c c n d i n g s l o p e . Z M P v a r i a t i o n w i t h T o r s o a n g l e 5 3 0 - 1 0 d e g l D y n a m i c 5 1 0 4 9 0 - 1 5 d e g s t a b i l i t y m a r g i n - 4 7 0 - 2 0 d e g ~ 4 5 0 - 2 5 d e g - 3 0 d e g ; ; : : 4 3 0 - 3 5 d e g 2 4 1 0 N 3 9 0 3 7 0 . ! 3 5 0 3 3 0 0 1 2 3 4 5 T i m e ( s e c ) F i g u r e 6 . 3 - Z M P v a r i a t i o n w i t h t o r s o a n g l e a t 1 5 d e g . s l o p e w i t h f u l l s t e p l e n g t h 5 2 6 . 2 E f f e c t o f T o r s o a n g l e o n d y n a m i c s t a b i l i t y a t d i f f e r e n t s t e p s i t 1 c · b i p e d a l r o b o t w a l k i n g o n 5 1 1 a s c e n d i n g s l o p e w h i l e v a r y i n g s t e p l e n g t h i s c o n s i d e r e d ! l l t h e f o l l o w i n g c a s e s . T h e d a t a r e l e v a n t t o t h e Z M P c a l c u l a t i o n o f e l i f T e r e n t s t e p s a r e , , c t t c : a t c d i n T a b l e 6 . 2 . t t l k 6 . 2 - P a r a m e t e r s f o r t h e c a l c u l a t i o n o f Z M P o f b i p e d a l r o b o t a t d i f f e r e n t s t e p s , . ! . " I 4 6 0 m m . x 1 1 0 0 m m . ! 1 8 0 m m L 1 8 0 0 m m , . I . = = i 4 8 0 m m m , ' 3 5 K g S L 1 7 0 0 m m . 3 5 0 1 1 1 1 1 1 , 1 7 5 1 1 1 1 1 1 H 7 0 0 1 1 1 1 1 1 I 9 . 8 1 m / s - m 1 • m . , 5 . 0 K g - - - - · - - - - - . \ _ 1 3 5 0 1 1 1 1 1 1 . 1 7 5 m m , 8 7 . 5 1 1 1 m [ m 2 • m 1 I 5 . 2 K g g j T - - 0 s e c 1 5 s e c I a 1 1 1 . 2 5 s e c f o 3 . 7 5 s e c I , j 5 . 0 0 s e c h . 2 . 1 W a l k i n g o n f u l l s t e p 1 , · I M P c a l c u l a t i o n h a s b e e n c a r r i e d o u t f o r d i f f e r e n t t o r s o a n g l e s u s i n g t h e M A T L A B ' I l c : ' T a l 1 1 i \ . 1 0 i n a p p e n d i x A a n d t h e Z M P v a l u e s a r e p l o t t e d a n d i n d i c a t e d i n F i g u r e 6 . 4 . Z M P v a r i a t i o n w i t h T o r s o a n g l e 5 5 0 . E E 5 0 0 ~ 4 5 0 a . . 2 N ~10 d e g - 1 5 d e g - 2 0 d e g - 2 5 d e g - 3 0 d e g - 3 5 d e g 1 2 3 T i m e ( s e c ) 4 5 F i g u r e 6 . 4 - Z M P v a r i a t i o n w i t h t o r s o a n g l e a t 5 d e g . s l o p e w i t h f u l l s t e p l e n g t h 5 3 h . 2 . 2 W a l k i n g o n h a l f s t e p . · / \ 1 ! > c a l c u l a t i o n h a s b e e n c a r r i e d o u t f o r d i f f e r e n t t o r s o a n g l e s u s i n g t h e M A T L A B , c > t : t t l i \ . 1 1 i n a p p e n d i ' . A a n d t h e Z M P v a l u e s a r e p l o t t e d a n d i n d i c a t e d i n F i g u r e 6 . 5 . . . . . . l 3 5 0 I 3 0 d e g 1 - 3 3 d e g - 3 5 d e g 3 o o r l - 4 o d e s I = 2 5 0 1 " ' · ~ 2 0 0 0 0 . 5 Z M I ' v a r i a t i o n w i t h T o r s o a n g l e D y n a m i c S t a b i l i t y M a r g i n T i m e ( s e c ) 1 . 5 . r 2 2 . 5 I i g u r e 6 . 5 - Z M P v a r i a t i o n w i t h t o r s o a n g l e a t 5 d e g . s l o p e w i t h h a l f s t e p l e n g t h 6 . 2 . 3 W a l k i n g o n q u a r t e r s t e p I h e I M P c a l c u l a t i o n h a s b e e n c a r r i e d o u t f o r d i f f e r e n t t o r s o a n g l e s u s i n g t h e M A T L A B p r o g r a m i \ . 1 4 i n a p p e n d i x A a n d t h e c a l c u l a t e d Z M P v a l u e s a r e p l o t t e d a n d i n d i c a t e d i n I i g u r e C 1 . 6 . 0 . 2 Z M P v a r i a t i o n w i t h t o r s o a n g l e 0 . 4 0 . 6 T i m e ( s e c ) 0 . 8 - + - - - 3 3 d e g - - e - - - 3 5 d e g - - - + - - - 4 0 d e g - 4 5 d e g - - ! 3 - 5 0 d e g - - + - 5 5 d e g - - ' < T - 6 0 d e g 1 . 2 f i g u r e 6 . 6 - Z M P v a r i a t i o n w i t h t o r s o a n g l e a t 5 d e g . s l o p e w i t h q u a r t e r s t e p l e n g t h 5 4 1 J h o v e t h r e e c a s e s t h e s t e p l e n g t h h a s b e e n v a r i e d t o f i n d o u t t h e b e h a v i o r o f d y n a m i c I ti~Jiit) o r t h e b i p e d a l r o b o t . A t l e r o b s e r v i n g a l l t h r e e g r a p h s i t i s o b s e r v e d t h a t t h e t o r s o - ' k '-~ttl h e i n c r e a s e d i n s t e e p s l o p e s w h i l e k e e p i n g d y n a m i c s t a b i l i t y . I n F i g u r e 6 . 6 i t c a n ' ' - ' ' - ' t l t h d t t h e t o r s o a n g l e c a n b e i n c r e a s e d u p t o 6 0 d e g r e e s w h i l e m a i n t a i n i n g t h e J . t m i c c ; t , t b i l i t ) . T h e r e a s o n f o r t h i s i s v , h e n t h e b i p e d a l r o b o t i s t a k i n g s m a l l e r s t e p s i t i s , . _ , , , t o m a i n t a i n t h e d y n a m i c s t a b i l i t y b e c a u s e Z M P i s c l o s e r t o t h e d y n a m i c s t a b i l i t y t r : 2 i 1 1 r a t h c r t h a n t a k i n g l a r g e s t e p s . 6 . 3 E f f e c t o n d y n a m i c s t a b i l i t y a t d i f f e r e n t s t e p d u r a t i o n s . . I . : I \ 1 P c a l c u l a t i o n h a s b e e n c a r r i e d o u t k e e p i n g t o r s o a n g l e a t I 0 d e g r e e s v a r y i n g s t e p l t t l l L ' . T h e M A T L A B p r o g r a m A . l 0 i n a p p e n d i x A h a s b e e n u s e d w i t h d a t a i n T a b l e 6 . 3 a n d t l 1 ' - · / f ' v 1 P v a l u e s a r e p l o t t e d a n d s h o w n i n F i g u r e 6 . 7 . ! . t h k 6 . . 1 - P a r a m e t e r s f o r t h e c a l c u l a t i o n o f Z M P o f b i p e d a l r o b o t a t d i f f e r e n t s t e p t i m e s 4 6 0 m m i X I 1 0 0 m m 4 8 0 m l 1 1 X , 4 5 0 m m - - - - - - - - - - · - -~- - - - - - - - - - - - - - - - - - 1 8 0 m m x , 8 0 0 m m 7 0 0 m m I H 7 0 0 m m I - - I . f - - r - 1 . ! t - - - • I I . . I - - + - - -~~- - - + - - ~ g _ - 1 - x - + - - 1 _ l _ _ _ I . 9 . 8 1 1 1 1 / s - m 1 .m~ 5 . 0 K g 3 5 0 1 1 1 m 1 7 1 2 . 1 1 1 3 5 . 2 K g - - - · 8 0 0 m m i m l 3 5 K g - - - - - - - · . . . l t i I i g u r e 6 . 7 i ' : M P v a r i a t i o n o f t h i s b i p e d a l r o b o t h a s b e e n t e s t e d b y v a r y i n g t h e t i m e t a k e n t t l ~'' i n g t h e l e g . I t c a n b e s e e n t h e r o b o t i s v e r y u n s t a b l e i f i t w a l k s v e r y s l o w l y . B u t a s l o n g a~ i t w a l k s f a s t t h e Z M P i m p r o v e s . T h e r e f o r e w i t h t h i s r e s u l t i t i s p o s s i b l e t o s t a t e t h a t t h e b i p e d a l r o b o t i s c a p a b l e o f f a s t w a l k i n g k e e p i n g i t s d y n a m i c b a l a n c e Z M P V a r i a t i o n V s S t e p T i m e ' " " ' .~' - 1 0 s e c : : : t l = . = : : - : d ~ c 5 5 0 · . . . . ~---- 1 5 0 0 4 5 0 m a r g i n 1 5 0 1 D O 5 0 0 o~~~~~~~~0~0~~~~~~20r0~~~~~~3~0~0~~~~~~40r0~~~~~~5~0~0~~~===--6L00~------~7JOO S t e p L e n g t h ( m m ) F i g u r e 6 . 7 - Z M P v a r i a t i o n w i t h t o r s o a n g l e a t 5 d e g . s l o p e w i t h f u l l s t e p l e n g t h v a r y i n g s t e p t i m e 5 5 6 . - 1 . E f f e c t o f T o r s o m a s s o n d y n a m i c s t a b i l i t y o f r o b o t j , s e c t i o n t h e e f f e c t o f t h e d y n a m i c s t a b i l i t y o f t h e b i p e d a l r o b o t o n v a r y i n g t o r s o m a s s r H l i y / e c l . T w o d i f f e r e n t c a s e s a t t w o d i f f e r e n t t o r s o a n g l e s h a v e b e e n a n a l y z e d . r ' 1 c 6 . 4 - P a r a m e t e r s f o r t h e c a l c u l a t i o n o f Z M P o f b i p e d a l r o b o t w i t h d i I T e r e n t m a s s e s o f I o r s o / . I I 4 6 0 - I 1 1 1 1 1 1 I x , 1 0 0 m m - - - - 1 1 1 1 1 1 ' 4 5 0 m m x 2 , ; 4 8 0 I . / , 1 1 1 1 1 1 i X , 8 0 0 m m 1 1 1 1 1 1 I H i 7 0 0 m m _ _ _ _ _ _ _ _ _ , . , - - - - - - i . . . 1 8 0 - - + - - - 7 0 0 ! . I ' S l l ! 9 . 8 1 1 1 ; ' , " I b : 3 5 ( ) 1 1 1 1 1 1 ! ! I -~- - - - + - I m " . m . - + - - - - - 5 s e c I r - - - - - - ' 5 . 2 K o ~ - 1 - - - b I 1 . 2 5 s e c / ( ) 0 s e c -~- - - t . , i 5 . 0 0 s e c - · - - - 3 . 7 5 s e c - L j - - j 8 0 0 m r n - - - y t , I ' 6 . - l . l D y n a m i c s t a b i l i t y w i t h d i f f e r e n t m a s s e s o n t o r s o a t t o r s o a n g l e o f I 0 ° l l : l l l t p c d a l r o b o t \ \ a l k i n g o n 5 1 1 a s c e n d i n g s l o p e w i t h f u l l s t e p h a s b e e n c o n s i d e r e d f o r t h i s C N I i l L ' f \ 1 A T I . ! \ l 3 p r o g r a m / \ . 1 0 i n a p p e n d i x A h a s b e e n u s e d w i t h d a t a i n T a b l e 6 . 4 a n d t h L · / \ I P v a l u e s a r e p l o t t e d a n d s h m v n i n F i g u r e 6 . 8 . 5 5 0 r t - 1 0 K - 1 5 K g - 2 0 K g - 2 5 K g 5 o o H _ 3 0 K g - 3 5 K g E f - 4 0 K g . . § _ 4 5 0 - 4 5 K g a . - s o K g 2 N - 5 5 K g . 6 0 K g 0 0 . 5 Z M P V a r i a t i o n w i t h M a s s o f T o r s o 1 . 5 2 2 . 5 3 T i m e ( s e c ) D y n a m i c s t a b i l i t y m a r g i n 3 . 5 4 4 . 5 I i g u r e 6 . 8 - Z M P v a r i a t i o n k e e r i n g t o r s o a n g l e a t I 0 d e g . a n d v a r y i n g t o r s o m a s s 5 5 6 h . - l . 2 D y n a m i c s t a b i l i t y w i t h d i f f e r e n t m a s s e s o n t o r s o a t t o r s o a n g l e o f 1 5 ° I l i e b i p e d a l r o b o t w a l k i n g o n 5 1 1 a s c e n d i n g s l o p e w i t h f u l l s t e p h a s b e e n c o n s i d e r e d f o r t h i s - : a s e . T h e M A T L A B p r o g r a m A . l 0 i n a p p e n d i x A h a s b e e n u s e d w i t h d a t a i n T a b l e 6 . 4 a n d r i l e / M P v a l u e s a r e p l o t t e d a n d s h o w n i n F i g u r e 6 . 9 5 5 0 I - 1 0 K g - 1 5 K g 5 o o H - 2 o K g - 2 5 K g E 11=~~~~ E c . . 4 5 0 - 4 0 K g 2 - 4 5 K g N - 5 0 K g 4 0 0 H - 5 5 K g 6 0 K g 0 0 . 5 Z M P V a r i a t i o n w i t h M a s s o f T o r s o D y n a m i c S t a b i l i t y m a r g i n 1 . 5 2 2 . 5 3 3 . 5 4 4 . 5 T i m e ( s e c ) I · i g u r e 6.9~ZMP v a r i a t i o n k e e p i n g t o r s o a n g l e a t 1 5 d e g . a n d v a r y i n g t o r s o m a s s 5 I t o m F i g u r e 6 . 8 i t c a n b e s e e n t h a t b y a d d i n g t o r s o a w e i g h t d y n a m i c s t a b i l i t y c a n b e m p r m c d . T h e F i g u r e 6 . 9 s h o v v s t h e t o r s o a n g l e a t 1 5 d e g r e e s a n d s t i l l i t i s p o s s i b l e t o t 1 c r c a s c t h e s t a b i l i t y b y a d d i n g m o r e w e i g h t s t o t h e t o r s o . A l s o s e e n t h a t b y a d d i n g i n i t i a l ' ' e i g h t s I 0 - 3 5 K g t h e d y n a m i c s t a b i I i t y h a s i n c r e a s e d b y m u c h w i d e r g a p r a t h e r t h a n a t l a t t e r " l L ' ) l S o f \ \ e i g h t s 3 5 - 6 0 K g . B y o b s e r v i n g t h i s b e h a v i o r i t c a n b e n o t e d m u c h i m p r o v e m e n t c~ttmot b e e x p e c t e d a f t e r s o m e m a r g i n a l w e i g h t . 6 . 5 E f f e c t o n l e n g t h o f t o r s o o n d y n a m i c s t a b i l i t y o f r o b o t I h e c f l c c t o f l e n g t h o f t o r s o h a s o n t h e d y n a m i c s t a b i l i t y o f t h e b i p e d a l r o b o t i s a n a l y z e d i n t h i s s e c t i o n . I h e M A T L A B p r o g r a m A . l 0 i n a p p e n d i x A h a s b e e n u s e d w i t h d a t a i n T a b l e 6 . 5 a n d t h e / V I P v a l u e s a r e p l o t t e d a n d s h o w n i n F i g u r e 6 . 1 0 . 5 7 . . . L _ · 1 I L ' ( J . ) - l ) a r a m e t e r s f o r t h e c a l c u l a t i o n o f Z M P o f b i p e d a l r o b o t w i t h d i f f e r e n t l e n g t h o f l o r s o ( l " " X T I ' 5 5 0 - 3 0 0 m m - 4 0 0 m m 5 0 0 1 [ - 5 0 0 m m - 6 0 0 m m E ~~-700 m m _ § _ _ 4 5 0 - a o o m m c . . [ - g o o m m : 2 N 4 0 0 0 0 . 5 4 b u - - 4 8 0 1 1 1 1 1 1 x , I 1 0 0 1 1 1 1 1 1 x , I 4 5 0 1 1 1 1 1 1 1 1 1 1 1 1 _ . . _ . 1 1 1 1 1 1 9 . 8 1 m / s - X I 8 0 0 1 1 1 1 1 1 H I 7 0 0 1 1 1 1 1 1 m l , m 4 5 . 0 K g 3 5 0 m m m 2 , m , 5 . 2 K g 5 s e c t o 0 s e c ' ~- s e c t 2 3 . 7 5 s e c 5 . 0 0 s e c a 5 0 Z M P v a r i a t i o n w i t h L e n g t h o f T o r s o 1 . 5 2 2 . 5 3 3 . 5 4 4 . 5 T i m e ( s e c ) I i g u r e 6 . 1 0 - Z M P v a r i a t i o n k e e p i n g t o r s o a n g l e a t I 0 d e g . a n d v a r y i n g t o r s o l e n g t h 5 l r 1 I i g u t ' l : 6 . 1 0 t h e v a r i a t i o n o f Z M P c a n b e s e e n b y i n c r e a s i n g t h e l e n g t h o f t o r s o . T h e r e i s c t g r e a t e r i n c r e a s e i n d y n a m i c s t a b i l i t y m a r g i n c l u e t o t h e i n c r e a s e i n t o r s o l e n g t h . B u t t h e r e j , ~~practical l i m i t a t i o n . 6 . 6 E f f e c t o n w e i g h t s o f l o w e r b o d y o n d y n a m i c s t a b i l i t y o f r o b o t I h e e l f e c t o f l e n g t h o f t o r s o o n t h e d y n a m i c s t a b i l i t y o f t h e b i p e d a l r o b o t i s a n a l y z e d i n t h i s tc~t c a s e . T h e M A ' I L A B p r o g r a m A . l 0 i n a p p e n d i x A h a s b e e n u s e d w i t h d a t a i n T a b l e 6 . 6 a n d t h e Z M P v a l u e s a r e p l o t t e d a n d s h o w n i n F i g u r e 6 . 1 1 . 5 8 l 1 I ' l L ' ( J . 6 - P a r a m e t e r s f o r t h e c a l c u l a t i o n o f Z M P o f b i p e d a l r o b o t w i t h d i f f e r e n t w e i g h t s o r ! o w e r b o d y I . ! . 4 6 0 1 1 1 1 1 1 ! I X I 1 0 0 1 1 1 1 1 1 I I 4 8 0 1 1 1 1 1 1 x , 4 5 0 m m ' I 8 0 m m i x , i 8 0 0 m m - - - 1 - - - - - - - - \ l 7 0 0 1 1 1 1 1 1 I - - - - I 1 - 1 I 7 0 0 m m g 9 . 8 l m / s - X I 3 5 0 m m l I 5 s e c / ( I 0 s e c I I - - - - - - - [ - - 1 . 2 5 s e c / 2 3 . 7 5 s e c I I I i 5 . 0 0 s e c 1 0 ° I I I 0 1 - I _ j Z M P v a r i a t i o n w i t h M a s s e s o f L 1 & L 2 700ir===~==~.---,----,---,----,---,---,---,--~ - 2 . 5 / 2 . 6 K g ~5/5.2 K g 6 o o r 1 - 1 o 1 1 o . 4 K g - 1 5 / 1 5 . 6 K g E 5 0 0 E 2 0 0 0 0 . 5 1 . 5 2 2 . 5 3 3 . 5 4 4 . 5 5 T i m e ( s e c ) fi~urc 6 . 1 1 - Z M P v a r i a t i o n k e e p i n g t o r s o a n g l e a t I 0 d e g . a n d v a r y i n g t h e w e i g h t s o f l o w e r b o d y ! h e l i g u r e 6 . 1 I s h o w s t h e b e h a v i o r o f Z M P w h e n v a r y i n g t h e w e i g h t s o f t h e l e g s ! , & L o n s l o p e a n g l e o f 5 ° . T h e g r a p h c l e a r l y s h o w s t h e l o s s o f d y n a m i c s t a b i l i t y w i t h i n c r e a s e o f w e i g h t s o f L 1 & L 2 . 5 9 6 . ~ B e h a v i o r a l a n a l y s i s o f j o i n t a n g l e s '~lriation o f j o i n t a n g l e s a n g u l a r p o s i t i o n . a n g u l a r v e l o c i t y a n d a n g u l a r a c c e l e r a t i o n a r c c~llcd i n l i g u r e s 6 . 8 . 6 . 9 . 6 . 1 0 a n d 6 . 1 1 . - , m o u t h v a r i a t i o n e n s u r e s t h e s m o o t h m o t i o n o f t h e b i p e d a l r o b o t . 0 8 ( ) I , , 0~, ~; ( 1 " ! ( ) 1 ~' 1 h t " ' t c l 1 A r i ( J L M I ; p o o , r t r u r r I J ' o T r r n P r . r n e 0 G I 5 " " 0 2 - 0 0 4 { ) 0 5 0 O C J ~ 0 1 ? - ( ) 1 2 < D 1 4 - 0 1 6 l h P t a l - A n q u l a r A r : { e l e r C J I I O r l V s T r m e 2 5 T i m e ( s e c ) t h e t a t - A n g u l n r w l o u t v V s T i m e " I l i T H - ! ( ! - , t " C J 3 5 4 5 e i r e ( J . I 2 : A n g u l a r p o s i t i o n . A n g u l a r v e l o c i t y a n d A n g u l a r a c c e l e r a t i o n o f j o i n t a n g l e ( } 1 V : , T r r r w 1 1 : : , 1 · 1 1 3 T i m e t s e c ) 1 5 - 0 0 1 - - ; ; ; - 0 0 2 u c E 0 O J g g : i 0 0 4 - 0 O : : J t h o t a 2 - A r r g u l a r a c c l e m t r o n V s T i m e 2 5 T i m e ( s e c ) t h o t a 2 - A n g u l a r w l o u t y I f ' > - l r r n f - ! T m w ( s e c ) 3 5 4 5 _ l l l l ' 6 . 1 3 : A n g u l a r p o s i t i o n . A n g u l a r v e l o c i t y a n d A n g u l a r a c c e l e r a t i o n o f j o i n t a n g l e ( } , 6 0 A I I ' J l 1 d ' ~, c , 1 1 1 J I 1 ' r I I l l t T r m e r s e c 1 u u 0 O b : J w - , E~ : : _ _ : 0 4 g ~ 0 0 0 3 ~ 0 0 2 thcta~-Arrgular a c c l e r a t 1 o n V s T 1 m e : - ! 5 : 1 r n e tiluta~>-Arrq u l d l v c l v t . l l l ' l l r n f c ? . I T 1 m e ( S e c ) ' 3 ~ 4 5 · ~urc 6 . 1 - + : A n g u l a r p o s i t i o n , A n g u l a r v e l o c i t y a n d A n g u l a r a c c e l e r a t i o n o f j o i n t a n g l e 0 5 t t H - : t a G - A n g u l a r p o s 1 ! 1 0 n V s l 1 r n e ~ I ! 0 5 I 5 0 . 0 4 - : ; ; 0 0 6 ~ 0 . 0 8 ~ u 0 " · 0 1 > m ~ 0 . 1 2 < · 0 1 4 t r 1 e l a ( , i \ n q u l d · a c c l s r o ; . u r r V s T 1 r n c 2 . 5 T r m e ( s e c ) H l e t a 6 - A n g u l a r v c l o c r t y V s T r m e r r r n e ( s e c ) 3 . 5 4 5 1 g u r e 6 . 1 5 : A n g u l a r p o s i t i o n , A n g u l a r v e l o c i t y a n d A n g u l a r a c c e l e r a t i o n o f j o i n t a n g l e 0 6 6 1 R e f e r e n c e s : \ 1 . I I . R a i b e r t l o e g g c d R o h o t s T h o r B a l a n c e . C a m b r i d g e . M A : M I T P r e s s . 1 9 8 6 . I~ \ l i o m i r V u k o h r a t o v i c a n d B r a n i s i a v . " Z e r o - M o m e n t P o i n t - T h i r t y f i v e y e a r s o r i t s l i f e : · l n t c n w l i o n a i . J o u r n a l o f H u m a n o i d R o h o t i c s . v o l . l , N o . ! , p p . l 5 7 - 1 7 3 . 2 0 0 4 . I~ I . 1 . Y a m a g u c h i . Y . T a k a n i s h i a n d I . K a t o , ' ' D e v e l o p m e n t o f a b i p e d w a l k i n g r o b o t c o m p e n s a t i n g f o r t h r e e a x i s m o v e m e n t b y t r u n k motion,"/EE/RS~/ I n t e r n a t i o n a l t t n r k s l w p o n i n t e l l i g e n r r o h o t i c s a n d s y s t e m s . v o l . 2 , p p . 5 6 1 - ) 6 6 . 1 9 9 9 . ! ~ i \ . A g r a ' h a l a n d S . K . A g r a w a L ' ' A n a p p r o a c h t o i d e n t i f y j o i n t m o t i o n s f o r d~ n a m i c a l l y s t a b l e ' h a l k i n g . " A S / v i E T r a n s . M e c h a n i c a l D e s i g n . v o l . l 2 8 . p p . 6 4 9 - 6 5 3 . P . 2 0 0 6 . I~ I C l o s w a m i . A . . " F o o t r o t a t i o n i n d i c a t o r ( F R I ) p o i n t : A n e w g a i t p l a n n i n g t o o l t o e v a ! u a t e p o s t u r a l s t a b i I i t y o f b i p e d r o b o t s ' ' . R o h o t i c s o n d A u t o m a t i o n . / I J I J r J J > m c c c d i n g s . } ( ) ( ) ! ) l i : L E l n t . C o n f e r e n c e o n . V o l u m e : I . 1 9 9 9 . p p . 4 7 - 5 2 v o l . l . ! 1 1 I l l o n d a C o r p o r a t i o n . S a y h e l l o t o a s i m o . h t t p : / / a s i m o . h o n d a . c o m . A c c e s s e d D e c e m b e r 2 7 . 2 0 0 9 . i ' I L . G e p p e r t . Q r i o . t h e r o b o t t h a t c o u l d . I E E E S p e c t r u m , 4 1 ( 5 ) : 3 4 - 3 7 , M a y 2 0 0 4 . i ' l I M a s s a c h u s e t t s I n s t i t u t e o f T e c h n o l o g y ( M I T ) L e g l a b o r a t o r y . h t t p : / / w w w . a i . m i t . e d u / r r o j c c t s / l e g l a b / r o b o t s / r o b o t s . A c c e s s e d S e p t e m b e r 2 1 . 2 0 0 9 . · 1 I K . K a n e k o . F . K a n e h i r o . S . K a j i t a , H . H i r u k a w a . T . K a w a s a k i , M . H i r a t a . K . A k a c h i . a n d T . l s o z u m i . H u m a n o i d r o b o t H R P - 2 . I n P r o c I E E E I n t C o n f R o b o t i c s a n d A u t o m a t i o n ( I C R A ' 0 4 ) . p a g e s I 0 8 3 - 1 0 9 0 , A p r i l 2 0 0 4 . ! 1 1 1 K . ~ishiwaki. T . S u g i h a r a . S . K a g a m i . F . K a n e h i r o , M . I n a b a . a n d I I . I n o u e . D e s i g n a n d d e v e l o p m e n t o f r e s e a r c h p l a t f o r m f o r p e r c e p t i o n - a c t i o n i n t e g r a t i o n i n h u m a n o i d r o b o t : H 6 . I n I E E E / R S . I I n t e r n a t i o n a l C o n f e r e n c e o n I n t e l l i g e n t R o b o t s a n d S y s t e m s ( I R O S ' O O ) . v o l u m e 3 . r a g e s 1 5 5 9 - 1 5 6 4 , 2 0 0 0 . I I I u j i t s u A u t o m a t i o n . I l u m a n o i d r o b o t . h t t p : / / w w w . t e c h j a p a n . c o m / A r t i c l e I 0 3 7 . h t m l . i \ c c e s s e d A u g u s t 2 5 . 2 0 0 9 . · 2 1 M . ( ] i e n g c r K . L f t l e r a n d F . P f e i f f e r . S e n s o r s a n d c o n t r o l c o n c e p t o r w a l k i n g " ' J o h n n i e ' ' . I n t J R o b o t R e s . 2 2 ( 3 ) : 2 2 9 - 2 3 9 , M a r c h 2 0 0 3 . I 1 I J . - H . K i m ; J . - H . O h . R e a l i z a t i o n o f d y n a m i c w a l k i n g f o r t h e h u m a n o i d r o b o t p l a t f o r m k h r - 1 . A d v R o b o t i c s . 1 8 ( 7 ) : 7 4 9 - 7 6 8 . A u g u s t 2 0 0 4 . 6 4 • ! l~.R. l y 1 u r p h y . T r i a l b y f i r e [ r e s c u e r o b o t s ] . I E E E R o b o t i c s a n d A u t o m a t i o n M a g a z i n e . '\1~):~0-61. 2 0 0 4 . " - , _ 1 \ o b c r h o n . M e e t p a c k b o t : T h e n C \ \ e s t r e c r u i t . h t t p : / / a r c h i v e s . c n n . c o m / 2 0 0 2 / T I : C l - I I " c i c n c c / 0 8 / 0 I / p a c k b o t / . A u g u s t I . 2 0 0 2 . A c c e s s e d J u n e 2 5 . 2 0 0 5 . ' , i . \ 3 a r c s a n d D . W e t t e r g r e e n . D a n t e I I : T e c h n i c a l d e s c r i p t i o n . r e s u l t s a n d l e s s o n s l c c t r n e d . I n t J R o b o t R e s . 1 8 ( 7 ) : 6 2 1 - 6 4 9 . ! 9 9 9 . l a l i l ' o r n i a I n s t i t u t e o f T e c h n o l o g y N A S A J e t P r o p u l s i o n L a b o r a t o r y . M a r s e x p l o r a t i o n r o v e r m i s s i o n . h t t p : / / m a r s r o v e r s . j p l . n a s a . g o v / h o m i i / , A c c e s s e d A u g u s t 1 5 . 2 0 0 9 . I I X I R . K . M i t t a L a n d I . J . N a g r a t h . R o b o t i c s a n d C o n t r o L T a t a M c G r a w - H i l L 2 0 0 3 . I I ' ! I . J o h n . 1 . C r a i g . I n t r o d u c t i o n t o R o b o t i c s : M e c h a n i c s a n d C o n t r o L P r e n t i c e H a l l . 2 0 0 4 . \ ' 1 1 1 \~oho\\nrks - A t o o l C o r r e a l t i m e i n t e r a c t i v e 3 D m o d e l i n g a n d a n i m a t i o n w i t h d i s t r i b u t e d s i m u l a t i o n . h t t p : / / w w w . n e w t o n i u m . c o m . A c c e s s e d D e c e m b e r 2 8 . 2 0 0 9 . \~l! I . P i c s t a n . W . G . J e s s y . R . W e s t e r v e ! L A . G a b r i a L " S t a b l e v v a l k i n g o f a 7 - D o F b i p e d r o b o t . " I E E E T r a n s . R o b o u \ u t o m . . v o \ . 1 9 . p p . 6 5 3 - 6 6 8 . 2 0 0 3 . . . . . . . . . . . . 1~21 \ l . ( i . J \ . P A b e y r a t n c . " : V 1 o d e l l i n g o f ' b i p e d a l r o b o t n e g o t i a t i n g s l o p e s " M a s t e r T h e s i s . l n i \ \ : T s i t y o f M o r a t u \ \ a . J a n u a r y 2 0 1 0 . 6 5 :\I'I'E~DIX- A : M A T L A B p r o g r a m s P r o : ; r a m A . I : P r o J . : r a m t o c a l c u l a t e t r a j e c t o r y c o e f f i c i e n t s s y m b o l i c a n v ' - r : : : : a ~ , - ' - - ' : \ a ' - : 3 2 f l l , - , > : ; ~ " - - · : / ' / < _ / . ( - ( \ c - i - - ' ' c - -~ , : . . - , . - , ' , c · : _ n c L ! -~:l c : 6 c - / f ' 2 x Y ' 3 * c : c s (a~c'ca) A 3 ; X J • - ~3~d2_I~l p l I~~ l f ' : - l 0 ; A ] ; ~ ~:c-:1lp'r1a; t-~~/ ( x J _ * e n s ( : : : ; l~J~la) + x 2 * - ' ! = ' h a : : - - 3 / ~; 1 2 1 ! ) n 1 1 / , ~3 I q ( x 2 * c o ( a l p h a ) + x J * c : o s ( a J p h a ) ) r 2 1 4 x / . " : i r 1 : , d J 1 - f I>~ ; x -~ k : < l : : ( r : l p h a ) J _ f I 2 · , . ; P r o g r a m A . 2 : P r o J . : r a m t o c a l c u l a t e t r a j e c t 0 1 : v c o e f f i c i e n t s n u m e r i c a n v . . . . . . . . . [c:C,c:~,c2,c3l=coe~ficient(xl,x2,x3,alpha,f) 3•x3'sin(aip~al+xlA3'f-xlA3*x2*sin(alpha)­ SlC(alpha)xx3~2~J*xlA2*x3•x2xsin(alpha)- , 1 - , x ~ "~ -~ x~~} - : - - - , x ~ " ' ' ? • - x L - " " 2 - k s _ : _ r : (a~ p l 1 a ) 1 x l * s i n ( a l p : n a ) - r x 3 A 3 + 2 _ . x 2 _ * = z J A . 2 - , f , : - 1 ' ' " " " - ~- 11 -' ; - z j 2 . : : : v : L - x "~~rf- ~ ' { ) A 2 - ~ _ -~·L's~~ rxL''i~-)"s_i_c(uJpha)·kx/.".3"x3)"xL,~,x3/(- -,~-j~~ '>:~-=,·-~~-x) ~:~:l-x2:, I ( x J _ - / . * x 3 - - r - x 2 ) ; - ~~ : ; _ _ ' z - 3 / ' -~ " : : 3 ~-:, ( a 2 _ p ~! (~ ;~ - 1 : 3 " x 2 - " ' 4 - k s i :-~ : : ~ ~-; h a ) A x ; ' ~ , , 3 • x 2 ' ~ : = : y ' : d - L ~ z : X : 1 ~ ~ r 1 : ~ 2 _ ~) ~1 a · ; - 1 3 - " ~~ - 3 , , 3 ~ x 2 - " ' 2 + . s i n ( a J ) - _ _ , _ _ j ' j " C A > C . ! ' ; ; 1 - I / ' 3 ' 1 - ' ) ( 3 - 3 k X l ' h ; 3 " ' ' c 3 j _ p ) \ 3 J ·~·x2'xl~6*x3"'2*~·z2"'2+2'x~"'~'f- ' , - ; ~ r _ - : l ' t c - t : . 2 _t,'-'-~~·j "''c:__~"1 L + c~ k , - - _ : ; " ' -,"f"~·~,- * x 3 , k , · > : 2 ' ' 2 " s i n ( a 2 _ p L c ) k l A ? - • s i n ( a 1 p n a l · x ' ' 2 8 * x 3 A 2 • x 2 ' s i n ( a l p h a ) x x l A 2 + ] q * x 3 A 3 * x 2 ' s j n ( a L p h 3 * x 3 y s i n , a i r l : : a . ) ' x 2 ) I ( - x 2 + x 3 ) I ( x l - x 3 ) I ( 2 * x l - x 3 - x 2 ) I ( x l - x 2 ) I ( x l - ~~ " 1 - I - / . ; - : _ j c : s , : a _ j ~ha ; , ; - ~/~ l A 3 .,~, x 3 - , . ; s i n ~: a J ~~; i ·-~ ·~ a l D ; 1 a ) _ _ , _ x , k , : : - ; : ) I X } A 3 * x 2 * s i n ( a l p h a ) + 3 * x l A J * f - J A 2 * x 3 * x 2 * s i n ( a l p h a ) - 5 * x l A 2 * x 2 A 2 * s i n ( x J A J - '.~-1 ) 'Kz~ 2"xJr2;_xi+s~:1(alp~a) " x 2 A 5 - • ~- 1 ' -~ • x l ~ 1 · ! · : ~-: ~- p~ ~ ~~) " : . : . 3 / ' 2 * x 2 - ) - ' [ l - f - , _ : _ A - l - ' ' X ) ' ' ~, + - - - r . 3 " x . ? ) , - , / . " C k x ? - , , / I - ~ ; _ z - : 3 t ~ f s j [ 1 ( a L I J [ - : a ) c K X~~ A f l - - r - - 2 7 ~ n i : a l p l ' t 0 : 1 ~ ~{ 2 - " ' : 3 K : · 7 • : j - , - z 2 ) I ( x ~- x ? ) / ( z J - 7 ) ; x : 3 - + - x 2 ) I ( a l p l 1 a : · , / , 2 ; " ) ' . - -~ - 1 - ' < _ ~ \ , - ; K ' J - · . · : 1 • : v : : 2 . / - x j _ _ J X:;_":-+-".<-~' ~ ; •s~nralpha)Txl•sin(alpha) " z 2 + x 3 ' s i n ( a l p h a ) • x 2 - ( - x 2 " x 3 ) l ( x J - x 3 ) / ( 2 • x l - x 3 - x 2 ) / i x l - x 2 i l ( x l - 6 6 11 m g r a m A . 3 : P r o g r a m t o c a l c u l a t e j o i n t a n g l e ( / , ' t ~:e+_3~,, ~~t'r.et_a :=c~Lput 3 [ X , Y , L : , L 2 l 2,',;-~21-\L:/2.~ u - - - , J ~~ l ( l - ( C t ~~ ~; L d 2 ' " ' L ; : ) I ( 2 + 1 _ , l k I _ , 2 ) ,~'--:.? ( ~~ti-le 1 ,~a?, ~--t~tc", a ) ; =L·,:~; ·~het 2 P r o g r a m A . 4 : P r o g r a m t o c a l c u l a t e j o i n t a n g l e R 1 t_nP~_ ( _ j l I =:;u~pu~- c = , ( X , Y , r~1et 2 ; : u n~"= ( - . ~~ 1 1 x : n~ ~e ~:;l_~_/ci; P r o g r a m A . 5 : P r o g r a m t o c a l c u l a t e j o i n t a n g l e ( } 5 I - 1--~t:::; t I ~ I - ' ( 1 ~ J 2 . ' ~ ~2 ' :~ l > ( c i . . : ) ) , - r : a c J • " 7 : ~-;X; _ j _ '~7 ' , - ( l _ , -~ • ? . , u . · . ~) : 1 ; ; P r o g r a m A . 6 : P r o g r a m t o c a l c u l a t e j o i n t a n g l e ( ) 6 : J l ? l ' = - ( ) _ ) . , _ _ . . 1 ~ 2 C ) ~ ( S X 1 [ ; , _ 1 . - = : l c - : 3 . ~-) ) ~- ' ' L i ' , ; : ) - " l a n _ ( ~- - = ~::. ~ : J . ( , - - r 1 / ~- j ; ; P r o g r a m A . 7 : P r o g r a m t o c a l c u l a t e j o i n t a n g l e t r a j e c t o r y c o e f f i c i e n t s ~ymho/ically , . . , . . . ~- ( _ : ::_~ _ : _ _ ' ~ 2 . ' '~~;:_14' , ~~ q 6 J = o · , _ 2 _ ~~-put l 0 8 3 ( t h e L a s , t l , G t a~ , + ' - r - l c : ~ c,r~cta?e,rs,~~ t : , L 2 , a 0 , a l , a 2 , a 3 , a 4 , a 5 ) f d a:·~s~a2'LS 2~ ]'LSAJ+a4•tsA4•a5*~sA5; - • ' a e t a O J ' • t e + a ? ' L c 2+a3'LeA3~a4*teA4+a5*teAS; - r : : R + c t 2 1 a ~ t - 2 - x a 2 " ' t s - r - 3 * ' c t 3 - - x t s , . , 2 + 4 * a 4 * t s A 3 + S 7 : : a 5 * t s A i } ; - c - - e _ ~1ct a l e + 2 * a::::--x-~e-r-3 ~-- a 3 x - c e A 2 + 4 " * a 4 * t e / " 3 - \ - 5 - - x - a S * t e A 4 ; - _ - 1 I " ' r - c 1 ; ; s r - 2 ' a . 2 + 6 ' a 3 k t l r - 1 2 * a 4 • L l A 2 + 2 0 * a 5 ~ t l A 3 ; , , i ; o · 2 ! 1 : ) ' J k ; - 2 t - l 2 k a 4 k t 2 A 2 + - 2 0 * a 5 ' t 2 A 3 ; , , ' / , , ~ : - ) l l = o _ j _ " T e ( _ e c : 1 ) , e q 2 , e q 3 , e q 4 , t - ; , a O , a l , a 2 , a 3 , a 4 , a S l s , ~l1eta 6 7 J•,,,~mm. 1 . 8 : P r o g r a m t o c a l c u l a t e j o i n t a n g l e t r a j e c t o r y c o e f f i c i e n t s numerical~v _ i . ! - : f .~; 1 - ' i _ ~ 1 ~ I _ ',~ \ t-_L~~ d : : : ; , - ! : = _ : - : c t , · - n e · ::::~, t a::_r~' ~, t - - ; ~ ) ~ i - - i * r':.~r ~ J ( , I K ~- t = i ' 3 ~ t 1 ' ~~ " " t .·-~ t l ~ · ,-,~-"-- ~ • + - ' - h u 2 _ :~' • ' I C ~ + : : _ e -~ J " . ~ l " ' 3 f _ ,~ s 2 * : _ 2 " t ~, e L > - = : l . ~- r l _ ~ t = ' ~ t t = : ' ~- l j~L2 • : I ' l ~1 \ • + : ; '~lie'~ : . : ' " " I } k • ~. ;~ ~ ~ - ) > = : ' - ) ( ' 1 j ( , • \~ I • 4 A t _ l / ' · 3 " ' t ; . ) I ' • * t h ( _ ; t c = t ~~ e T 3 2 " ~ C _ : I ' · 2 ) ; t ) ~ + 2 A 2 * t 3 * : . ? " 2 * i _ : : 3 2 ' " L ~ 1 C - ? t~ o ~,_ s + 2 0 k ~ e " · " · t l " 3 k t s A ? k L 2 k - c , h c t a e _ _ _ : _ 6 0 + - - : e " ' . ? * ~ l " 3 " : : - : t 1 . : = : ; : - _ -~: s - - - r - 2 ~ " - : . ) "'-~ k ; : - _ e " 5 " ' t~~ e~ a s - '4•t~A?·~~~~ae+20*~2A3*tsA4*teA?*thetale- ~ b*~e·the~ale-36'~l·teA4*t2A2*tsA2•thetale­ ;·c~A6•cheras,l08'~?·tsAS*tlA2*thetae+24*tlA2*tsA5*teA3*~hcta2e•~20 ·~?A • r n e t sT:28*t2A2*~sA3*tlA3*thetae+24*t2A2'tsA6•th~t~e+180' '''e*Lherae-368'~2A2'~sA2•tlA3*re*thetae- A : _ - f - . : _ j " t L(~' c • J G • r ? • c s A 4 · c l ? • t e A 2 • t h c t a l e - c - : 0•t2•csA~'t!A2·tc•thetale- : : : ~ T 1 ' I , . - L S : " t ~"~ 7 : - : j _ d 2 - ~10 ·tl•lsA~'Lc"t2A2'thetaie- * L e ~, ' / ~ - ; : - - h ,~ ' , " • c . e 1 L * t 1 · - ) " r _ ~- : J " - r - _ ~-l < : - ; t 2 c - - 1 - 2 ~ ~ t l k t s / ' - 4 " ' t ~; / ' , : J K ~--he t a 2 c + 2 ,~ k L . 1 2 , . _ t r : . . : : : c . + - l__'--~-~t.jt 1 "' "'-~::--./'4".-te/'- · 1 - - t h e t . c t ' i _ , _ j 1 : - - - ; • - ! ( -~ 't~T~o~··l·reA~•r?A2·t~£tasT240·tc 3 · c 2 l'tl'r~·~~etas ·~ • t , _ , ~ l : - ; + - , - - - ) " - ' _ ) -~ 1 l - , , * ~- , - - - ~ t : 3 / ' 3 - k t h . : - _ : t = 2 s * . t e / ' 3 - . , -~ 1 - - t 1 h 3 " + _ ~; _ c • c-:B~'~!A2*ts"3*t2*~eA2'1 r : H G ' t l ' 2 ' t s ' 2 ' t 2 ' / ' 3 * ~~ne"= ~-:; , : / . k - t : . ! - : _ c , - j ) . · · . e - ~·· ?•re·~·tn~tals,JE'tl~2•:sA3'~2*teA3*tteta~c+:RO*tl 2·c~·j•c ( ! · k + _ L 3 " L . : - - ; / ' ~~ " / ' . 3 * t . > e t u 2 s - 2 8 * t . 2 A 3 * 1 _ sA~~ * L c : : : * t ' n e t a<~- /•~cA3'tl.etals~24*tlA2'csAS*tcA2*thetale+24'llA2*tsA6•t~·t~eta 2 • t s s • t ' r h e t J e - 2 0 * t 2 ' 3 ' t s A 3 * t h e t a e - ·-~ / ' . - = - . " t - c : . " ~ : - - --c~ a-~ 1 : 2 0 ~ t~ l · " 3 " ~sA 4 * ' - c e * L h c - r _ : . a e - ~sA~·te"2'theta~e~3•tl*tsA6*tcA3·theta2e+l20'tlA2*tsA4•te'2*thcta i • A~•ts'2•thetals- ; . . 1 ' 2 • t 2 A j ' L h e t • _ . t e r - 2 t ; • c 2 A 2 ' t e A 4 • t s A 3 * t h c t a l e - ~·r:A2•t2A3•ts•thetals\36C*te*tlA2*L2A3•tsA2*thetae­ ··~eAJ•t A3*t~ctals- " " - : - . ~- -~ ~ - r - _ r 1 e : - . d s · L C " !~ 3 " ' t c " i } " c : : : : ; * ' r . h e t a _ : : : ; + 3 6 k ~) k ~- R " 4 ~ 1 ' - : _ _ 2 ~ t " ~ " t h e l _ ( · , " ( l + - r _ : " · . : : / * L . s *t-r~e-c ~s-t-3Kt2"te/'i'*1_sA2kllleta?~-;- , , > : . . . 4 _ n c · - ~ 1 3 "'~- ) ' " 1 c / ' . 6 * ~--he~ 2 s - r ~ s / ' 3 - : : . _ - 1 _ - r - ? ' 7 : : ~-j , - , • , - l l ~; 2 " ' *·-s~"'-l-,*te•·-r::-r-:.c~::: - J e - " - : ~, , :~ " + : : · r ' j " ~-? - 2 ~- t ; ) , - : ~ ' . 3 , . ~ l l e l . ~:.~ 1 e I-/~ U l - + . c ; /'<~~ * L ~ ~ " ' L 1 * e : / , . r , - ) , + - ' , ' 1 _ ~_-:-:::..:::c.- ~ t - ~ ( ; 1 - ' . - ) ~ t , 1 ~ ~-:: s 7 * ~ ! - - ? / ' . ? " t } 1 c : t _ c . = l 2 c ; - 3 * : · t e - - 1 " - : . _ / _ - ; , * . 'c~·~LA)•t2A3•tsAJ!~~~[ t-120~'-J: - 1~ 1 - 1 e 1 1 s , - 2 : J · 1 , . J , - . 3 ' * c e / ' , 0 . - - r l s A k L h e t a 1 s + 1 : ? ( ) ~ t 2 / ' j " t . E . , 4 " t ~"' t ~=;-;. · , _ ;~ e ' ' : . r . = ; , , 2 r t S J " ' + - \ - , ? + * L 2 / ' 2 - ~ ' L C j ' l h 8 C d S " t ) ' 2 + i 0'tl•tsA2•te'3•tnetae'~2A2- ~e~ • t i ' t s A 6 • t c • t h e t a e J 6 • t l * t s A S • t e A 2 * t h e t a e + 4 Q • t 1 ' 3 • ' ( - - , ' r _ c / ' ? " - l : = . h e ~·~het 1e-:G0'~ 1 A?•tsA3*teA?•thctae- 6 8 . . . . . . - ' , I , _ _ ( ~f~l1e:~c _ , c ; - - ' - - 1 ( ) " 1 _ 1 / ' ' - 3 , . . - t r J c t * t : s " / * t e / ' : 3 j 0 " ' t l r t , 3 / ' I * t _ l l e t r l e - 2 * + : : n P L ;~ e - ) 4 " ' ~ ~ / ' ~) * L e / ' 4 * t _ . c ; ' " * t h e t 1 e - 2 C J - A - t _ -~ ' " j ~ ~~- " ~~ ' A - c _ :::~ ~- L : - , ~ t a > J - [ _ I ' T ~-· k • - h P - ; d - j S - ~ (~I·;~ A t _ ~ k t _ ~ " - , ~) k , : = 2 A 2 K t S * t _ h e t a 1 ; : ) - ~ } t ~ t_::--le:'-::-a~::~-+ / : / 1 " t / , 2 " t _ , - ' I - , • • c ; / ~:' ~ " * - r - i _ - L · : - L a : - · ~ 1 - e ( , ' I r - · - c - L - 1 - > - ~ r _ ' > - ; ' I , c . ' l L - - ( _ J ' _ ) • 1 _ ~~; '~ ' ·~ / / j ~ t _ j _ ) , t _ p / ' : 3 -~ t _ : : 1 C : f - _ / ( , 1 --~. I _ / j ' L : C ~ r l ' ' t . ' < ' ' j ' ' c - , _ > e " t _ c - ' , ; - c ~ ' t -~ ,~,- t _ , ~~ , . t _ ~: ! , ; t a e t - t e / , 2 - ~,. • L : j · ~-s . ~ c " C : ~~ ' t j , . ~ e / ' -~ ) , - : . _ r : c t 2 1 : : 3 - . . ~ ; - . ' , ( _ ' ; T -~l s A 4 · : 2 ' 2 ' : c 2':heta~s- J , ~ + - , • . - - , - < - : - - - • - - < - t - ~ ~ .~ t . - - i l ( ' - ) " * ~ S · " ' ] r - r - ~~ E - : ? : l - - - : = : . 2 ' * t c~ A 7 - ~'-::1 rc;.~;.,.t-s L + :/~-~-~ )*l~e·'2~<'Lheta2::;Kt.s/'G 2,~*-:l ~-""'tcA~JAts~--=--h,~;Ls: s * - c ; · .i~'! 1 . : : : . ' ~ - , - - . , , . l (':lA~'te ~·~te:as-3*tl·tc'6•~sAJ•:~e ? e - 3 - , t 2 " - t : 1 c t C J . s - t 2 < 0 k - c s A J " ' t 1 3 " t e , . - ~ . 2 * - : _ : : - - : e - a e - ' ~ - ' ' ) , _ s , . Lh~; ~JE•:l*:eA •t?A2•:sA2•thc~ : 2 o • t e ' ? · · l A J · t 2 ' , - 1 e t r : _ . : . l ~ - i':l'J*t2•tnctaellBO•tl*LeA2*tsA3*thctae*t2'2~36*tl*teA3*tsA3*t~et -~ ' t l · t e A 3 * t s A 3 * 1 h c t a ' c ' t 2 A 2 - ~·:,,•J•~heL - E • t : · t s ' 2 * t h c t a J s * t e A 6 c 6 * : l * t s A 3 * t h e t a : s • t c ' + G ' t l , _ - , ) t 1 * 1 ~ ' l ~ ~ ( - ; " .~1 - . J ; - t : 1 c t , : : : ; . 1 c _ ' · C ) : _ . : * t = . _ * ' L :--~etc:. : 3 * t . s / J + t 4 - - ( _ '-~ " A t s - k t ~ * t s ' ' ' " 7 -*~c.,_: · a 1 ; _ ; j i 1 . . - --~ 4 * ~ c 4 , . t I _ a 2 _ s + / : ~ k t / / , 2 j ~- / ' , • 1 ) * ~ e o~- ~he : k + - • t _ , - - " ( 4 " t : 2 A ? , , ~J " t e : 3 * t ~~ ~ t . s - _ . " r ' , . ~ - ' - - : : { T _ / A t .~; / , r : l * : _ : : : ~-'" ' ) - · t _ h ·~; t a : ? G ~t )*~:=:-; 5*~(' 4*t~leL 2 s - J r t 2 k t _ c , . t • t " ? - c • I ~·L:'2' ~ 'lsA3*Lhe~a·s- A I : - ) . : 2 I L : * L 3'LeA~*Ls'2*theta2s+21•t?'2* e A G * L ' t h e t a " - -=-~ " - i L / ~' : > - " t e ; - : : : : ~:; + . 2 ~1 ~ ' 2 1 : - - - : = _ e A ( J * t " ' e t a s - - - ) ' * . ) ~[ A ~ ~ "r_rj~: + O * : s ' L • t ? · : e ' ] * : h c t a e t E • • • 1~2 • t " ' • t : : " o t a l e t J · t . 2 l e - _ 2 1 ' t l A 2 ' c e A 3 * L h c r a e • : s A 3 - ~+r-,' : 3 " - . - h e - : c . . ' ' _ - ) - Las•cs'J•LeA~- • t 2 · t · e t a s * t c ' 6 * t s t 6 • t 2 * t h e t a s * : e ' 3 • t s A 2 - ~-t~!cLa2s~4·ts'4':hcta2s*teA6t6•tsA6*t?•:e'2':~e:alc­ •'t2':c'c~e:ae-G•tsAS't2'teA2•thetae­ ~ls•:eA7*t2·lB'ts'4*t2*te'4'theta:s)/(l80*tsA2*tlA2*tc•L?A3- - J • t l*t2-:o•~sA3*t2A3+12*L2A2*tsA6~10*tcA5*t2AJ- ' ' I "2't,'3-~4'tl"2•:2•~eA5+60*tsA4*tl*t2A3+5~*tlA2•t2*ts"5- ' l 5!~ • - 1 • ? A 2 • L e 5 · 6 0 ' t c " J • : l 2 * : 2 A J - 6 0 * L e 1•tl_•~ A J - ·-2'LlA~- • t s A 2 * t 2 ' t e A 5 - 8 0 ' t s A 2 • t c A * : 2 A 3 - J • t s ' G • : e • : 2 - o·~2 3 + 6 0 • t A 2 • t s ' 3 " t l " J - OLcAIJ+~2'r7'2•~sA5•cc-3•t~A6•ts*t2-lO*leA~kLs*t?AJ- ) + l l • L e " 2 • t /~ . ( . _ _ . c . ; , : ) , - = ~=::, ~- ~ · : ) S ' 3 • t ? A c6Q*t2•~eA4•tlAJ-6Q•t2A)*re l•~~ ]':L"l-~3"'·1~ ~)~I- 3 " - : 2 K L e " ' 1 1 + 6 : J * " = L · ' > ; - = - .-~ / ~ t ( - ; ~--; " - t - ( - : c . " · - , , - j - '~I r 7 l / ~ t ~ " ' _ _ e - " - : - .c~ 3 - ~- . . ) i , c ; : _ _ ~ A L e : 3 , . ; = . L 3 ' ~) c - . - - t l 1 : - : - _ 2 ) " ~ e · ' - : : - : ) 4 - + - 2 - < t : : : _ l - < ~ • 1 c • , - l / - : ; • : 2 • · : s • t e · 3 • + - l ' ' 3 - P ' t l l-Ie~ i ? ' l S " t 2 ' t l A J - ~~ 1 \ * • ( : : _ ; / ' - - ; ~ · _ / · ; ~- l _ / ' ; A l . I - : _ _ 2 " [ _ S A G k t ~ A 2 - l 8 r t } ' " 3 ' A t : 3 ' " r = J ~ S * ' = . s / - ; - > ; : ] _ ~ t e ' 4 - · r "tlA2+GCxtsA4'te'2*tlA2~3*tsA6*tl*re- , . , 't~'~tRll':lAJ•tsA2*te"J-lQ•t}AJ*ts*teA4tlQxLlAJ*textSA1- 6 9 + 1 ; . t J 6 - O • t ] 2 cs'2·~eA4- • - :-~- ~ ' i " : : r ; : - * " r : : S ~;- 3 ' k - : : _ J k - : - _ P h S k t _ S " \ : 2 - 1 ? * ~ S * t : _ _ ' ' 7 r - : : : C r . 1 : - ; . : · , ~- / ' ) 1 - + > C 1 _ _ ) : . ( . - ; - - - 3 * t h e t a l s - ; : > O * L / A 3 * t s A * U J e t _ a · e - ' f r ~-=; > : e - : - s · ~) x t , , 6 * t c " ' 3 T t h e L a 2 ( ? + / 4 * t e A 4 * ~ / / , 2 - k ~ o A 2 * t - - 1 - ~ c,~ A z-~ r - . : · - ~ ( ! • '~, ~ - - / " : 2 " ' t _ e , . 2 * - a / s - ~ ~- h ( _ _ l i : : : - J j , j ·teAS•t2A2*~sA2*tne~a?s-6*LeA6•t ·t?*~hetal - < , ~-- l = 1 l t = · - - - -~ C : · t ~ r t ~ " ) * ~sA 3 ~ t : 1 e t a~ s + 3 6 - k L e A ~1 * I _ ' " ' t ; . . : c~ t c J 1 c K ' / A : 2 - (-~ f - - , . - - ~ : . : ) " ' ( , ~- t - k t 2 * t l l ~ t : : _ _ c - ? 4 k t ~ 7 t > = ; " r : : ~ ; " " t ~-:_ p t , : _ , - : _ : , , _ , - , _ · - · - , ' = , . t - 3 • ~ , : : ~: • t l · : e 1~ a l s - ' - I F : t a 2 . s - 1 3 6 0 · t : : : ; ' t j · • 2 • : . 1 * ~ r ' : A 3 • t ~eta c - _ \ I : - 1 1 I , - : : _ * T~ ~; L ; " ' ) " ' : - _ : : : _ _ " " r e x t r : e - : : : : a 1 s - t j _ ( ) ·:~ * - S " t L ' ' ) " ' ~ c " L . )"":-~ ~,}-, / ' - ) ' t ~t-,(~-::-_a : : J ,--,,,'Ct.-:J''~~'"'t2A:~kt(;/'Jkt::_ 7 -,_llc--:-.ulc- - < 7 t : - J , , h < ? · _ : , l -·+;'·,}~-, ''(;~-i ' ] t - ; t ,-_::+t_(=;"-'::-2A2+3A~,--, f - l , c . - : ; A / k : h e t - l 2 P - -~~"3 1 ·t:-=: · l * t h e t . a l * t 2 - ; " • < : • t : . : c t a · e · t 2 / - s · : i J * t ' · ~~ . l ~~ ' J ' 1 : h c r : - ' * c ' 4 I 2 0 * - : t _ ' ; * t ~-~ " ' ' \ f t · , > - , t ;~_;..? :::_~ A - 1 < - } - ~: , ) - r ~ : - - f ; ~~A['}-} t s A 2 , 1 ; ~ > } '_l~ a l c 1 6 * L A4'LCA3xt2·th~t3 e - 2 ' ' i ; ' t ~, L~ -~·r2•1he-:a2s+2~·te' ' L / " 2 • t s • t h c t a l s t 6 • t e " 7 • - : *~s*thcLa?st2C*t " / ' t : : e t a J c - 6 · t c " S • t s 2·~2·the: ls+2ll*t2A3*rs"L*~e• · a e - _ _ : _ : : : . : • - r - ' ( l , : - : . / . 1 _ - . ~ " " t _ , ~*~e*+- t a l c + J 6 * t s A 3 * t l A 2 · t c " 3 * t : h c l s - e . - , , - . _ ; : ) . - . ::-:-~~p- •re'4'~hetals+~6*tsA4'rl"2*tcA2•thctals+24*ts"~*tl"2*teA2•the J o - 1 2•rc"2•t:)JCt~S-Jl C*ts"3•tJ"2*:eA2*thetae+3lJ*tSA3•tl*tc"5•~hCtC : · c ? . - • t : _ · t e " 2 · t • · - · I e A~ ' 1 t ' t , c • · _ I · , , . ' I ~ a~s- - 3 c • s"J·t?"2•rl•tc*~het t s + J 6 • r 2'~2"/•tl•teA2·t~etae- ' + . / • I _ l / • i 2 • t : p A 3 • : l t ' - ' - . c 1 p + - 2 ( I * t , , , - 2 • t ! . 2 • - ~ j ·~ / ' c : ; } 1~ t '-~I_ c J ~ ~ t ~· 4 : J * ~ ~ ) * : : : : _ _ / * t L . A J ~ t c : A t h ~ t , j = - - s - L ! - - ) t , . a~ · : : " J * r . s " 3 ~ r _ l A;~ * ' L / \ ~1 ~ , ' . - 1 e t 2 : ? e - 1 ~ / ~-=- ' ' - - ' ~_)" L " i ; : ~ · , f " ' ) At~'..P·,~a)c- ; _ _ _ , ' - : - _ j - '-~- ~)*i-f""/'~--.- ] 6 C " t ~."' : _ ] _ / . 2 + · ~ 7 " t c " ' 3 - r t h c::~- a ~3- ~ f -~ f - ( ' · / \ : ? · - - - : : h ~~ · a e 1 ~ 2 : ' " " + : : - s / \ : - 3 _ . _ - ~ 1 / \ 2 _ . _ - r _ ; / A 3 * t h e - : a : : _ r ' - / ' I · , · 5 ' t d / ( : ; - a 2'r~ela2s+"2J•tl*t2"J*ten~·t~etals-l8CJ*ts"2*tJA2'te 3't~elac l · ; ' • t r • · · J · t ' : c t . a : 3 - ( 0 • · . s " 2 * t l • t e " 6 · t h e t a 2 e - 3 r t s A 4 ""t.l-~eta=._c;- _ j • t C _ ' r 4 k t ' : C ' t a 1 : 3 - J - l 2 0 * t : 2 k t S A 4 k : . l r . 3 * t r " e t a 1 e - ' - : • . 3 * t l 3 * t h c o · . J ~ e 1 1 2 l l • r e A 3 *the~- J e ' t 2 * t s A 3 - \ * - h e r a s · t 2 • ' s A 3 - 1 2 0 • t s • L 1 • 2 * t 2 A 3 * t : e A 2 * t h e t a l s - ?*t2AJ•te*thetas+720*ts*tlA2*t2"3*te*thetac~lC8*ts*:lA2•L2*~eA4 ·~ " : ~" * t f 1 c : t ~ / ' s J - : . e " ( ) - ·t~ te'2't'~ ~T~ J*t2*rl'3*~s*:e"/*theLa ;,+-(~ _<~t::f 1 _hf-::::·,:_ll~-· ~, ' - - " - : . o , 3 ' + - : : - > ~ · _ t 1 e , - _ a l E - " - - - r - 2 · 1 0 ~ ; _ s ' 2 " ' t e 't2'3't~_eLac- e"2't2A3*thct~s~~2C·t2"2'ti"3*teA3*:.hetals+6*t " 7 • t 2 * t h e t a l e - ? · t t i e L a l c - J O · t s 4 ' " 2 A 2 • t e " 3 • t h e t a 2 s - . . . / . ~~ ' C t r ~ e · ' - " . ( - ; -~ ~ t 2 : J T t c · A 2 * L - A L h t : t a s " ~ 2 A 2 + 6 0 " " t e / , 2 * t 2 "~ 3 " - ' : s A J k r _ : - e " r _ / _ · " ' , l - " L r - : _ , / ' ) " ' , h e L : - l l - b 0 * : : = . s / · : 2 r - 2 r J - - x ~ e " ' 3 * - : = _ : ' - e - ; : : : _ a _ : _ _ c - " 3 * t c ' ·'-'+:~} + l 2 C l ' t ? • t " ) • t _ l A 3 * t c " ' ' t ! l ' " L a l ;"·'-lr~:-- (~-rl,;~,~tJ .!_*t:-:::~~~·t_ ~:_her=-a~s- n c - 6 l s · r c ~- 't~·tc 4 • c ' 3 * a 1 s - 7 0 * " - - - L • , c - : _ 1 . < - ·(~ · ' I * L :--~ ) c t ~-! 8 t-~ c~ J + ( ) k ~- l r t c ; " ' : ) " T ~~: L " + - f-~ ~ I , I _ : ; ) - _ l ~ l ' t e ' 3 • t n c ; t : _ c • t s . : ; - : ~ L ~, r ; + - ( i ] " t S A 3 - 6 A - t 1 * t ' L - l " 1 ~· 2 " l _ . r : t ; J 1 ' ' 1 - l _ ' 1 j j _ ~--, ! ~ } j _ " 3 · I h c t c , : : . " - b • t l ' : - f ; ' < , , - t _ h > - ; - d ) t = : - ~ i -~. ' 2 " ' - : - : _ a ? -~*~ ~E•~he- ~s -~ j - " ' ' 1 ) 1 , - ) ~ - ; - : - ~ ~ : ,~: 1 - - 1 ~ r _ ) * t s k + : - - _ ) " " ' 2 " * - t 1 r ] . , . _ - '~ r ; , , 2 k L I - l e : _ a j _ s ~ 1 ~ ~~, ~- ! _ 1 " - _ ; - ) ' - - 3 k • " t < ' t p/'-~-+:_hc~02_ ·t~Ctd c 1 ~~·ts~2•te"3*1hctas•t2 £ - ) ' t ' e t . a e ' 1 2 ~720"ts•L2"?*t~~3*te•thetas- , , _ ! ' t l " 3 • t e • t n c t - a e t 2 4 0 " t s • t 2 A 2 • t l A 3 * t e A 2 * t h e t a l e + : : . 2 0 • L l * t 2 " 3 • L s " ? -J~(·~'·C " J • t '2•t-e•the~ae+4 0 * t l * t 2 A 3 • t A2.•t-e•~~e~as- _ ; ' J C j . . ~ , - , / , ' " " j , _ - , / . ? * - _s+~HO*tl*L2A3*ts*~eA2*the' s - 1 . . < . 1 . ] 1 ' _ - : 1 ; : 4 k : ~sA K - - = . 1 A . 2 ~ L r~ e t c 1 ~ e + 6 * t s " ' - - - - : ~-- _ j _ ,~,:: e * r n e l a 2 e + ~ ' + / l~- , ~ T : / ' , ' 3 } : - ; _ c l s - r ~~ · _ ;~ ~..--::-- ~; / \ S · o \ - t ) / ' 2 } - - : - L - 7 - c ' : _ e t a - L R f / ) ~ t : = " ! - , 3 ' t 1 " 2 " ' t : / * , _ ~ " t I , . + ~ " - / · · + - r . - : e - ' - · ) 6 : J • t : c ; • • _ l * t i ~ 2 • t e • t : ; • t : : e 1 - . . , s - ; - 1 , + , - ' 3 u ~-) ; : . t 2 r t ~ k - ; : : : h e : : : : - : : : : ) - , - ) . - + - : 1 , - . , k · _ . : ! . . ~ . . "'~e·thstdl l " ' " 3 ' - ' , ' - - \ " l c . : - , : " 1 ~ , . t : ' e ~- :~, ( ~ C , . c 2 " " L s , . , / 7 - t 1 ' ~ " r - ' - t ; ' , e t d ~- r , ~~ 1 ; ~ 4 '~ * ~? . c : : k : + _ ~~ " / ' " ' , 1 A 3 < : - : e ; k ~ ~ ~ ~ , _ ,~ = - • t - l ( ) A , t - = : : : : , - J . - j '~ ! ; - ' : ~~ ( ' • - !J~ t : - : . : : : _ 2 - - - t . , I , , / . . . . - r ' " ' 6 " r : r : ; : : - t _ d ) < . r - l _ _ _ ~-~ f~ 1 - - t - : 2 : u ~; / " t - l ' ' ) " t e A 3 " " : h c t _ 0 j _ _ _ _ : _ 6 U " L , , , ? ~- t - 1 , , J +~ t P / ' j ~ ·~- I - 1 ( • t --~ ,~,te/'i~ * : _ h c t _ _ - : : 1 : - J - ? ' t •~net lc+~C'ts~S*tlA3"te*the~a2er20*~1A3·LeA~•tne'a~s­ (~"*thctul~+C,"-;--~i 7 ·-;:-~~A7Kt'neta~s- 1 E : ; . - : ; * J : - ( : 0 . " ' * - , - , C > - + - - - = - : / - - , . - : t . S A 3 _ _ _ : _ _ 4 ~) * t S A ? * t 1 A 3 * t e A t } t , · . _ l l c - = : t a . 2 ~- ·~ c · . . ! . c i _ l "+-l~E:-"l_o:;_,_=; t :~)4 . , ' "7·~lA3•teA2•Ltetas- ~···eA/*thet • I P ~;"' . . t a L I O • t A 2 ' t l A 2 ' t e · t 2 A 3 - 3 * L l * t e A I - 3 ' t s " / • L 2 - ~~ 2 • t 2 A 2 " t S 6-:0•ten~*L2"3-60*tSA3*tln2*L2A3- , - , r J t t - : u ' - + - ~~: / ' ( t ' t,~: A f - 2 ' ' ' 3 - t 1 , l ; l j _ A ? k ~ 2 k t S A 5 - ' ) 4 " - r - l * - : - : - : : _ _ A , _ , teA~;~ 6 ~J * l e A 3 * t 2 _ / · / * ~ 2 A 3 - 6 C J 1 ~ ~ " " ~~ * L 2 _ } ; t : ! A-~- -:.~ f ' ) ~ - - ' , A : - , C l k t ~· ? f t A ~~ k ~~ 2 A 3 - 3 • t 6 ° ~: ( ; 0 l 2 - ' + - , - . r . , " ' . l ; I 6 ' t s • t 2 T l •Le"4':s'~2 3 - l ~ ~) c I~ ~ + - ' ) k t - " " ' < l - - ) . ; +~ 1 _ A 3 - 6 0 t 1 c ; / ' * t l _ s - 4 - I l l - , ; I , _ . ( k + - / k - A / l j 6 ~ < ~ t l j ( - j " ~ ; : , A ~- r _ , · J . ~- 1 A 2 * L 2 " ' ~:__ e ' : . ' . , ' ' ' . i . - - , C } ' 1 ' ) r - * t · · : ' 3 · < ; ~ / j , - ( ) k L ~ K t / A 2 " " · P " L ~3" ' 1 + ~ L C J + l _ S A J . , . T : ( ' ; * " - ! - l > ~~/*' ~.; C ' / ' 3 ; I ) - : : : ; L * l ~' * -~ - , c ~~ + : ~ 0 ~ t _ 2 " " ' ! k t s - k - - / ' . ~! * t J A -~ - : : ; * ' -:->~~"' : " L 2 - + 3 " t ~) , , ; " - t J - j _ 2 * " : : : . s / ' , G r t 1 A 2 - + ~ C l k - c 1 , , , 3 - " t s ) - j - l l = - ) - . _ 1 _ ; : ; r ' . > ; f - - l j _ - ' r l ' . c _ j l~· 1 , ; ~ - = s r l ' : ! " : - _ 2 _ 2 , 3 * 1 . • s A 6 k t 1 " " ' 1 ' - c - > - ? , -~)-+-~_<(" • ~- 1 "-3~ 1 . _ _ : - · . / ' : : J - x : - ; : - : / ' 3 - l C A - - : l / ' ')·k~skr_eA4- 1 lD-x:-rl A'3"~ek-':_s'"/t- )~ f - ~~l:--;/'3+:2"'1- 1 2"*LCA6-(~:J* 1 ,_-_j_A)+-~.::;'"2x~eA4- L ' r +]'~i*tcA2•rs"~-]*tl*teA~*tsA2+l2•ts*tlA2·~eA • ! ~xt-~eta2e*rl*teA7- •r~A?"theta2s•~eA4":2•t2A2*~eA5"theta?s*ts+3*t2*teA2•the~a2s*ts"5- ~_::. t r~ 2 : - ' * l s , , 4 - -~ , ! t - ~~ " r : e / ' 7 * L :~ • . ; t C l ~~ : : = ; - 1 - t~' 0 - x : t s A 2 · k t h c t ~) - k ~ ~ ' " ~-_; K ~ ? / , ? ~ / . : : · • i _ : : : : . / : ' I ~ + ' , • . + - 1 j " i _ _ ,-,2,.t,_tl~ . . ta-Je+-l?0"'t/~''2*t: "'5* 1 ~_s''2"-~~lE::1_al J . r , , · t l , _ : : _ t - · ) ( , ( : : ~ l -~- + - --~,~=s'tfJe~ s - - < ! y • , L k j " " L _ ( " l_.t'l(:_:T~ C 1 ( : : : - ~· " T ! - 1 ( ; t ~_; 1 ' , 1 - J b I ~_:: / / ' - ; I ) _ : : _ k t , A t 1 - f + ' - - ~ C t ( J _ : C 7 1 1 y : · - - " t • ' ( - , . . . - - (-~ l : - : - - < , " " ~·~t~ac+ > - - < ( l " t k + - ' * r r l t - · _ : - - : J " - : - : ( ' ' t / " 1 ( t - 1 , c ; / ' ~. * r _ l l e r -3b~t-/ L~t-~3 ~-i"i-rl·--=~~l· -r--"rl!~"1(' 2 " 2 " - t e 4 - ; o ; l _ _ _ : _ r t _ _ h c t _ : s + 1 c~~-t1 ' L L ' · L -~ J ' -~ e - ) ' ~~ .~: ) f I ~-j P ( = i l ~--; - 3 " t l A / : f . - 1 , - _ 2 . ' " ' ] r - = * - c l l P t a - < , " ' ~ ~" j d , : - ) - - - r - ~3 ( * ~~ = - - ' ) ) - - t _ 2 1 - t _ S / ' ( " : _ ~C; - L 1 S + 6 + - ~- '/~ ~ t S - , L~ ' t f-~ · '~ : 1 _ , _ _ : _ ( ' ) ~ i e = , r I - he~ ,_:-:s(~ * 1 _ 1 ?~t? " · L c : : A c J r-thetu1r_~- ~sA4•,•eta?eil?O'~ A?·~lA3*ts*te•th~tnis- • r ] A 3 * i e • t *the~al~- s~J·~] • , e • t h e t a l s · 3 6 " t 2 A 2 * t s A 3 * t e * t J * t h e t a l c - • + - P " 3 ~- l _ ~ ' + . s ) . " 7 - I 1 c : t _ a j _ + 3 ( . * ~~:?.A 2 x - : = _ e A 3 + - - t : _ s k t 1 * ' = . h e t a 1 e - _'li"l8A2'tSA2*L~CL~lST36•t2A2*tl*teA2•tsA2*theCale­ ~·tl*tSA)•te*thetae-180*t2A2•tl*cSA2•~e*thetas~l8Q*~2A2•ts•tl'lCA2 · - 1 8 *t2A2*ts•tJ*teA2·~~ctae- ~·r~As•thcta2e+lD•tJA3*teAS*the~a2e- A . 6 ' t h e t . ; ; e + 1 2 * t l " 2 * t . s A 6 ' t h e t a 2 e + 3 6 • t l A 2 • t 2 • r s • t e ' 3 • t h e t a i s - ~-: - y ~- w - - , 5 " 1 ~-! c - : : : - r : - _ ~~ J l r t 2 L ' t~ A 3 x - : _ s " " - : e k t ht.:~ 1 ' - - " ' - ~ , . · - - : - _ : : - - ; ~ c c ~ t~:et~a- s - > . - - - ~ * " - - , , + : : - : 1 1 e t _ -~ 1 ~ ~ 1 - - · 1 " " -::-~ 1 A ' 2 ; \ ~- 2 * 1 , _ s , , 4 x t ; ' . r ; ~ a l e - ~~ " + : : t • c~ - L ·t~·~·~cA4*~s*chcta2e+l20"t: ~·~?"3'' '~·thclJ: 2 ' j - J . - -~-~ n•~ , ' - ) ! - - ~ · + - " "'2*"~2*"~J?/''3*LheLr-le+24~~kt_ 4 "'·?*~ 3 kt-~c" 2 ~the; _ _ c - L ] " t :--:.~::t:: c - 1 _ 1 _ e - · - . , ' t ~; , j ; ( : : , -~~ ( - , ( , I ~ t - ~+-~ 2'"J*ts_~.thctC12+j68)~t~-""'''*'-'-- 1 " j X 1 ,J;~ 1 ,_~[~2L : 3 - - ; " - j - - t - _ : , ;I;~- ~-j E - - : t : : - ; - " -'~ ~ t _ : · " - 2 - ) . I 2 * t_~ ( ; A 4 A t h r _ - : I _ a 1 :__~ - - f c~ 0 - ) . - - " ( / : , , 3 r ~ ~ * - l : S ~ ; I ; t - ;~ e ( l : : : - - ; - :~ I + _ h ( ~ - · - ' 1 ~ I : r t : : ~;~ · 3 ; I ; 1 - l ; I ; t 2 ) ; - t h e t _ - t 2 . 4 u ~ 1 _ ~ K ~ _ : _ _ < ; - - : : : e A ? ; I ; t ~: e r : ~_it;- A - t - ( J · - ~ } _ _ : - : : ; ) . t _ r - - ' -~ > t h ~-~ t c~i l : : : - > i 2 ~j : ~ ' 1 _ _ ~ ~3 "~ t - L " t _ : 3 / ' , 2 r L r - - e L a e ; j _ 0 k : - _ 2 . 3 r L l " " l - J ~<1~~~e:_ci'----'- S · " ' L ~ ' ? * c ·~hctJ:e~lG8•t:'2*~sA2•te*t2*thctac- ; · : : : 2 • t c • L 2 * t r 1 e r c L · · 3 6 " t . l A . 7 ' t . s ' · 2 ' L e A 2 • t 2 * t h e t a l s - 'cs·te'2·t2'tJle~as lBO•tl'2•ts•t.c~2*L2*lhetae+36•tlA2*t2•te•tsA3' - , ( ; ( ' 1 + t _ 2 A~~ k t : ~" teA;~ *-~ t _ k ~ rl(~ t a 1 s T 2 4 8 7 < t l A 3 + - L s A : ? ' k L 2 ) . - L r l ( ; " ( _ c l s - ' • - s · 2 • t 2 • t f ' , e t a e - [ i · : ' 1 . l A 3 * t s A 2 • t e A 3 * t h e t a 2 e - : . , . , i•t~.'thr"tc=il':i1 ' * t l ' 3 * t e " 2 " t s ' 3 • t h e t a 2 e - ' - l R O • t 1 · ' 3 ' t e A 3 • 1 . 2 • t : h e r . a : ~/'I r_;A/:rt:L"t::het_as~-00"·~;.A3*t}_*te*ts/'2*t~leLa2_,~- / ) ; i - : _ - - ; ~ t ~ i j _ · · - : - - - - ' " : : - : = : "t-_het-_.~11· . 2 . 1 1 o : t ? " 3 " t _ : ~r_:-3-~ac- ' . . " : : : ~~ ~ - : : : 1 - - : ? ' - : - : : l ' ' r t ; - - - ' , " t _ , , - c e l · - c s - 1 t ~-_;- ( - - ( _ . ' " ' L ~ · " -~~ x L ~_-;" 2 k t . e k t . : 2 * L h e t a 2 _ s - : ) 4 ' ) • ' 2 . ' : 3 ' · i c • t . s · t 2 • l h e ' · : 3 - o·~:A3*~eA2Jt2·t~etae- ~. ' - ' ; ' K - ; - _ ~-, ~ T _ '~1 ~';I', C : " + - ' " ' ] * = - ! _ ; h :~ " ' ~~- 2 -~ L l~e j a l e - 4 5 7 L h 1 _ ? t _ a 2 e } - t s ' 3 r - ' : _ : * + - ( _ · - · L , ·-~1·-:=_i d 2 " t _ /'L~ 1 - _ l " " L e r j t ] k - c _ h _ r = : - : = _ e " : . l *t.e/'·c"~._s- ' o - ) - t : : l . ~ j - - " i / : , , ~- t t : - ; " 5 * i _ _ , < : A / 6 C ~ t _ ~ r , ~ + e " ~-- s L < : t 2 ~ t :-~ ' ? - ; - _ c : . . l c : + - i - { l _ j ~ I_~) t _ c : : " - : ; ; : : _ _ _ - = ; + - L 6T1~*tsA3•chet22s·'2"' : ; * t - .::-~ / s ! 1 : ' 4 r ~~ r - : c L a . 2 - . - t _ e j L 2 / , 3 - 3 k t . 2 * + - A s * 1 , :-~ e i - d / . s ~ : : : : _ . . , / . - ,~ f·tneta~s·tsT6c•- 2 ~ ~- c : : " 2 k t : r 1 e L a ? s * L . s A 4 T l 2 K L 2 / , ? . k - c e · " E " - t _ h e l _ , : : : - : . ~ ~:;- :·~~e~a2s't.s•t?-3- '''~eLa s * t e A ? ' t 7 ' 3 r i O * c s A S * t h e t a 2 s x t 2 A 3 + 3 * t s A 7 • t h e L a 2 s * t 2 l / : l a D • l s ·~2 3 - l * t l * t e ' 7 - 3 • t s A 7 * t 2 - ·-?"l~l2·L2A2*ts 6 i J O • t e A 5 ' t 2 " J - 6 Q • t s n 3 * t l n 2 * t 2 A 3 - 't~·~e"CT6Q*ts":;•t]•~2"3+54*tlA2*~2*tSA5- 1 [S~5~54*Ll*\ A ) • t e n 5 + 6 D • t e A J k t l A 2 * t 2 A 3 - 6 0 * t e A 4 * t l * t 2 A 3 - · r s ~·c2•tlA3~3*rsA2*~2•teA5-80*tsA2*teA3*t2A3-3*tsA6*tc•~2- ~·~ "t2~3+6D•t?A2'tccA3*~1A3- t~ ~-~ · " ~· l 1 + I : 7 . - " , 2 * T 5•~CT3*te"6•tsrt2+10'tCA4•ts't2A3- ' * t ' ) - - 1 - ~~ C · * 1 ' ' " ~) K ~- c : , l " + - ? A - : : : ) T ( ) * j 2 * t - c / ' , 4 * t 1 A 3 - ( - - ) 0 ~ -t.-~ 2 A 2 + t e 3 . . . _ . - : - - _ - 1 r , j - 7 2 t - .~~ / , ~ + : . = · ) - - = e / ' , - ) ~ · : _ ? * t s A 4 - 4 5 * t . s A 3 * t _ 2 ' k L e A 4 + 6 c • k t 2 A 2 7 < - j _ : : 3 . " , / r t C : / ' 4 - ' t ' t · - L 1 0 1 r S ) ' t " , _ * t 2 A 3 - 9 0 * t l A 2 * l : 2 * t e ' t : : ; • " 4 - 2 J - - ) _ _ ) + _ . · - : : - - . *·t~ *te_~"3~t2A3+90*~1*t2A~~"'-L * t : : : - : ; ~~-1/ " ' > ~; . ' t _ , _ : : : , X " + - . 2 1 ; • :~·-"t•:; 1-1~~:~<--t f t *teA'3k'~1A.3- ) ? , _ ( ' ' . - - . : ( ) ' t : - ~) ' ' ' 2 · k t . c ; { - t . t - = : ; A . ? f , : : 1 A ~~ ' ' I c " ' t . 2 1 ) ' • • • · " - , ! • 1 - l 2 ' ~ S A ' l . l r 2 I : : _ ( ) 1 t l " J * I .-~ 1 ! - ' - , ' ] - • - ' - 1 . i . j . , - + : : 1 " ' - = c - ~ . , · - - c - ( ' j - 2 _ ( ) ~ \ 1 3 l ; - - = : " f \:~ r , 4 + ] _ . : ; / . - 3 ' - ; : - c ; ' } ' t _ . ' - : ; c . + - _ r 1 ~· ' ' -~- 1 ) " ' - : _ l _ ' t G - 0 ' t 1 A 2 ' t s A 2 * t e A 4 - ( ; " + - s - 1 - J * 1 J k t F : - ; " ) " " : - s ~~ ~ L : : _ * t e / ' , 5 k t s A 2 , : : _ 2 k t s " t 1 A ;~ · k t c A ~ ; - • · · , ' t e 2 ' : : 2 ' 1 ' ; : · • e · s - l 8 * L . ' 2 * t s A 5 * I h e t a 2 e ' 6 ( ) ' t e " / ' t / • 2 * t . · ; ' t h e · a · - . , . - , . , : : : : - : ; " • t s ' t t e : a c - 1 2 • t e A * t 2 A 2 * t s A 2 * t h e t a l s - 3*the::a/e+teA6·::s*theta2e+48*teA4*t2A2*t~etals+tcA7·t~ela?e-::.2 ? • : s 2':~etale-/ ' t 2 A 3 • t s ' t e A 2 * : h e t a l e + 1 2 ' t 2 A 2 ' t s A 3 ' t e * t h e t a l e - , · · · • ; • • - - : e ' : : - - - , e t a l E ' C - 2 ' t l A 3 ' " t s A 3 ' t h P t a l s - 1 2 * t l A 2 ' L e A ; ; * : h e t a l e - ·~ h ~ . 2 c · - 8 0 · k t 2 A ] * 1 .~ e , . t s ,~. - : h e - c a e + 1 8 + - : . _ : _ 2 A 2 k t e A S x t h c t a 2 s - L h e t 2 2 s * + = - _ . s · " 7 - - , - 1 2 : : - ) r :~) · ' ; f : _ S * t e A L J - s·-sA3*te*t2A3T40·t~e:a2s*:s*teA3't2A3+30*theta2s*t?A2'tc•tsA~+2 c c · L~ ' : S ' 't2AJ-:!_8*t:::_et_c";2S't2n/'tSA5+::_)"-_Cta2S*t.S~'6•te­ • t c 6 • : s + B O w t i A J * t s A 2 * : h e t a s - 2 0 * t : r J * t s A 4 ' t h e t a 2 e - - ~ " " : : : ~ h 2 ~- t ~-~Pta T D , . . t - 3 k t e A 4 k t h c t a 2 e + 6 C + - t 1 A 3 * t e A 3 x t h c tal:::::;+~ ( ) * L - ; · " 3 x · _ e j 1 : - ~ r~~ ~: t l , , 3 " ' · . . ~ · " 2 ; < . . L . - . e t 8 - G C k : : = _ " 3 * t . s A j · k t h e 1 . _ a : 2 . e + ~3 ~ " " ' t - A 3 . . . . t c < ' 2 - , . . ~-he I _ a s - } · - ' - ~~ U < ; t j"'L2A2-.--t~tet e - l 8 x t l A2*-::-_P-A~*'L~-et 3 r 7 :~c~ta·: 1 ' ' r t _ : . _ / \ 2 " L 2 A 3 k t h e L a s - f 8 X t 1 A 2 k L e / \ ~~ k : _ h e e - e - 4 Atl'~·2*~s" :--~ ~ T ' t 1 · - K t h e r a J S + 4 T 1 , _ 1 • " ' > 3 f - = S , 0 , _ j l f _ n r t ~-:_p t c _ 1 : 2 e _ _ _ ( _ _ _ : ; y - 8 : _ . , , _ . ~·t ·-~·th"CI. - s J } ; t 1 / \ j * t S J . - - : - . ~; X t - l - l ~ t c 1 c - . ~ A~·rr·t~ 0 IJ::.s+ / 0 · ! A 3 ' t 2 2 · t s ' t h : t a : s i l 2 0 ' 1 1 " 3 · c A 2 · t s • t - 2 1 ~·t A 2 ' t c ' t · ~:~- 0 * t ' A 3 ' t e ' 2 * t s ' t h e t a l s - ·:s'2'~~e-~::. +2lJ"L:A3'teA/':s•thctalc16C*::A2'Le'2*~s·- e t A , - = < . * - _ - , j - r . a l t ' ' l L • t l A 2 . , t s " 3 · - - c e *-~he~ a 1 s - · t s ' 2 • r r * t h e t s - i 2 ''Ll"2't2AJ•te'thetale~l20*:1'2•t2'3*Le't~etals­ ·:sA4•tc•thcta2e~J • : : : . A 2 ' t e A 4 * t s * t h e t a 2 e - 1 2 0 * t : A 2 * r s * t 2 ' 3 w L h e t a 1 2 · t s ' t 2 ' 3 • : h e t a 1 e - L'trA~*:tetals-l5*:"C'J•:s'4*theta2e+20*t2'3*tsA2*te'theLals­ s'3•te'theta:s+80*tcA2*t2A3*:hetae+80*t2'3*ts"2*ttetac+l2*te'3*~ :a:e-20'te'3*t?A3't~eta1e- A 1 ' t h c t a l e + 6 *t2'3*~s'3*thetale-::.2•teA3*t2'2*ts•LheLa:s · ~~c:c·"ec'3·t)'2·--:het s - - . j T / ~- ( r ; . n - 2 - 1 . I 2 3 * · - / ' , 3 " " t h e t 1 s - 6 0 * - c 2 A 2 · k t . s / ' , ~ ; < . j _ ; - 1 e ! _ _ a e - l " t _ l l c _ _ t a 2 . ~~- ~. ~::~ " L , ; : 3 * L s I ' ? * - : f 1 e ~as- , • 1 - ~ . . t : _ : : ; " t r~ E_~ r : : a : = - - ; : ( , ~~ k : : : ' - - " L K : -• • s A 2 " ' l e , . _ - L ~~etas- ~ ~·le·~~e: e+lS·tsA3*tc'1*t~ela2s-~s~:sA4'thcta2 y t e A J - · _ l / 2 ~ t~ n ( ; t c l e · - 6 . , * ' ' = - s " 3 * L ~ A ? . " t f : e t a ( - ; - + - L : : - 3 A 5 * t e " 2 " . t I 1 e ~ / . : = ; - - - : - 6 0 + t ; - ; . , 2 . _ _ ~ 1 ; - , , _ _ ; · ? + 1 / * L ~-=-; 2 ° , ~ - " , 2 * e 2 ' * " : . ~~~:-_a l . - : - ) - = : _ _ 2 k t : : _ ; A / K t 1 A 2 k L e 2 " - · , _ h r-:=~ ~ 1 r , _ ; - ·-~~2·-:·2 ~2'ts·~hcta: *t0A3ytlA2-60*the~ ·~·t2.'2- ~· • 1 ~ L _ / : " L ' · - : e : _ le-:~,,,, i"~JlCT_r__;,2s- ' : - : { - ; - - ; 2 " t f : ( ; : _ a 2 ~'· + ( ) : , : y l _ 2 A / } - \ _ S ' " ' 3 f , t h f ? t _ : _ a S ' 2 ( ) k t : 2 , . , ] ' K ~ : = = J r t f : , , / f t ' I . t : _ ' ; - ~~' , ' : 2 , - . , : 0 e / ' . J . , - t _ : _ · , e ~.::a:__ I ~: ~ ) . ~~ . = ; . . . : : _ : : " . t _ ~ / • L 2 ~ · = - ? . , . , s i"'L~-lU _ : : : ; • l 2 J~~?·t2A2'ts'6Tl0'"le'5•L2AJ- . = - - • ' , 2 - - , k ' J / ' : 3 - ~) / : 7 < f - l A 2 j t K " = A 5 + - 6 c ) : ~~ S A ~ * I ] _ - J ; · ~ 2 A 3 + ~ 4 ; \ t l " . 2 k - - : _ 2 ) . · t : : 3 , " , 5 - -~·tsA5+54·~1*L2A?•teA5+60*teA3*tl'2*l2A3-60*t nq•~l*:?'<­ 't2·~lAJ~Jk~s'2k~2·~e'5-80*tsA2*tcA * t 2 ' 3 - 3 * t s A G • t c • t 2 - 't2A3~GO*t2A2*t A ] ' : l A J - e ~+:?·:2A2•ts'5'te~3*teA6'ts•t21lC*teA4•Ls"t2A3- 7 3 ~·~)+ O·~c ? * t s ~'t2~3+6C't?*t~"4'tl"3-6C•t2"?'t "3·~:-~- H 4+L~•:e'3't2'~s"4-4 * t s " 3 * t 2 * t e " 4 + 6 0 * t ? " 2 * : ' 2 w t e " 4 - ' , 1 1 ~ = - - : 2 * t - : : 3 :~ ~ t - 1 A t c _ . _ - = 2 A 3 - 9 0 k t _ _ 1 A . ? * L * t c k t s I ' ~ T r - = - - : ) , . t - : - ; / ' 3 - + - : ~) ~,! . - r - " " : _ : : : _ _ " t p ~ 3 r : : - _ / / , : 3 i ( ) k t 2 _ * t . 2 A 2 k ~- e r - - : : :~ ' " ' 4 f ~- 2 * i - ~A . ; : k + - p " A - : - - : A'~ I -~C· 2 · r s • - e 3*~1-~- ) > 1 s " j _ S / : 2 < e - : : - . 2 _ A ~~ _ • · - . 1 . 1 . )~c;' ~~- ~- ' L , I U ~~ " t .~~ ' ' ~ • : 2 t ~ _ / · 2 ' 3 A t ; 3 r . 6 k t A l _ e - ' l ~80·-1"3•Ls"7*Le"3-l •:~"3'ts*:e"4+JO*ll"J*:c'~ • I ' o ~-~•ts"3t:2·~1"?· c " 6 - 6 D • t l " 2 * - s " 2 • : e " 4 - 3 • t l • r r 2 ' L s 5-3':l·~e"5':s"2tl2*ts*tl')·Le"5); ·~-.--~ c_.+'~·:~~~t?'3"t.-::·'d_k::LcL2ls-l-2-"'L f , · t _ e t j 0 L - ' ' , _ f t ' '-'.~---I f - L~"t ctcJ1:::;-;~r,:. ~-:.: * 1 _ 8 • ) " + - r 1 e ~ l C . L , , -~ 0 : _ s K t _ · r : 1 e : _ f0*:2A3*ts't~etae+J'l•L?·Ls"3*:h J e - 9 r t s i ' S " t / * : : ' · e - : a 2 s - 2 * ~e-aJe-lC•-s"?·- "l*t~cta2c- · - : : c : a 2 c c t - • t _ 2 ' t e 4 • :___~let a 1 e + J : J *teA~; * t s " 4 x t h c ~a 2 c t 1 5 • : _ s A . - ; • t 2 • t e * L h ·-2~3·~s•te'theta.s+6•t2*ts"3*te*thctalc+30*tl"3'~e*LsA/'*t~eta2e­ ; . t c ; * t c ; x t h e t a l s - ,.~ " 2 ' t h c t a 2 c t 6 'tJ."3*t2xtsxt~etale+20*tlA3*Le*Ls*thetale+l0'tl <·~ La)e-2:J*~1'3*ts~2xt~e~als-60•tlA3•ts*thctac•60'tlA3*~s*thetas- • r _ 2 * t h e t a s - d j O * t : : _ A ~*te * t h e t a s - e · .he~ae+28*t1A3'teA2*thetaJ.e+J.O*tsAJxtheta2sxt2A3+60•tlA3*ts*t2 ,_c-~8·tl~J•te't2x:he:als+l20xtlA3*t2*thetae+6*ts"2'teA?*t?*ttJe:a1e- !1~-:a.?s * -c_r~: K~? "~~-]·; k t sA;~ ' - * : t c * t 2 k : - _ h c - c a e t - 3 0 * - : s " ' 2 x L c k t . 2 -kt~"~Qtas- ~· ' t 2 • r n c ; : - c : : s - l · L~' ; - _ 2 * t e A 4 * : : h E ' ! t : a 2 s - , - 3 ( ) * t . s * : _ e A 2 ' t 2 * t h e · : s - ' . ' * I _ · _ , _ : : : e - G C 1 ~ ~- * +_~-:~ " * ~e* - : r : e " : a 1 e - ( , - ~/~--~;-~:::__ ~ A t - ' " 2 * t - _ h e t a = - s + 9 * t = - * t s A s - K t h e t u . 2 e - ; · L . c t a e t l _ ; ; * t l ' t / ' . 3 • t h e t a s + : J ( J k t l * t e A 3 * t h e t a e - • • t : ' t .'~ _ _ ; _ _ _ ~ · : · · • t l " - t - ~-· " J + L h e t ; 3 e - he~ s + C · t l ' t s 4':cetaJs+2'teA6·t~eta2e+3D·ts•thcta2s'tcA2*t2A : : * - e * · _ h e ; _ - - - ' / : , . t~ ( . c - : a 2 : = - ' - - 1 - 2 1 j J . - ' : = : - : , , z ] " ' ( t 1 ' - = I~(:;-= a l 0 1 - 2 · 1 ; - t : : : - 3 A s * t c r -:=.he::.~~: . 2 : - - ; - • t l ·~;.~tal 6 ' t c 3 • t l ' t s ' t h c t a : : . s - 6 * t c A J x t s * t ' . • t h c t a l c - s · r h e l a 2 s - 2 •teA4·~:·L~eLals+10*Le"4*Ls"2*lheta/'s­ J · · . I l e : a 1 e - c t a 2 c 6 *tl*t2A3·tc*thctalc~6C*ti*t2A3*te*thetals­ c " 4 * : e * t h e : ) e + l _ 5 * l ] ' : e A 4 x t s * t h e t a 2 e - 6 0 * t l * t s • t 2 A J * l h e l a l s - ·:s':2AJ*thetalc-6•t2*~e*tsAJ*thetals- ~"-=--~A2* l e + 3 8 * t 2 * t e A 3 * t h e t a s - ·LeA3*thetae+6*tl·~e"2*tsA2*thetals­ e A 2 ' t s A 2 • t h e t a : e T . 3 0 ' t l * t s A 2 * t e * t h e t a e - 3 0 * t l * t s A 2 * t e x t h e t a s - · " 1 ·reA2'the~as+3D•t · t l * t e A 2 * t n e t a e - 2 * t h e t a 2 s * t e A 6 - ~~ ' r i l s ~c·r~ ~·:sA2'thc~ : : . c 2 * t c A S x t s x t h c t a 2 i , ; , : ; + · ; : ; • , t e , • ~ ' e t a ? . c 3 - 1 : ] • t he~ a 2 s • t e A 3 + ~ 2 · ' 3 - , : : fli~'t d . S t } t~ c - ~~ )~ - : : - - ' ' t 2 " 3 ' t c * t h c t a c o 2 d * t 2 * t c A 4 * t h c t a i s ) / ( J H ( ) * c s A 2 w t l 2 ' t e • t ? . - " C • c s ' S * t 2 ' 3 f ! 2 x t 2 A 2 • t s A 6 i l : J • t c " 5 " : 2 ] - 2•~2 3-~~·tl"2•t2"teA516Q•~sA4*tl't2"Jt54*tl_A2*t2*tsA~­ " · s"~"S~··i·~2'2' A~·E * t AJ•t:"2*t2A3-60*teA4•t1*~2AJ­ ,,3•3'ts•2*t2·t•:A5-R:J•tsA2xtcA3*~2A3-3*ts 6 • t e * t 2 - .·~e*t2A3168'~2A · t s A 3 * t l 3 - s'~eA5+12'L?A?.·L "5*te+3*~eA6*ts•t2+J:JxteA4*Ls*'2"3- '~-:, · - I _ 2 T " 2 • t s 3 • t 2 A 3 c 6 0 * t 2 * t e A 4 * t l A 3 - 6 0 ' t 2 A 2 ' t c A J x : : A 3 - 2 • t s " 4 ~~·tcA3't2·:s"1-45•ts"3*t2•teA4+60·~2"2*tsA?•teA4- · L e A 4 - 1 ? . 0 ' L ' 3 ' ' - 'ce*t?.A3-90'~J"?.*t2'te'tsA4- 7 4 ~, i 2 ' j ' t : : : . ; ;~<.-. A L e j " t : ) ' ' j + ~ k t 1 k ~ 2 A . i " t r - : _ " i - ~ , - . /~ - + - L ~~ J ~ + : : :::~ r - , ,~ t i K~~~ k · t : : J f t p·~jA < ~ " . 3 - ~"~_2_ ~~ ·~~')•t ·tc'?·~l' l ' t ) ) 7 t - j · - ; · t l - 1 : 2 * t 6 • t 1 2+10*~1 J·ts'~- + · 4 , - ' j _ ' ) . I : '~ / . + ( ) c~ > C t s r , L 1 r : e _ ; " t : : = - "'J*-:~:;~'' * : l ' 2 T J * t s A 6 ' t l * L e - •tc'3-10"tl'3•ts*te'4~10*t1'3•tp':s· - < • ~- p > * . ~i2*tl'7•tc'E-60•tl'2*:s'2*:e·4- 6 * - s 3 · t l • t e ' 2 " L s S - 3 ' t l * . e A S • t s ' 2 + 1 2 * t s * t l ' 2 • t c • l f - : - ; " ' ' 3 A t ~- a l s - ) 4 * t l · t s ' 2 * : h c . s - I I • _ ' t _ _ , , - - ( • ' . - . " . - I l _ ( ' ~ th~ l _ -~~\ ' 1 : h c t ~:c- I H - 1 / - - . . - j A t ~;_t-2~ -~ : - _ ; - ~ - ~ _ e : _ a 2 , , - G " - : : - ; " · L _ " - t · ) ; t " : - - : e t ~h 0 ) t24·~ ' 2 ' : 2 • : h e t a s - t , : : . 1 ~: ~::; - - j - • , - ) . , T ~::.· / ' ~~ l - - ~ 2 c + · , _ [~ ; , : - . . s / ' 7 / ( t - , - , ) * t ( : · r t _ ' n e t d ~; + r ; + · j _ 1 : 3 - ~ f , l I _ l , - ( _ _ : _ ' F c c : , • : l s - 3 + ' . c + r _ l ; - - 2 A / - , - t c~ ~- r~ r : - - : : : - : : - : d J I e - J·,~·~)·t t ~s+l7•t2 2 ' t s • t ·t~Pl ! s - s ~ - f t ~: · ' : :--~ 1 ; \ * - - ' / " ' L * ' _ : : ; -~ - P _ , ? " ' t j P t c _ / s - l * : : 2 ! ' . 2 k t : _ : 3 ~ " · _ _ ( ? " t > > - - : - - : t L s - 1 . ( . _ · _ . : : : _ : t r _ , _ , ~ t s ) } - : - : ~- e : _ - L 4 7 - - : : : c ~ t 5 * t 2 * t r - 1 e - c a \ " ? - . - _ " ' - L c - = : t a(=>-;.-~ " ' - : . . P t . G e c : : a 2 e - : : A 1 s ~ f : J < : : . ) / ' , : 2 . . ' k i · : - l s t a : 2 s - ·~~Ptas+36·t7 2·ts•thctae-l2*t2"2*ts'2*thetalst8·:e'2*ts'3*L~et ~·rhcla/e- 4 • r 2 ~-teA2*thetals+6't2*~e'4*rhera2s- ~ , , , ' l - 1 " ~ : - : :~- c•t::ctas+36*L7'2'te*thctae+l8*te'3*t2*ttetals+24*te'/•t2*t_~etas­ ~e-1P·ts• ' t 2 * - h e t lc-2~*t2"2*tsA2*L~ctale- • [f)(~' c : : l ' " _1A2•t::'*r:;*L:1etc~l + . l 6 • r l * l / 2 · t e • : c o :-.a~:t•- ' " · : h c : - ; - c _ ) " ' t ~~-" t : . : . J " " L " ' L ) . , . t ~e t . ~;. 1 e - L e - : : : ~-~ ~ ~;, (-~ . . . [ , - ) "'~ • t _ · , e t 1 8 T ( ) { ~ C ? ' • . 3 J c t ~ . , _ ~- 1 · - --~ ~-; ::~· / ' , : - : : " + - h > ? · - :._~) t.e"'~::c_;-a.?sx:-_s"L- . . , . t _ r : ; A 2 k - : - J.?~~ . . , . t . s ' ' J - . . 1 ~-\ · , l . . /~t_:--.· ~):.:_)" j ; ] k . . 1 . , - - : : : t : · -~ 2 " ; ' ; l s ) . ; t l · : c · l L c ' t c : J · 2 ' L r e t a 1 s - 1 8 · t l ' r e ' • t b e t l s - · : · - t -.~:_=:..;...> · r - [ " t _ • r I t e ; t . c , c - - : l l * t _ • t 3 • r n e t _ · c : · J > ? . i . 3 0 • t s i ( " t _ _ - , t ; t _ ' ; - - r ~~ * +-h_r~ t _ -~- ~- ~~~a]c-6•tl·to ~s'2·L~etale+74•tl*ts•to•t_r.:etae­ • • ' r h e · st_2·ts·t:~;•c · t h e t a l c - ;.tc*t~eta_st~6·t_~2·t ' t h e t a s - s•~nctae+2~*L1'2'tc 2 * t h c t a : s + l 2 * t ' ' 2 ' t e A 2 ' t h e l a l e ) / ( l 8 0 • t _ s ' 2 * t ] - 3 - ) • · . 1 ' r c · - : - J · r 7 •t2-lO•csA5*t2~3~12*t2"2•ts~6ilO•te'5*L2'3- ~ ~ ! _ _ 1 ~ 2 + - 2 * L ~~ h 5 I 6 8 * - : : : : : ) A 4 k L 1 k t 2 " 3 + ~ L i k t l / " / * : : _ 2 · k L s . : : L ; k - : = : 1 * ~-- 2 ·tc'~+EO•tp"J*rl'2*:2'3-60*teA~·Ll ' t 2 " 3 - ~·t2'L1"3i3"ls'2*t2't~A5-PO·tsA2*:eA3*~2A3-Jxt ' 6 * t e * t 2 - . . J • : • ' : • 1 : : ) ~- 3 I ( i • t ; } A • : ' - ' , , 3 • . 1 A 3 - t~ 2*:s•teA5+:7·t2"2*ts'5*Le+3*te"6*ts*t2~10*teA4*ts*t2A3- ·ts"5•t2+80•te~2•:s•J•t2'3+60*t2*te•4*tlA3-60*t2A2•tc•3*tlA3- 2'~s 4 + 4 5 * t e ' 3 · : 2 * L s " 4 - 4 5 · t '3*L2"teA4+6C·~?"2•tsA2'te"4- . · ' : ' ' • t e ~ 4 - 1 2 C i ' t • ) • c : 1 • t e • 1 2 A 3 - C J 0 • t : : _ A 2 • L 2 * t e • l • 1 - . . t I " / , : 2 ~- - - ) / - ~~ - - 1 ) ~ . ; - . s ) . - t _ - J t R A ] * ~ 2 A 3 - 0 * - t : : ' . 1 f t 2 h 2 + - t . - : - _ 5 A , 4 j : _ 2 C ) * t : - 3 / ' , ' 3 * ~ c J . . . : - ·-~ ' 7 + - 1 : 2 . . . t _ ~ " L · t (~' - . : : - = - . 2 , . _ _ , : . . . ' 2 " " t ~. " " , . : _ e / · ) * . - . , . l ' · 3 - ' - . _ j . ~ · -~ u J j t 2 / ' , " t ~J - l . t e " ' 2 " ' L ~ , - . ~ , I j , ~ • · . : 1 - l > • - : - ; . ( , • t ; _ • . 2 + l 0 * - l , - _ , • t . ' i ' . > · : 1 : , · t o ; ~ · t . I · t · ( : ( ' 1 J . ' / - L ] A ; - , ) ~ : - = 2 - - J J . t : , : : ; • t · • t e - 7 5 , _ ' ' : ~ ; - - - - ~ * 1 _ .~3 ' - e 1 + 1 · + - } - r , /+l~~L~~·i P r o w a m A . 9 : P r o g r a m t o c a l c u l a t e Z M P o f l o w e r b o d y " -~ l ( ' I L J I = - J L ' ) · : ' J > _ - ; t l,~_h-?ta/,th,?-r~a~), :~f1cr ' c 1 ' q 2 ' .(~' - t , : - 1 ' ' l ' 1 3 -~ n l - ~: t _ : - 1 , - , 1 : . h e t a 2 r ; / * : - 3 ~L [ ; i : - ) _ l - ~--. ~ 1 _ , 2 J ) : : ~ ~; : j ' r , _ ; (~ "~ - ( ~-~ _ - : _ / 3 6 :~ ,-~, , - 1 ( ' l ln~:p:/-)6~~ - - I t _ - " ' > ? t _ - l , - - : ) + t h : ; ' _ ) ) + J . 1 ~ ( r : i I 3 6 ) ; · : _ : ; f . .~ (~.het r o • + . C J ' L l ' s i : : ( p i - ( t f l e t S - : : u e l a c j : ) _ } _ " , (:~;)~~· J - - - : - C . ( : ( ) ( ) A - . ' ' 4 ; • c J ::~ ( , (~ " t - ·t.'2+.~12:J't : J ; C ) l - : " ' i - , , - ) / - 2 2 • t . n J - / . Q J 6 8 5 e - J * t . A 4 ; L ( 1 · u : C : : ~ 1 - - - u • U 8 6 - • - ~~ 2 + . : J l l 6 / I · 1 * t . A 3 ; ?~JL•t+0.32CJ8't.-/-.0996•t.A3+.0l*t.n4; ~~5~+.6 :6*::-.)988·~.n2+.0~*~.n); 'L-0.10~6•t. 2~.032~*t.n3-3.2/6~e-3*t.A4; . - r ; . 7 l U · ~. ~ . : : C J ' 2 • t . A 2 - . : _ . 2 9 0 6 e - 2 • L . · 3 ; ) c J L i t - ; - ; \ - J ~ ~~ · , T : ' ! • 1 . • r 2 i · · 1 . ' r ( ) ! l t I.~, . * ( q 1 3 . A / ) . * s i n ( q 4 ) - c r . • ; · o ( q 3 q 4 : : . ' ' . ' ::;lq~)-Ll.•qlJ.n2-'-'l.' : : . 4 . ' 2 - : J L ' _,.~T,l.·:11. / ; ~ .~) - t - . . • ' ) . ' L l . ' ) . L r " ' .~ . ~- -~-.-k~~-k(ql1+ql2:'.~'2; \·~"- 1+q~L); - : . / ' , / ; 2 ; 1 ' 2 . '~ l ? + , _ ; 2 : ' 1 . ' t ~1; I l . ' Y - . . K ( b - ) -~1 c i . * y l ; > : 2 . ;~; (d-,~~ - r n = - _ . " c . * y 2 ; II':L_;.A:-~J.J ( f - c _ ; - r t ' . * ' / 3 ; ' 1 +~y: l ] . A - X L ; . -~ ( k - C J ) - n L . ' * j . * y ~ ; , - , - ( ; 1 ) ! / • · " " . - " ' I 1 , r 1 2 7 6 P r o g r a m A . / 0 : P r o g r a m t o c a l c u l a t e Z M P a t 5 d e g r e e s ~, J -~ _ , L l , _ _ : - : ; x , y , t : 1 0 ~- c 1 , e ~ a 2 , t ~: e t _ a : ; , - - _ _ P a t~ , t _ _ > ? t , - l - - - ; , · - : I 1 , _ 1 ] ' ] -~ J 1 L l l J 1 : T l C ) r ' J 1 X r Y } ' I i _ I 1 , , : : l n . : : : , , - ) • n s : 'sin,,r:he::h~- t y - e ' , * s i n ( p i - ( t h e t 1-t:_~_et : . _ - L 2 - k c : c s ( p i - : t : - . > ? t o - ~t~:·;:-:·~a2)); c : , > . - T _ 2 - r s : :--~ ( ; ) i - ( t h e t a l ' t h e t a 2 ) ) ; r c o s ( L r : . e t a b ) + 0 . 1 ~) k c o s ( p i I J 6 ) ; ? ' t n ( t h e t a 6 ) + 8 . c ; 5 * s : : _ n ( p : ; , / 3 6 ) ; J • ) , 1 • · = u s ( p i - ( t : , , c t : c U ' U \ e : . a 6 ) ) + 0 . l T c 0 s ( D i I 3 6 ) ; ' ' ' ' i n ( l ? i I 3 6 ) + L;~ • s i : - ; ( L h e t a 6 ) + C . 3 ' L 1 * s i n ( - ( t ' c e t a 5 + t h e t a 6 : ) ; . l :~ ' c : o s ( D i I 3 6 ) - s x + 0 . S * L 5 ' c o s (theta~ - t h e t a 6 -~he t a 7 ) ) ~- s y + D . S * L S * ~;i:-: (t:~eta5+theta6-U:eta7); 4C6't+.2034*L.n2-0.C67•t.n3~0.006*~.n4; -_;~ G+0.~06 " I - . 1 8 6 · - . A 2 + . 24·~-A3; ll~ '+-:~:·.c·-,9LJl_rt__."~- . C 2 2 " - - .~3+~~.':1C6S~e-3-At."'4; , , r - = - . r-~ g ~~ , - ) * - r - - , . - · - : - r - . ( J . L -~ : j ( _ ; ' . ' ) r · t . ' ) . o · = . 6 2 1 4 ' t . · · 3 ; ' ~) ' ) A~ ~ ~- • ~) / 1 -~ T _ A -~ ; , f ' 3 - -~. 2 2 r - ; ': 1 \::t~-~~ I + - / ' , " ' • . . ' 4 ; '~ i ! ' ' 3 · L ~4112.• , q l 3 . 2 ) .'Slf\(Cj'J)-q.·::_~o ( q _ j + q 4 ) _ : - ; l c . . 'ql-~­ ' - 1 1 : · , ~ ( l-~- \ • q l · + ( h - g \ , , - l : : . ) . - 1 \ : , '-~ I~·~{]) u • s . . * L / . )~=-."'2/17; . A 2 / 1 2 ; l .~ ' • j . · ' 1 / , _ ] L . . . . ( r - - - . ! . : _ c ; l : f ! ) - L J . ' q ' . 3 . ~2<_1 j - 1 ·~-r l j ; · ; '~ ( q l : 1 - 8 . 5 . k L l . + - : = t = - = - . A 2 ; s ( q l ) f [ ; . . * L l . * q ? / ; - . , - I , . * ~ 2 . ' ( r _ J : : _ J + q : : _ 2 j . • A 2 ; . * _ 2 . • ( q 7 1 · · q 2 / j ; 2 ; 2~~."'2; ~ : l • r · • "L~-: 1 3 · · : r l 4 l c l 7 J • ~-) , A f _ 1 ~ • . , - ( q 2 3 + Q 2 ! } + C ? . 2 ) ) - : r _ - ' · ' l . · y L ; : ( ] - · - · - : r ~ \ . / . ) ' k . ' j · - l ' r \ / . 5 ; ~ y 4 ; - ; - r r l A y~~. ~ ~: . v , (~-q, -a.~l. A \l~). f y~; -~-g) ; I ~q); . k - q ) ; ' I \ i 6 - q ; ' - ; : : : ; - - 1 - - \ r ~.:-) ~ ~ I ( u t ' J - - 1 - - ' d 1 r ,~-t-v 6 6 ) 4 7 7 P r o g r a m A . l l : P r o g r a m t o c a l c u l a t e Z M P a t 1 0 d e g r e e s f ._-J/~t ~~~y I 1 I·~.? f , q ( , q / , q l , c / ',~t- ,In/,r1_~,r:L/±,T 1 _~.,q . '~ l j _ : _ ; - f ( p ~ / = - :~ ' '_;l~~( ~-,~,~11'~)', 'L2·=cs:p~-:ql q 2 ' : : : - : , - lql+q~)'• ~·;,·· . j j ' + , ; ; : ( u i ; - , 8 ) ) ~ : : ; - j ~ c_(~ : : -~ C J • 4 ~[!( / 1 8 ) ; _ , • I~ l ' • ] . : : + C : . 1 - " c c s ( p i / 1 8 ) ; , ; . . (~),/1 )t~L'Sl:(q3)+0.3•Ll•sl:l(pL-(q4rqJ)); I J ·cos(pi/l8)+sx~O.:';•L~*cos(q4+q3-q·;;; ~_)c)+sy-~o. 5KL~*5in ( q 4 + q 3 - c ( l ~; 9 8*~~0.238~*~.A2-0.0688*t.A3+6.5e-3*t.A4; 8+0.~77*t-0.2Q6~*l."2+2.6e-2*t.A3; - - . r : r 1 f J • ' = J 0 . 0 2 1 ; l * t . ' 2 - 7 . 2 e - 3 * t . A 3 + 6 . 8 0 6 5 e - 4 * t . A 4 ; - ' n 1 0 E · 0 . 0 4 8 6 * t - 2 . 1 6 e - 2 * L . A 2 + 2 . 7 2 2 6 e - 3 * t . " 3 ; , , _ r l ) r ) t , * t + 0 . 2 4 3 C ) ' t . A 2 - . 0 F J 4 • t . A 3 + 7 e - 3 * t . A 4 ; ' · - • 1 - . 4 ~ 1 J ' t - . ) I l 2 ' t : . A 2 . , . 2 . 8 e - 2 * t . A 3 ; ~) i ! ~ * t t - • ' / ~' q " ' t • A L~- • :~. 2 2 * - = • A 3 - 2 • : _ l . 6 1 ( ? - 3 k t 0 A 4 ; 9~2-.l~lR't-.06fJ*~.'2+ . 1 6 4 4 c - J • t . " ; ( q 4 ) t1.'•j/j-~l.•c2 t~2.*(cl3.A2) . k s i ( c 4 ) - g . * c c s ( u 3 · c 4 ) 3 . ' s i ' · ( ' 1 4 ) - - 2 . I q 1 ; . A 2 . • c C ) s ( q I ; ) - L l . • q 1 3 . A 2 - L : . • q l L l • A 2 - ' 1 = - -~ . * 1 ~ - q . . s i _ :-~ : q 3 q 4 ) ; : - ' : + ( J . - q ) . " _ I t ( : _ ) - 0 . c ) • • L i . • q l l . " 2 ; ; - - - ; - - • ' . . _ ; X I J ~ ; _ , J + - u . "c,~- ) + , _ . S . ~ L L . " : : q 2 l +c~2 2 ) ; - 0 . j 0 . .~2/12; . ' 2 / 1 2 ; ( q 1 3 + q l f J ) • ~ 2 . ' ~-1 ; * ( q 2 3 + q 2 4 ) . * : : : . 1 ; ' ' 2 ; 0 2 ; 2 ; r q 7 l - ! · . * s , _ 1 1 ( c . - l - : : . ~) . , 1 s . * : q 1 3 + q 1 c ; r - q 1 7 l . A 2 ; . . · o ' i n ( ) + l . * C C J s ( q 7 ) + C ; • 5 . ' L 5 . * ( q 2 3 + q 2 4 + q 2 7 ) ; • - 1 , J - 1 n J - : v : 1 . * t b - c j - r r t ~ . " " a . ~- y : ; " > - : > ·:._1-~J - n . " - c . > k v 2 ; *'--~ ~=-'J . - J e . + - } < 3 ; I < ) ' . , . ' ) " 4 . A (k-~· . j J . · y . - ; ; T l - ' - 1 ' \ . , - - , J ; I J - \ _ 1 , ' . , J I - 1 ( ' v r - : r : · . . . · k y c • • f - \ 0 , 1 7 8 P r o J . t r a m A . I 2 : P r o g r a m t o c a l c u l a t e Z M P a t 1 5 d e g r e e s =-~J~~ ( t , L l , L 2 , T : J , s x , s y , q : _ , q 2 , q 3 , q 4 , q 7 , q 2 _ 7 , q 2 ' / ,ml,n/:·,rn~-3,rn!l,r:L:J, . : _ _ - ' 1*,·~;:---;fr;~ . l ! - ) l - < l l ' ' ' I - < - ; 1 : _ , 1 • , _ : 1 ~~, : : : : ' O : C : + :3(p~/~:/·, r~ioJ-;· I c , 1 / p / 1 1 - C ' . : : : ! 2 J ~ / l ~~ ' ~IJ./; 1 L ) · : : ; i _ ; > ) ~ C . J '~~ ~ ' s i n ( p i - : q 4 1 o 3 ) / l / : · , - S X J . ~ r ~ 1 ~) - f , C C . : : 3 ( q 4 - 1 q 3 - ; · _ : r / ) ; , ) - + ' q 4 - : : ( / ) ; t - , . / : (~ I ~ : 1 ~ f t 0 _ 3 I - - , . ~j p - * t . ~ 1 ; 3 0 . 5 3 6·~-0.2364·~.A2~3e-2*t.AJ; •~r-~•t-6.3e-3*l.A2+ 8 8 le-J*~.A3-l.R518e-4*t.A~; . . . , - i - ~. 2 -2'c+~.6 ~3 - 3 * c . A 2 - 7 . 1 0 1 2 e - 4 * c . A 3 ; 7 2 * : : T 0 . 2 2 B * t . 2 - . 0 G 6 * c . A 3 + 6 . 5 e - 3 * t . A 4 ; ' ~- . 4 ~1 ~/ 6 * : _ - . J ' j 8 k l _ . / , 2 + 2 . G e - 2 * t . · ' 3 ; , , " " ' , R • t : + . D 6 8 7 • : : . A 2 - . ( J 2 0 4 • t . • 3 + 1 . 9 8 9 5 e - 3 * t . A 4 ; 4 R + . : 3 7 4 ' t - . C 6 C 2 * t . n 2 + 7 . 9 5 8 e - J x t . n 3 ; ' ' ~i . • r c : ' ' ( 0 4 ) T ; ; . * 3 " L l . * q ? · 1 + L 2 . * ( q 1 3 . A ? ) . ~ s i n ( q 4 ) - g . * c 0 s ( q ·~ + q 4 : · i :~ ( q 4 ) - ~ 2 . • q 1 3 . / 2 . • c 0 s ( q : ) ) - L l . * q 1 3 . A 2 - L : . X q 1 4 . A 2 - -~ ~ ; • " _ : _ L } - g . A . : = : ; . _ n , · J 1 · = 1 L~ ) ; ' r ~- c ; , : . ~ ~- _ r 1 ( ,~ _ - U . ~ • , - l _ j _ : _ _ • ~ c~ l 1 ) ~ ' \ , r l ] / + u-~- q ) . r c ( ) :~ ( < J l ) T • = ) . * L l . ~ q 2 ; : ; : + t J . · s i : - , : q : : J - J . : , . * L 2 . ' ( q l 1 r q l ? ) . ' 2 ; 1r~ ~:;:-t-::._).T .:~r.:·-+-- .:).f~/."·,:q2l~-q;::2;:·; l 3 ' ~j· ~-T.~. ·:Il~~ - , 2 ; (~ 2 ) ; ; ~ / 1 . : : _ _ _ ; - ~~ ~:; • > c " ~ ':~ • A 2 I 1 2 ; .).'(q~J7ql4) . n / . ' L } ; } . ' ; . ' ( q 2 3 + q 2 4 ) . *~l; ( ' c 3 ( q / ) - h . * s i~~ ( q / ) - 0 . 5 . k L S . ' ( ql3~ q l 4 + q U ) . A 2 ; lrl 1 ""<~;---r-l-' . • " ' - c c s ( q 1 t ( ; . 3 . * L 5 . * ' J c e 1-:r~l. r x : . • ) - m J . • a . ' y l ; ?~n2.*x2.*(d-g)-ml.*c.*y2; _ ? i - ~r.3 . x x 3 . * ( f - g ) -~n3 . k e . * y 3 ; ' l · c~2S- r o g r a m A . / 3 : P r o g r a m t o c a l c u l a t e Z M P a t h a l f s t e p ~ n " ' . , ( t , : : : _ _ : , _ : 2 , ~-) , ~-:; z , y , 1 _ h e c a : : : _ _ , t h e t 2 2 , the~ _ ) , r n e - = a G , ~ h t ? + - r l / , (~ ~ , : _ 1 , r _ 1 2 - ,l~l~,r-:5, , g , z , y ) ; - I ( - _ ' c~ ~ " ~ ~) ~- I~ / j r+-1--,~~r-- ~-y--~_L_"- in(r=:i-(th~t l-t~~eta2) · c o s : - ( - h " t l + t r J 2 - c J 2 ) ' 1 ~ T , ) * : : - = ; ! n ( p ~ - ( : : - ~-l e ~ c : = . _ + t l - : e t ( J 2 ) ) ; ·~ c:~ C ~' . ' , - I ' ; , : - ; r l ~:"=net_ • " - . \ ~~ i I G ; : ~~. e · - :_ne~=~G:~ ) - I -~.1~ s ' , r i / 3 6 ) · : 1 . , , i r • - ( L ' / ~ ( t _ - ~: ' - - ' - j f C ) - t : ~-- e t -~.: ~i : : - , - t t _ .~; r _ ) - : = l ~t:: a ' \ . - , ! _ - _ ; 2 ~ ~ c J ; " ~- . J ] ' l ) ! ~ ; j . c - - , _ ) -_rl(~:_ ~:: l - - ; ' -~~-~L"?- 1 11-+- - < ; - l _ ~) r _ · . - ~ ~ • 4 ; -~- - } , > ' - - : E : - _ ' r ' l · j . , 4 ' ' C ~~- A - t i o ( ) 9 ~ - , . J C j r - = ; - " - = . / ' ~- ( _ ) . ~ ' / ( j 7 c - U / 2 · t ~)-, (),~:.=) r t . " ' 4 ; / - . 0 2 • t . A 3 ; 2-.6?1B·~.n3+.11HS•~.n~; \ r : · , ~ • _ l _ ~J 1 2 ~ - : - : - - , . : . , - / 1 1 ~± ' - • -~)--+ • 4 - / 1 * t . / \ - - ) ; ' ' L 3 ( q ( - . : . · 3 L~.·c~2L;i'-"/.'(ql3."2).*si_c"( " s ~ n ( ~ 4 ) - L : . 2 . " q : _ ? . , , ~: . * c : o s ( q 4 ) - J . l . * q : 2 _ 3 . A 2 - L 1 . * 3 . * q ] 4 - C J . r s i : ; : : c { 3 - q 4 ) ; 1 1 \ r ( : - ' J : ' ' s i _ n ( : j ' ) - 0 " : J • K L l . X o l 1 . " 2 ; ( h - q ) . ' c _ ; o c : , : q l ) t C . 5 . * L l . * q 2 2 ; . . - : : = = : l ~ . - ' : 1 • * L~. k ' : 1 : _ : _ - + - q : 2 ' " 2 ; 1 ' - , - ! . ' [ . . ' , ! ; L ' :;~ / 1 ;~ ; l _ ! _ . , ~. ' I ~ · L -~-(~~' 4 - l l . t 2 _ l l : : ~; . · - ) . _ ! _ j : ) • ( c~ : " t _ [!~ + q l " / ) + ~ K ( ' : + ' . : , . ' ! , ' ' ( q 2 + q 2 4 - J c ? / ) l~:rJ. ' X : . • ( n - g - r n J . * a . ' y l ; r : c ' · · : r c : ? . ' x 2 . · ( c i - q - r : l l . * c . * y 2 ; . J ) H r n 3 . * x 3 . * ( f : - g - J ; L 3 . * e . * y 3 ; r > 4 t m 4 . * x / , . * ( k - q -mL.*~.*y4; . " '~12 i r - m 5 . " X 5 . . _ ( V ) - r n 5 . * v S . x y 5 ; J ; ' J . - T I - I , , r C - · c + · i ' I ( 1 1 I r~ _ J _ · \ I 6 C ) / _ ; ~. - , k ( - O S ( q - ) - , q 4 ; 1 _ 4 . " 2 - 8 0 18 ( 9 9 _/_\ -J --"" l"~ _j_ -'~ 1 ", ', I l .-J =--, ! Cf\)._ 'C1l\'f' L- ,: D-g,-· (fl-Y ( t-T : / : v /~ f • ~ ._ • t m g r w n £ ] : S i m u l a t i o n p r o g r a m o f s t a n c e l e g ··~ P o o t - • : " ' I : J R r J U P l : : : : : TR~J.JSFOPJvi~TIOf'.J ST.AF~.Tl + TF'AJ.JSL~TIOr·.Jl : ; p F~.OT A . T I O r • . J l ~ C U E : E 1 3 T F : A r \ J S F O F : r v 1 A T I O r \ J S T O P l - ··~ ( J R O U P 2 : : : : T F~Ar\J S F 0 F : r v 1 A T I O r · . J S T A R T l l m C V L ! r \ J D E R + W A r · . J S L A T I O r · . J B ~ C U E : E :~) R O T A T r o r · . J 1 3 + W A N S L A T I O r o . J 2 0 m C Y L l r ' - J D E R + TR~J.JSLATIOr'-t::l : / F~OTATIOro.m; + TR~J'.JS.LA.TICrr·.J46 m 1 1 . . 8 7 ' ' m g r w n F 3 : S i m u l a t i o n p r o g r a m f o r s i m u l a t i o n o f h o t h l e g s --~ F~.oot ··~ S l o p e : : : : : : T F : J \ I . . l 5 - F O F : I v 1 A T l ' J r ' l S T A . F : T 9 : j F : C l T A.TI01'~92 + T R A J . J S L . A . T I O I • . J 1 4 6 Q g C U E : E 1 3 T R A I ' < J S F O P i v 1 A T I O I ' · l S T O P l - ··~ E : o t h L t : g c = ! T R A I ' · l S F O F : . I v 1 A T I O I \ l S T A P T 1 2 m , = v 1 u w m + T R A J . J S L A T I O I . . J 5 5 Q g C U E : E : ) WTATIO~·J41 + T P J l J . J S L A T I O I . . J 3 8 m C Y m . J D E R + TRAJ~SL.ATIOI.,J35 : ) R O T A T I O I \ J 3 5 + TRA~·lSLATJOI'.J44 m C Y L I I . , l D E F : + T R A r · . J S L . A T I O I · i 6 3 + T F : A f ' . J S L . A T I O I . , J 9 1 I I I C ' • ( L l i ' . J D E R + T F : J \ J J S L A T I O I ' . J l l 4 ( } F : . C I T A . T J ! J r · . J 7 1 + T R A N S L . A T ! C l i ' . J 1 4 1 m '=YLII·~DER + T R A J . J S L . A T I O I \ J l 7 0 : ; p F:.OT.ATIOI·~92 + T R A J . J S L A T I O I · . J 1 9 9 I I I C Y L I I ' . J D E F : . + T R . A J . J S L . A . T I O I " i 2 6 0 : ' ) F : O T A T I O N 1 0 9 Q g C U E : E 1 3 ~--------------~ . . 8 8 l ' m g n 1 1 1 1 t ' . - 1 · S i m u l a h o n J 7 r o g r a m o f h i p e d a ! r o h o t l o w e r b o d v 1 1 ' i l h t o r s o ··~ R o o t ··~ S l o p e : : : : : : : T R A J . J S F O R M A T I O r . . J S T A . R T 9 ~:¥ R O T A T I O r ' - J 9 2 + T R A J . J S L A T I O r • . J l 4 6 Q g C U B E 1 3 TRAr'.JS.FOF'J.,~1A.TIOr'-J S T O P 1 -~ E : o t h L t : g c = : TRAJ.JSFOF~f./1ATIOr·.J S T A H T 1 2 m C ' i l l r ' - J D E F : + TF~Ar·.JSLATIDr'.JSS Q g C U E : E ~} R O T A T I O r ' - J 4 1 + TF:.Ar·.JSLATIOr'-J3:~: m CYLI~·JDER + T F : . A N S L A T I O r . . J 3 5 ~} P J J T A T I O r o . J 3 5 + WAr'~SLATIDr'H4 m CVLH'~DER + TF:Ar'.JSL.ATIOr'-J6~: + TF~#JSLATIO~·J91 m C Y L i r · . J D E R = : T P A r . . JSFOF~J·.,~1ATIOro.J S T A F ' T 6 1 + T R A J . J S L A . T I O r . . J 1 1 4 ~) R O T A T I O r · . J 1 6 1 + T R A J . J S L A T U J r · B 2 5 m C i L 1 r . . J D E F ' 1 3 T R A J . J S F O F ' f · i 1 A T I O r · . J S T O P 4 9 = : T RA.r·~ S F 0 R r v 1 A . T I O r • J S T A.R.T:~:l + T R A J . J S L A T I O r · B O O m C Y L I N D E R + TRM~SLAT!Or . . J 2 9 1 ~) R O T A . T I O ~.J127 + TF:Ar'.JSLATION2:~:6 m C V L i r · : J D E R + T F : A N S L A T I O r · . J 2 2 : 7 ~) R O T . A T r o r · n 2 4 + T F ' A J ' - J S L A T I O r • B O O I I I c Y L I r · m E F~ + T R A J . J S L A T I O r . , E : n + T F ' A N S L A . T I O r ' - 1 4 0 6 ~:)•" F : C I T A T I O r • . J 1 4 3 Q g C U B E 1 3 TF~AJ'-lSFOF~Jv1A.TIOr•J S T O P 6 1 8 9