Sistem Dinamik
Indrazno Siradjuddin
February 14, 2017
1
Sistem Dinamik Pergerakan Pegas
Gambar 1: Sistem pegas
1.1
Metode Lagrange (Lagrangian Method)
L = T − V (1)
dimana L adalah fungsi Lagrange, T adalah energi kinetik dan V adalah energi potensial. Untuk sistem konservatif (tidak ada energi yang hilang) maka per-samaan gerak (an equation of motion) dapat dihasilkan dari diferensial dari fungsi Lagrange, hasil persamaan berikut ini disebut juga Lagrange’s equation
d dt ∂L ∂ ˙qi − ∂L ∂qi = 0 (2)
dimana qi adalah sistem koordinat dari pergerakan sistem yang digeneralisasi. Untuk kasus pegas yang memiliki satu sumbu pergerakan bebas i = 1 atau dengan kata lain sistem 1 DOF (degrees of freedom), maka subscript i tidak perlu ditulis
tanpa mengihangkan arti, untuk itu koordinat dari sistem pegas q1 = q = x. Persamaan Lagrange untuk kasus sistem pegas menjadi
d dt ∂L ∂ ˙x − ∂L ∂x = 0 (3)
dimana ˙x adalah turunan pertama dari x. Energi kinetik T dari sistem pegas adalah
T = 1
2m ˙x 2
(4) sedangkan energi potensial V dari sistem pegas adalah
V = 1
2kx
2− mg(l + x) (5)
dimana k adalah konstanta pegas dengan satuan N/m dan l adalah panjang pegas. Lebih lanjut, turunan pertama fungsi Lagrange didapatkan persamaan sebagai berikut: d dt ∂L ∂ ˙x = d dt ∂ 12m ˙x2−1 2kx 2+ mg(l + x) ∂ ˙x ! (6) = m¨x (7) ∂L ∂x = ∂ 1 2m ˙x 2− 1 2kx 2+ mg(l + x) ∂x (8) = −kx + mg (9)
oleh karena itu, persamaan Lagrange untuk sistem pegas menjadi
m¨x + kx − mg = 0 (10)
m¨x = mg − kx (11)
Persamaan Lagrange yang dihasilkan diatas dapat di validasi dengan menggunakan hukum Newton dan Hooke.
1.2
Hukum Hough dan Newton
X f = w − fk (12) X f = mg − kx (13) m¨x = mg − kx (14) ¨ x = mg − kx m (15)
1.3
Simulasi dengan Python
1 i m p o r t p y l a b 2 from s c i p y . i n t e g r a t e i m p o r t o d e i n t 3 d e f MassSpring ( s t a t e , t ) : 4 # unpack t h e s t a t e v e c t o r 5 x = s t a t e [ 0 ] 6 xd = s t a t e [ 1 ] 7 # t h e s e a r e our c o n s t a n t s 8 k = 2 . 5 # Newtons p e r metre 9 m = 1 . 5 # K i l o g r a m s 10 g = 9 . 8 # m e t r e s p e r s e c o n d 11 # compute a c c e l e r a t i o n xdd 12 xdd = (( −k∗x ) /m) + g 13 # r e t u r n t h e two s t a t e d e r i v a t i v e s 14 r e t u r n [ xd , xdd ] 15 16 s t a t e 0 = [ 0 . 0 , 0 . 0 ] 17 t = p y l a b . a r a n g e ( 0 . 0 , 1 0 . 0 , 0 . 1 ) 18 19 s t a t e = o d e i n t ( MassSpring , s t a t e 0 , t ) 20 21 p y l a b . p l o t ( t , s t a t e ) 22 p y l a b . x l a b e l (’TIME ( s e c ) ’) 23 p y l a b . y l a b e l (’STATES ’) 24 p y l a b . t i t l e (’ Mass−S p r i n g System ’) 25 p y l a b . l e g e n d ( ( ’ $x$ (m) ’, ’ $ \ d o t {x} $ (m/ s e c ) ’) ) 26 p y l a b . s a v e f i g (” t e s t . s v g ”)Program 1: Sistem pegas
1.4
Gaya redaman
Model dan simulasi sistem pegas diatas adalah diturunkan berdasarkan asumsi tidak ada gaya eksternal yang mempengaruhi sistem. Sesungguhnya, pendekatan yang lebih realistis adalah bahwa sistem pegas tidak selamanya akan berosilasi terus menerus, melainkan lambat laun osilasi akan berkurang/teredam sehingga pegas akan kembali pada posisi normal (rest). Oleh sebab itu sistem pegas bukan merupakan sistem konservatif. Gaya redaman yang dinotasikan fd adalah propor-sional dengan kecepatan, dapat dirumuskan sebagai berikut
fd = −c ˙x (16)
dimana c adalah konstanta redaman viskos (viscous damping) dengan satuan N.s/m, tanda ”-” diberikan sebagai tanda yang selalu berlawanan dengan arah gerak benda. Persamaan Lagrange menjadi
d dt ∂L ∂ ˙qi − ∂L ∂qi = Qi (17)
dimana Qi adalah gaya eksternal. Sebagai perbandingan dengan sistem yang kon-servatif, nilai Qi adalah sama dengan nol. Untuk kasus sistem pegas maka
d dt ∂L ∂ ˙x −∂L ∂x = −c ˙x (18)
dengan cara yang sama seperti diatas, maka diperoleh persamaan gerak dinamik pegas sebagai berikut
m¨x = mg − kx − c ˙x (19)
Simulasi dengan Python berikut ini dengan menggunakan c = 2 N.s/m
1 i m p o r t p y l a b 2 from s c i p y . i n t e g r a t e i m p o r t o d e i n t 3 d e f MassSpring ( s t a t e , t ) : 4 # unpack t h e s t a t e v e c t o r 5 x = s t a t e [ 0 ] 6 xd = s t a t e [ 1 ] 7 # t h e s e a r e our c o n s t a n t s 8 k = 2 . 5 # Newtons p e r metre 9 m = 1 . 5 # K i l o g r a m s 10 g = 9 . 8 # m e t r e s p e r s e c o n d 11 c = 2 . 0 12 # compute a c c e l e r a t i o n xdd 13 xdd = (( −k∗x − c ∗ xd ) /m) + g 14 # r e t u r n t h e two s t a t e d e r i v a t i v e s
15 r e t u r n [ xd , xdd ] 16 17 s t a t e 0 = [ 0 . 0 , 0 . 0 ] 18 t = p y l a b . a r a n g e ( 0 . 0 , 1 0 . 0 , 0 . 1 ) 19 20 s t a t e = o d e i n t ( MassSpring , s t a t e 0 , t ) 21 22 p y l a b . p l o t ( t , s t a t e ) 23 p y l a b . x l a b e l (’TIME ( s e c ) ’) 24 p y l a b . y l a b e l (’STATES ’) 25 p y l a b . t i t l e (’ Mass−S p r i n g System ’) 26 p y l a b . l e g e n d ( ( ’ $x$ (m) ’, ’ $ \ d o t {x} $ (m/ s e c ) ’) ) 27 p y l a b . s a v e f i g (” t e s t . s v g ”)
Program 2: Sistem pegas dengan damper
2
Sistem Pergerakan Pendulum Pegas
Gambar 4: Sistem pendulum pegas
Energi kinetik total sistem pendulum pegas adalah merupakan gabungan antara energi kinetik translasi (tangensial atau translational) dan rotasi (radial atau ro-tational)
T = 1
2m
˙x2 + (l + x)2θ˙2 (20)
Catatan: Energi kinetik untuk benda berputar dengan jarak dari titik pusat r adalah
Tr =
1 2Iω
2 (21)
dimana I adalah momen inersia dan ω adalah kecepatan putaran (ω = ˙θ). Jika masa pegas diabaikan, maka momen inersia dari pendulum pegas adalah
I = mr2 (22)
untuk kasus pendulum pegas, r = (l + x). Energi potensial sistem pendulum pegas diakibatkan oleh gravitasi dan sifat pegas itu sendiri, dituliskan sebagai berikut
V = −mg(l + x) cos θ + 1
2kx
2 (23)
Fungsi Lagrange sistem pendulum pegas dapat diformulasikan sebagai berikut
L = 1 2m ˙x2 + (l + x)2θ˙2+ mg(l + x) cos θ −1 2kx 2 (24)
Dapat diketahui dari persamaan diatas bahwa terdapat dua sistem koordinat pada sistem pendulum pegas yaitu sistem koordinat translasi yang direpresentasikan oleh x dan sistem koordinat rotasi yang direpresentasikan oleh θ. Sehingga per-samaan Lagrange pada kedua koordinat tersebut dapat diformulasikan sebagai berikut, terhadap x d dt ∂L ∂ ˙x − ∂L ∂x = 0 (25) (m¨x) − (m(l + x) ˙θ2+ mg cos θ − kx) = 0 (26) ¨ x = (l + x) ˙θ2− g cos θ + kx m (27) dan terhadap θ d dt ∂L ∂ ˙θ = ∂L ∂θ (28) d dt m(l + x)2θ˙ = −mg(l + x) sin θ (29) m(l + x)2θ + 2m(l + x) ˙x ˙¨ θ = −mg(l + x) sin θ (30) (l + x)¨θ + 2 ˙x ˙θ = −g sin θ (31) ¨ θ = −(g sin θ + 2 ˙x ˙θ) (l + x) (32)
3
DO NEXT!!!
1. Python code to simulate the spring pendulum 2. Include the damping force
3. Python code to simulate the spring pendulum with the damping force 4. Hardcoded ODE Euler
4
Single Joint Elbow Kinematic
Gambar 5: 1 DOF lengan robot
Kinematic terdiri dari forward kinematic dan inverse kinematic. Dalam kinemat-ics, akan dipelajari hubungan atau fungsi yang memetakan input (input space) atau join robot (joint space) untuk robot manipulator menjadi output (output space) atau task space atau work space. Gambar 5 adalah contoh sederhana dari sistem 1 DOF (degrees of freedom) dimana jumlah input hanya satu yaitu sebuah motor pada posisi (0, 0) atau base, yang menggerakkan lengan l. Gerakkan putaran motor (dalam hal ini ditunjukkan oleh posisi sudut θ) ditujukan untuk mempo-sisikan ujung lengan (end-effector) pada posisi koordinat tertentu yang dinotasikan oleh (xe, ye). Dengan trigonometri dapat dengan mudah didapatkan
xe = l cos θ (33)
ye = l sin θ (34)
Problem pada inverse kinematic adalah untuk mencari posisi sudut motor θ jika dinginkan posisi end-effector pada (xe, ye). Inverse kinematic dari kasus ini dapat dihitung dengan tan θ = ye xe (35) θ = arctan ye xe (36)
4.1
Simulasi 1 DOF lengan dengan Python
1 from p y l a b i m p o r t ∗
2 #from math i m p o r t s i n , c o s , r a d i a n s , p i 3 t = l i n s p a c e ( 0 , 1 , 2 0 0 )
4 t h e t a = s i n ( 2 ∗ p i ∗ t / 4 ) 5 f i g u r e ( )
6 s u b p l o t ( 2 , 4 , ( 1 , 2 ) ) 7 p l o t ( t , t h e t a ∗180/ p i ) 8 x l a b e l (’TIME ( s e c ) ’)
9 y l a b e l (’ELBOW ANGLE ( deg ) ’) 10# compute hand p o s i t i o n Hx , Hy 11 l = 0 . 4 5 12 Hx = l ∗ c o s ( t h e t a ) 13 Hy = l ∗ s i n ( t h e t a ) 14 s u b p l o t ( 2 , 4 , ( 5 , 6 ) ) 15 p l o t ( t , Hx , ’ b− ’) 16 p l o t ( t , Hy , ’ r− ’) 17 x l a b e l (’TIME ( s e c ) ’) 18 y l a b e l (’HAND POSITION (m) ’) 19 l e g e n d ( (’Hx ’,’Hy ’) , l o c=’ l o w e r r i g h t ’) 20 s u b p l o t ( 2 , 4 , ( 3 , 8 ) ) 21 p l o t ( ( 0 , Hx [ 0 ] ) , ( 0 , Hy [ 0 ] ) ,’ g− ’) 22 p l o t ( ( 0 , Hx[ − 1 ] ) , ( 0 , Hy[ − 1 ] ) ,’ r− ’) 23 p l o t (Hx [ 0 : − 1 : 1 0 ] , Hy [ 0 : − 1 : 1 0 ] ,’ k . ’) 24 x l a b e l (’X (m) ’) 25 y l a b e l (’Y (m) ’) 26 a x i s (’ e q u a l ’) 27 p y l a b . s a v e f i g (” t e s t 1 d o f . s v g ”)
Program 3: Simulasi kinematics 1 DOF
5
2 DOF Kinematic
Gambar 7: 2 DOF lengan robot
5.1
Forward Kinematic
x1 = l1cos θ1 (37) y1 = l1sin θ1 (38) x2 = l1cos θ1+ l2cos(θ1+ θ2) (39) y2 = l1sin θ1+ l2sin(θ1+ θ2) (40) 1 from p y l a b i m p o r t ∗ 2 d e f j o i n t s t o h a n d ( a1 , a2 , l 1 , l 2 ) : 3 Ex = l 1 ∗ c o s ( a1 ) 4 Ey = l 1 ∗ s i n ( a1 ) 5 Hx = Ex + ( l 2 ∗ c o s ( a1+a2 ) ) 6 Hy = Ey + ( l 2 ∗ s i n ( a1+a2 ) ) 7 r e t u r n Ex , Ey , Hx , Hy 8 9 # l i m b geometry 10 l 1 = 0 . 3 4 # m e t r e s 11 l 2 = 0 . 4 6 # m e t r e s 12 13# d e c i d e on a r a n g e o f j o i n t a n g l e s 14 n 1 s t e p s = 10 15 n 2 s t e p s = 10 16 a 1 r a n g e = l i n s p a c e ( 0 ∗ p i / 1 8 0 , 120∗ p i / 1 8 0 , n 1 s t e p s ) # s h o u l d e r 17 a 2 r a n g e = l i n s p a c e ( 0 ∗ p i / 1 8 0 , 120∗ p i / 1 8 0 , n 2 s t e p s ) # elbow 1819# sample a l l c o m b i n a t i o n s and p l o t j o i n t and hand c o o r d i n a t e s 20 f=f i g u r e ( f i g s i z e = ( 8 , 1 2 ) ) 21 f o r i i n r a n g e( n 1 s t e p s ) : 22 f o r j i n r a n g e( n 2 s t e p s ) : 23 s u b p l o t ( 2 , 1 , 1 ) 24 p l o t ( a 1 r a n g e [ i ] ∗ 1 8 0 / p i , a 2 r a n g e [ j ] ∗ 1 8 0 / p i , ’ r+ ’) 25 ex , ey , hx , hy = j o i n t s t o h a n d ( a 1 r a n g e [ i ] , a 2 r a n g e [ j ] , l 1 , l 2 ) 26 s u b p l o t ( 2 , 1 , 2 ) 27 p l o t ( hx , hy , ’ r+ ’) 28 s u b p l o t ( 2 , 1 , 1 ) 29 x l a b e l (’ S h o u l d e r Angle ( deg ) ’) 30 y l a b e l (’ Elbow Angle ( deg ) ’) 31 t i t l e (’ J o i n t Space ’) 32 s u b p l o t ( 2 , 1 , 2 ) 33 x l a b e l (’ Hand P o s i t i o n X (m) ’) 34 y l a b e l (’ Hand P o s i t i o n Y (m) ’) 35 t i t l e (’ Hand Space ’) 36 a1 = a 1 r a n g e [ n 1 s t e p s / 2 ] 37 a2 = a 2 r a n g e [ n 2 s t e p s / 2 ] 38 ex , ey , hx , hy = j o i n t s t o h a n d ( a1 , a2 , l 1 , l 2 ) 39 s u b p l o t ( 2 , 1 , 1 ) 40 p l o t ( a1 ∗180/ p i , a2 ∗180/ p i ,’ bo ’, m a r k e r s i z e =5) 41 a x i s (’ e q u a l ’) 42 x l = g e t ( g e t ( f , ’ a x e s ’) [ 0 ] ,’ x l i m ’) 43 y l = g e t ( g e t ( f , ’ a x e s ’) [ 0 ] ,’ y l i m ’) 44 p l o t ( ( x l [ 0 ] , x l [ 1 ] ) , ( a2 ∗180/ p i , a2 ∗180/ p i ) ,’ b− ’) 45 p l o t ( ( a1 ∗180/ p i , a1 ∗180/ p i ) , ( y l [ 0 ] , y l [ 1 ] ) ,’ b− ’) 46 s u b p l o t ( 2 , 1 , 2 ) 47 p l o t ( ( 0 , ex , hx ) , ( 0 , ey , hy ) ,’ b− ’) 48 p l o t ( hx , hy , ’ bo ’, m a r k e r s i z e =5) 49 a x i s (’ e q u a l ’) 50 x l = g e t ( g e t ( f , ’ a x e s ’) [ 1 ] ,’ x l i m ’) 51 y l = g e t ( g e t ( f , ’ a x e s ’) [ 1 ] ,’ y l i m ’) 52 p l o t ( ( x l [ 0 ] , x l [ 1 ] ) , ( hy , hy ) ,’ b− ’) 53 p l o t ( ( hx , hx ) , ( y l [ 0 ] , y l [ 1 ] ) ,’ b− ’) 54 p y l a b . s a v e f i g (” t e s t 2 d o f . s v g ”)
Gambar 8: Hasil simulasi 2 DOF lengan robot
5.1.1 Inverse Kinematic !!!
5.2
Jacobian
Working space atau task space ξ = [x, y]T dan input space atau joint space θ = [θ1, θ2]T. Forward kinematic
ξ = f (θ) (41)
merupakan hubungan posisi sudut pada join θ dengan posisi end-effector ξ pada Euclidean space. Dengan mencari turunan pertama dari fungsi forward kine-matic, hubungan kecepatan sudut pada join dengan kecepatan end-effector pada
taskspace dapat diketahui dξ dt = df (θ) dt (42) dξ dt = ∂f (θ) ∂θ dθ dt (43) ˙ξ = J ˙θ (44)
dimana J disebut sebagai matriks Jacobian. Untuk kasus 2 DOF, matriks Jacobian dapat dicari dengan
J = ∂f (θ)x ∂θ1 ∂f (θ)x ∂θ2 ∂f (θ)y ∂θ1 ∂f (θ)y ∂θ2 (45)
Tuliskan program Python dibawah ini untuk menghitung secara simbolik kompo-nen matriks J 1 # i m p o r t sympy 2 from sympy i m p o r t ∗ 3 # d e f i n e t h e s e v a r i a b l e s a s s y m b o l i c ( no t numeric ) 4 a1 , a2 , l 1 , l 2 = s y m b o l s (’ a1 a2 l 1 l 2 ’) 5 # a1 and a2 r e p r e s e n t t h e t a 1 and t h e t a 2 , r e s p e c t i v e l y 6 # f o r w a r d k i n e m a t i c s f o r Hx and Hy 7 f x = l 1 ∗ c o s ( a1 ) + l 2 ∗ c o s ( a1+a2 ) 8 f y = l 1 ∗ s i n ( a1 ) + l 2 ∗ s i n ( a1+a2 ) 9 # u s e sympy d i f f ( ) t o g e t p a r t i a l d e r i v a t i v e s f o r J a c o b i a n m a t r i x 10 J11 = d i f f ( f x , a1 ) 11 J12 = d i f f ( f x , a2 ) 12 J21 = d i f f ( f y , a1 ) 13 J22 = d i f f ( f y , a2 ) 14 p r i n t J11 15 p r i n t J12 16 p r i n t J21 17 p r i n t J22
Program 5: Menghitung secara simbolik komponen matrix J akan didapatkan hasil matriks Jacobian J sebagai berikut
J = −l1sin(θ1) − l2sin(θ1+ θ2) −l2sin(θ1+ θ2) l1cos(θ1) + l2cos(θ1+ θ2) l2cos(θ1+ θ2)
(46) Program simulasi berikut memberikan gambaran aplikasi mencari nilai kecepatan
1 from sympy i m p o r t ∗ 2 from numpy i m p o r t ∗ 3 d e f j a c o b i a n (A, aparams ) : 4 ””” 5 Given j o i n t a n g l e s A=(a1 , a2 ) 6 r e t u r n s t h e J a c o b i a n m a t r i x J ( q ) = d f /dA 7 ””” 8 l 1 = aparams [ ’ l 1 ’] 9 l 2 = aparams [ ’ l 2 ’] 10 dFxdA1 = −l 1 ∗ s i n (A [ 0 ] ) − l 2 ∗ s i n (A[ 0 ] +A [ 1 ] ) 11 dFxdA2 = −l 2 ∗ s i n (A[ 0 ] +A [ 1 ] ) 12 dFydA1 = l 1 ∗ c o s (A [ 0 ] ) + l 2 ∗ c o s (A[ 0 ] +A [ 1 ] ) 13 dFydA2 = l 2 ∗ c o s (A[ 0 ] +A [ 1 ] )
14 J = m a t r i x ( [ [ dFxdA1 , dFxdA2 ] , [ dFydA1 , dFydA2 ] ] ) 15 r e t u r n J 16 17 aparams = {’ l 1 ’ : 0 . 3 3 8 4 , ’ l 2 ’ : 0 . 4 5 5 4 } 18 A = a r r a y ( [ 4 5 . 0 , 9 0 . 0 ] ) ∗ p i /180 # j o i n t a n g l e s 19 Ad = m a t r i x ( [ [ − 5 . 0 ] , [ 3 . 0 ] ] ) ∗ p i /180 # j o i n t v e l o c i t i e s 20 J = j a c o b i a n (A, aparams ) 21 Hd = J ∗Ad 22 p r i n t Hd
Program 6: Velocity kinematic
Kecepatan end-effector dihitung jika diketahui θ1 = 45◦, θ2 = 90◦, ˙θ1 = −5◦s−1 dan ˙θ2 = 3◦s−1 dan dihasilkan ˙x = 0.03212204 m s−1 dan ˙x = −0.00964106 m s−1.
5.3
Persamaan Dinamik
5.3.1 Forward Acceleration Kinematics d dt ˙ξ = ¨ξ = d dt J ˙θ (47) = Jd dt ˙θ + ˙θ d dt(J) (48) = J¨θ + ˙J ˙θ (49)
Sebagai contoh, untuk kasus 2 DOF lengan robot
˙J = d dt(j1,1) d dt(j1,2) d dt(j2,1) d dt(j2,2) (50)
dimana d
dt(j1,1) = d
dt(−l1sin(θ1) − l2sin(θ1+ θ2)) (51)
= −l1cos(θ1) ˙θ1− l2cos(θ1+ θ2) ˙θ1− l2cos(θ1+ θ2) ˙θ2 (52) d dt(j1,2) = d dt(−l2sin(θ1+ θ2)) (53) = −l2cos(θ1+ θ2) ˙θ1− l2cos(θ1+ θ2) ˙θ2 (54) d dt(j2,1) = d dt(l1cos(θ1) + l2cos(θ1+ θ2)) (55)
= −l1sin(θ1) ˙θ1− l2sin(θ1+ θ2) ˙θ1− l2sin(θ1+ θ2) ˙θ2 (56) d
dt(j2,2) = d
dt(l2cos(θ1+ θ2)) (57)
= −l2sin(θ1+ θ2) ˙θ1− l2sin(θ1+ θ2) ˙θ2 (58) 5.3.2 Inverse Acceleration Kinematics
¨
ξ = J¨θ + ˙J ˙θ (59)
¨
θ = J−1¨ξ − ˙J ˙θ (60)
dimana J−1 adalah inverse dari J.
6
Persamaan Gerak 1 DOF (Variasi Kasus)
Linear/Translational Kinetic Energy: T1 = 1 2mv 2 (61) = 1 2m( ˙x 2 + ˙y2) (62)
Rotational Kinetic Energy:
T2 = 1 2I ˙θ
2 (63)
Total Kinetic Energy:
T = T1+ T2 (64) = 1 2m( ˙x 2+ ˙y2) + 1 2I ˙θ 2 (65)
Dengan mentransformasikan koordinat titik pusat massa m, (x, y) → (r, θ) yang dapat diperoleh dengan cara sebagai berikut,
x = r sin(θ) (66)
y = −r cos(θ) (67)
˙x = r cos (θ) ˙θ (68)
˙
y = r sin (θ) ˙θ (69)
substitusikan, Kinetic Energy menjadi
T = 1 2m((r cos(θ) ˙θ) 2+ (r sin(θ) ˙θ)2) + 1 2I ˙θ 2 (70) = 1 2mr 2θ˙2(cos2θ + sin2θ) + 1 2I ˙θ 2 (71) = 1 2mr 2θ˙2+1 2I ˙θ 2 (72) Energi potensial V = mgh (73)
dimana h adalah ketinggian benda dari permukaan tanah. Dengan mendifinisikan bahwa energi potensial nol adalah saat lengan menghadap tegak lurus kebawah, θ = 0◦. Maka,
Dengan demikian fungsi Lagrange dapat ditulis sbg berikut L = T − V (75) = 1 2mr 2˙ θ2+1 2I ˙θ 2− mgr(1 − cos(θ)) (76) Untuk dapat merumuskan derivative dari fungsi Lagrange, perlu diketahui
∂L ∂θ = mgr sin(θ) (77) dan ∂L ∂ ˙θ = mr 2˙ θ + I ˙θ (78) dan d dt ∂L ∂ ˙θ = mr2θ + I ¨¨ θ (79)
Dalam kasus ini koordinat system hanya satu yaitu θ, persamaan EULER-LAGRANGE dapat ditulis sebagai berikut
d dt ∂L ∂ ˙θ − ∂L ∂θ = Q (80)
dalam hal Q adalah torque yang diberikan kepada system. Dengan kata lain, hal ini dapat digunakan untuk menjawab pertanyaan, ”berapa torque yang dibutuhkan untuk mengahasilkan state dinamik yang diinginkan”.
Q = mr2θ + I ¨¨ θ + mgr sin(θ) (81)
Persamaan forward dynamic dari gerak pendulum dapat dihitung dengan ¨
θ = Q − mgr sin(θ)
I + mr2 (82)
Program Python dibawah ini adalah implementasi sistem dinamik 1 dof
1 from s c i p y . i n t e g r a t e i m p o r t o d e i n t 2 from p y l a b i m p o r t ∗ 3 d e f o n e j o i n t a r m ( s t a t e , t ) : 4 t h e t a = s t a t e [ 0 ] # j o i n t a n g l e ( r ad ) 5 t h e t a d o t = s t a t e [ 1 ] # j o i n t v e l o c i t y ( rad / s ) 6 m = 1 . 6 5 # kg 7 r = 0 . 5 0 # l i n k l e n g t h (m)
9 i = 0 . 0 2 5 # moment o f i n e r t i a ( kg m m) 10 t h e t a d d o t = −(m∗ g ∗ r ∗ s i n ( t h e t a ) ) / ( i + (m∗ r ∗ r ) ) 11 r e t u r n [ t h e t a d o t , t h e t a d d o t ] 12 13 t = l i n s p a c e ( 0 . 0 , 1 0 . 0 , 1 0 0 1 ) # 10 s e c o n d s sampled a t 1000 Hz 14 s t a t e 0 = [ 9 0 . 0 ∗ p i / 1 8 0 . 0 , 0 . 0 ] # 90 deg i n i t i a l a n g l e , 0 deg / s e c i n i t i a l v e l o c i t y 15 s t a t e = o d e i n t ( o n e j o i n t a r m , s t a t e 0 , t ) 16 p r i n t s t a t e 17 f i g u r e ( ) 18 p l o t ( t , s t a t e ∗180/ p i ) 19 l e g e n d ( (’ t h e t a d o t ’,’ t h e t a d d o t ’) ) 20 x l a b e l (’TIME ( s e c ) ’)
21 y l a b e l (’THETA DOT ( deg ) & THETA DDOT ( deg / s e c ) ’) 22 p y l a b . s a v e f i g (” t e s t 1 d o f d y n a m i c . s v g ”)
Program 7: Simulasi dinamik 1 DOF Hasil plot
Gambar 10: Hasil simulasi sistem dinamik 1 DOF lengan robot Program dibawah ini skeleton animasi pendulum
1 from s c i p y . i n t e g r a t e i m p o r t o d e i n t 2 from p y l a b i m p o r t ∗ 3 4 d e f animate arm ( s t a t e , t ) : 5 l = 0 . 5 6 f i g u r e ( f i g s i z e = ( 1 2 , 6 ) ) 7 p l o t ( 0 , 0 ,’ r . ’) 8 p , = p l o t ( ( 0 , l ∗ s i n ( s t a t e [ 0 , 0 ] ) ) ,(0 , − l ∗ c o s ( s t a t e [ 0 , 0 ] ) ) ,’ b− ’) 9 t t = t i t l e (” %4.2 f s e c ” % 0 . 0 0 ) 10 x l i m ([ − l − . 0 5 , l + . 0 5 ] ) 11 y l i m ([ − l , . 1 0 ] )
12 s t e p = 3 13 f o r i i n x r a n g e( 1 , s h a p e ( s t a t e ) [ 0 ] − 1 0 , s t e p ) : 14 p . s e t x d a t a ( ( 0 , l ∗ s i n ( s t a t e [ i , 0 ] ) ) ) 15 p . s e t y d a t a ((0 , − l ∗ c o s ( s t a t e [ i , 0 ] ) ) ) 16 t t . s e t t e x t (” %4.2 f s e c ” % ( i ∗ 0 . 0 1 ) ) 17 draw ( ) 18 19 t = l i n s p a c e ( 0 . 0 , 1 0 . 0 , 1 0 0 1 ) 20 s t a t e = o d e i n t ( o n e j o i n t a r m , s t a t e 0 , t ) 21 animate arm ( s t a t e , t )
Program 8: Simulasi dinamik 1 DOF
7
Equations of motion for a two-joint arm
8
Motor Control: Muscle
Gambar 12: Model dari muscle dengan torque yang konstan
