機(jī)械動態(tài)仿真實(shí)驗(yàn)指導(dǎo)

1、機(jī)械動態(tài)仿真實(shí)驗(yàn)指導(dǎo)書一課程簡介機(jī)械動態(tài)仿真是為機(jī)械工程等相關(guān)專業(yè)開設(shè)的一門技術(shù)基礎(chǔ)課，主要介紹基于MATLAB軟件平臺的機(jī)械運(yùn)動仿真模塊SimMechanics和Simulink的應(yīng)用，目的在于培養(yǎng)學(xué)生了解和掌握機(jī)械動態(tài)仿真的基本理論、仿真方法及技術(shù)，為將來從事機(jī)械產(chǎn)品動態(tài)特性分析奠定基礎(chǔ)。二課程實(shí)驗(yàn)?zāi)康膶?shí)驗(yàn)?zāi)康模菏煜ず驼莆誐ATLAB軟件的函數(shù)編程，掌握基于MATLAB軟件平臺的機(jī)械仿真模塊SimMechanics和Simulink的建模方法、仿真技術(shù)。通過實(shí)驗(yàn)教學(xué)，使學(xué)生學(xué)會把實(shí)際問題抽象為數(shù)學(xué)模型、進(jìn)而建立仿真模型的方法，增強(qiáng)分析和解決實(shí)際問題的能力。三實(shí)驗(yàn)方式與基本要求實(shí)驗(yàn)方式：學(xué)生

2、獨(dú)立在計(jì)算機(jī)上進(jìn)行MATLAB軟件編程，建立四連桿機(jī)構(gòu)、曲柄滑塊機(jī)構(gòu)、二連桿平面機(jī)器人等機(jī)構(gòu)的數(shù)學(xué)模型，進(jìn)而建立基于Simulink和SimMechanics動態(tài)仿真模型，設(shè)置仿真運(yùn)行參數(shù)，觀察不同參數(shù)下的仿真結(jié)果?；疽螅?為保證實(shí)驗(yàn)順利進(jìn)行，要求學(xué)生掌握MATLAB語言的基礎(chǔ)知識；2由指導(dǎo)教師講清實(shí)驗(yàn)的基本要求、目的、建模要點(diǎn)及注意事項(xiàng)；3要求學(xué)生嚴(yán)格遵守實(shí)驗(yàn)課守則，認(rèn)真實(shí)驗(yàn)，按時完成實(shí)驗(yàn)報(bào)告。四實(shí)驗(yàn)報(bào)告每個實(shí)驗(yàn)均應(yīng)撰寫實(shí)驗(yàn)報(bào)告。實(shí)驗(yàn)報(bào)告內(nèi)容應(yīng)包括：實(shí)驗(yàn)名稱、目的、內(nèi)容、實(shí)驗(yàn)結(jié)果分析等。鼓勵學(xué)生以小論文形式整理和撰寫實(shí)驗(yàn)報(bào)告。五考試（考核）方法與規(guī)定本課程的實(shí)驗(yàn)考核采取理論測試與操作考核

3、相結(jié)合方式，并考慮平時實(shí)驗(yàn)及實(shí)驗(yàn)報(bào)告完成情況，按百分制綜合平定成績。實(shí)驗(yàn)1-1 小車彈簧系統(tǒng)的運(yùn)動仿真參考書圖6-17 質(zhì)量-阻尼-彈簧機(jī)械系統(tǒng)的仿真。應(yīng)用牛頓定律可以用數(shù)學(xué)模型描述：式中，y是質(zhì)量m的位移；u為作用質(zhì)量m上的力。采用現(xiàn)代控制理論的狀態(tài)方程，可以將單自由度的二階微分方程寫成兩個一階微分方程：設(shè) x1=y, x2=dy/dx, 則有 （1-2）根據(jù)（1-2）的數(shù)學(xué)模型即可編制仿真程序，之后即可上機(jī)調(diào)試運(yùn)行。步驟：1在MATLAB快捷菜單欄中，打開仿真庫，彈出Simulink Library Browser窗口。2在Simulink Library Browser中點(diǎn)擊File-&

4、gt;New->Model，以創(chuàng)建仿真模型。（注意與MATLAB命令窗口建立的模型不同。）3將輸入信號f視為正弦波，根據(jù)數(shù)學(xué)模型創(chuàng)建Simulink仿真模型如下：4在MATLAB命令窗口對質(zhì)量m、剛度k、阻尼系數(shù)B賦值后，即可對模型仿真。 實(shí)驗(yàn)1-2 子系統(tǒng)模塊在創(chuàng)建模型時，如果需要一個子系統(tǒng)也可直接在子系統(tǒng)窗口創(chuàng)建。這樣可以省去了上面壓縮子系統(tǒng)和重新安排窗口的步驟。x3要使用子系統(tǒng)模塊創(chuàng)建新的子系統(tǒng)，先從Function & Table模塊庫中拖一個子系統(tǒng)模塊到模型窗口中。雙擊子系統(tǒng)模塊就會出現(xiàn)一個子系統(tǒng)編輯窗口。單個小車的運(yùn)動方程如下：注意符號的寫法。先建立單個小車的系統(tǒng)的子

5、系統(tǒng)。使用子系統(tǒng)模塊創(chuàng)建如圖所示的子系統(tǒng)。此子系統(tǒng)用來模擬一個小車的運(yùn)動。子系統(tǒng)的輸入為小車的左距xn-1和右距xn+1，輸出為小車的當(dāng)前位置xn。 子系統(tǒng)完成后，關(guān)閉子系統(tǒng)窗口。復(fù)制兩次此子系統(tǒng)模塊，并如圖所示連接起來。 為了可以對每個小車的參數(shù)賦值，要做以下設(shè)置：建立文件set_k_m.m% set the spring constants and block mass valuesk1=1; k2=2; k3=4; k0=0;m1=1; m2=3; m3=2;仿真開始之前，在MATLAB命令窗口中運(yùn)行此M文件。然后，指定示波器模塊把顯示數(shù)據(jù)保存到工作空間中。并設(shè)置仿真的起止時間為（010

6、0）。仿真結(jié)束后，在MATLAB命令窗口把所得到的小車3的顯示數(shù)據(jù)繪制成圖。實(shí)驗(yàn)2-1 通過運(yùn)動學(xué)仿真求解曲柄滑塊的速度一、 曲柄滑塊的數(shù)學(xué)模型，采用閉環(huán)矢量方程yR2R3R1q3q2x （3-1）將此矢量方程分解到x和y坐標(biāo)軸上，得到 （3-2） （3-3）將上邊兩式對時間求導(dǎo)，有 （3-4） （3-5） 其中是矢量R1大小的變化率，也是滑塊相對于地面的平移速度。若曲柄的角速度已知，則方程改寫為 （3-6）二、曲柄滑塊運(yùn)動學(xué)的Simulink仿真將w2視為仿真的輸入，可以用數(shù)值積分從速度中計(jì)算出q2、q3和r1。顯然，勻速時w2為常量，作為仿真的輸入。選擇constant模塊。另外兩個速度可

7、以從閉環(huán)矢量方程中求得。實(shí)現(xiàn)這個過程需要三個積分模塊作為仿真的開始。第一個輸入是w2，這里設(shè)為常量并作為仿真的輸入，選擇constant模塊。另外兩個速度可以從閉環(huán)矢量方程中求得。為此編寫一個MATLAB函數(shù)來求解該方程。函數(shù)名為compvel.m內(nèi)容如下：function x=compvel(u)r2=1.0;r3=4.0;a=r3*sin(u(3) 1;-r3*cos(u(3) 0;b=-r2*u(1)*sin(u(2);r2*u(1)*cos(u(2);x=inv(a)*b; 三、參數(shù)設(shè)置（建立初始條件）初始條件必須是機(jī)構(gòu)在某個真實(shí)位置的角度和長度。假定曲柄的初始位置q2=0，此時連桿與

8、曲柄處于同一條線上。顯然，q3=0，滑塊距原點(diǎn)的距離為r2+r3的位置，即r1=5.0（in）。在MATLAB命令窗口鍵入：>> theta-2=0; theta-3=0; r1=5.0; 為了顯示滑塊的位移r1隨時間的變化圖形，可以用下列MATLAB命令：plot(simout(:,1),simout(:,6)xlabel('Time(sec) ')ylabel('Piston Displacment (in)')類似地，連桿的速度變化可以用下列命令畫出：plot(simout(:,1),simout(:,3)xlabel('Time(se

9、c) ')ylabel('Connectin Rod Speed (rad/s) ')實(shí)驗(yàn)2-2 通過運(yùn)動學(xué)仿真求解曲柄滑塊的加速度速度方程是對閉環(huán)矢量方程求關(guān)于時間的一階導(dǎo)數(shù)而得到的。加速度方程要對時間求二階導(dǎo)數(shù)，這時要特別注意對時間變量乘積的求導(dǎo)過程。閉環(huán)矢量方程對時間的二階導(dǎo)數(shù)方程由下式給出： （4-7） （4-8）在仿真時，位移（q2、q3和r1）被視為已知量（因?yàn)樗鼈兪欠e分的結(jié)果）。類似地，如果將仿真擴(kuò)展并計(jì)入加速度，則速度（w2、w3和r¢1）也被視為已知量。 （4-9）比較加速度方程和速度方程左端矩陣，可以看出左端2´2矩陣完全一樣。加

10、速度仿真的大部分模塊結(jié)構(gòu)和前面集合的速度仿真結(jié)構(gòu)一樣，可以修改前面的模型。求解上面的加速度方程（4-9）需要一個新的函數(shù)文件compacc.m。該函數(shù)需要額外的輸入a2、（仿真輸入）w2、w3。Mux模塊被擴(kuò)展，仿真系統(tǒng)增加了三個新的積分模塊。修改后的仿真系統(tǒng)如圖。輸出矢量也相應(yīng)地要加以改變，變量simout現(xiàn)在包括6列數(shù)據(jù)：t、q2、w3、q3、r1和r1。仿真的初始條件theta-2=0; theta-3=0; r1=5.0; omega-2=0; omega-3=0; r1-dot=0輸入加速度為10rad/s2; 仿真運(yùn)行時間為4秒。當(dāng)4秒結(jié)束時，曲柄轉(zhuǎn)速應(yīng)為40 rad/s，曲柄共旋

11、轉(zhuǎn)80 rad （大約13圈）下圖給出了連桿的角速度所經(jīng)歷的加速過程仿真曲線。注意到圖中給出的周期數(shù)和所確定的曲柄圈數(shù)13相吻合。加速度不是按正弦規(guī)律變化?？v坐標(biāo)是角速度omega-3實(shí)驗(yàn)3-1 四桿機(jī)構(gòu)的位置問題q2q3q4桿長r1=12, r2=4, r3=10, r4=7在機(jī)構(gòu)分析過程中，首先要進(jìn)行位置分析。就單自由度機(jī)構(gòu)而言，需要回答以下問題：若已知機(jī)構(gòu)中某一根連桿的位置（相對于地面），那么機(jī)構(gòu)中其他連桿的位置應(yīng)如何確定？例如：若給定連桿2的轉(zhuǎn)角q2和所有連桿的長度，則q3和q4可完全由方程求出。然而這組方程是關(guān)于q3和q4的非線性超越方程，非常難以求解。這里介紹非線性方程的數(shù)值解法。

12、采用牛頓辛普森方法是求解非線性方程的一種迭代法，它從某一給定的初始向量開始不斷地給增量直到所得結(jié)果足夠接近精確解。在前面推導(dǎo)得到了四連桿機(jī)構(gòu)的閉環(huán)矢量方程以及其在x方向和y方向的投影方程， （2-2） （2-3）將方程重新組合得到如下方程：因此位置問題可表述為：對給定的一組連桿長度和它的，尋求適當(dāng)?shù)?，使得函?shù)f1和f2等于零。由于f1和f2是非線性和超越的（超越函數(shù)中包含未知量），故線性的矩陣分析方法已不適用，牛頓辛普森方法正適合于求解這類非線性方程。首先，以名義解的形式重新定義變量，認(rèn)為名義解接近精確解，其間差值由以下修正因子描述： 其中：q3和q4代表問題的解；和為接近解的名義解；Dq3和

13、Dq4即為修正因子。運(yùn)用上述泰勒級數(shù)，將結(jié)果表達(dá)為如下矩陣方程：四連桿機(jī)構(gòu)位置問題的MATLAB求解求位置的方程function th3, th4=possol4(th,rs)th2=th(1);th3bar=th(2); %(guess)th4bar=th(3); %(guess)epsilon=1.0e-6;f=rs(3)*cos(th3bar)-rs(4)*cos(th4bar)+rs(2)*cos(th2)-rs(1);rs(3)*sin(th3bar)-rs(4)*sin(th4bar)+rs(2)*sin(th2);while norm(f)>epsilon J=-rs(3)

14、*sin(th3bar) rs(4)*sin(th4bar);rs(3)*cos(th3bar) -rs(4)*cos(th4bar); dth=inv(J)*(-1.0*f); th3bar=th3bar+dth(1); th4bar=th4bar+dth(2); f=rs(3)*cos(th3bar)-rs(4)*cos(th4bar)+rs(2)*cos(th2)-rs(1);rs(3)*sin(th3bar)-rs(4)*sin(th4bar)+rs(2)*sin(th2); norm(f);end;th3=th3bar;th4=th4bar;輸入初始條件：rs=12,4,10,7th

15、=0,45*pi/180,135*pi/180觀察輸出結(jié)果，正確輸出方法：th3,th4=possol4(th,rs)上面求解的特點(diǎn)是只要給定連桿的長度，輸入角度以及初始估計(jì)，即可得到四連桿機(jī)構(gòu)中其它兩個連桿的角度。fourbar.mD2R=pi/180.0;rs=12.0,4.0,10.0,7.0;th=0.0,45*D2R,100*D2R;dth=5*D2R;for i=1:72 ths=possol4(th,rs) angles(i,:)=th(1)/D2R ths(1)/D2R ths(2)/D2R; th(1)=th(1)+dth; th(2)=ths(1); th(3)=ths(2

16、);end腳本文件將結(jié)果存儲于名為angles的矩陣中（72行3列），則可由plot命令繪圖。plot(angles(:,1), angles(:,2), angles(:,1), angles(:,3)axis(0 360 0 160)>> text(50,35,'Theta-3')>> text(110,110,'Theta-4')>> ylabel('Solved angles (degrees)')>> xlabel('Input angles (degrees)') c=-

17、rs(3)*sin(th3) rs(4)*sin(th4);rs(3)*cos(th3) -rs(4)*cos(th4)c = -69.5269 69.5269 71.8750 8.1250d=-250*rs(2)*sin(0);250*rs(2)*cos(0)d = 0 10000inv(c)*dans = 125 125這些初始條件是很容易被實(shí)現(xiàn)的。若曲柄以250rad/s的轉(zhuǎn)速旋轉(zhuǎn)，機(jī)構(gòu)在0.025s之內(nèi)旋轉(zhuǎn)一周。如果仿真運(yùn)行0.1s，則大致模擬4圈。實(shí)驗(yàn)3-2 有曲柄條件的仿真求解。ADCBBDr1r2r3r4ACC2C2q四桿機(jī)構(gòu)中有的連架桿能作整周回轉(zhuǎn)而成為曲柄，有的則不能。在機(jī)械

18、原理的教科書中，導(dǎo)出鉸鏈四桿機(jī)構(gòu)有曲柄的條件為：連架桿和機(jī)架中必有一桿是最短桿。1 最短桿與最長桿的長度之和小于其它兩桿長度之和。上述兩個條件必須同時滿足，否則機(jī)構(gòu)中就不存在曲柄，只能是雙搖桿機(jī)構(gòu)。這里假定鉸鏈四桿機(jī)構(gòu)中存在曲柄，進(jìn)行可動性分析所關(guān)心的問題是：機(jī)構(gòu)輸入構(gòu)件與輸出構(gòu)件之間起連接作用的連桿長度所許可的變化。對于一系列輸入曲柄的位置來說，最關(guān)心的是能夠裝配的極小和極大連桿長度。分析：對于問題（1），可以用解析法求得以輸入和輸出角度位置的函數(shù)表示的連桿長度的極小或極大值。用上面的具有典型性的四桿機(jī)構(gòu)尺寸，曲柄為最短桿，搖桿小于機(jī)架長度：r1=12, r2=4, r4=7，求連桿r3的極

19、小和極大值。建立一個MATLAB命令的腳本文件，起名為ganminmax.m，% calculate the condition of fourbar having crankD2R=pi/180.0;rs=12.0,4.0,7.0;th=0.0;dth=5*D2R;for i=1:72 c=(rs(2)*sin(th)2+(rs(1)-rs(2)*cos(th)2; a=c0.5-rs(3); b=c0.5+rs(3); angles(i,:)=th/D2R a b; th(1)=th(1)+dth;endangles(37,2),angles(1,3)plot(angles(:,1), a

20、ngles(:,2), angles(:,1), angles(:,3)axis(0 360 0 25)在MATLAB窗口運(yùn)行該命令得到下圖和>> ganminmaxans =9 15這兩個值代表?xiàng)Ur2由0轉(zhuǎn)過360度，能夠保證有曲柄的連桿取值范圍。 采用同樣的方法能夠確定，r4從0到360度時，容許連桿取值的范圍。 腳本文件取名為lminmax.m% calculate the condition of fourbar having crankD2R=pi/180.0;rs=12.0,4.0,7.0;th=0.0;dth=5*D2R;for i=1:72 c=(rs(3)*sin

21、(th)2+(rs(1)+rs(3)*cos(th)2; a=c0.5-rs(2); b=c0.5+rs(2); angles(i,:)=th/D2R a b; th(1)=th(1)+dth;endangles(37,2),angles(1,3)plot(angles(:,1), angles(:,2), angles(:,1), angles(:,3)axis(0 360 0 25)僅有個別參數(shù)需要修改，總體上和前面的程序大致相同。實(shí)驗(yàn)4 四桿機(jī)構(gòu)速度和加速度仿真為進(jìn)一步說明運(yùn)動學(xué)仿真的概念，將對曲柄滑塊機(jī)構(gòu)的仿真進(jìn)行修改，用來對四連桿機(jī)構(gòu)進(jìn)行運(yùn)動仿真。這里只需對Simulink模型作少許

22、修改即可完成。兩個函數(shù)compacc.m和conchk.m必須作修改以反映適當(dāng)?shù)拈]環(huán)矢量方程。寫出四連桿的閉環(huán)矢量方程： （4-1）投影到x軸和y軸： （4-2） （4-3）對時間求一次導(dǎo)數(shù) （4-4） （4-5）假定連桿2與一臺電機(jī)相連，該電機(jī)能夠提供足夠大的驅(qū)動力矩使得w2相對保持常量。在此條件下，w2就稱為機(jī)構(gòu)輸入，重寫（4-4）和（4-5） （4-6） （4-7）在傳統(tǒng)的動力學(xué)分析中，確定角速度在某個時刻的大小，是在求解位置之后進(jìn)行的。也就是說，在某個時刻所有連桿轉(zhuǎn)角的角度是已知的。方程可寫成矩陣的形式： （4-8）再次對時間求導(dǎo)并整理后：（4-11）*利用式（4-8）求解四連桿機(jī)構(gòu)在

23、圖示位置時連桿3、4的角速度。設(shè)輸入連桿2的角速度為100rad/s。各連桿長度及在圖示位置的轉(zhuǎn)角如下：r1=12.0; r2=4.0; r3=10.0; r4=7.0;th2=45*pi/180;th3=24.652*pi/180;th4=90.6794*pi/180;j=-r3*sin(th3) r4*sin(th4);r3*cos(th3) -r4*cos(th4)b=100*r2*sin(th2);-100*r2*cos(th2)om34=inv(j)*b*可見速度和加速度方程左端的2×2矩陣完全相同。function x=compacc4(u)r1=12;r2=4;r3=1

24、0.0;r4=7.0;a=-r3*sin(u(6) r4*sin(u(6);r3*cos(u(6) -r4*sin(u(7);b(1)=r2*u(1)*sin(u(5)+r2*u(2)2*cos(u(5)+r3*u(3)2*cos(u(6)-r4*u(4)2*cos(u(7);b(2)=-r2*u(1)*cos(u(5)+r2*u(2)2*sin(u(5)+r3*u(3)2*sin(u(6)-r4*u(4)2*sin(u(7);x=inv(a)*b'四連桿仿真框圖只需作少許修改，具體如圖：實(shí)驗(yàn)5-1 平面鉸接二連桿的運(yùn)動仿真（基于Simulink的建模方法）本章討論一種在機(jī)器人學(xué)文獻(xiàn)中

25、經(jīng)常遇到的開鏈?zhǔn)綑C(jī)構(gòu)的動力學(xué)仿真。平面連桿機(jī)構(gòu)是一種簡單的兩自由度的機(jī)械裝置，具有一定的復(fù)雜動力特性。下面導(dǎo)出這種機(jī)構(gòu)的矢量方程和動力學(xué)方程。最終的動力學(xué)仿真要求具有兩個輸入?yún)?shù)，即由兩個電動機(jī)所產(chǎn)生的扭矩。矢量方程平面兩連桿機(jī)器人的矢量方程為： （5-1）注意到方程和我們前面所有遇到的方程形式有所不同，原因在于矢量角的定義方法不同。所給出的角度是相對于前一個連桿的方位，而不是相對于整體坐標(biāo)系的x軸。這種習(xí)慣的表示方法在機(jī)器人學(xué)中很普遍，其源于安裝在機(jī)器人手臂上的傳感器所測得的是連桿的相對轉(zhuǎn)角，而不是絕對轉(zhuǎn)角。將矢量方程投影到x軸和y軸上 （5-2） （5-3）對上式求導(dǎo)數(shù)有 （5-4） （5

26、-5）令C12=cos(q1+q2)，S12=sin(q1+q2)，將上兩式重新整理并寫成如下矩陣形式 （5-6）這個矩陣是人們所熟知的雅可比矩陣，它是一種有效的控制算法的基礎(chǔ)。對矩陣方程再求導(dǎo)，得到加速度方程 （5-7）上述推導(dǎo)出的方程構(gòu)成了進(jìn)行動力學(xué)仿真的基礎(chǔ)，它們表明了有效負(fù)荷的加速度與兩節(jié)點(diǎn)處電動機(jī)的角速度和角加速度之間的關(guān)系。確定兩連桿質(zhì)心處的加速度和節(jié)點(diǎn)變量之間的關(guān)系是十分重要的，而這些關(guān)系推導(dǎo)如下 （5-9） （5-10） （5-11）（5-12） 動力學(xué)方程按照前面所講的方法分別取兩個連桿的隔離體進(jìn)行分析。從第一根連桿可以推導(dǎo)出下面三個運(yùn)動方程 （5-13） （5-14） （5

27、-15）同樣，第二根連桿的受力如圖，有下述三個運(yùn)動方程 （5-16） （5-17） （5-18）最后研究作用在機(jī)器人上的有效載荷。注意到有效載荷的變化是直接與第二根連桿相聯(lián)系的，所以可以與連桿集中在一起考慮。由于有效載荷的質(zhì)量是隨著機(jī)器人舉起的不同物體而變化的，并且我們關(guān)心的是維持有效載荷所需要的力的大小，因而，再增加幾個方程對研究問題更加方便。將加載物視為集中質(zhì)量，得到兩個方程 （5-19） （5-20）綜上所述，從矢量方程中得到六個標(biāo)量方程，從動力學(xué)方程中得到八個標(biāo)量方程，如果再將電動機(jī)的扭矩考慮進(jìn)來，就有下面的14個未知量 聯(lián)立約束方程聯(lián)立六個運(yùn)動方程和八個動力學(xué)方程，即可得到一個關(guān)于1

28、4個未知量的線性方程組，如下這個矩陣方程嵌入到MATLAB函數(shù)中，隨后也可以嵌入到Simulink仿真中。上圖為兩連桿機(jī)器人的Simulink仿真模型，未連接的輸出代表冗余的加速度和約束力。為了在仿真中獲得較高的可信度，需要做一個簡單的實(shí)驗(yàn)。回想一下機(jī)器人在垂直平面工作時在重力作用下的受力圖。因此，如果讓機(jī)器人從任何初始位置開始運(yùn)動，將輸入的扭矩值設(shè)置為，那么機(jī)器人將在自身重力的作用下下落，最后達(dá)到兩個連長都在一條鉛垂線上的位置。參照圖7-1中節(jié)點(diǎn)角的定義，此時相應(yīng)的節(jié)點(diǎn)角度是q1p/2和q20 rad。對于初始條件，選擇q10和q2p/2 rad。這時對應(yīng)于加載物的位置（就是機(jī)器人操縱裝置的

29、末點(diǎn)位置）xpl=1.0和ypl=0.8。如同所有的仿真一樣，積分求解器的初始條件必須是相容的。求解矩陣方程的函數(shù)程序如下：function out=robot(u)% u(1)=omege-1% u(2)=theta-1% u(3)=omege-2% u(4)=theta-2% u(5)=Torque-1% u(6)=Torque-2%g=9.8067;r1=1.0;rc1=0.5;r2=0.8;rc2=0.1;m1=2.5;m2=1.8;I1=0.15;I2=0.05;%mpl=2.0;S1=sin(u(2);S12=sin(u(2)+u(4);C1=cos(u(2);C12=cos(u(2)+u(4);%a=zeros(14,14);b=zeros(14,1);a(1,1)=r1*S1+r2*S12;a(1,2)=r2*S12;a(1,7)=1;a(2,1)=-r1*C1-r2*C12;a(2,2)=-r2*C12;a(2,8)=1;a(3,1)=rc1*S1;a(3,3)=1;a(4,1)=-rc1*C1;a(4,4)=1;a(5,1)=r1*S1+rc2*S12;a(5,2)=rc2*S12;a(5,5)=1;a(6,1)=-r1*C1-rc2*C12;a(6