Capitolul 4MODELAREA DINAMICĂ A ROBOŢILOR INDUSTRIALI ARTICULAŢI

4.1. Consideraţii teoretice

Confom cu [Dră97], studiul dinamic al roboţilor industriali articulaţi, precum şi cei seriali, se poate realiza utilizând diferite metode. O metodă ar fi, folosirea ecuaţiilor lui Lagrange de speţa II-a sub formă dinamică, fără să fie luate în considerare reacţiunile din cuple şi frecările, se pot deduce ecuaţiile diferenţiale ale mişcării. O altă metodă ar fi utilizarea formalismul Newton-Euler, cu ajutorul căruia se pot determina reacţiunile din cuple, neglijând frecările.

4.1.1. Ecuaţiile lui Lagrange de speţa II-a, [Rip77]

Utilizând anumite artificii introduse de Lagrange, ecuaţiile de speţa I-a exprimate prin relaţia (4.1), pot fi aduse la o altă formă.

(4.1)

Astfel, din ecuaţiile (4.1) se obţine relaţia:

(4.2)

Membrul drept din relaţia (4.2), reprezintă forţa generalizată Qk. Acceleraţia a punctului

Mi poate fi scrisă ca derivata a vitezei în raport cu timpul. Astfel relaţia (4.2), poate fi scrisă

în felul următor:

, (4.3)

în care viteza se exprimă ca derivată în raport cu timpul a vectorului de poziţie

, (4.4)

dacă (4.5)

sau

1

, (4.6)

dacă

(4.7)

Dacă se utilizează regula de derivare a unui produs, se obţine:

. (4.8)

Analizând relaţiile (4.4) şi (4.6), se poate observa că viteza depinde de coordonatele generalizate, de vitezele generalizate şi de timp. Lagrange a considerat coordonatele generalizate independente de vitezele generalizate , astfel că viteza va depinde de 2h+1 parametri independenţi: h coordonate generalizate, h viteze generalizate şi timpul t.

Astfel, din relaţia (4.6) se obţine:

, (4.9)

întrucât vectorii şi nu depind de vitezele generalizate, ci numai de

coordonatele generalizate şi, eventual, de timp. În urma acestei observaţii se poate scrie expresia:

. (4.10)

Întrucât viteza depinde de coordonatele prin intermediul funcţiilor

şi , având în vedere relaţia (4.6), se obţine următoarea expresie:

. (4.11)

După cum se poate observa în relaţiile (4.10) şi (4.11), membrii doi sunt identici, astfel că:

. (4.12)

Având în vedere relaţiile (4.8), (4.9) şi (4.12), relaţia (4.3) devine:

. (4.13)

2

Întrucât, , , (4.14)

termenii din primul membru al relaţiei (4.13) devin succesiv

(4.15)

, (4.16)

deoarece reprezintă energia cinetică a sistemului material.

Dacă se introduc relaţiile (4.15) şi (4.16) în relaţia (4.13), rezultă relaţia (4.17)

(4.17)

care reprezintă ecuaţiile lui Lagrange de speţa a II-a, conform cu [Isp94].Ecuaţiile lui Lagrange de speţa a II-a formează un sistem de h ecuaţii diferenţiale scalare de

ordinul doi în care intervin energia cinetică Ec a sistemului şi forţele generalizate Qk, (k=1÷h), în funcţie de coordonatele generalizate , vitezele generalizate şi timpul t, conform cu [Isp04].

Forţa generalizată se determină fie cu relaţia

, (4.18)

fie dând o deplasare virtuală sistemului compatibilă cu legăturile, astfel ca să varieze numai coordonata generalizată , adică

(4.19)

Având în vedere relaţiile de mai jos

(4.20)

se poate determina lucrul mecanic virtual al forţelor active corespunzător unei deplasări elementare virtuale a sistemului material compatibilă cu legăturile olonome şi ideale pe care acesta le posedă. Astfel, expresia lucrului mecanic elementar virtual este următoarea:

3

(4.21)

în care forţa generalizată Qk are expresia (4.18).Conform cu (4.19), din (4.21) rezultă forţele generalizate:

(4.22)

4.1.2. Ecuaţiile dinamice Newton-Euler

De două mişcări simultane suprapuse este nevoie pentru determinarea mişcării generale a unui solid rigid şi anume: o mişcare de translaţie efectuată cu viteza şi acceleraţia ale

centrului de masă al rigidului şi o mişcare sferică efectuată în jurul centrului de masă cu viteza unghiulară şi acceleraţia unghiulară .

Pentru efectuarea studiului dinamic al mişcării rigidului aflat într-o mişcare generală se aplică teorema mişcării centrului de masă, adică ecuaţia lui Newton şi teorema momentului cinetic aplicată în raport cu centrul de masă, adică ecuaţia dinamică a lui Euler, [Det09]. Potrivit acestei metode, pentru determinarea ecuaţiilor dinamice ale roboţilor trebuie aplicat principiul lui D’Alembert pentru fiecare element care intră în structura mecanică a robotului.

Conform cu [Isp04], în figura 4.1, se consideră elementul i, i=1÷n, din structura mecanică a unui robot. Pentru elementul i, se cunosc anumiţi parametrii mecanici şi anume: masa M i, poziţia centrului de masă Ci şi tensorul inerţial relativ la un sistem de referinţă având originea în centrul de masă.

Asupra acestui element i acţionează un sistem de forţe exterioare, care în raport cu centrul de masă se reduce la o forţă rezultantă şi la un moment rezultant . Acest sistem de forţe exterioare imprimă elementului i o mişcare generală, caracterizată de către parametrii cinematici

care reprezintă viteza şi acceleraţia centrului de masă Ci, respectiv viteza şi

acceleraţia unghiulară cu care elementul i se roteşte în jurul centrului său de masă.

4

(i-1)

Oi

Elementul i(Mi, Ji

*) Ci

(i+1)Oi+1

civ

ciMiF

ii

cia

Figura 4.1. Elementul (i) din structura mecanică a unui robot

Teorema mişcării centrului de masă şi teorema momentului cinetic în raport cu centrul de masă sunt exprimate prin relaţiile:

sau . (4.23)

În conformitate cu (4.23), se pot scrie următoarele relaţii:

(4.24)

(4.25)

relaţii în care , reprezintă elementele torsorului de reducere în raport cu centrul maselor ale sistemului de forţe exterioare date şi de legătură care acţionează asupra elementului i.

Relaţia (4.24) reprezintă ecuaţia lui Newton, iar relaţia (4.25) reprezintă ecuaţia dinamică a lui Euler, conform cu [Isp04]. Cu ajutorul figurii 4.2 se pot scrie ecuaţiile dinamice ale robotului. În conformitate cu modelul geometric direct al unui robot, se ataşează elementului i un sistem de referinţă (Ti) cu originea în Oi. Elementul i este caracterizat din punct de vedere al distribuţiei

maselor, de către următorii parametrii: masa Mi, vectorul de poziţie al centrului de masă în

raport cu sistemul (Ti), tensorul inerţial în raport cu un sistem (Ti*), plasat cu originea în centrul

de masă Ci, care are axele paralele cu ale sistemului (Ti).

Sistemele de forţe exterioare date şi de legătură, reduse în raport cu centrul de masă C i al elementului i, respectiv în raport cu originea Oi a sistemului (Ti), conduc la doi torsori de reducere:

şi .

Elementele acestor torsori sunt:

5

xi (i-1)

(Ti)

yi

zi

Elementul i ()

ix

Ci

yi+1

(i+1)

Oi+1(Ti+1)

zi+1

Oi xi+1

il 1i

F

il 1iO

M

iy

iz

ij ic

F

ij ic

M

ici

M

ici

vici

ai

iici

r

iiF

iliF

il iO

M

i1ir (Ti

*)

Figura 4.2. Elementul i separat de elementele învecinate

- vectorul rezultant al forţelor date exprimat în sistemul (Ti);

- momentul rezultant al sistemului de forţe exterioare date, determinat în raport cu centrul de

masă Ci şi exprimat în sistemul (Ti);

- vectorul rezultant al forţelor de legătură dintre elementele i-1, i, aplicat în Oi şi exprimat în

sistemul (Ti);

- momentul rezultant al forţelor de legătură dintre elementele i-1, i, determinat în raport cu

Oi şi exprimat în sistemul (Ti).Dacă se iau în considerare forţele de legătură care apar între elementele i, i+1, atunci în

calcule se introduc şi elementele ale torsorului de reducere al acestui sistem de forţe în

raport cu originea Oi+1 a sistemului de referinţă (Ti+1), exprimate în sistemul de referinţă (Ti).

Mişcarea elementului i este caraterizată de către următorii parametrii cinematici: , , ,

, dacă se alege ca pol de referinţă centrul de masă Ci al elementului. Având în vedere aceste

condiţii, se introduce în Ci elementele ale torsorului forţelor de inerţie (fig.4.2).

Ecuaţiile dinamice pentru elementul i, se pot scrie conform cu principiul lui D’Alembert, astfel:

(4.26)

Având în vedere că şi , se pot obţine din

sistemul (4.26) expresiile vectorilor . Astfel rezultă:

(4.27)

Aplicând relaţia (4.24), respectiv în raport cu Oi relaţia (4.25), se poate obţine, deasemenea, sistemul de ecuaţii (4.27).

Forţele generalizate motoare din axa cinematică i se obţin prin proiectarea

vectorilor pe axa cuplei de versor . Astfel,

(4.28)

4.1.3. Metoda iterativă

Cu ajutorul acestei metode iterative, se stabilesc ecuaţiile dinamice ale unui robot, care pune în evidenţă variabilele generalizate, forţele generalizate motoare şi forţele de legătură care apar între elementele componente ale robotului.

6

dacă cupla este de translaţie, dacă cupla este de rotaţie.

,kM

,kFQ

ii

il

ii

ili

m

iO

i

Algoritmul de calcul, conform cu [Fu87] se bazează pe metoda Luh-Walker-Paul şi se compune din două părţi şi anume:

1. Iteraţii spre exteriorul structurii robotului. Utilizând ecuaţiile dinamice Newton-Euler, se determină pentru fiecare element i, (i=1÷n),

component al robotului, vitezele şi acceleraţiile liniare şi unghiulare, respectiv forţele şi momentele forţelor exterioare.

2. Iteraţii spre interiorul structurii robotului. În acest caz, se determină pentru fiecare element i, (i=1÷n), al robotului torsorul forţelor de

legătură dintre elementele i, i+1, respectiv forţele generalizate motoare din axele cinematice. În conformitate cu structura cinematică a unui robot cu n grade de libertate (fig.4.3), [Isp04]

şi [Sic09], se pot stabili ecuaţiile dinamice ale unui robot.

Astfel, pentru fiecare element i se determină matricea de transformare omogenă:

. (4.29)

Dacă se aplică metoda operatorilor compuşi de tip DH în varianta a doua, elementele matricei (4.29) au următoarele semnificaţii:

reprezintă matricea de rotaţie care defineşte orientarea fiecărei axe a sistemului (T i) în raport cu sistemul (Ti-1) şi are expresia:

; (4.30)

7

yn+1

iiε

Figura 4.3. Structura cinematică a unui robot cu n grade de libertate

y0x0

z0

(T0)

xi(i-1)

(Ti)

Oi

yi

Elementul i (Mi, Ji

*)

Ci(Ti

*)

yi+1

(i+1)Oi+1

(Ti+1)

zi+1

xi+1

xnOn (Tn)

yn

zn+1

(Tn+1)

On+1

1nl 1nO

M

1il 1i

F

ici

r

nmQ

zn

yi*

1imQ

1il 1iO

M

xi*

zi*

iii

ii qMG

i1ir

O0

zi

iiii

iia

iiv

ici

vici

ai

iiiF

ici

M

1nl 1n

F

xn+1

imQ

iqiq

reprezintă vectorul unicoloană care defineşte poziţia originii Oi a sistemului (Ti) în raport cu originea Oi+1 a sistemului (Ti+1), având expresia matriceală:

(4.31)

Se determină inversa matricei de rotaţie :

adică,

(4.32)

Pentru fiecare element i se determină următorii parametrii:

Mi – masa elementului i cu relaţia în care

- vectorul de poziţie al centrului de masă Ci în raport cu originea Oi a sistemului de

referinţă (Ti), cu relaţia:

(4.33)

unde reprezintă matricea care defineşte poziţia şi orientarea fiecărei axe a sistemului de referinţă (Ti) în raport cu sistemul de referinţă de tip DH, (TiD) şi care se poate determina, conform [Isp90], cu relaţia:

(4.34)

Ji*i – tensorul inerţial al elementului i în raport cu sistemul de referinţă (T i

*) cu originea în centrul de masă Ci. Acesta se determină conform cu [Isp90], cu următoarea relaţie:

, (4.35)

în care elementele matricei sunt momentele de inerţie mecanice axiale şi centrifugale ale elemen-tului i determinate în raport cu sistemul de referinţă (TiD), având originea în centrul de masă Ci .

Asupra robotului acţionează sistemul forţelor de greutate corespunzătoare fiecărui element i, (i=1÷n) şi un sistem de forţe exterioare situate la extremitatea robotului. Sistemul forţelor exterioare se reduce în raport cu originea On+1 a sistemului de referinţă (Tn+1), solidar cu dispozitivul de

8

,1,1

jdacă elementul j rămâne în componenţa elementului i;dacă elementul j se elimină;

prehensiune şi obiectul de manipulat prins în cleşte. Astfel, se obţine torsorul de reducere al forţelor

exterioare, având ca elemente: vectorul rezultant şi momentul rezultant , exprimaţi în

sistemul de referinţă (Tn+1).Dacă robotul se află în mişcare, atunci pe fiecare axă de mişcare sunt puse în evidenţă

variabilele generalizate . Parametrii cinematici care caracterizează la un moment

dat mişcarea elementului i sunt: cărora li se adaugă parametrii cinematici

ai elementului fix (0), adică:

(4.36)

în care g este acceleraţia gravitaţională.Conform cu [Isp04] şi [Sic09], prin aplicarea metodei Newton-Euler se determină ecuaţiile

dinamice ale robotului, din care ulterior se obţin forţele generalizate motoare .Aceste rezultate se obţin parcurgând cele două etape ale algoritmului de calcul şi

întroducând notaţia:

(4.37)

1. Iteraţii spre exteriorul structurii mecanice a robotului. Aplicând algoritmul de calcul prezentat în cadrul metodei iterative de modelare cinematică, se determină următorii parametri cinematici, conform cu [Isp04] şi [Det07]:

- viteza unghiulară a elementului i în raport cu sistemul fix (T0) de la baza robotului, exprimată în sistemul (Ti) cu relaţia:

(4.38)

- acceleraţia unghiulară a elementului i în raport cu sistemul (T0), exprimată în sistemul (Ti) cu relaţia:

(4.39)

- acceleraţia liniară a originii sistemului (Ti) în raport cu sistemul fix (T0), exprimată în sistemul (Ti) cu relaţia:

(4.40)

- acceleraţia centrului de masă Ci al elementului i, determinată în raport cu sistemul fix (T0) şi exprimată în raport cu sistemul (Ti) prin relaţia:

9

n1ii

1, dacă cupla este de rotaţie

0, dacă cupla este de translaţie.

(4.41)

Elementele torsorului de reducere al sistemului forţelor exterioare, se obţin prin aplicarea pentru fiecare element i, a ecuaţiilor dinamice Newton-Euler (4.24)-(4.25) şi au următoarele expresii:

(4.42)

2. Iteraţii spre interiorul structurii mecanice a robotului. Utilizând ecuaţiile din relaţia (4.27), pentru fiecare element i, se poate determina torsorul de reducere al sistemului forţelor de legătură. Elementele acestui torsor au expresiile:

(4.43)

Relaţiile (4.43) s-au obţinut prin transformarea vectorilor şi în vectorii:

(4.44)

Având în vedere relaţia (4.28), se determină forţele generalizate motoare , care de fapt reprezintă modelul dinamic al robotului:

(4.45)

în care reprezintă, în conformitate cu [Fu87], forţa generalizată datorată frecărilor şi are următoarea expresie:

(4.46)

Parametrii bi şi din relaţia (4.46) reprezintă:

bi – coeficientul de frecare vâscoasă;

- forţa generalizată datorată frecărilor uscate (frecări coulombiene) şi are expresia:

(4.47)

În relaţia de mai sus, ci reprezintă coeficientul de frecare uscată, iar di este diametrul fusului din cupla de rotaţie.

Sistemul (4.45) de ecuaţii dinamice se poate scrie sub forma:

(4.48)

10

şi reprezintă, modelul dinamic al robotului posedând n grade de libertate.În cadrul problemei directe a dinamicii robotului se cunoaşte vectorul coloană al forţelor

generalizate motoare. Astfel, se pot deduce funcţiile:

(4.49)

care reprezintă legea de mişcare a robotului în spaţiul configurativ al stărilor.În cadrul problemei inverse a dinamicii robotului, denumită modelul dinamic invers, sunt

cunoscute funcţiile şi se determină cu relaţia (4.45) forţele generalizate motoare . Utilizând metoda iterativă Newton-Euler, cu relaţia (4.43) se pot determina elementele torsorului forţelor de legătură dintre elementele componente ale robotului.

În concluzie, metoda iterativă Newton-Euler cuprinde următoarele etape:1. Se modelează geometric structura mecanică a robotului cu n grade de libertate şi se

determină pentru i=1÷n, matricele (4.29) şi inversele acestora.2. Se calculează pentru fiecare element i, i=1÷n, parametrii care caracterizează distribuţia

maselor şi anume: masa , vectorul de poziţie al centrului de masă Ci în raport cu Oi cu (4.33),

(4.34) şi tensorul cu relaţia (4.35).3. Se calculează, prin iteraţii spre exterior, parametrii cinematici (4.36)-(4.41) şi torsorul

forţelor exterioare (4.42).4. Se determină, prin iteraţii spre interior, torsorul forţelor de legătură ale cărui elemente

sunt date de (4.43), precum şi forţele generalizate motoare prin utilizarea ecuaţiilor dinamice (4.45).

4.2. Modelarea dinamică a robotului articulat 5R utilizând metoda Newton-Euler

Schemă cinematică a robotului articulat 5R este prezentată în figura 4.4 iar modelarea dinamică se va realiza utilizând metoda Newton-Euler în conformitate cu [Det07], [Ghi04], [Mar93] şi [Jen92].

Pentru aplicarea metodei Newton-Euler este necesar ca modelarea geometrică şi cinematică să fie finalizate. De asemenea ar mai fi necesar determinarea parametrilor de distribuţie a maselor, precum şi impunerea anumitor ipoteze simplificatoare, cum sunt cele enumerate mai jos:

- se aleg centrele maselor Ci în originile Oi ale sistemelor de referinţă carteziene Oixiyizi, i = 1÷5 şi astfel vectorii de poziţie a centrelor de masă sunt nuli;

- alegând sistemele de referinţă mobile astfel încât axele lor să coincidă cu direcţiile principale de inerţie aferente originii acestor sisteme, rezultă că momentele de inerţie mecanice centrifugale devin nule.

11

Parametrii de distribuţie a maselor care trebuiesc determinaţi sunt următorii: masele elementelui i, centrele de masă şi tensorii inerţiali.

1. Masele care definesc structura robotului articulat sunt: M1, M2, M3, M4, M5; 2. Centrele de masă ale robotului articulat 5R sunt:

, , , , ;

3. Tensorii inerţiali:

, , ,

, .

Componentele tensorului inerţial, , , , i = 1, 2, 3, 4, 5 reprezintă momentele de inerţie mecanice axiale în raport cu sistemul i, cu originea în centrul maselor C i şi având aceeaşi orientare cu sistemul ataşat fiecărui element al robotului.

12

Figura 4.4. Schema cinematică a robotului articulat 5R

Acceleraţiile corespunzătoare centrelor de masă se determină conform cu relaţia (4.41), enunţată în paragraful 4.1.3. Astfel rezultă următoarele acceleraţii:

,

; (4.50)

, (4.51)

; (4.52)

În Anexa 1 se regăsesc relaţiile acceleraţiilor centrelor de masă 3, 4, şi 5 deoarece acestea sunt foarte complexe, iar în continuare vor fi prezentate relaţiile de calcule cu care au fost determinate acestea:

, (4.53)

, (4.54)

. (4.55)

După cum ştim prin metoda Newton-Euler structura mecanică a unui robot este parcursă atât prin iteraţii spre exterior, cât şi prin iteraţii spre interior. În cele ce urmează este prezentată prima metodă şi anume iteraţii prin exterior, unde rezultă sistemul forţelor exterioare conform paragrafului 4.1.3, astfel:

, (4.56)

; (4.57)

, (4.58)

; (4.59)

13

Relaţiile forţelor exterioare pentru celelalte trei elemente ale robotului articulat sunt foarte complexe, de aceea, acestea pot fi regăsite în Anexa 1. Determinarea acestor forţe exterioare sunt date de relaţiile de mai jos:

, (4.60)

, (4.61)

. (4.62)

Momentele forţelor exterioare se obţin cu relaţia (4.42) şi au următoarea formă:

, ; (4.63)

, (4.64)

;(4.65)

; (4.66)

; (4.67)

. (4.68)

Relaţiile momentelor forţelor exterioare pentru ultimele trei elemente ale robotului articulat sunt foarte complexe, de aceea, acestea pot fi regăsite în Anexa 1.

Torsorul forţelor şi momentelor de legătură, respectiv forţele generalizate motoare din cuplele robotului se determină conform cu cel de-al doilea algoritm a metodei lui Newton-Euler, care constă în faptul că structura mecanică a robotului este parcursă prin iteraţii spre interior.

Forţele de legătură precum şi momentele forţelor de legătură au, conform cu (4.43) şi [Isp04], următoarele formule de calcul:

, (4.69)

14

, (4.70)

, (4.71)

, (4.72)

. (4.73)

(4.74)

(4.75)

(4.76)

(4.77)

(4.78)

dar datorită faptului că expresiile atât a forţelor de legătură cât şi a momentelor forţelor de legătură sunt complexe, acestea se regăsesc la Anexa 1.

Forţele generalizate motoare sunt determinate conform cu [Det07] şi [Isp04]. Expresiile acestor forţe reprezintă sistemul de ecuaţii diferenţiale dinamice ce caracterizează modelul dinamic al robotului articulat 5R.

Având relaţia (4.45), forţele generalizate motoare au următoarele formule de calcule:

, (4.79)

, (4.80)

, (4.81)

, (4.82)

15

. (4.83)

Datorită faptului că expresiile tuturor calculelor efectuate înainte sunt destul de complexe, acestea determină complexitatea forţelor generalizatoare motoare, care de asemenea se pot regăsi la Anexa 1.

4.3. Modelarea dinamică a robotului articulat 6R utilizând metoda Newton-Euler

Modelarea dinamică a robotului articulat 6R, a cărui schemă cinematică este prezentată în figura 4.5 va fi realizată în conformitate cu [Det07], [Ghi04], [Mar93] şi [Jen92], prin aplicarea metodei Newton-Euler. Această metodă este implementată în programul de modelare simbolică Robot_Symbolic, modulul Robot_Dynamics din cadrul programului Matlab.

Pentru aplicarea metodei se impune modelarea geometrică şi cinematică, determinarea parametrilor de distribuţie a maselor, precum şi impunerea anumitor ipoteze simplificatoare:

- se aleg centrele maselor Ci în originile Oi ale sistemelor de referinţă carteziene Oixiyizi, i = 1÷6 şi astfel vectorii de poziţie a centrelor de masă sunt nuli;

- alegând sistemele de referinţă mobile astfel încât axele lor să coincidă cu direcţiile principale de inerţie aferente originii acestor sisteme, rezultă că momentele de inerţie mecanice centrifugale devin nule.

Parametrii de distribuţie a maselor: masele elementelui i, centrele de masă şi tensorii inerţiali:

- masele: M1, M2, M3, M4, M5, M6;

- centrele de masă: , , , , , ;

- tensorii inerţiali:

, , ,

, , .

16

Componentele tensorului inerţial, , , , i = 1, 2, 3, 4, 5, 6 reprezintă momentele de inerţie mecanice axiale în raport cu sistemul i, cu originea în centrul maselor C i şi având aceeaşi orientare cu sistemul ataşat fiecărui element al robotului.

Acceleraţiile corespunzătoare centrelor de masă se determină conform cu relaţia (4.41), enunţată în paragraful 4.1.3. Astfel rezultă următoarele acceleraţii:

,

; (4.84)

,

; (4.85)

, (4.86)

17

Figura 4.5. Schema cinematică a robotului articulat 6R

; (4.87)

Datorită faptului că relaţiile acceleraţiilor centrelor de masă 4, 5 şi 6 sunt foarte complexe, acestea pot fi regăsite în Anexa 2, iar în continuare vor fi prezentate relaţiile de calcule cu care au fost determinate acestea:

, (4.88)

, (4.89)

.

(4.90)

Conform paragrafului 4.1.3, prin metoda Newton-Euler structura mecanică a unui robot este parcursă prin iteraţii spre exterior, rezultând astfel sistemul forţelor exterioare:

, ; (4.91)

, ; (4.92)

, (4.93)

; (4.94)

Relaţiile forţelor exterioare pentru celelalte trei elemente ale robotului articulat sunt foarte complexe, de aceea, acestea pot fi regăsite în Anexa 2. Determinarea acestor forţe exterioare sunt date de relaţiile de mai jos:

, (4.95)

, (4.96)

18

. (4.97)

Conform cu relaţia (4.42), se obţin momentele forţelor exterioare, care au următoarea formă:

, ; (4.98)

, ; (4.99)

; (4.100)

; (4.101)

; (4.102)

.

(4.103)

Relaţiile momentelor forţelor exterioare pentru cele patru elemente ale robotului articulat sunt foarte complexe, de aceea, acestea pot fi regăsite în Anexa 2.

Cel de-al doilea algoritm a metodei lui Newton-Euler constă în faptul că structura mecanică a robotului este parcursă prin iteraţii spre interior. Astfel, se determină torsorul forţelor şi momentelor de legătură, respectiv forţele generalizate motoare din cuplele robotului.

Forţele de legătură precum şi momentele forţelor de legătură au, conform cu (4.43) şi [Isp04], următoarele formule de calcul:

, (4.104)

, (4.105)

, (4.106)

, (4.107)

, (4.108)

, (4.109)

(4.110)

(4.111)

19

(4.112)

(4.113)

(4.114)

(4.115)

dar datorită faptului că expresiile atât a forţelor de legătură cât şi a momentelor forţelor de legătură sunt complexe, se regăsesc la Anexa 2.

În conformitate cu [Det07] şi [Isp04], se determină forţele generalizate motoare, a căror expresii reprezintă sistemul de ecuaţii diferenţiale dinamice ce caracterizează modelul dinamic al robotului articulat 6R.

Având relaţia (4.45), forţele generalizate motoare au următoarele formule de calcule:

, (4.116)

, (4.117)

, (4.118)

, (4.119)

, (4.120)

. (4.121)

Datorită faptului că expresiile tuturor calculelor efectuate înainte sunt destul de complexe, acestea determină complexitatea forţelor generalizatoare motoare, care de asemenea se pot regăsi la Anexa 2.

20

ANEXA 1

Acceleraţiile corespunzătoare centrelor de masă ale robotului articulat 5R

vc33dot =

21

q1ddot*rc33z*sin(q2 + q3) - q1dot^2*rc33x - l3*q1ddot*cos(q2) - q1ddot*rc33y*cos(q2 + q3) - l1*q1ddot + 2*q1dot*q2dot*rc33z*cos(q2 + q3) + 2*q1dot*q3dot*rc33z*cos(q2 + q3) + 2*q1dot*q2dot*rc33y*sin(q2 + q3) + 2*q1dot*q3dot*rc33y*sin(q2 + q3) + 2*l3*q1dot*q2dot*sin(q2)

sin(q3)*(l3*q2ddot + g*cos(q2) + l1*q1dot^2*sin(q2) + (l3*q1dot^2*sin(2*q2))/2) - cos(q3)*(l3*((q1dot^2*cos(2*q2))/2 + q1dot^2/2 + q2dot^2) - g*sin(q2) + l1*q1dot^2*cos(q2)) - rc33y*((q1dot^2*cos(2*q2 + 2*q3))/2 + (q2dot + q3dot)^2 + q1dot^2/2) - rc33z*(q2ddot + q3ddot) + (q1dot^2*rc33z*sin(2*q2 + 2*q3))/2 + q1ddot*rc33x*cos(q2 + q3)

rc33y*(q2ddot + q3ddot) - rc33z*(q1dot^2*sin(q2 + q3)^2 + (q2dot + q3dot)^2) - sin(q3)*(g*sin(q2) - l3*(q1dot^2/2 - (q1dot^2*(2*sin(q2)^2 - 1))/2 + q2dot^2) + l1*q1dot^2*(2*sin(q2/2)^2 - 1)) - (2*sin(q3/2)^2 - 1)*(l3*q2ddot - g*(2*sin(q2/2)^2 - 1) + l1*q1dot^2*sin(q2) + (l3*q1dot^2*sin(2*q2))/2) + (q1dot^2*rc33y*sin(2*q2 + 2*q3))/2 - q1ddot*rc33x*sin(q2 + q3)

vc44dot =

cos(q4)*(l4*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) - l1*q1ddot - l3*q1ddot*cos(q2) + l4*q1dot*sin(q2 + q3)*(q2dot + q3dot) + 2*l3*q1dot*q2dot*sin(q2)) - sin(q4)*(l4*(q2ddot + q3ddot) + cos(q3)*(l3*q2ddot + g*cos(q2) + l1*q1dot^2*sin(q2) + (l3*q1dot^2*sin(2*q2))/2) + sin(q3)*(l3*((cos(2*q2)/2 + 1/2)*q1dot^2 + q2dot^2) - g*sin(q2) + l1*q1dot^2*cos(q2)) + (l4*q1dot^2*sin(2*q2 + 2*q3))/2) - rc44x*((sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))^2 + (q4dot + q1dot*sin(q2 + q3))^2) - rc44y*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot)) + rc44z*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) + rc44z*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) + rc44y*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))*(q4dot + q1dot*sin(q2 + q3))

rc44x*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot)) - cos(q3)*(l3*(q1dot^2*cos(q2)^2 + q2dot^2) - g*sin(q2) + l1*q1dot^2*cos(q2)) - l4*(q1dot^2*cos(q2 + q3)^2 + (q2dot + q3dot)^2) - rc44z*(sin(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) - q4dot*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q4)*(q2ddot + q3ddot)) - rc44y*((sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))^2 + (cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))^2) + sin(q3)*(l3*q2ddot + g*cos(q2) + l1*q1dot^2*sin(q2) + l3*q1dot^2*cos(q2)*sin(q2)) + rc44x*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))*(q4dot + q1dot*sin(q2 + q3)) + rc44z*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))*(q4dot + q1dot*sin(q2 + q3))

rc44y*(sin(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) - q4dot*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q4)*(q2ddot + q3ddot)) + cos(q4)*(l4*(q2ddot + q3ddot) + cos(q3)*(l3*q2ddot + g*cos(q2) + l1*q1dot^2*sin(q2) + (l3*q1dot^2*sin(2*q2))/2) + sin(q3)*(l3*((cos(2*q2)/2 + 1/2)*q1dot^2 + q2dot^2) - g*sin(q2) + l1*q1dot^2*cos(q2)) + (l4*q1dot^2*sin(2*q2 + 2*q3))/2) - rc44z*((cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))^2 + (q4dot + q1dot*sin(q2 + q3))^2) + sin(q4)*(l4*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) - l1*q1ddot - l3*q1ddot*cos(q2) + l4*q1dot*sin(q2 + q3)*(q2dot + q3dot) + 2*l3*q1dot*q2dot*sin(q2)) - rc44x*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) + rc44x*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) + rc44y*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))*(q4dot + q1dot*sin(q2 + q3))

vc55dot =

22

rc55y*(q5dot*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3))) + sin(q5)*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) - cos(q5)*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot))) - l5*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot)) + rc55z*(q5dot*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3))) + cos(q5)*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) + sin(q5)*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot))) - sin(q4)*(l4*(q2ddot + q3ddot) + cos(q3)*(l3*q2ddot + g*cos(q2) + l1*q1dot^2*sin(q2) + (l3*q1dot^2*sin(2*q2))/2) + sin(q3)*(l3*((cos(2*q2)/2 + 1/2)*q1dot^2 + q2dot^2) - g*sin(q2) + l1*q1dot^2*cos(q2)) + (l4*q1dot^2*sin(2*q2 + 2*q3))/2) - l6*((sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))^2 + (q4dot + q1dot*sin(q2 + q3))^2) + cos(q4)*(l4*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) - l1*q1ddot - l3*q1ddot*cos(q2) + l4*q1dot*sin(q2 + q3)*(q2dot + q3dot) + 2*l3*q1dot*q2dot*sin(q2)) - rc55x*((sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3)))^2 + (cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3)))^2) + l5*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))*(q4dot + q1dot*sin(q2 + q3)) + rc55y*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3)))*(q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) + rc55z*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3)))*(q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))

sin(q5)*(l5*(sin(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) - q4dot*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q4)*(q2ddot + q3ddot)) + cos(q4)*(l4*(q2ddot + q3ddot) + cos(q3)*(l3*q2ddot + g*cos(q2) + l1*q1dot^2*sin(q2) + (l3*q1dot^2*sin(2*q2))/2) + sin(q3)*(l3*((cos(2*q2)/2 + 1/2)*q1dot^2 + q2dot^2) - g*sin(q2) + l1*q1dot^2*cos(q2)) + (l4*q1dot^2*sin(2*q2 + 2*q3))/2) + sin(q4)*(l4*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) - l1*q1ddot - l3*q1ddot*cos(q2) + l4*q1dot*sin(q2 + q3)*(q2dot + q3dot) + 2*l3*q1dot*q2dot*sin(q2)) - l6*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) + l6*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) + l5*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))*(q4dot + q1dot*sin(q2 + q3))) - rc55x*(q5dot*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3))) + sin(q5)*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) - cos(q5)*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot))) - rc55z*(q5ddot - q4dot*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + sin(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + cos(q4)*(q2ddot + q3ddot)) - cos(q5)*(l4*(q1dot^2*cos(q2 + q3)^2 + (q2dot + q3dot)^2) - l6*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot)) + cos(q3)*(l3*(q1dot^2*cos(q2)^2 + q2dot^2) - g*sin(q2) + l1*q1dot^2*cos(q2)) + l5*((sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))^2 + (cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))^2) - sin(q3)*(l3*q2ddot + g*cos(q2) + l1*q1dot^2*sin(q2) + l3*q1dot^2*cos(q2)*sin(q2)) - l6*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))*(q4dot + q1dot*sin(q2 + q3))) - rc55y*((q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))^2 + (cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3)))^2) + rc55z*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3)))*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3))) + rc55x*(sin(q5)*(sin(q4)*(q2dot + q3dot) +

23

q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3)))*(q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))

cos(q5)*(l5*(sin(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) - q4dot*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q4)*(q2ddot + q3ddot)) + cos(q4)*(l4*(q2ddot + q3ddot) + cos(q3)*(l3*q2ddot + g*cos(q2) + l1*q1dot^2*sin(q2) + (l3*q1dot^2*sin(2*q2))/2) + sin(q3)*(l3*((cos(2*q2)/2 + 1/2)*q1dot^2 + q2dot^2) - g*sin(q2) + l1*q1dot^2*cos(q2)) + (l4*q1dot^2*sin(2*q2 + 2*q3))/2) + sin(q4)*(l4*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) - l1*q1ddot - l3*q1ddot*cos(q2) + l4*q1dot*sin(q2 + q3)*(q2dot + q3dot) + 2*l3*q1dot*q2dot*sin(q2)) - l6*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) + l6*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) + l5*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))*(q4dot + q1dot*sin(q2 + q3))) - rc55x*(q5dot*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3))) + cos(q5)*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) + sin(q5)*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot))) + rc55y*(q5ddot - q4dot*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + sin(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + cos(q4)*(q2ddot + q3ddot)) + sin(q5)*(l4*(q1dot^2*cos(q2 + q3)^2 + (q2dot + q3dot)^2) - l6*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot)) + cos(q3)*(l3*(q1dot^2*cos(q2)^2 + q2dot^2) - g*sin(q2) + l1*q1dot^2*cos(q2)) + l5*((sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))^2 + (cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))^2) - sin(q3)*(l3*q2ddot + g*cos(q2) + l1*q1dot^2*sin(q2) + l3*q1dot^2*cos(q2)*sin(q2)) - l6*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))*(q4dot + q1dot*sin(q2 + q3))) - rc55z*((q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))^2 + (sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3)))^2) + rc55y*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3)))*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3))) + rc55x*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3)))*(q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))

Forţele exterioare ale robotului articulat 5R

F33 =

M3*(q1ddot*rc33z*sin(q2 + q3) - q1dot^2*rc33x - l3*q1ddot*cos(q2) - q1ddot*rc33y*cos(q2 + q3) - l1*q1ddot + 2*q1dot*q2dot*rc33z*cos(q2 + q3) + 2*q1dot*q3dot*rc33z*cos(q2 + q3) + 2*q1dot*q2dot*rc33y*sin(q2 + q3) + 2*q1dot*q3dot*rc33y*sin(q2 + q3) + 2*l3*q1dot*q2dot*sin(q2))

M3*sin(q3)*(l3*q2ddot + g*cos(q2) + l1*q1dot^2*sin(q2) + (l3*q1dot^2*sin(2*q2))/2) - M3*rc33z*(q2ddot + q3ddot) - M3*cos(q3)*(l3*((q1dot^2*cos(2*q2))/2 + q1dot^2/2 + q2dot^2) - g*sin(q2) + l1*q1dot^2*cos(q2)) - M3*rc33y*((q1dot^2*cos(2*q2 + 2*q3))/2 + (q2dot + q3dot)^2 + q1dot^2/2) + M3*q1ddot*rc33x*cos(q2 + q3) + (M3*q1dot^2*rc33z*sin(2*q2 + 2*q3))/2

M3*rc33y*(q2ddot + q3ddot) - M3*(2*sin(q3/2)^2 - 1)*(l3*q2ddot - g*(2*sin(q2/2)^2 - 1) +l1*q1dot^2*sin(q2) + (l3*q1dot^2*sin(2*q2))/2) - M3*rc33z*(q1dot^2*sin(q2 + q3)^2 + (q2dot + q3dot)^2) - M3*sin(q3)*(g*sin(q2) - l3*(q1dot^2/2 + q2dot^2 - q1dot^2*(sin(q2)^2 - 1/2)) + l1*q1dot^2*(2*sin(q2/2)^2 - 1)) - M3*q1ddot*rc33x*sin(q2 + q3) + (M3*q1dot^2*rc33y*sin(2*q2 + 2*q3))/2

F44 =

24

M4*rc44z*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) - M4*rc44y*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot)) - M4*sin(q4)*(l4*(q2ddot + q3ddot) + cos(q3)*(l3*q2ddot + g*cos(q2) + l1*q1dot^2*sin(q2) + (l3*q1dot^2*sin(2*q2))/2) + sin(q3)*(l3*((cos(2*q2)/2 + 1/2)*q1dot^2 + q2dot^2) - g*sin(q2) + l1*q1dot^2*cos(q2)) + (l4*q1dot^2*sin(2*q2 + 2*q3))/2) - M4*rc44x*((sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))^2 + (q4dot + q1dot*sin(q2 + q3))^2) + M4*cos(q4)*(l4*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) - l1*q1ddot - l3*q1ddot*cos(q2) + l4*q1dot*sin(q2 + q3)*(q2dot + q3dot) + 2*l3*q1dot*q2dot*sin(q2)) + M4*rc44z*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) + M4*rc44y*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))*(q4dot + q1dot*sin(q2 + q3))

M4*rc44x*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot)) - M4*cos(q3)*(l3*(q1dot^2*cos(q2)^2 + q2dot^2) - g*sin(q2) + l1*q1dot^2*cos(q2)) - M4*l4*(q1dot^2*cos(q2 + q3)^2 + (q2dot + q3dot)^2) - M4*rc44z*(sin(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) - q4dot*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q4)*(q2ddot + q3ddot)) - M4*rc44y*((sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))^2 + (cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))^2) + M4*sin(q3)*(l3*q2ddot + g*cos(q2) + l1*q1dot^2*sin(q2) + l3*q1dot^2*cos(q2)*sin(q2)) + M4*rc44x*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))*(q4dot + q1dot*sin(q2 + q3)) + M4*rc44z*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))*(q4dot + q1dot*sin(q2 + q3))

M4*rc44y*(sin(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) - q4dot*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q4)*(q2ddot + q3ddot)) - M4*rc44x*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) + M4*cos(q4)*(l4*(q2ddot + q3ddot) + cos(q3)*(l3*q2ddot + g*cos(q2) + l1*q1dot^2*sin(q2) + (l3*q1dot^2*sin(2*q2))/2) + sin(q3)*(l3*((cos(2*q2)/2 + 1/2)*q1dot^2 + q2dot^2) - g*sin(q2) + l1*q1dot^2*cos(q2)) + (l4*q1dot^2*sin(2*q2 + 2*q3))/2) - M4*rc44z*((cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))^2 + (q4dot + q1dot*sin(q2 + q3))^2) + M4*sin(q4)*(l4*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) - l1*q1ddot - l3*q1ddot*cos(q2) + l4*q1dot*sin(q2 + q3)*(q2dot + q3dot) + 2*l3*q1dot*q2dot*sin(q2)) + M4*rc44x*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) + M4*rc44y*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))*(q4dot + q1dot*sin(q2 + q3))

F55 =

M5*rc55y*(q5dot*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3))) + sin(q5)*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) - cos(q5)*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot))) - M5*l5*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot)) + M5*rc55z*(q5dot*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3))) + cos(q5)*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) + sin(q5)*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) – cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot))) - M5*sin(q4)*(l4*(q2ddot + q3ddot) + cos(q3)*(l3*q2ddot + g*cos(q2) + l1*q1dot^2*sin(q2) + (l3*q1dot^2*sin(2*q2))/2) + sin(q3)*(l3*((cos(2*q2)/2 + 1/2)*q1dot^2 + q2dot^2) - g*sin(q2) + l1*q1dot^2*cos(q2)) + (l4*q1dot^2*sin(2*q2 + 2*q3))/2) - M5*l6*((sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))^2 + (q4dot + q1dot*sin(q2 + q3))^2) + M5*cos(q4)*(l4*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) -

25

q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) - l1*q1ddot - l3*q1ddot*cos(q2) + l4*q1dot*sin(q2 + q3)*(q2dot + q3dot) + 2*l3*q1dot*q2dot*sin(q2)) - M5*rc55x*((sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3)))^2 + (cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3)))^2) + M5*rc55y*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3)))*(q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) + M5*rc55z*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3)))*(q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) + M5*l5*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))*(q4dot + q1dot*sin(q2 + q3))

M5*sin(q5)*(l5*(sin(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) - q4dot*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q4)*(q2ddot + q3ddot)) + cos(q4)*(l4*(q2ddot + q3ddot) + cos(q3)*(l3*q2ddot + g*cos(q2) + l1*q1dot^2*sin(q2) + (l3*q1dot^2*sin(2*q2))/2) + sin(q3)*(l3*((cos(2*q2)/2 + 1/2)*q1dot^2 + q2dot^2) - g*sin(q2) + l1*q1dot^2*cos(q2)) + (l4*q1dot^2*sin(2*q2 + 2*q3))/2) + sin(q4)*(l4*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) - l1*q1ddot - l3*q1ddot*cos(q2) + l4*q1dot*sin(q2 + q3)*(q2dot + q3dot) + 2*l3*q1dot*q2dot*sin(q2)) - l6*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) + l6*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) + l5*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))*(q4dot + q1dot*sin(q2 + q3))) - M5*rc55y*((q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))^2 + (cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3)))^2) - M5*rc55x*(q5dot*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3))) + sin(q5)*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) - cos(q5)*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot))) - M5*rc55z*(q5ddot - q4dot*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + sin(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + cos(q4)*(q2ddot + q3ddot)) - M5*cos(q5)*(l4*(q1dot^2*cos(q2 + q3)^2 + (q2dot + q3dot)^2) - l6*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot)) + cos(q3)*(l3*(q1dot^2*cos(q2)^2 + q2dot^2) - g*sin(q2) + l1*q1dot^2*cos(q2)) + l5*((sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))^2 + (cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))^2) - sin(q3)*(l3*q2ddot + g*cos(q2) + l1*q1dot^2*sin(q2) + l3*q1dot^2*cos(q2)*sin(q2)) - l6*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))*(q4dot + q1dot*sin(q2 + q3))) + M5*rc55x*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3)))*(q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) + M5*rc55z*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3)))*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3)))

M5*cos(q5)*(l5*(sin(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) - q4dot*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q4)*(q2ddot + q3ddot)) + cos(q4)*(l4*(q2ddot + q3ddot) + cos(q3)*(l3*q2ddot + g*cos(q2) + l1*q1dot^2*sin(q2) + (l3*q1dot^2*sin(2*q2))/2) + sin(q3)*(l3*((cos(2*q2)/2 + 1/2)*q1dot^2 + q2dot^2) - g*sin(q2) + l1*q1dot^2*cos(q2)) + (l4*q1dot^2*sin(2*q2 + 2*q3))/2) + sin(q4)*(l4*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) - l1*q1ddot - l3*q1ddot*cos(q2) + l4*q1dot*sin(q2 + q3)*(q2dot + q3dot) + 2*l3*q1dot*q2dot*sin(q2)) - l6*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) + l6*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) + l5*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))*(q4dot + q1dot*sin(q2 + q3))) - M5*rc55z*((q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))^2 + (sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3)))^2) - M5*rc55x*(q5dot*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3))) + cos(q5)*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 +

26

q3) + q1dot*q3dot*cos(q2 + q3)) + sin(q5)*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot))) + M5*rc55y*(q5ddot - q4dot*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + sin(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + cos(q4)*(q2ddot + q3ddot)) + M5*sin(q5)*(l4*(q1dot^2*cos(q2 + q3)^2 + (q2dot + q3dot)^2) - l6*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot)) + cos(q3)*(l3*(q1dot^2*cos(q2)^2 + q2dot^2) - g*sin(q2) + l1*q1dot^2*cos(q2)) + l5*((sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))^2 + (cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))^2) - sin(q3)*(l3*q2ddot + g*cos(q2) + l1*q1dot^2*sin(q2) + l3*q1dot^2*cos(q2)*sin(q2)) - l6*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))*(q4dot + q1dot*sin(q2 + q3))) + M5*rc55x*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3)))*(q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) + M5*rc55y*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3)))*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3)))

Momentele forţele exterioare ale robotului articulat 5R

N33 =

(q2dot + q3dot)*(Iyx3s*q1dot*cos(q2 + q3) - Izx3s*q1dot*sin(q2 + q3)) - Ixy3s*(cos(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) + sin(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*cos(q2 + q3)) + Ixz3s*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + Ix3s*(q2ddot + q3ddot) + q1dot*cos(q2 + q3)*(Iyz3s*q1dot*cos(q2 + q3) + Iz3s*q1dot*sin(q2 + q3)) - q1dot*sin(q2 + q3)*(Iy3s*q1dot*cos(q2 + q3) + Izy3s*q1dot*sin(q2 + q3))

(Izx3s*(q2dot + q3dot) + Ix3s*q1dot*cos(q2 + q3))*(q2dot + q3dot) + Iy3s*(cos(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) + sin(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*cos(q2 + q3)) + Iyz3s*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) - Iyx3s*(q2ddot + q3ddot) - q1dot*cos(q2 + q3)*(Iz3s*(q2dot + q3dot) + Ixz3s*q1dot*cos(q2 + q3)) + q1dot*sin(q2 + q3)*(Izy3s*(q2dot + q3dot) - Ixy3s*q1dot*cos(q2 + q3))

q1dot*sin(q2 + q3)*(Iy3s*(q2dot + q3dot) + Ixy3s*q1dot*sin(q2 + q3)) - Izy3s*(cos(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) + sin(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*cos(q2 + q3)) - Iz3s*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) - Izx3s*(q2ddot + q3ddot) - q1dot*cos(q2 + q3)*(Iyz3s*(q2dot + q3dot) - Ixz3s*q1dot*sin(q2 + q3)) - (Iyx3s*(q2dot + q3dot) + Ix3s*q1dot*sin(q2 + q3))*(q2dot + q3dot)

N44 =

(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))*(Iyz4s*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + Iz4s*(q4dot + q1dot*sin(q2 + q3))) + (cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))*(Iyx4s*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - Izx4s*(q4dot + q1dot*sin(q2 + q3))) - Ixy4s*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) + Ix4s*(sin(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) - q4dot*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q4)*(q2ddot + q3ddot)) - Ixz4s*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot)) - (Iy4s*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + Izy4s*(q4dot + q1dot*sin(q2 + q3)))*(q4dot + q1dot*sin(q2 + q3))

(Ix4s*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + Izx4s*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)))*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) -

27

(Ixz4s*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + Iz4s*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)))*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - (Ixy4s*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - Izy4s*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)))*(q4dot + q1dot*sin(q2 + q3)) + Iy4s*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) - Iyx4s*(sin(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) - q4dot*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q4)*(q2ddot + q3ddot)) - Iyz4s*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot))

Iz4s*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot)) - (sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))*(Iyz4s*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - Ixz4s*(q4dot + q1dot*sin(q2 + q3))) - Izy4s*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) - (cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))*(Iyx4s*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) + Ix4s*(q4dot + q1dot*sin(q2 + q3))) - Izx4s*(sin(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) - q4dot*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q4)*(q2ddot + q3ddot)) + (Iy4s*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) + Ixy4s*(q4dot + q1dot*sin(q2 + q3)))*(q4dot + q1dot*sin(q2 + q3))

N55 =

Ixz5s*(q5dot*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3))) + sin(q5)*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) - cos(q5)*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot))) - Ixy5s*(q5dot*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3))) + cos(q5)*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) + sin(q5)*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot))) - (sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3)))*(Iy5s*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3))) + Izy5s*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3)))) + (cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3)))*(Iyz5s*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3))) + Iz5s*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3)))) + Ix5s*(q5ddot - q4dot*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + sin(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + cos(q4)*(q2ddot + q3ddot)) + (Iyx5s*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3))) - Izx5s*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3))))*(q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))

Iy5s*(q5dot*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3))) + cos(q5)*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) + sin(q5)*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot))) + Iyz5s*(q5dot*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3))) + sin(q5)*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) - cos(q5)*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) -

28

cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot))) - Iyx5s*(q5ddot - q4dot*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + sin(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + cos(q4)*(q2ddot + q3ddot)) - (cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3)))*(Ixz5s*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3))) + Iz5s*(q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))) - (sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3)))*(Ixy5s*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3))) - Izy5s*(q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))) + (Ix5s*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3))) + Izx5s*(q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)))*(q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))

(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3)))*(Ixy5s*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3))) + Iy5s*(q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))) - Izy5s*(q5dot*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3))) + cos(q5)*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) + sin(q5)*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot))) - Izx5s*(q5ddot - q4dot*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + sin(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + cos(q4)*(q2ddot + q3ddot)) - Iz5s*(q5dot*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3))) + sin(q5)*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) - cos(q5)*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot))) + (cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3)))*(Ixz5s*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3))) - Iyz5s*(q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))) - (Ix5s*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3))) + Iyx5s*(q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)))*(q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))

Forţele de legătură ale robotului articulat 5R

f55 =

fx6 - M5*l5*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot)) + M5*rc55y*(q5dot*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3))) + sin(q5)*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) - cos(q5)*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot))) + M5*rc55z*(q5dot*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3))) + cos(q5)*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) + sin(q5)*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot))) - M5*sin(q4)*(l4*(q2ddot + q3ddot) + cos(q3)*(l3*q2ddot + g*cos(q2) + l1*q1dot^2*sin(q2) + (l3*q1dot^2*sin(2*q2))/2) + sin(q3)*(l3*((cos(2*q2)/2 + 1/2)*q1dot^2 + q2dot^2) - g*sin(q2) + l1*q1dot^2*cos(q2)) +

29

(l4*q1dot^2*sin(2*q2 + 2*q3))/2) - M5*l6*((sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))^2 + (q4dot + q1dot*sin(q2 + q3))^2) + M5*cos(q4)*(l4*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) - l1*q1ddot - l3*q1ddot*cos(q2) + l4*q1dot*sin(q2 + q3)*(q2dot + q3dot) + 2*l3*q1dot*q2dot*sin(q2)) - M5*rc55x*((sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3)))^2 + (cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3)))^2) + M5*rc55y*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3)))*(q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) + M5*rc55z*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3)))*(q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) + M5*l5*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))*(q4dot + q1dot*sin(q2 + q3))

fy6 - M5*rc55y*((q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))^2 + (cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3)))^2) + M5*sin(q5)*(l5*(sin(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) - q4dot*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q4)*(q2ddot + q3ddot)) + cos(q4)*(l4*(q2ddot + q3ddot) + cos(q3)*(l3*q2ddot + g*cos(q2) + l1*q1dot^2*sin(q2) + (l3*q1dot^2*sin(2*q2))/2) + sin(q3)*(l3*((cos(2*q2)/2 + 1/2)*q1dot^2 + q2dot^2) - g*sin(q2) + l1*q1dot^2*cos(q2)) + (l4*q1dot^2*sin(2*q2 + 2*q3))/2) + sin(q4)*(l4*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) - l1*q1ddot - l3*q1ddot*cos(q2) + l4*q1dot*sin(q2 + q3)*(q2dot + q3dot) + 2*l3*q1dot*q2dot*sin(q2)) - l6*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) + l6*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) + l5*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))*(q4dot + q1dot*sin(q2 + q3))) - M5*rc55x*(q5dot*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3))) + sin(q5)*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) - cos(q5)*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot))) - M5*rc55z*(q5ddot - q4dot*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + sin(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + cos(q4)*(q2ddot + q3ddot)) - M5*cos(q5)*(l4*(q1dot^2*cos(q2 + q3)^2 + (q2dot + q3dot)^2) - l6*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot)) + cos(q3)*(l3*(q1dot^2*cos(q2)^2 + q2dot^2) - g*sin(q2) + l1*q1dot^2*cos(q2)) + l5*((sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))^2 + (cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))^2) - sin(q3)*(l3*q2ddot + g*cos(q2) + l1*q1dot^2*sin(q2) + l3*q1dot^2*cos(q2)*sin(q2)) - l6*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))*(q4dot + q1dot*sin(q2 + q3))) + M5*rc55x*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3)))*(q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) + M5*rc55z*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3)))*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3)))

fz6 - M5*rc55z*((q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))^2 + (sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3)))^2) + M5*cos(q5)*(l5*(sin(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) - q4dot*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q4)*(q2ddot + q3ddot)) + cos(q4)*(l4*(q2ddot + q3ddot) + cos(q3)*(l3*q2ddot + g*cos(q2) + l1*q1dot^2*sin(q2) + (l3*q1dot^2*sin(2*q2))/2) + sin(q3)*(l3*((cos(2*q2)/2 + 1/2)*q1dot^2 + q2dot^2) - g*sin(q2) + l1*q1dot^2*cos(q2)) + (l4*q1dot^2*sin(2*q2 + 2*q3))/2) + sin(q4)*(l4*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) - l1*q1ddot - l3*q1ddot*cos(q2) + l4*q1dot*sin(q2 + q3)*(q2dot + q3dot) + 2*l3*q1dot*q2dot*sin(q2)) - l6*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) + l6*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) +

30

l5*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))*(q4dot + q1dot*sin(q2 + q3))) - M5*rc55x*(q5dot*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3))) + cos(q5)*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) + sin(q5)*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot))) + M5*rc55y*(q5ddot - q4dot*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + sin(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + cos(q4)*(q2ddot + q3ddot)) + M5*sin(q5)*(l4*(q1dot^2*cos(q2 + q3)^2 + (q2dot + q3dot)^2) - l6*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot)) + cos(q3)*(l3*(q1dot^2*cos(q2)^2 + q2dot^2) - g*sin(q2) + l1*q1dot^2*cos(q2)) + l5*((sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))^2 + (cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))^2) - sin(q3)*(l3*q2ddot + g*cos(q2) + l1*q1dot^2*sin(q2) + l3*q1dot^2*cos(q2)*sin(q2)) - l6*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))*(q4dot + q1dot*sin(q2 + q3))) + M5*rc55x*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3)))*(q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) + M5*rc55y*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3)))*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3)))

f44 =

fx6 + M4*rc44z*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) - M5*l5*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot)) - M4*rc44y*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot)) + M5*rc55y*(q5dot*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3))) + sin(q5)*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) - cos(q5)*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot))) + M5*rc55z*(q5dot*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3))) + cos(q5)*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) + sin(q5)*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot))) - M4*sin(q4)*(l4*(q2ddot + q3ddot) + cos(q3)*(l3*q2ddot + g*cos(q2) + l1*q1dot^2*sin(q2) + 1/2*l3*q1dot^2*sin(2*q2)) + sin(q3)*(l3*((1/2*cos(2*q2) + 1/2)*q1dot^2 + q2dot^2) - g*sin(q2) + l1*q1dot^2*cos(q2)) + 1/2*l4*q1dot^2*sin(2*q2 + 2*q3)) - M5*sin(q4)*(l4*(q2ddot + q3ddot) + cos(q3)*(l3*q2ddot + g*cos(q2) + l1*q1dot^2*sin(q2) + 1/2*l3*q1dot^2*sin(2*q2)) + sin(q3)*(l3*((1/2*cos(2*q2) + 1/2)*q1dot^2 + q2dot^2) - g*sin(q2) + l1*q1dot^2*cos(q2)) + 1/2*l4*q1dot^2*sin(2*q2 + 2*q3)) - M5*l6*((sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))^2 + (q4dot + q1dot*sin(q2 + q3))^2) - M4*rc44x*((sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))^2 + (q4dot + q1dot*sin(q2 + q3))^2) + M4*cos(q4)*(l4*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) - l1*q1ddot - l3*q1ddot*cos(q2) + l4*q1dot*sin(q2 + q3)*(q2dot + q3dot) + 2*l3*q1dot*q2dot*sin(q2)) + M5*cos(q4)*(l4*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) - l1*q1ddot - l3*q1ddot*cos(q2) + l4*q1dot*sin(q2 + q3)*(q2dot + q3dot) + 2*l3*q1dot*q2dot*sin(q2)) - M5*rc55x*((sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3)))^2 + (cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3)))^2) + M5*rc55y*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3)))*(q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) + M5*rc55z*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot +

31

q1dot*sin(q2 + q3)))*(q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) + M4*rc44z*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) + M5*l5*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))*(q4dot + q1dot*sin(q2 + q3)) + M4*rc44y*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))*(q4dot + q1dot*sin(q2 + q3))

fy6*cos(q5) - fz6*sin(q5) - M4*l4*q1dot^2 - M4*l4*q2dot^2 - M5*l4*q1dot^2 - M4*l4*q3dot^2 - M5*l4*q2dot^2 - M5*l5*q1dot^2 - M5*l4*q3dot^2 - M5*l5*q2dot^2 - M5*l5*q3dot^2 - M4*q1dot^2*rc44y - M4*q2dot^2*rc44y - M4*q3dot^2*rc44y - 2*M4*q2dot*q3dot*rc44y + M4*l4*q1dot^2*cos(q2)^2 + M4*l4*q1dot^2*cos(q3)^2 + M5*l4*q1dot^2*cos(q2)^2 + M5*l4*q1dot^2*cos(q3)^2 + M5*l5*q1dot^2*cos(q2)^2 + M5*l5*q1dot^2*cos(q3)^2 + M4*q1dot^2*rc44y*cos(q2)^2 + M4*q1dot^2*rc44y*cos(q3)^2 - M4*q2ddot*rc44z*cos(q4) - M4*q3ddot*rc44z*cos(q4) - M5*q5ddot*rc55z*cos(q5) + M4*l3*q2ddot*sin(q3) + M5*l3*q2ddot*sin(q3) + M5*l6*q2ddot*sin(q4) + M5*l6*q3ddot*sin(q4) + M4*q2ddot*rc44x*sin(q4) + M4*q3ddot*rc44x*sin(q4) + M5*q2ddot*rc55x*sin(q4) + M5*q3ddot*rc55x*sin(q4) - M5*q5ddot*rc55y*sin(q5) - M4*l3*q2dot^2*cos(q3) - M5*l3*q2dot^2*cos(q3) - M5*q1dot^2*rc55y*cos(q5) - M5*q2dot^2*rc55y*cos(q5) - M5*q3dot^2*rc55y*cos(q5) - M5*q5dot^2*rc55y*cos(q5) + M4*g*cos(q2)*sin(q3) + M4*g*cos(q3)*sin(q2) + M5*g*cos(q2)*sin(q3) + M5*g*cos(q3)*sin(q2) + M5*q1dot^2*rc55z*sin(q5) + M5*q2dot^2*rc55z*sin(q5) + M5*q3dot^2*rc55z*sin(q5) + M5*q5dot^2*rc55z*sin(q5) - 2*M4*l4*q2dot*q3dot - 2*M5*l4*q2dot*q3dot - 2*M5*l5*q2dot*q3dot - M5*q1dot^2*rc55z*cos(q2)^2*sin(q5) - M5*q1dot^2*rc55z*cos(q3)^2*sin(q5) - M5*q2ddot*rc55z*cos(q4)*cos(q5) - M5*q3ddot*rc55z*cos(q4)*cos(q5) - M5*q2ddot*rc55y*cos(q4)*sin(q5) - M5*q3ddot*rc55y*cos(q4)*sin(q5) - 2*M4*l4*q1dot^2*cos(q2)^2*cos(q3)^2 - 2*M5*l4*q1dot^2*cos(q2)^2*cos(q3)^2 - 2*M5*l5*q1dot^2*cos(q2)^2*cos(q3)^2 - 2*M4*q1dot^2*rc44y*cos(q2)^2*cos(q3)^2 - M4*l1*q1dot^2*cos(q2)*cos(q3) - M5*l1*q1dot^2*cos(q2)*cos(q3) + M4*l1*q1dot^2*sin(q2)*sin(q3) + M5*l1*q1dot^2*sin(q2)*sin(q3) + 2*M5*l6*q2dot*q4dot*cos(q4) + 2*M5*l6*q3dot*q4dot*cos(q4) + 2*M4*q2dot*q4dot*rc44x*cos(q4) + 2*M4*q3dot*q4dot*rc44x*cos(q4) + 2*M5*q2dot*q4dot*rc55x*cos(q4) - 2*M5*q2dot*q3dot*rc55y*cos(q5) + 2*M5*q3dot*q4dot*rc55x*cos(q4) + 2*M4*q2dot*q4dot*rc44z*sin(q4) + 2*M4*q3dot*q4dot*rc44z*sin(q4) + 2*M5*q2dot*q3dot*rc55z*sin(q5) - M4*l3*q1dot^2*cos(q2)^2*cos(q3) - M5*l3*q1dot^2*cos(q2)^2*cos(q3) + M5*q1dot^2*rc55y*cos(q2)^2*cos(q5) + M5*q1dot^2*rc55y*cos(q3)^2*cos(q5) + M5*l6*q1ddot*cos(q2)*cos(q3)*cos(q4) + M4*q1ddot*rc44x*cos(q2)*cos(q3)*cos(q4) + M5*q1ddot*rc55x*cos(q2)*cos(q3)*cos(q4) + M4*q1ddot*rc44z*cos(q2)*cos(q3)*sin(q4) - M5*l6*q1ddot*cos(q4)*sin(q2)*sin(q3) - 2*M5*q1dot^2*rc55y*cos(q2)^2*cos(q3)^2*cos(q5) - M4*q1ddot*rc44x*cos(q4)*sin(q2)*sin(q3) - M5*q1ddot*rc55x*cos(q4)*sin(q2)*sin(q3) + 2*M5*q1dot^2*rc55z*cos(q2)^2*cos(q3)^2*sin(q5) - M4*q1ddot*rc44z*sin(q2)*sin(q3)*sin(q4) - M4*q1dot^2*rc44z*cos(q2)*cos(q4)*sin(q2) - M4*q1dot^2*rc44z*cos(q3)*cos(q4)*sin(q3) + M4*l3*q1dot^2*cos(q2)*sin(q2)*sin(q3) + M5*l3*q1dot^2*cos(q2)*sin(q2)*sin(q3) + M5*l6*q1dot^2*cos(q2)*sin(q2)*sin(q4) + M5*l6*q1dot^2*cos(q3)*sin(q3)*sin(q4) + M4*q1dot^2*rc44x*cos(q2)*sin(q2)*sin(q4) + M5*q1dot^2*rc55x*cos(q2)*sin(q2)*sin(q4) + M4*q1dot^2*rc44x*cos(q3)*sin(q3)*sin(q4) + M5*q1dot^2*rc55x*cos(q3)*sin(q3)*sin(q4) - 2*M5*q2dot*q5dot*rc55y*cos(q4)*cos(q5) - 2*M5*q3dot*q5dot*rc55y*cos(q4)*cos(q5) + 2*M5*q2dot*q4dot*rc55z*cos(q5)*sin(q4) + 2*M5*q2dot*q5dot*rc55z*cos(q4)*sin(q5) + 2*M5*q3dot*q4dot*rc55z*cos(q5)*sin(q4) + 2*M5*q3dot*q5dot*rc55z*cos(q4)*sin(q5) + 2*M5*q2dot*q4dot*rc55y*sin(q4)*sin(q5) + 2*M5*q3dot*q4dot*rc55y*sin(q4)*sin(q5) + M5*q1ddot*rc55y*cos(q2)*cos(q3)*sin(q4)*sin(q5) - M5*q1ddot*rc55z*cos(q5)*sin(q2)*sin(q3)*sin(q4) - M5*q1ddot*rc55y*sin(q2)*sin(q3)*sin(q4)*sin(q5) - M5*q1dot^2*rc55z*cos(q2)*cos(q4)*cos(q5)*sin(q2) - M5*q1dot^2*rc55z*cos(q3)*cos(q4)*cos(q5)*sin(q3) + 2*M4*l4*q1dot^2*cos(q2)*cos(q3)*sin(q2)*sin(q3) + 2*M5*l4*q1dot^2*cos(q2)*cos(q3)*sin(q2)*sin(q3) + 2*M5*l5*q1dot^2*cos(q2)*cos(q3)*sin(q2)*sin(q3) + 2*M4*q1dot^2*rc44y*cos(q2)*cos(q3)*sin(q2)*sin(q3) - M5*q1dot^2*rc55y*cos(q2)*cos(q4)*sin(q2)*sin(q5) - M5*q1dot^2*rc55y*cos(q3)*cos(q4)*sin(q3)*sin(q5) + 2*M4*q1dot*q4dot*rc44z*cos(q2)*cos(q3)*cos(q4) - 2*M5*l6*q1dot*q4dot*cos(q2)*cos(q3)*sin(q4) - 2*M4*q1dot*q4dot*rc44x*cos(q2)*cos(q3)*sin(q4) - 2*M5*q1dot*q4dot*rc55x*cos(q2)*cos(q3)*sin(q4) - 2*M4*q1dot*q4dot*rc44z*cos(q4)*sin(q2)*sin(q3) + 2*M5*l6*q1dot*q4dot*sin(q2)*sin(q3)*sin(q4) + 2*M4*q1dot*q4dot*rc44x*sin(q2)*sin(q3)*sin(q4) + 2*M5*q1dot*q4dot*rc55x*sin(q2)*sin(q3)*sin(q4) + 2*M4*q1dot^2*rc44z*cos(q2)*cos(q3)^2*cos(q4)*sin(q2) + 2*M4*q1dot^2*rc44z*cos(q2)^2*

32

cos(q3)*cos(q4)*sin(q3) - 2*M5*l6*q1dot^2*cos(q2)*cos(q3)^2*sin(q2)*sin(q4) - 2*M5*l6*q1dot^2*cos(q2)^2*cos(q3)*sin(q3)*sin(q4) - 2*M4*q1dot^2*rc44x*cos(q2)*cos(q3)^2*sin(q2)*sin(q4) - 2*M4*q1dot^2*rc44x*cos(q2)^2*cos(q3)*sin(q3)*sin(q4) - 2*M5*q1dot^2*rc55x*cos(q2)*cos(q3)^2*sin(q2)*sin(q4) - 2*M5*q1dot^2*rc55x*cos(q2)^2*cos(q3)*sin(q3)*sin(q4) + M5*q1ddot*rc55z*cos(q2)*cos(q3)*cos(q5)*sin(q4) + 2*M5*q1dot^2*rc55y*cos(q2)*cos(q3)*cos(q5)*sin(q2)*sin(q3) - 2*M5*q1dot^2*rc55z*cos(q2)*cos(q3)*sin(q2)*sin(q3)*sin(q5) + 2*M5*q1dot*q4dot*rc55z*cos(q2)*cos(q3)*cos(q4)*cos(q5) + 2*M5*q1dot*q4dot*rc55y*cos(q2)*cos(q3)*cos(q4)*sin(q5) +2*M5*q1dot*q5dot*rc55y*cos(q2)*cos(q3)*cos(q5)*sin(q4) - 2*M5*q1dot*q4dot*rc55z*cos(q4)*cos(q5)*sin(q2)*sin(q3) - 2*M5*q1dot*q5dot*rc55z*cos(q2)*cos(q3)*sin(q4)*sin(q5) - 2*M5*q1dot*q4dot*rc55y*cos(q4)*sin(q2)*sin(q3)*sin(q5) - 2*M5*q1dot*q5dot*rc55y*cos(q5)*sin(q2)*sin(q3)*sin(q4) + 2*M5*q1dot^2*rc55z*cos(q2)*cos(q3)^2*cos(q4)*cos(q5)*sin(q2) + 2*M5*q1dot^2*rc55z*cos(q2)^2*cos(q3)*cos(q4)*cos(q5)*sin(q3) + 2*M5*q1dot*q5dot*rc55z*sin(q2)*sin(q3)*sin(q4)*sin(q5) + 2*M5*q1dot^2*rc55y*cos(q2)*cos(q3)^2*cos(q4)*sin(q2)*sin(q5) + 2*M5*q1dot^2*rc55y*cos(q2)^2*cos(q3)*cos(q4)*sin(q3)*sin(q5)

fz6*cos(q5) + fy6*sin(q5) - M5*l6*q4ddot - M4*q4ddot*rc44x - M5*q4ddot*rc55x - M4*q1dot^2*rc44z - M4*q4dot^2*rc44z - M4*q2dot^2*rc44z*cos(q4)^2 - M4*q3dot^2*rc44z*cos(q4)^2 + M4*l4*q2ddot*cos(q4) + M4*l4*q3ddot*cos(q4) + M5*l4*q2ddot*cos(q4) + M5*l4*q3ddot*cos(q4) + M5*l5*q2ddot*cos(q4) + M5*l5*q3ddot*cos(q4) + M4*q2ddot*rc44y*cos(q4) + M4*q3ddot*rc44y*cos(q4) + M5*q5ddot*rc55y*cos(q5) - M4*l1*q1ddot*sin(q4) - M5*l1*q1ddot*sin(q4) - M5*q5ddot*rc55z*sin(q5) - M5*q1dot^2*rc55z*cos(q5) - M5*q4dot^2*rc55z*cos(q5) - M5*q5dot^2*rc55z*cos(q5) - M5*q1dot^2*rc55y*sin(q5) - M5*q4dot^2*rc55y*sin(q5) - M5*q5dot^2*rc55y*sin(q5) - M5*q2dot^2*rc55y*cos(q4)^2*sin(q5) - M5*q3dot^2*rc55y*cos(q4)^2*sin(q5) + M4*l3*q2ddot*cos(q3)*cos(q4) + M5*l3*q2ddot*cos(q3)*cos(q4) - 2*M4*q2dot*q3dot*rc44z*cos(q4)^2 + M5*q2ddot*rc55y*cos(q4)*cos(q5) + M5*q3ddot*rc55y*cos(q4)*cos(q5) - M4*l3*q1ddot*cos(q2)*sin(q4) - M5*l3*q1ddot*cos(q2)*sin(q4) - M5*l6*q1ddot*cos(q2)*sin(q3) - M5*l6*q1ddot*cos(q3)*sin(q2) - M4*q1ddot*rc44x*cos(q2)*sin(q3) - M4*q1ddot*rc44x*cos(q3)*sin(q2) - M5*q1ddot*rc55x*cos(q2)*sin(q3) - M5*q1ddot*rc55x*cos(q3)*sin(q2) - M5*q2ddot*rc55z*cos(q4)*sin(q5) - M5*q3ddot*rc55z*cos(q4)*sin(q5) + M4*g*cos(q2)*cos(q3)*cos(q4) + M5*g*cos(q2)*cos(q3)*cos(q4) + 1/2*M4*l3*q1dot^2*cos(q4)*sin(q3) + M4*l3*q2dot^2*cos(q4)*sin(q3) + 1/2*M5*l3*q1dot^2*cos(q4)*sin(q3) + M5*l3*q2dot^2*cos(q4)*sin(q3) + M5*l6*q2dot^2*cos(q4)*sin(q4) + M5*l6*q3dot^2*cos(q4)*sin(q4) + M4*q2dot^2*rc44x*cos(q4)*sin(q4) + M4*q3dot^2*rc44x*cos(q4)*sin(q4) + M5*q2dot^2*rc55x*cos(q4)*sin(q4) + M5*q3dot^2*rc55x*cos(q4)*sin(q4) - M4*g*cos(q4)*sin(q2)*sin(q3) - M5*g*cos(q4)*sin(q2)*sin(q3) - M5*q2dot^2*rc55z*cos(q4)^2*cos(q5) - M5*q3dot^2*rc55z*cos(q4)^2*cos(q5) - 2*M5*q2dot*q3dot*rc55z*cos(q4)^2*cos(q5) - M4*l4*q1ddot*cos(q2)*cos(q3)*sin(q4) - M5*l4*q1ddot*cos(q2)*cos(q3)*sin(q4) - M5*l5*q1ddot*cos(q2)*cos(q3)*sin(q4) - 2*M5*q2dot*q3dot*rc55y*cos(q4)^2*sin(q5) - M4*q1ddot*rc44y*cos(q2)*cos(q3)*sin(q4) + M4*l4*q1ddot*sin(q2)*sin(q3)*sin(q4) + M5*l4*q1ddot*sin(q2)*sin(q3)*sin(q4) + M5*l5*q1ddot*sin(q2)*sin(q3)*sin(q4) + M4*q1ddot*rc44y*sin(q2)*sin(q3)*sin(q4) + M4*l1*q1dot^2*cos(q2)*cos(q4)*sin(q3) + M4*l1*q1dot^2*cos(q3)*cos(q4)*sin(q2) + M5*l1*q1dot^2*cos(q2)*cos(q4)*sin(q3) + M5*l1*q1dot^2*cos(q3)*cos(q4)*sin(q2) + M4*q1dot^2*rc44z*cos(q2)^2*cos(q3)^2*cos(q4)^2 - 2*M5*l6*q1dot*q2dot*cos(q2)*cos(q3) - 2*M5*l6*q1dot*q3dot*cos(q2)*cos(q3) - 2*M4*q1dot*q2dot*rc44x*cos(q2)*cos(q3) - 2*M4*q1dot*q3dot*rc44x*cos(q2)*cos(q3) - 2*M5*q1dot*q2dot*rc55x*cos(q2)*cos(q3) - 2*M5*q1dot*q3dot*rc55x*cos(q2)*cos(q3) - 2*M5*q2dot*q5dot*rc55z*cos(q4)*cos(q5) - 2*M5*q3dot*q5dot*rc55z*cos(q4)*cos(q5) + M4*q1dot^2*rc44z*cos(q4)^2*sin(q2)^2*sin(q3)^2 + 2*M5*l6*q2dot*q3dot*cos(q4)*sin(q4) - 2*M4*q1dot*q4dot*rc44z*cos(q2)*sin(q3) - 2*M4*q1dot*q4dot*rc44z*cos(q3)*sin(q2) + 2*M4*q2dot*q3dot*rc44x*cos(q4)*sin(q4) + 2*M5*q2dot*q3dot*rc55x*cos(q4)*sin(q4) - 2*M5*q2dot*q5dot*rc55y*cos(q4)*sin(q5) - 2*M5*q3dot*q5dot*rc55y*cos(q4)*sin(q5) + 2*M4*l3*q1dot*q2dot*sin(q2)*sin(q4) + 2*M5*l3*q1dot*q2dot*sin(q2)*sin(q4) + 2*M5*l6*q1dot*q2dot*sin(q2)*sin(q3) + 2*M5*l6*q1dot*q3dot*sin(q2)*sin(q3) + 2*M4*q1dot*q2dot*rc44x*sin(q2)*sin(q3) + 2*M4*q1dot*q3dot*rc44x*sin(q2)*sin(q3) + 2*M5*q1dot*q2dot*rc55x*sin(q2)*sin(q3) + 2*M5*q1dot*q3dot*rc55x*sin(q2)*sin(q3) + 1/2*M4*l3*q1dot^2*cos(q2)^2*cos(q4)*sin(q3) + 1/2*M5*l3*q1dot^2*cos(q2)^2*cos(q4)*sin(q3) - 1/2*M4*l3*q1dot^2*cos(q4)*sin(q2)^2*sin(q3) -

33

1/2*M5*l3*q1dot^2*cos(q4)*sin(q2)^2*sin(q3) + M5*q1ddot*rc55z*cos(q2)*cos(q3)*sin(q4)*sin(q5) - M5*l6*q1dot^2*cos(q2)^2*cos(q3)^2*cos(q4)*sin(q4) - M4*q1dot^2*rc44x*cos(q2)^2*cos(q3)^2*cos(q4)*sin(q4) - M5*q1dot^2*rc55x*cos(q2)^2*cos(q3)^2*cos(q4)*sin(q4) + M5*q1ddot*rc55y*cos(q5)*sin(q2)*sin(q3)*sin(q4) - M5*q1ddot*rc55z*sin(q2)*sin(q3)*sin(q4)*sin(q5) -M5*l6*q1dot^2*cos(q4)*sin(q2)^2*sin(q3)^2*sin(q4) - M4*q1dot^2*rc44x*cos(q4)*sin(q2)^2*sin(q3)^2*sin(q4) - M5*q1dot^2*rc55x*cos(q4)*sin(q2)^2*sin(q3)^2*sin(q4) + M4*l3*q1dot^2*cos(q2)*cos(q3)*cos(q4)*sin(q2) + M5*l3*q1dot^2*cos(q2)*cos(q3)*cos(q4)*sin(q2) + M5*q1dot^2*rc55z*cos(q2)^2*cos(q3)^2*cos(q4)^2*cos(q5) + M5*q1dot^2*rc55y*cos(q2)^2*cos(q3)^2*cos(q4)^2*sin(q5) + M5*q1dot^2*rc55z*cos(q4)^2*cos(q5)*sin(q2)^2*sin(q3)^2 - 2*M5*q1dot*q4dot*rc55z*cos(q2)*cos(q5)*sin(q3) - 2*M5*q1dot*q4dot*rc55z*cos(q3)*cos(q5)*sin(q2) + M5*q1dot^2*rc55y*cos(q4)^2*sin(q2)^2*sin(q3)^2*sin(q5) + 2*M4*l4*q1dot*q2dot*cos(q2)*sin(q3)*sin(q4) + 2*M4*l4*q1dot*q2dot*cos(q3)*sin(q2)*sin(q4) + 2*M4*l4*q1dot*q3dot*cos(q2)*sin(q3)*sin(q4) + 2*M4*l4*q1dot*q3dot*cos(q3)*sin(q2)*sin(q4) + 2*M5*l4*q1dot*q2dot*cos(q2)*sin(q3)*sin(q4) + 2*M5*l4*q1dot*q2dot*cos(q3)*sin(q2)*sin(q4) + 2*M5*l4*q1dot*q3dot*cos(q2)*sin(q3)*sin(q4) + 2*M5*l4*q1dot*q3dot*cos(q3)*sin(q2)*sin(q4) + 2*M5*l5*q1dot*q2dot*cos(q2)*sin(q3)*sin(q4) + 2*M5*l5*q1dot*q2dot*cos(q3)*sin(q2)*sin(q4) + 2*M5*l5*q1dot*q3dot*cos(q2)*sin(q3)*sin(q4) + 2*M5*l5*q1dot*q3dot*cos(q3)*sin(q2)*sin(q4) + 2*M4*q1dot*q2dot*rc44y*cos(q2)*sin(q3)*sin(q4) + 2*M4*q1dot*q2dot*rc44y*cos(q3)*sin(q2)*sin(q4) + 2*M4*q1dot*q3dot*rc44y*cos(q2)*sin(q3)*sin(q4) + 2*M4*q1dot*q3dot*rc44y*cos(q3)*sin(q2)*sin(q4) - 2*M5*q1dot*q4dot*rc55y*cos(q2)*sin(q3)*sin(q5) - 2*M5*q1dot*q4dot*rc55y*cos(q3)*sin(q2)*sin(q5) + M4*l4*q1dot^2*cos(q2)*cos(q3)^2*cos(q4)*sin(q2) + M4*l4*q1dot^2*cos(q2)^2*cos(q3)*cos(q4)*sin(q3) + M5*l4*q1dot^2*cos(q2)*cos(q3)^2*cos(q4)*sin(q2) + M5*l4*q1dot^2*cos(q2)^2*cos(q3)*cos(q4)*sin(q3) + M5*l5*q1dot^2*cos(q2)*cos(q3)^2*cos(q4)*sin(q2) + M5*l5*q1dot^2*cos(q2)^2*cos(q3)*cos(q4)*sin(q3) + M4*q1dot^2*rc44y*cos(q2)*cos(q3)^2*cos(q4)*sin(q2) + M4*q1dot^2*rc44y*cos(q2)^2*cos(q3)*cos(q4)*sin(q3) - M4*l4*q1dot^2*cos(q2)*cos(q4)*sin(q2)*sin(q3)^2 - M4*l4*q1dot^2*cos(q3)*cos(q4)*sin(q2)^2*sin(q3) - M5*l4*q1dot^2*cos(q2)*cos(q4)*sin(q2)*sin(q3)^2 - M5*l4*q1dot^2*cos(q3)*cos(q4)*sin(q2)^2*sin(q3) - M5*l5*q1dot^2*cos(q2)*cos(q4)*sin(q2)*sin(q3)^2 - M5*l5*q1dot^2*cos(q3)*cos(q4)*sin(q2)^2*sin(q3) - M4*q1dot^2*rc44y*cos(q2)*cos(q4)*sin(q2)*sin(q3)^2 - M4*q1dot^2*rc44y*cos(q3)*cos(q4)*sin(q2)^2*sin(q3) + 2*M5*l6*q1dot*q2dot*cos(q2)*cos(q3)*cos(q4)^2 + 2*M5*l6*q1dot*q3dot*cos(q2)*cos(q3)*cos(q4)^2 + 2*M4*q1dot*q2dot*rc44x*cos(q2)*cos(q3)*cos(q4)^2 + 2*M4*q1dot*q3dot*rc44x*cos(q2)*cos(q3)*cos(q4)^2 + 2*M5*q1dot*q2dot*rc55x*cos(q2)*cos(q3)*cos(q4)^2 + 2*M5*q1dot*q3dot*rc55x*cos(q2)*cos(q3)*cos(q4)^2 - M5*q1ddot*rc55y*cos(q2)*cos(q3)*cos(q5)*sin(q4) - 2*M5*l6*q1dot*q2dot*cos(q4)^2*sin(q2)*sin(q3) - 2*M5*l6*q1dot*q3dot*cos(q4)^2*sin(q2)*sin(q3) - 2*M4*q1dot*q2dot*rc44x*cos(q4)^2*sin(q2)*sin(q3) - 2*M4*q1dot*q3dot*rc44x*cos(q4)^2*sin(q2)*sin(q3) - 2*M5*q1dot*q2dot*rc55x*cos(q4)^2*sin(q2)*sin(q3) -2*M5*q1dot*q3dot*rc55x*cos(q4)^2*sin(q2)*sin(q3) + 2*M4*q1dot*q2dot*rc44z*cos(q2)*cos(q3)*cos(q4)*sin(q4) + 2*M4*q1dot*q3dot*rc44z*cos(q2)*cos(q3)*cos(q4)*sin(q4) + 2*M5*q1dot*q5dot*rc55z*cos(q2)*cos(q3)*cos(q5)*sin(q4) + 2*M5*q1dot*q2dot*rc55y*cos(q2)*cos(q5)*sin(q3)*sin(q4) + 2*M5*q1dot*q2dot*rc55y*cos(q3)*cos(q5)*sin(q2)*sin(q4) + 2*M5*q1dot*q3dot*rc55y*cos(q2)*cos(q5)*sin(q3)*sin(q4) + 2*M5*q1dot*q3dot*rc55y*cos(q3)*cos(q5)*sin(q2)*sin(q4) + 2*M5*q1dot*q5dot*rc55y*cos(q2)*cos(q3)*sin(q4)*sin(q5) - 2*M4*q1dot*q2dot*rc44z*cos(q4)*sin(q2)*sin(q3)*sin(q4) - 2*M4*q1dot*q3dot*rc44z*cos(q4)*sin(q2)*sin(q3)*sin(q4) - 2*M5*q1dot*q2dot*rc55z*cos(q2)*sin(q3)*sin(q4)*sin(q5) - 2*M5*q1dot*q2dot*rc55z*cos(q3)*sin(q2)*sin(q4)*sin(q5) - 2*M5*q1dot*q3dot*rc55z*cos(q2)*sin(q3)*sin(q4)*sin(q5) - 2*M5*q1dot*q3dot*rc55z*cos(q3)*sin(q2)*sin(q4)*sin(q5) - 2*M5*q1dot*q5dot*rc55z*cos(q5)*sin(q2)*sin(q3)*sin(q4) + M5*q1dot^2*rc55y*cos(q2)*cos(q3)^2*cos(q4)*cos(q5)*sin(q2) + M5*q1dot^2*rc55y*cos(q2)^2*cos(q3)*cos(q4)*cos(q5)*sin(q3) - 2*M5*q1dot*q5dot*rc55y*sin(q2)*sin(q3)*sin(q4)*sin(q5) - 2*M4*q1dot^2*rc44z*cos(q2)*cos(q3)*cos(q4)^2*sin(q2)*sin(q3) - M5*q1dot^2*rc55y*cos(q2)*cos(q4)*cos(q5)*sin(q2)*sin(q3)^2 - M5*q1dot^2*rc55z*cos(q2)*cos(q3)^2*cos(q4)*sin(q2)*sin(q5) - M5*q1dot^2*rc55y*cos(q3)*cos(q4)*cos(q5)*sin(q2)^2*sin(q3) - M5*q1dot^2*rc55z*cos(q2)^2*cos(q3)*cos(q4)*sin(q3)*sin(q5) + M5*q1dot^2*rc55z*cos(q2)*cos(q4)*sin(q2)*sin(q3)^2*sin(q5) + M5*q1dot^2*rc55z*cos(q3)*cos(q4)*sin(q2)^2*sin(q3)*sin(q5) + 2*M5*l6*q1dot^2*cos(q2)*cos(q3)*cos(q4)*sin(q2)*sin(q3)*sin(q4) + 2*M4*q1dot^2*rc44x*cos(q2)*cos(q3)*cos(q4)*sin(q2)*sin(q3)*sin(q4) + 2*M5*q1dot^2*rc55x*cos(q2)*cos(q3)*cos(q4)*sin(q2)*sin(q3)*sin(q4) + 2*M5*q1dot*q2dot*rc55z*cos(q2)*cos(q3)*cos(q4)*cos(q5)*sin(q4) + 2*M5*q1dot*q3dot*rc55z*cos(q2)*cos(q3)*cos(q4)*cos(q5)*sin(q4) + 2*M5*q1dot*q2dot*rc55y*cos(q2)*

34

cos(q3)*cos(q4)*sin(q4)*sin(q5) + 2*M5*q1dot*q3dot*rc55y*cos(q2)*cos(q3)*cos(q4)*sin(q4)*sin(q5) - 2*M5*q1dot*q2dot*rc55z*cos(q4)*cos(q5)*sin(q2)*sin(q3)*sin(q4) - 2*M5*q1dot*q3dot*rc55z*cos(q4)*cos(q5)*sin(q2)*sin(q3)*sin(q4) - 2*M5*q1dot*q2dot*rc55y*cos(q4)*sin(q2)*sin(q3)*sin(q4)*sin(q5) - 2*M5*q1dot*q3dot*rc55y*cos(q4)*sin(q2)*sin(q3)*sin(q4)*sin(q5) - 2*M5*q1dot^2*rc55z*cos(q2)*cos(q3)*cos(q4)^2*cos(q5)*sin(q2)*sin(q3) - 2*M5*q1dot^2*rc55y*cos(q2)*cos(q3)*cos(q4)^2*sin(q2)*sin(q3)*sin(q5)

Momentele forţelor de legătură ale robotului articulat 6R

n55 =

nx6 - Ixy5s*(q5dot*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3))) + cos(q5)*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) + sin(q5)*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot))) + Ixz5s*(q5dot*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3))) + sin(q5)*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) - cos(q5)*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot))) + rc55y*(M5*cos(q5)*(l5*(sin(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) - q4dot*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q4)*(q2ddot + q3ddot)) + cos(q4)*(l4*(q2ddot + q3ddot) + cos(q3)*(l3*q2ddot + g*cos(q2) + l1*q1dot^2*sin(q2) + 1/2*l3*q1dot^2*sin(2*q2)) + sin(q3)*(l3*((1/2*cos(2*q2) + 1/2)*q1dot^2 + q2dot^2) - g*sin(q2) + l1*q1dot^2*cos(q2)) + 1/2*l4*q1dot^2*sin(2*q2 + 2*q3)) + sin(q4)*(l4*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) - l1*q1ddot - l3*q1ddot*cos(q2) + l4*q1dot*sin(q2 + q3)*(q2dot + q3dot) + 2*l3*q1dot*q2dot*sin(q2)) - l6*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) + l6*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) + l5*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))*(q4dot + q1dot*sin(q2 + q3))) - M5*rc55z*((q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))^2 + (sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3)))^2) - M5*rc55x*(q5dot*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3))) + cos(q5)*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) + sin(q5)*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot))) + M5*rc55y*(q5ddot - q4dot*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + sin(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + cos(q4)*(q2ddot + q3ddot)) + M5*sin(q5)*(l4*(q1dot^2*cos(q2 + q3)^2 + (q2dot + q3dot)^2) - l6*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot)) + cos(q3)*(l3*(q1dot^2*cos(q2)^2 + q2dot^2) - g*sin(q2) + l1*q1dot^2*cos(q2)) + l5*((sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))^2 + (cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))^2) - sin(q3)*(l3*q2ddot + g*cos(q2) + l1*q1dot^2*sin(q2) + l3*q1dot^2*cos(q2)*sin(q2)) - l6*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))*(q4dot + q1dot*sin(q2 + q3))) + M5*rc55x*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3)))*(q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) + M5*rc55y*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3)))*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3)))) + rc55z*(M5*rc55y*((q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))^2 + (cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3)))^2) - M5*sin(q5)*(l5*(sin(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) - q4dot*(sin(q4)*(q2dot +

35

q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q4)*(q2ddot + q3ddot)) + cos(q4)*(l4*(q2ddot + q3ddot) + cos(q3)*(l3*q2ddot + g*cos(q2) + l1*q1dot^2*sin(q2) + 1/2*l3*q1dot^2*sin(2*q2)) + sin(q3)*(l3*((1/2*cos(2*q2) + 1/2)*q1dot^2 + q2dot^2) - g*sin(q2) + l1*q1dot^2*cos(q2)) + 1/2*l4*q1dot^2*sin(2*q2 + 2*q3)) + sin(q4)*(l4*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) - l1*q1ddot - l3*q1ddot*cos(q2) + l4*q1dot*sin(q2 + q3)*(q2dot + q3dot) + 2*l3*q1dot*q2dot*sin(q2)) - l6*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) + l6*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) + l5*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))*(q4dot + q1dot*sin(q2 + q3))) + M5*rc55x*(q5dot*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3))) + sin(q5)*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) - cos(q5)*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot))) + M5*rc55z*(q5ddot - q4dot*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + sin(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + cos(q4)*(q2ddot + q3ddot)) + M5*cos(q5)*(l4*(q1dot^2*cos(q2 + q3)^2 + (q2dot + q3dot)^2) - l6*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot)) + cos(q3)*(l3*(q1dot^2*cos(q2)^2 + q2dot^2) - g*sin(q2) + l1*q1dot^2*cos(q2)) + l5*((sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))^2 + (cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))^2) - sin(q3)*(l3*q2ddot + g*cos(q2) + l1*q1dot^2*sin(q2) + l3*q1dot^2*cos(q2)*sin(q2)) - l6*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))*(q4dot + q1dot*sin(q2 + q3))) - M5*rc55x*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3)))*(q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - M5*rc55z*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3)))*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3)))) - (sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3)))*(Iy5s*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3))) + Izy5s*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3)))) + (cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3)))*(Iyz5s*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3))) + Iz5s*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3)))) + Ix5s*(q5ddot - q4dot*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + sin(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + cos(q4)*(q2ddot + q3ddot)) + (Iyx5s*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3))) - Izx5s*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3))))*(q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))

ny6 + Iy5s*(q5dot*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3))) + cos(q5)*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) + sin(q5)*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot))) + Iyz5s*(q5dot*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3))) + sin(q5)*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) - cos(q5)*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot))) - rc55x*(M5*cos(q5)*(l5*(sin(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) - q4dot*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q4)*(q2ddot + q3ddot)) + cos(q4)*(l4*(q2ddot + q3ddot) + cos(q3)*(l3*q2ddot + g*cos(q2) + l1*q1dot^2*sin(q2) + 1/2*l3*q1dot^2*sin(2*q2)) + sin(q3)*(l3*((1/2*cos(2*q2) + 1/2)*q1dot^2 + q2dot^2) - g*sin(q2) + l1*q1dot^2*cos(q2)) + 1/2*l4*q1dot^2*sin(2*q2 + 2*q3)) + sin(q4)*(l4*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) -

36

cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) - l1*q1ddot - l3*q1ddot*cos(q2) + l4*q1dot*sin(q2 + q3)*(q2dot + q3dot) + 2*l3*q1dot*q2dot*sin(q2)) - l6*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) + l6*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) + l5*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))*(q4dot + q1dot*sin(q2 + q3))) - M5*rc55z*((q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))^2 + (sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3)))^2) - M5*rc55x*(q5dot*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3))) + cos(q5)*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) + sin(q5)*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot))) + M5*rc55y*(q5ddot - q4dot*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + sin(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + cos(q4)*(q2ddot + q3ddot)) + M5*sin(q5)*(l4*(q1dot^2*cos(q2 + q3)^2 + (q2dot + q3dot)^2) - l6*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot)) + cos(q3)*(l3*(q1dot^2*cos(q2)^2 + q2dot^2) - g*sin(q2) + l1*q1dot^2*cos(q2)) + l5*((sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))^2 + (cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))^2) - sin(q3)*(l3*q2ddot + g*cos(q2) + l1*q1dot^2*sin(q2) + l3*q1dot^2*cos(q2)*sin(q2)) - l6*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))*(q4dot + q1dot*sin(q2 + q3))) + M5*rc55x*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3)))*(q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) + M5*rc55y*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3)))*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3)))) - fz6*l7 - Iyx5s*(q5ddot - q4dot*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + sin(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + cos(q4)*(q2ddot + q3ddot)) - (cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3)))*(Ixz5s*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3))) + Iz5s*(q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))) - (sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3)))*(Ixy5s*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3))) - Izy5s*(q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))) + (Ix5s*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3))) + Izx5s*(q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)))*(q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) + rc55z*(M5*rc55y*(q5dot*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3))) + sin(q5)*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) - cos(q5)*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot))) - M5*l5*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot)) + M5*rc55z*(q5dot*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3))) + cos(q5)*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) + sin(q5)*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot))) - M5*sin(q4)*(l4*(q2ddot + q3ddot) + cos(q3)*(l3*q2ddot + g*cos(q2) + l1*q1dot^2*sin(q2) + 1/2*l3*q1dot^2*sin(2*q2)) + sin(q3)*(l3*((1/2*cos(2*q2) + 1/2)*q1dot^2 + q2dot^2) - g*sin(q2) + l1*q1dot^2*cos(q2)) + 1/2*l4*q1dot^2*sin(2*q2 + 2*q3)) - M5*l6*((sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))^2 + (q4dot + q1dot*sin(q2 + q3))^2) + M5*cos(q4)*(l4*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) - l1*q1ddot - l3*q1ddot*cos(q2) + l4*q1dot*sin(q2 + q3)*(q2dot + q3dot) + 2*l3*q1dot*q2dot*sin(q2)) - M5*rc55x*((sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3)))^2 + (cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot

37

+ q1dot*sin(q2 + q3)))^2) + M5*rc55y*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3)))*(q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) + M5*rc55z*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3)))*(q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) + M5*l5*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))*(q4dot + q1dot*sin(q2 + q3)))

nz6 - Iz5s*(q5dot*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3))) + sin(q5)*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) - cos(q5)*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot))) - Izy5s*(q5dot*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3))) + cos(q5)*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) + sin(q5)*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot))) - rc55x*(M5*rc55y*((q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))^2 + (cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3)))^2) - M5*sin(q5)*(l5*(sin(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) - q4dot*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q4)*(q2ddot + q3ddot)) + cos(q4)*(l4*(q2ddot + q3ddot) + cos(q3)*(l3*q2ddot + g*cos(q2) + l1*q1dot^2*sin(q2) + 1/2*l3*q1dot^2*sin(2*q2)) + sin(q3)*(l3*((1/2*cos(2*q2) + 1/2)*q1dot^2 + q2dot^2) - g*sin(q2) + l1*q1dot^2*cos(q2)) + 1/2*l4*q1dot^2*sin(2*q2 + 2*q3)) + sin(q4)*(l4*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) - l1*q1ddot - l3*q1ddot*cos(q2) + l4*q1dot*sin(q2 + q3)*(q2dot + q3dot) + 2*l3*q1dot*q2dot*sin(q2)) - l6*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) + l6*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) + l5*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))*(q4dot + q1dot*sin(q2 + q3))) + M5*rc55x*(q5dot*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3))) + sin(q5)*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) - cos(q5)*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot))) + M5*rc55z*(q5ddot - q4dot*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + sin(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + cos(q4)*(q2ddot + q3ddot)) + M5*cos(q5)*(l4*(q1dot^2*cos(q2 + q3)^2 + (q2dot + q3dot)^2) - l6*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot)) + cos(q3)*(l3*(q1dot^2*cos(q2)^2 + q2dot^2) - g*sin(q2) + l1*q1dot^2*cos(q2)) + l5*((sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))^2 + (cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))^2) - sin(q3)*(l3*q2ddot + g*cos(q2) + l1*q1dot^2*sin(q2) + l3*q1dot^2*cos(q2)*sin(q2)) - l6*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))*(q4dot + q1dot*sin(q2 + q3))) - M5*rc55x*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3)))*(q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - M5*rc55z*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3)))*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3)))) + fy6*l7 - Izx5s*(q5ddot - q4dot*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + sin(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + cos(q4)*(q2ddot + q3ddot)) + (sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3)))*(Ixy5s*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3))) + Iy5s*(q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))) + (cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3)))*(Ixz5s*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3))) - Iyz5s*(q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))) - (Ix5s*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 +

38

q3))) + Iyx5s*(q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)))*(q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - rc55y*(M5*rc55y*(q5dot*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3))) + sin(q5)*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) - cos(q5)*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot))) - M5*l5*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot)) + M5*rc55z*(q5dot*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3))) + cos(q5)*(q4ddot + q1ddot*sin(q2 + q3) + q1dot*q2dot*cos(q2 + q3) + q1dot*q3dot*cos(q2 + q3)) + sin(q5)*(q4dot*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) - cos(q4)*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) + sin(q4)*(q2ddot + q3ddot))) - M5*sin(q4)*(l4*(q2ddot + q3ddot) + cos(q3)*(l3*q2ddot + g*cos(q2) + l1*q1dot^2*sin(q2) + 1/2*l3*q1dot^2*sin(2*q2)) + sin(q3)*(l3*((1/2*cos(2*q2) + 1/2)*q1dot^2 + q2dot^2) - g*sin(q2) + l1*q1dot^2*cos(q2)) + 1/2*l4*q1dot^2*sin(2*q2 + 2*q3)) - M5*l6*((sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4))^2 + (q4dot + q1dot*sin(q2 + q3))^2) + M5*cos(q4)*(l4*(sin(q3)*(q1ddot*sin(q2) + q1dot*q2dot*cos(q2)) - cos(q3)*(q1ddot*cos(q2) - q1dot*q2dot*sin(q2)) + q1dot*q3dot*sin(q2 + q3)) - l1*q1ddot - l3*q1ddot*cos(q2) + l4*q1dot*sin(q2 + q3)*(q2dot + q3dot) + 2*l3*q1dot*q2dot*sin(q2)) - M5*rc55x*((sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3)))^2 + (cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3)))^2) + M5*rc55y*(sin(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) + cos(q5)*(q4dot + q1dot*sin(q2 + q3)))*(q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) + M5*rc55z*(cos(q5)*(sin(q4)*(q2dot + q3dot) + q1dot*cos(q2 + q3)*cos(q4)) - sin(q5)*(q4dot + q1dot*sin(q2 + q3)))*(q5dot + cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4)) + M5*l5*(cos(q4)*(q2dot + q3dot) - q1dot*cos(q2 + q3)*sin(q4))*(q4dot + q1dot*sin(q2 + q3)))

ANEXA 2

Acceleraţiile corespunzătoare centrelor de masă ale robotului articulat 6R

39

vC44dot = cos(q4)*l2*sin(q2)*q1ddot+2*cos(q4)*q2dot*cos(q2)*q1dot*l2+cos(q4)*l3*q1ddot*sin(q3+q2)+2*cos(q4)*l3*q2dot*q1dot*cos(q3+q2)+2*cos(q4)*l3*q3dot*q1dot*cos(q3+q2)+sin(q4)*g*sin(q3+q2)-sin(q4)*cos(q3)*q2ddot*l2+1/2*sin(q4)*q1dot^2*l2*sin(q3+2*q2)-1/2*sin(q4)*sin(q3)*l2*q1dot^2-sin(q4)*sin(q3)*l2*q2dot^2-sin(q4)*l3*q2ddot-sin(q4)*l3*q3ddot+sin(q4)*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3

-sin(q4)*l2*sin(q2)*q1ddot-2*sin(q4)*q2dot*cos(q2)*q1dot*l2-sin(q4)*l3*q1ddot*sin(q3+q2)-2*sin(q4)*l3*q2dot*q1dot*cos(q3+q2)-2*sin(q4)*l3*q3dot*q1dot*cos(q3+q2)+cos(q4)*g*sin(q3+q2)-cos(q4)*cos(q3)*q2ddot*l2+1/2*cos(q4)*q1dot^2*l2*sin(q3+2*q2)-1/2*cos(q4)*sin(q3)*l2*q1dot^2-cos(q4)*sin(q3)*l2*q2dot^2-cos(q4)*l3*q2ddot-cos(q4)*l3*q3ddot+cos(q4)*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3 g*cos(q3+q2)+sin(q3)*q2ddot*l2+1/2*q1dot^2*l2*cos(q3+2*q2)-1/2*cos(q3)*l2*q1dot^2-cos(q3)*l2*q2dot^2-l3*q1dot^2+l3*q1dot^2*cos(q3+q2)^2-l3*q2dot^2-2*l3*q2dot*q3dot-l3*q3dot^2 vC55dot = cos(q4)*l2*sin(q2)*q1ddot+2*cos(q4)*q2dot*cos(q2)*q1dot*l2+cos(q4)*l3*q1ddot*sin(q3+q2)+2*cos(q4)*l3*q2dot*q1dot*cos(q3+q2)+2*cos(q4)*l3*q3dot*q1dot*cos(q3+q2)+sin(q4)*g*sin(q3+q2)-sin(q4)*cos(q3)*q2ddot*l2+1/2*sin(q4)*q1dot^2*l2*sin(q3+2*q2)-1/2*sin(q4)*sin(q3)*l2*q1dot^2-sin(q4)*sin(q3)*l2*q2dot^2-sin(q4)*l3*q2ddot-sin(q4)*l3*q3ddot+sin(q4)*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3

-cos(q5)*sin(q4)*l2*sin(q2)*q1ddot-2*cos(q5)*sin(q4)*q2dot*cos(q2)*q1dot*l2-cos(q5)*sin(q4)*l3*q1ddot*sin(q3+q2)-2*cos(q5)*sin(q4)*l3*q2dot*q1dot*cos(q3+q2)-2*cos(q5)*sin(q4)*l3*q3dot*q1dot*cos(q3+q2)+cos(q5)*cos(q4)*g*sin(q3+q2)-cos(q5)*cos(q4)*cos(q3)*q2ddot*l2+1/2*cos(q5)*cos(q4)*q1dot^2*l2*sin(q3+2*q2)-1/2*cos(q5)*cos(q4)*sin(q3)*l2*q1dot^2-cos(q5)*cos(q4)*sin(q3)*l2*q2dot^2-cos(q5)*cos(q4)*l3*q2ddot-cos(q5)*cos(q4)*l3*q3ddot+cos(q5)*cos(q4)*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3+sin(q5)*g*cos(q3+q2)+sin(q5)*sin(q3)*q2ddot*l2+1/2*sin(q5)*q1dot^2*l2*cos(q3+2*q2)-1/2*sin(q5)*cos(q3)*l2*q1dot^2-sin(q5)*cos(q3)*l2*q2dot^2-sin(q5)*l3*q1dot^2+sin(q5)*l3*q1dot^2*cos(q3+q2)^2-sin(q5)*l3*q2dot^2-2*sin(q5)*l3*q2dot*q3dot-sin(q5)*l3*q3dot^2

sin(q5)*sin(q4)*l2*sin(q2)*q1ddot+2*sin(q5)*sin(q4)*q2dot*cos(q2)*q1dot*l2+sin(q5)*sin(q4)*l3*q1ddot*sin(q3+q2)+2*sin(q5)*sin(q4)*l3*q2dot*q1dot*cos(q3+q2)+2*sin(q5)*sin(q4)*l3*q3dot*q1dot*cos(q3+q2)-sin(q5)*cos(q4)*g*sin(q3+q2)+sin(q5)*cos(q4)*cos(q3)*q2ddot*l2-1/2*sin(q5)*cos(q4)*q1dot^2*l2*sin(q3+2*q2)+1/2*sin(q5)*cos(q4)*sin(q3)*l2*q1dot^2+sin(q5)*cos(q4)*sin(q3)*l2*q2dot^2+sin(q5)*cos(q4)*l3*q2ddot+sin(q5)*cos(q4)*l3*q3ddot-sin(q5)*cos(q4)*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3+cos(q5)*g*cos(q3+q2)+cos(q5)*sin(q3)*q2ddot*l2+1/2*cos(q5)*q1dot^2*l2*cos(q3+2*q2)-1/2*cos(q5)*cos(q3)*l2*q1dot^2-cos(q5)*cos(q3)*l2*q2dot^2-cos(q5)*l3*q1dot^2+cos(q5)*l3*q1dot^2*cos(q3+q2)^2-cos(q5)*l3*q2dot^2-2*cos(q5)*l3*q2dot*q3dot-cos(q5)*l3*q3dot^2 vC66dot = -cos(q6)*sin(q4)*sin(q3)*l2*q2dot^2-1/2*sin(q6)*sin(q5)*cos(q3)*l2*q1dot^2+cos(q6)*sin(q4)*g*sin(q3+q2)-cos(q6)*sin(q4)*l3*q2ddot-cos(q6)*sin(q4)*l3*q3ddot+sin(q6)*sin(q5)*g*cos(q3+q2)-sin(q6)*sin(q5)*l3*q2dot^2-sin(q6)*sin(q5)*l3*q3dot^2+cos(q6)*cos(q4)*l2*sin(q2)*q1ddot+2*cos(q6)*cos(q4)*q2dot*cos(q2)*q1dot*l2+cos(q6)*cos(q4)*l3*q1ddot*sin(q3+q2)+2*cos(q6)*cos(q4)*l3*q2dot*q1dot*cos(q3+q2)+2*cos(q6)*cos(q4)*l3*q3dot*q1dot*cos(q3+q2)-cos(q6)*sin(q4)*cos(q3)*q2ddot*l2+1/2*cos(q6)*sin(q4)*q1dot^2*l2*sin(q3+2*q2)-1/2*cos(q6)*sin(q4)*sin(q3)*l2*q1dot^2+cos(q6)*sin(q4)*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3-sin(q6)*cos(q5)*sin(q4)*l2*sin(q2)*q1ddot-2*sin(q6)*cos(q5)*sin(q4)*q2dot*cos(q2)*q1dot*l2-sin(q6)*cos(q5)*sin(q4)*l3*q1ddot*sin(q3+q2)-2*sin(q6)*cos(q5)*sin(q4)*l3*q2dot*q1dot*cos(q3+q2)-2*sin(q6)*cos(q5)*sin(q4)*l3*q3dot*q1dot*cos(q3+q2)+sin(q6)*cos(q5)*cos(q4)*g*sin(q3+q2)-sin(q6)*cos(q5)*cos(q4)*cos(q3)*

40

q2ddot*l2+1/2*sin(q6)*cos(q5)*cos(q4)*q1dot^2*l2*sin(q3+2*q2)-1/2*sin(q6)*cos(q5)*cos(q4)*sin(q3)*l2*q1dot^2-sin(q6)*cos(q5)*cos(q4)*sin(q3)*l2*q2dot^2-sin(q6)*cos(q5)*cos(q4)*l3*q2ddot-sin(q6)*cos(q5)*cos(q4)*l3*q3ddot+sin(q6)*cos(q5)*cos(q4)*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3+sin(q6)*sin(q5)*sin(q3)*q2ddot*l2+1/2*sin(q6)*sin(q5)*q1dot^2*l2*cos(q3+2*q2)-sin(q6)*sin(q5)*cos(q3)*l2*q2dot^2-2*sin(q6)*sin(q5)*l3*q2dot*q3dot-sin(q6)*sin(q5)*l3*q1dot^2+sin(q6)*sin(q5)*l3*q1dot^2*cos(q3+q2)^2

-cos(q6)*cos(q5)*sin(q4)*l2*sin(q2)*q1ddot+sin(q6)*sin(q4)*sin(q3)*l2*q2dot^2-2*sin(q6)*cos(q4)*q2dot*cos(q2)*q1dot*l2-sin(q6)*sin(q4)*g*sin(q3+q2)+sin(q6)*sin(q4)*l3*q2ddot+sin(q6)*sin(q4)*l3*q3ddot+cos(q6)*sin(q5)*g*cos(q3+q2)-cos(q6)*sin(q5)*l3*q2dot^2-cos(q6)*sin(q5)*l3*q3dot^2-1/2*cos(q6)*cos(q5)*cos(q4)*sin(q3)*l2*q1dot^2-cos(q6)*cos(q5)*sin(q4)*l3*q1ddot*sin(q3+q2)-sin(q6)*cos(q4)*l2*sin(q2)*q1ddot-sin(q6)*cos(q4)*l3*q1ddot*sin(q3+q2)-2*sin(q6)*cos(q4)*l3*q2dot*q1dot*cos(q3+q2)-2*sin(q6)*cos(q4)*l3*q3dot*q1dot*cos(q3+q2)+sin(q6)*sin(q4)*cos(q3)*q2ddot*l2-1/2*sin(q6)*sin(q4)*q1dot^2*l2*sin(q3+2*q2)+1/2*sin(q6)*sin(q4)*sin(q3)*l2*q1dot^2-sin(q6)*sin(q4)*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3-2*cos(q6)*cos(q5)*sin(q4)*q2dot*cos(q2)*q1dot*l2-2*cos(q6)*cos(q5)*sin(q4)*l3*q2dot*q1dot*cos(q3+q2)-2*cos(q6)*cos(q5)*sin(q4)*l3*q3dot*q1dot*cos(q3+q2)+cos(q6)*cos(q5)*cos(q4)*g*sin(q3+q2)-cos(q6)*cos(q5)*cos(q4)*cos(q3)*q2ddot*l2+1/2*cos(q6)*cos(q5)*cos(q4)*q1dot^2*l2*sin(q3+2*q2)-cos(q6)*cos(q5)*cos(q4)*sin(q3)*l2*q2dot^2-cos(q6)*cos(q5)*cos(q4)*l3*q2ddot-cos(q6)*cos(q5)*cos(q4)*l3*q3ddot+cos(q6)*cos(q5)*cos(q4)*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3+cos(q6)*sin(q5)*sin(q3)*q2ddot*l2+1/2*cos(q6)*sin(q5)*q1dot^2*l2*cos(q3+2*q2)-1/2*cos(q6)*sin(q5)*cos(q3)*l2*q1dot^2-cos(q6)*sin(q5)*cos(q3)*l2*q2dot^2-2*cos(q6)*sin(q5)*l3*q2dot*q3dot-cos(q6)*sin(q5)*l3*q1dot^2+cos(q6)*sin(q5)*l3*q1dot^2*cos(q3+q2)^2 sin(q5)*sin(q4)*l2*sin(q2)*q1ddot+2*sin(q5)*sin(q4)*q2dot*cos(q2)*q1dot*l2+sin(q5)*sin(q4)*l3*q1ddot*sin(q3+q2)+2*sin(q5)*sin(q4)*l3*q2dot*q1dot*cos(q3+q2)+2*sin(q5)*sin(q4)*l3*q3dot*q1dot*cos(q3+q2)-sin(q5)*cos(q4)*g*sin(q3+q2)+sin(q5)*cos(q4)*cos(q3)*q2ddot*l2-1/2*sin(q5)*cos(q4)*q1dot^2*l2*sin(q3+2*q2)+1/2*sin(q5)*cos(q4)*sin(q3)*l2*q1dot^2+sin(q5)*cos(q4)*sin(q3)*l2*q2dot^2+sin(q5)*cos(q4)*l3*q2ddot+sin(q5)*cos(q4)*l3*q3ddot-sin(q5)*cos(q4)*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3+cos(q5)*g*cos(q3+q2)+cos(q5)*sin(q3)*q2ddot*l2+1/2*cos(q5)*q1dot^2*l2*cos(q3+2*q2)-1/2*cos(q5)*cos(q3)*l2*q1dot^2-cos(q5)*cos(q3)*l2*q2dot^2-cos(q5)*l3*q1dot^2+cos(q5)*l3*q1dot^2*cos(q3+q2)^2-cos(q5)*l3*q2dot^2-2*cos(q5)*l3*q2dot*q3dot-cos(q5)*l3*q3dot^2

Forţele exterioare ale robotului articulat 6R

F44 = 1/2*M4*(2*cos(q4)*l2*sin(q2)*q1ddot+4*cos(q4)*q2dot*cos(q2)*q1dot*l2+2*cos(q4)*l3*q1ddot*sin(q3+q2)+4*cos(q4)*l3*q2dot*q1dot*cos(q3+q2)+4*cos(q4)*l3*q3dot*q1dot*cos(q3+q2)+2*sin(q4)*g*sin(q3+q2)-2*sin(q4)*cos(q3)*q2ddot*l2+sin(q4)*q1dot^2*l2*sin(q3+2*q2)*sin(q4)*sin(q3)*l2*q1dot^2-2*sin(q4)*sin(q3)*l2*q2dot^2-2*sin(q4)*l3*q2ddot-2*sin(q4)*l3*q3ddot+2*sin(q4)*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3) -1/2*M4*(2*sin(q4)*l2*sin(q2)*q1ddot+4*sin(q4)*q2dot*cos(q2)*q1dot*l2+2*sin(q4)*l3*q1ddot*sin(q3+q2)+4*sin(q4)*l3*q2dot*q1dot*cos(q3+q2)+4*sin(q4)*l3*q3dot*q1dot*cos(q3+q2)-2*cos(q4)*g*sin(q3+q2)+2*cos(q4)*cos(q3)*q2ddot*l2-cos(q4)*q1dot^2*l2*sin(q3+2*q2)+cos(q4)*sin(q3)*l2*q1dot^2+2*cos(q4)*sin(q3)*l2*q2dot^2+2*cos(q4)*l3*q2ddot+2*cos(q4)*l3*q3ddot-2*cos(q4)*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3) -1/2*M4*(-2*g*cos(q3+q2)-2*sin(q3)*q2ddot*l2-q1dot^2*l2*cos(q3+2*q2)+cos(q3)*l2*q1dot^2+2*cos(q3)*l2*q2dot^2+2*l3*q1dot^2-2*l3*q1dot^2*cos(q3+q2)^2+2*l3*q2dot^2+4*l3*q2dot*q3dot+2*l3*q3dot^2) F55 =

41

1/2*M5*(2*cos(q4)*l2*sin(q2)*q1ddot+4*cos(q4)*q2dot*cos(q2)*q1dot*l2+2*cos(q4)*l3*q1ddot*sin(q3+q2)+4*cos(q4)*l3*q2dot*q1dot*cos(q3+q2)+4*cos(q4)*l3*q3dot*q1dot*cos(q3+q2)+2*sin(q4)*g*sin(q3+q2)-2*sin(q4)*cos(q3)*q2ddot*l2+sin(q4)*q1dot^2*l2*sin(q3+2*q2)-sin(q4)*sin(q3)*l2*q1dot^2-2*sin(q4)*sin(q3)*l2*q2dot^2-2*sin(q4)*l3*q2ddot-2*sin(q4)*l3*q3ddot+2*sin(q4)*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3)

-1/2*M5*(2*cos(q5)*sin(q4)*l2*sin(q2)*q1ddot+4*cos(q5)*sin(q4)*q2dot*cos(q2)*q1dot*l2+2*cos(q5)*sin(q4)*l3*q1ddot*sin(q3+q2)+4*cos(q5)*sin(q4)*l3*q2dot*q1dot*cos(q3+q2)+4*cos(q5)*sin(q4)*l3*q3dot*q1dot*cos(q3+q2)-2*cos(q5)*cos(q4)*g*sin(q3+q2)+2*cos(q5)*cos(q4)*cos(q3)*q2ddot*l2-cos(q5)*cos(q4)*q1dot^2*l2*sin(q3+2*q2)+cos(q5)*cos(q4)*sin(q3)*l2*q1dot^2+2*cos(q5)*cos(q4)*sin(q3)*l2*q2dot^2+2*cos(q5)*cos(q4)*l3*q2ddot+2*cos(q5)*cos(q4)*l3*q3ddot-2*cos(q5)*cos(q4)*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3-2*sin(q5)*g*cos(q3+q2)-2*sin(q5)*sin(q3)*q2ddot*l2-sin(q5)*q1dot^2*l2*cos(q3+2*q2)+sin(q5)*cos(q3)*l2*q1dot^2+2*sin(q5)*cos(q3)*l2*q2dot^2+2*sin(q5)*l3*q1dot^2-2*sin(q5)*l3*q1dot^2*cos(q3+q2)^2+2*sin(q5)*l3*q2dot^2+4*sin(q5)*l3*q2dot*q3dot+2*sin(q5)*l3*q3dot^2) 1/2*M5*(2*sin(q5)*sin(q4)*l2*sin(q2)*q1ddot+4*sin(q5)*sin(q4)*q2dot*cos(q2)*q1dot*l2+2*sin(q5)*sin(q4)*l3*q1ddot*sin(q3+q2)+4*sin(q5)*sin(q4)*l3*q2dot*q1dot*cos(q3+q2)+4*sin(q5)*sin(q4)*l3*q3dot*q1dot*cos(q3+q2)-2*sin(q5)*cos(q4)*g*sin(q3+q2)+2*sin(q5)*cos(q4)*cos(q3)*q2ddot*l2-sin(q5)*cos(q4)*q1dot^2*l2*sin(q3+2*q2)+sin(q5)*cos(q4)*sin(q3)*l2*q1dot^2+2*sin(q5)*cos(q4)*sin(q3)*l2*q2dot^2+2*sin(q5)*cos(q4)*l3*q2ddot+2*sin(q5)*cos(q4)*l3*q3ddot-2*sin(q5)*cos(q4)*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3+2*cos(q5)*g*cos(q3+q2)+2*cos(q5)*sin(q3)*q2ddot*l2+cos(q5)*q1dot^2*l2*cos(q3+2*q2)-cos(q5)*cos(q3)*l2*q1dot^2-2*cos(q5)*cos(q3)*l2*q2dot^2-2*cos(q5)*l3*q1dot^2+2*cos(q5)*l3*q1dot^2*cos(q3+q2)^2-2*cos(q5)*l3*q2dot^2-4*cos(q5)*l3*q2dot*q3dot-2*cos(q5)*l3*q3dot^2) F66 = -1/2*M6*(-2*cos(q6)*sin(q4)*g*sin(q3+q2)+2*cos(q6)*sin(q4)*l3*q2ddot-2*sin(q6)*sin(q5)*g*cos(q3+q2)+2*cos(q6)*sin(q4)*l3*q3ddot+2*sin(q6)*sin(q5)*l3*q2dot^2+2*sin(q6)*sin(q5)*l3*q3dot^2+2*sin(q6)*sin(q5)*l3*q1dot^2-sin(q6)*sin(q5)*q1dot^2*l2*cos(q3+2*q2)+2*sin(q6)*cos(q5)*cos(q4)*cos(q3)*q2ddot*l2+2*sin(q6)*cos(q5)*cos(q4)*sin(q3)*l2*q2dot^2+2*sin(q6)*cos(q5)*cos(q4)*l3*q2ddot+sin(q6)*sin(q5)*cos(q3)*l2*q1dot^2-4*cos(q6)*cos(q4)*l3*q3dot*q1dot*cos(q3+q2)-cos(q6)*sin(q4)*q1dot^2*l2*sin(q3+2*q2)+cos(q6)*sin(q4)*sin(q3)*l2*q1dot^2-4*cos(q6)*cos(q4)*l3*q2dot*q1dot*cos(q3+q2)-4*cos(q6)*cos(q4)*q2dot*cos(q2)*q1dot*l2+4*sin(q6)*cos(q5)*sin(q4)*q2dot*cos(q2)*q1dot*l2+4*sin(q6)*cos(q5)*sin(q4)*l3*q2dot*q1dot*cos(q3+q2)+4*sin(q6)*cos(q5)*sin(q4)*l3*q3dot*q1dot*cos(q3+q2)-sin(q6)*cos(q5)*cos(q4)*q1dot^2*l2*sin(q3+2*q2)+sin(q6)*cos(q5)*cos(q4)*sin(q3)*l2*q1dot^2+4*sin(q6)*sin(q5)*l3*q2dot*q3dot+2*cos(q6)*sin(q4)*sin(q3)*l2*q2dot^2+2*sin(q6)*cos(q5)*cos(q4)*l3*q3ddot-2*sin(q6)*cos(q5)*cos(q4)*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3-2*sin(q6)*sin(q5)*sin(q3)*q2ddot*l2+2*sin(q6)*sin(q5)*cos(q3)*l2*q2dot^2-2*sin(q6)*sin(q5)*l3*q1dot^2*cos(q3+q2)^2-2*cos(q6)*cos(q4)*l3*q1ddot*sin(q3+q2)-2*cos(q6)*cos(q4)*l2*sin(q2)*q1ddot+2*cos(q6)*sin(q4)*cos(q3)*q2ddot*l2-2*cos(q6)*sin(q4)*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3+2*sin(q6)*cos(q5)*sin(q4)*l2*sin(q2)*q1ddot+2*sin(q6)*cos(q5)*sin(q4)*l3*q1ddot*sin(q3+q2)-2*sin(q6)*cos(q5)*cos(q4)*g*sin(q3+q2))

-1/2*M6*(-2*sin(q6)*sin(q4)*l3*q3ddot-2*sin(q6)*sin(q4)*l3*q2ddot-2*cos(q6)*sin(q5)*g*cos(q3+q2)+2*cos(q6)*sin(q5)*l3*q2dot^2+2*cos(q6)*sin(q5)*l3*q3dot^2+2*cos(q6)*sin(q5)*l3*q1dot^2+cos(q6)*sin(q5)*cos(q3)*l2*q1dot^2+4*sin(q6)*cos(q4)*l3*q2dot*q1dot*cos(q3+q2)+4*sin(q6)*cos(q4)*q2dot*cos(q2)*q1dot*l2+cos(q6)*cos(q5)*cos(q4)*sin(q3)*l2*q1dot^2+4*sin(q6)*cos(q4)*l3*q3dot*q1dot*cos(q3+q2)+sin(q6)*sin(q4)*q1dot^2*l2*sin(q3+2*q2)-sin(q6)*sin(q4)*sin(q3)*l2*q1dot^2+4*cos(q6)*cos(q5)*sin(q4)*q2dot*cos(q2)*q1dot*l2+4*cos(q6)*cos(q5)*sin(q4)*l3*q2dot*q1dot*cos(q3+q2)+4*cos(q6)*cos(q5)*sin(q4)*l3*q3dot*q1dot*cos(q3+q2)-cos(q6)*cos(q5)*cos(q4)*q1dot^2*l2*sin(q3+2*q2)-cos(q6)*sin(q5)*q1dot^2*l2*cos(q3+2*q2)+4*cos(q6)*sin(q5)*l3*q2dot*q3dot+2*sin(q6)*cos(q4)*l2*sin(q2)*q1ddot+2*sin(q6)*cos(q4)*l3*q1ddot*sin(q3+q2)-2*sin(q6)*sin(q4)*cos(q3)*q2ddot*l2+2*sin(q6)*sin(q4)*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3-2*cos(q6)*cos(q5)*cos(q4)*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3-2*cos(q6)*sin(q5)*sin(q3)*q2ddot*l2-2*cos(q6)*cos(q5)*cos(q4)*g*sin(q3+q2)+

42

2*cos(q6)*cos(q5)*cos(q4)*cos(q3)*q2ddot*l2+2*cos(q6)*cos(q5)*cos(q4)*sin(q3)*l2*q2dot^2+2*cos(q6)*cos(q5)*cos(q4)*l3*q2ddot+2*cos(q6)*cos(q5)*cos(q4)*l3*q3ddot+2*cos(q6)*sin(q5)*cos(q3)*l2*q2dot^2+2*cos(q6)*cos(q5)*sin(q4)*l2*sin(q2)*q1ddot-2*sin(q6)*sin(q4)*sin(q3)*l2*q2dot^2+2*cos(q6)*cos(q5)*sin(q4)*l3*q1ddot*sin(q3+q2)-2*cos(q6)*sin(q5)*l3*q1dot^2*cos(q3+q2)^2+2*sin(q6)*sin(q4)*g*sin(q3+q2)) 1/2*M6*(2*sin(q5)*sin(q4)*l2*sin(q2)*q1ddot+4*sin(q5)*sin(q4)*q2dot*cos(q2)*q1dot*l2+2*sin(q5)*sin(q4)*l3*q1ddot*sin(q3+q2)+4*sin(q5)*sin(q4)*l3*q2dot*q1dot*cos(q3+q2)+4*sin(q5)*sin(q4)*l3*q3dot*q1dot*cos(q3+q2)-2*sin(q5)*cos(q4)*g*sin(q3+q2)+2*sin(q5)*cos(q4)*cos(q3)*q2ddot*l2-sin(q5)*cos(q4)*q1dot^2*l2*sin(q3+2*q2)+sin(q5)*cos(q4)*sin(q3)*l2*q1dot^2+2*sin(q5)*cos(q4)*sin(q3)*l2*q2dot^2+2*sin(q5)*cos(q4)*l3*q2ddot+2*sin(q5)*cos(q4)*l3*q3ddot-2*sin(q5)*cos(q4)*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3+2*cos(q5)*g*cos(q3+q2)+2*cos(q5)*sin(q3)*q2ddot*l2+cos(q5)*q1dot^2*l2*cos(q3+2*q2)-cos(q5)*cos(q3)*l2*q1dot^2-2*cos(q5)*cos(q3)*l2*q2dot^2-2*cos(q5)*l3*q1dot^2+2*cos(q5)*l3*q1dot^2*cos(q3+q2)^2-2*cos(q5)*l3*q2dot^2-4*cos(q5)*l3*q2dot*q3dot-2*cos(q5)*l3*q3dot^2)

Momentele forţele exterioare ale robotului articulat 6R

N33 =

Ix3s*q2ddot+Ix3s*q3ddot-q1dot^2*cos(q3+q2)*Iy3s*sin(q3+q2)+q1dot^2*sin(q3+q2)*Iz3s*cos(q3+q2) Iy3s*q1ddot*sin(q3+q2)+Iy3s*q2dot*q1dot*cos(q3+q2)+Iy3s*q3dot*q1dot*cos(q3+q2)+q1dot*cos(q3+q2)*Ix3s*q2dot+q1dot*cos(q3+q2)*Ix3s*q3dot-Iz3s*q1dot*cos(q3+q2)*q2dot-Iz3s*q1dot*cos(q3+q2)*q3dot

Iz3s*q1ddot*cos(q3+q2)-Iz3s*q2dot*q1dot*sin(q3+q2)-Iz3s*q3dot*q1dot*sin(q3+q2)-q1dot*sin(q3+q2)*Ix3s*q2dot-q1dot*sin(q3+q2)*Ix3s*q3dot+Iy3s*q1dot*sin(q3+q2)*q2dot+Iy3s*q1dot*sin(q3+q2)*q3dot N44 = Ix4s*cos(q4)*q2ddot+Ix4s*cos(q4)*q3ddot+Ix4s*sin(q4)*q1ddot*sin(q3+q2)+Ix4s*sin(q4)*q2dot*q1dot*cos(q3+q2)+Ix4s*sin(q4)*q3dot*q1dot*cos(q3+q2)+Ix4s*q4dot*cos(q4)*q1dot*sin(q3+q2)-Ix4s*q4dot*sin(q4)*q2dot-Ix4s*q4dot*sin(q4)*q3dot-Iy4s*q1dot^2*cos(q3+q2)*cos(q4)*sin(q3+q2)+Iy4s*sin(q4)*q2dot*q1dot*cos(q3+q2)+Iy4s*sin(q4)*q3dot*q1dot*cos(q3+q2)-Iy4s*q4dot*cos(q4)*q1dot*sin(q3+q2)+Iy4s*q4dot*sin(q4)*q2dot+Iy4s*q4dot*sin(q4)*q3dot+Iz4s*q1dot^2*cos(q3+q2)*cos(q4)*sin(q3+q2)+Iz4s*q4dot*cos(q4)*q1dot*sin(q3+q2)-Iz4s*sin(q4)*q2dot*q1dot*cos(q3+q2)-Iz4s*q4dot*sin(q4)*q2dot-Iz4s*sin(q4)*q3dot*q1dot*cos(q3+q2)-Iz4s*q4dot*sin(q4)*q3dot

-Iy4s*sin(q4)*q2ddot-Iy4s*sin(q4)*q3ddot+Iy4s*cos(q4)*q1ddot*sin(q3+q2)+Iy4s*cos(q4)*q2dot*q1dot*cos(q3+q2)+Iy4s*cos(q4)*q3dot*q1dot*cos(q3+q2)-Iy4s*q4dot*sin(q4)*q1dot*sin(q3+q2)-Iy4s*q4dot*cos(q4)*q2dot-Iy4s*q4dot*cos(q4)*q3dot+Ix4s*cos(q4)*q2dot*q1dot*cos(q3+q2)+Ix4s*cos(q4)*q3dot*q1dot*cos(q3+q2)+Ix4s*q1dot^2*cos(q3+q2)*sin(q4)*sin(q3+q2)+Ix4s*q4dot*cos(q4)*q2dot+Ix4s*q4dot*cos(q4)*q3dot+Ix4s*q4dot*sin(q4)*q1dot*sin(q3+q2)-Iz4s*q1dot^2*cos(q3+q2)*sin(q4)*sin(q3+q2)-Iz4s*q4dot*sin(q4)*q1dot*sin(q3+q2)-Iz4s*cos(q4)*q2dot*q1dot*cos(q3+q2)-Iz4s*q4dot*cos(q4)*q2dot-Iz4s*cos(q4)*q3dot*q1dot*cos(q3+q2)-Iz4s*q4dot*cos(q4)*q3dot

-Iy4s*q3dot*q1dot*sin(q3+q2)+Ix4s*q3dot*q1dot*sin(q3+q2)-Iz4s*q2dot*q1dot*sin(q3+q2)-Iz4s*q3dot*q1dot*sin(q3+q2)+2*Ix4s*sin(q4)*q2dot*cos(q4)*q3dot+Ix4s*q2dot*q1dot*sin(q3+q2)+Iz4s*q4ddot+Ix4s*sin(q4)*q2dot^2*cos(q4)+Ix4s*sin(q4)*q3dot^2*cos(q4)-Iy4s*sin(q4)*q2dot^2*cos(q4)-Iy4s*sin(q4)*q3dot^2*cos(q4)-2*Ix4s*cos(q4)^2*q1dot*sin(q3+q2)*q2dot-2*Ix4s*cos(q4)^2*q1dot*sin(q3+q2)*q3dot+2*Iy4s*cos(q4)^2*q1dot*sin(q3+q2)*q2dot-2*Iy4s*sin(q4)*q2dot*cos(q4)*q3dot+2*Iy4s*cos(q4)^2*q1dot*sin(q3+q2)*q3dot-Ix4s*cos(q4)*q1dot^2*sin(q4)+Ix4s*cos(q4)*q1dot^2*sin(q4)*cos(q3+q2)^2+Iy4s*cos(q4)*q1dot^2*sin(q4)-Iy4s*cos(q4)*q1dot^2*sin(q4)*cos(q3+q2)^2-Iy4s*q2dot*q1dot*sin(q3+q2)+Iz4s*q1ddot*cos(q3+q2)

43

N55 = Ix5s*q5ddot+Ix5s*sin(q4)*q1ddot*sin(q3+q2)-Ix5s*q4dot*sin(q4)*q2dot-Ix5s*q4dot*sin(q4)*q3dot-Iy5s*cos(q5)*q4dot^2*sin(q5)+Iz5s*cos(q5)*q4dot^2*sin(q5)+Iz5s*sin(q5)*q3dot^2*cos(q5)*cos(q4)^2-Iz5s*cos(q4)*q1dot^2*sin(q3+q2)*cos(q3+q2)+2*Iy5s*cos(q5)^2*q4dot*sin(q4)*q2dot+2*Iz5s*cos(q5)*q1dot*cos(q3+q2)*sin(q5)*q4dot-2*Iz5s*sin(q5)*q2dot*cos(q5)*q3dot+2*Iy5s*sin(q5)*q2dot*cos(q5)*q3dot+Iy5s*sin(q5)*cos(q4)^2*q1dot^2*cos(q5)+Iz5s*sin(q4)*q2dot*q1dot*cos(q3+q2)-Iy5s*sin(q5)*q3dot^2*cos(q5)*cos(q4)^2-Iy5s*sin(q5)*cos(q4)^2*q1dot^2*cos(q5)*cos(q3+q2)^2-Iy5s*sin(q5)*q2dot^2*cos(q5)*cos(q4)^2+Iy5s*cos(q4)*q1dot^2*sin(q3+q2)*cos(q3+q2)+Iy5s*cos(q4)*q1dot*sin(q3+q2)*q4dot+Iz5s*sin(q5)*q2dot^2*cos(q5)*cos(q4)^2+Iz5s*sin(q4)*q3dot*q1dot*cos(q3+q2)-Iz5s*sin(q5)*cos(q4)^2*q1dot^2*cos(q5)+Iz5s*sin(q5)*cos(q4)^2*q1dot^2*cos(q5)*cos(q3+q2)^2-Iy5s*sin(q4)*q2dot*q1dot*cos(q3+q2)-Iy5s*sin(q4)*q3dot*q1dot*cos(q3+q2)-Iz5s*cos(q4)*q1dot*sin(q3+q2)*q4dot+2*Iz5s*sin(q5)*q2dot*cos(q5)*q3dot*cos(q4)^2+Iy5s*sin(q5)*q3dot^2*cos(q5)+Iz5s*sin(q4)*q3dot*q4dot+Iy5s*sin(q5)*q2dot^2*cos(q5)+Iz5s*sin(q4)*q2dot*q4dot-Iy5s*sin(q4)*q3dot*q4dot-Iz5s*sin(q5)*q2dot^2*cos(q5)-Iz5s*sin(q5)*q3dot^2*cos(q5)-Iy5s*sin(q4)*q2dot*q4dot-2*Iy5s*sin(q5)*q2dot*cos(q5)*q3dot*cos(q4)^2+Ix5s*sin(q4)*q3dot*q1dot*cos(q3+q2)+Ix5s*sin(q4)*q2dot*q1dot*cos(q3+q2)+Ix5s*q4dot*cos(q4)*q1dot*sin(q3+q2)-Iy5s*cos(q5)*q1dot^2*cos(q3+q2)^2*sin(q5)+2*Iy5s*cos(q5)^2*q4dot*sin(q4)*q3dot-2*Iy5s*sin(q5)*cos(q4)*q1dot*sin(q3+q2)*cos(q5)*sin(q4)*q2dot-2*Iy5s*sin(q5)*cos(q4)*q1dot*sin(q3+q2)*cos(q5)*sin(q4)*q3dot-2*Iy5s*cos(q5)^2*q1dot^2*cos(q3+q2)*cos(q4)*sin(q3+q2)+2*Iy5s*cos(q5)^2*q1dot*cos(q3+q2)*sin(q4)*q2dot+2*Iy5s*cos(q5)^2*q1dot*cos(q3+q2)*sin(q4)*q3dot-2*Iy5s*cos(q5)*q1dot*cos(q3+q2)*sin(q5)*q4dot-2*Iy5s*cos(q5)^2*q4dot*cos(q4)*q1dot*sin(q3+q2)+Iz5s*cos(q5)*q1dot^2*cos(q3+q2)^2*sin(q5)-2*Iz5s*cos(q5)^2*q4dot*sin(q4)*q2dot-2*Iz5s*cos(q5)^2*q4dot*sin(q4)*q3dot+2*Iz5s*sin(q5)*cos(q4)*q1dot*sin(q3+q2)*cos(q5)*sin(q4)*q2dot+2*Iz5s*sin(q5)*cos(q4)*q1dot*sin(q3+q2)*cos(q5)*sin(q4)*q3dot+2*Iz5s*cos(q5)^2*q1dot^2*cos(q3+q2)*cos(q4)*sin(q3+q2)-2*Iz5s*cos(q5)^2*q1dot*cos(q3+q2)*sin(q4)*q2dot-2*Iz5s*cos(q5)^2*q1dot*cos(q3+q2)*sin(q4)*q3dot+2*Iz5s*cos(q5)^2*q4dot*cos(q4)*q1dot*sin(q3+q2)+Ix5s*cos(q4)*q2ddot+Ix5s*cos(q4)*q3ddot

Iy5s*sin(q5)*q4ddot-Iy5s*sin(q5)*q2dot*q1dot*sin(q3+q2)-Iy5s*sin(q5)*q3dot*q1dot*sin(q3+q2)+Iy5s*q5dot*cos(q5)*q1dot*cos(q3+q2)+Iy5s*q5dot*sin(q5)*sin(q4)*q2dot+Iy5s*q5dot*sin(q5)*sin(q4)*q3dot+Iy5s*cos(q5)*cos(q4)*q3dot*q1dot*cos(q3+q2)-Iy5s*cos(q5)*q4dot*sin(q4)*q1dot*sin(q3+q2)-Iy5s*cos(q5)*q4dot*cos(q4)*q2dot-Iy5s*cos(q5)*q4dot*cos(q4)*q3dot+Iy5s*q5dot*cos(q5)*q4dot-Iz5s*q5dot*sin(q5)*sin(q4)*q3dot+Iz5s*q5dot*sin(q5)*cos(q4)*q1dot*sin(q3+q2)+Iy5s*cos(q5)*cos(q4)*q1ddot*sin(q3+q2)+Iy5s*cos(q5)*cos(q4)*q2dot*q1dot*cos(q3+q2)-Iy5s*q5dot*sin(q5)*cos(q4)*q1dot*sin(q3+q2)+Ix5s*cos(q5)*q4dot*cos(q4)*q2dot+Ix5s*cos(q5)*q4dot*cos(q4)*q3dot+Ix5s*cos(q5)*q4dot*sin(q4)*q1dot*sin(q3+q2)+Ix5s*q5dot*cos(q5)*q4dot+Ix5s*sin(q5)*sin(q4)*q2dot^2*cos(q4)+Ix5s*cos(q5)*cos(q4)*q2dot*q1dot*cos(q3+q2)+Ix5s*cos(q5)*cos(q4)*q3dot*q1dot*cos(q3+q2)+Ix5s*cos(q5)*q1dot^2*cos(q3+q2)*sin(q4)*sin(q3+q2)+Ix5s*q5dot*cos(q5)*q1dot*cos(q3+q2)+Ix5s*q5dot*sin(q5)*sin(q4)*q3dot-2*Ix5s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*q2dot-Iz5s*q5dot*cos(q5)*q4dot-Iz5s*cos(q5)*cos(q4)*q3dot*q1dot*cos(q3+q2)-Iz5s*cos(q5)*q4dot*cos(q4)*q3dot-Iz5s*sin(q5)*sin(q4)*q3dot^2*cos(q4)-Iz5s*cos(q5)*cos(q4)*q2dot*q1dot*cos(q3+q2)+Ix5s*sin(q5)*sin(q4)*q3dot^2*cos(q4)-Ix5s*q5dot*sin(q5)*cos(q4)*q1dot*sin(q3+q2)+2*Iz5s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*q2dot-2*Ix5s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*q3dot+2*Ix5s*sin(q5)*sin(q4)*q2dot*cos(q4)*q3dot+Ix5s*q5dot*sin(q5)*sin(q4)*q2dot-Iz5s*cos(q5)*q1dot^2*cos(q3+q2)*sin(q4)*sin(q3+q2)-Iz5s*cos(q5)*q4dot*sin(q4)*q1dot*sin(q3+q2)-Iz5s*cos(q5)*q4dot*cos(q4)*q2dot-Iz5s*sin(q5)*sin(q4)*q2dot^2*cos(q4)-2*Iz5s*sin(q5)*sin(q4)*q2dot*cos(q4)*q3dot+2*Iz5s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*q3dot-Iy5s*cos(q5)*sin(q4)*q2ddot-Iy5s*cos(q5)*sin(q4)*q3ddot-Iz5s*q5dot*cos(q5)*q1dot*cos(q3+q2)-Iz5s*q5dot*sin(q5)*sin(q4)*q2dot+Iy5s*sin(q5)*q1ddot*cos(q3+q2)+Ix5s*sin(q5)*q3dot*q1dot*sin(q3+q2)-Iz5s*sin(q5)*q3dot*q1dot*sin(q3+q2)+Ix5s*sin(q5)*q2dot*q1dot*sin(q3+q2)-Iz5s*sin(q5)*q2dot*q1dot*sin(q3+q2)-Ix5s*sin(q5)*cos(q4)*q1dot^2*sin(q4)+Ix5s*sin(q5)*cos(q4)*q1dot^2*sin(q4)*cos(q3+q2)^2+Iz5s*sin(q5)*cos(q4)*q1dot^2*sin(q4)-Iz5s*sin(q5)*cos(q4)*q1dot^2*sin(q4)*cos(q3+q2)^2 Iz5s*sin(q5)*q4dot*sin(q4)*q1dot*sin(q3+q2)+Iz5s*sin(q5)*q4dot*cos(q4)*q2dot+Iz5s*sin(q5)*q4dot*cos(q4)*q3dot-Iz5s*cos(q5)*q2dot*q1dot*sin(q3+q2)-Iz5s*cos(q5)*q3dot*q1dot*sin(q3+q2)-Iz5s*q5dot*sin(q5)*q1dot*cos(q3+q2)+Iz5s*q5dot*cos(q5)*sin(q4)*q2dot-Iz5s*q5dot*cos(q5)*cos(q4)*q1dot*

44

sin(q3+q2)+Iz5s*cos(q5)*q4ddot-Ix5s*q5dot*cos(q5)*cos(q4)*q1dot*sin(q3+q2)+Iy5s*q5dot*sin(q5)*q4dot+Iz5s*sin(q5)*sin(q4)*q2ddot+Iz5s*sin(q5)*sin(q4)*q3ddot+Iz5s*cos(q5)*q1ddot*cos(q3+q2)-Iz5s*q5dot*sin(q5)*q4dot-Ix5s*q5dot*sin(q5)*q4dot-Iy5s*cos(q5)*sin(q4)*q2dot^2*cos(q4)-2*Iy5s*cos(q5)*sin(q4)*q2dot*cos(q4)*q3dot+2*Iy5s*cos(q5)*cos(q4)^2*q1dot*sin(q3+q2)*q2dot+Iy5s*sin(q5)*cos(q4)*q2dot*q1dot*cos(q3+q2)+Iy5s*sin(q5)*q4dot*cos(q4)*q2dot-Iy5s*cos(q5)*sin(q4)*q3dot^2*cos(q4)+2*Iy5s*cos(q5)*cos(q4)^2*q1dot*sin(q3+q2)*q3dot+Iy5s*sin(q5)*cos(q4)*q3dot*q1dot*cos(q3+q2)+Iy5s*sin(q5)*q4dot*cos(q4)*q3dot+Iy5s*sin(q5)*q1dot^2*cos(q3+q2)*sin(q4)*sin(q3+q2)+Iy5s*sin(q5)*q4dot*sin(q4)*q1dot*sin(q3+q2)-Iy5s*q5dot*cos(q5)*sin(q4)*q2dot-Iy5s*q5dot*cos(q5)*sin(q4)*q3dot+Iy5s*q5dot*cos(q5)*cos(q4)*q1dot*sin(q3+q2)+Iy5s*q5dot*sin(q5)*q1dot*cos(q3+q2)-Iz5s*sin(q5)*cos(q4)*q1ddot*sin(q3+q2)-Iz5s*sin(q5)*cos(q4)*q2dot*q1dot*cos(q3+q2)-Iz5s*sin(q5)*cos(q4)*q3dot*q1dot*cos(q3+q2)+Iz5s*q5dot*cos(q5)*sin(q4)*q3dot-Ix5s*sin(q5)*cos(q4)*q2dot*q1dot*cos(q3+q2)-Ix5s*sin(q5)*cos(q4)*q3dot*q1dot*cos(q3+q2)-Ix5s*sin(q5)*q1dot^2*cos(q3+q2)*sin(q4)*sin(q3+q2)-Ix5s*q5dot*sin(q5)*q1dot*cos(q3+q2)-Ix5s*sin(q5)*q4dot*cos(q4)*q2dot-Ix5s*sin(q5)*q4dot*cos(q4)*q3dot-Ix5s*sin(q5)*q4dot*sin(q4)*q1dot*sin(q3+q2)+Ix5s*cos(q5)*sin(q4)*q2dot^2*cos(q4)+2*Ix5s*cos(q5)*sin(q4)*q2dot*cos(q4)*q3dot+Ix5s*q5dot*cos(q5)*sin(q4)*q2dot+Ix5s*cos(q5)*sin(q4)*q3dot^2*cos(q4)+Ix5s*q5dot*cos(q5)*sin(q4)*q3dot-2*Ix5s*cos(q5)*cos(q4)^2*q1dot*sin(q3+q2)*q2dot-2*Ix5s*cos(q5)*cos(q4)^2*q1dot*sin(q3+q2)*q3dot+Ix5s*cos(q5)*q2dot*q1dot*sin(q3+q2)+Ix5s*cos(q5)*q3dot*q1dot*sin(q3+q2)-Iy5s*cos(q5)*q2dot*q1dot*sin(q3+q2)-Iy5s*cos(q5)*q3dot*q1dot*sin(q3+q2)+Iy5s*cos(q5)*cos(q4)*q1dot^2*sin(q4)-Iy5s*cos(q5)*cos(q4)*q1dot^2*sin(q4)*cos(q3+q2)^2-Ix5s*cos(q5)*cos(q4)*q1dot^2*sin(q4)+Ix5s*cos(q5)*cos(q4)*q1dot^2*sin(q4)*cos(q3+q2)^2 N66 = Iy6s*sin(q6)*cos(q5)*q4dot*cos(q4)*q2dot+Ix6s*cos(q6)*sin(q4)*q1ddot*sin(q3+q2)+Ix6s*sin(q6)*q5dot*sin(q5)*sin(q4)*q2dot-Ix6s*q6dot*sin(q6)*cos(q4)*q3dot+Ix6s*cos(q6)*cos(q4)*q2ddot+Ix6s*cos(q6)*cos(q4)*q3ddot+Ix6s*sin(q6)*sin(q5)*q4ddot+Iy6s*sin(q5)*q3dot*sin(q6)*q1dot*sin(q3+q2)+Ix6s*cos(q6)*q5ddot+Iz6s*cos(q5)*q1dot^2*cos(q3+q2)^2*cos(q6)*sin(q5)+Iz6s*q6dot*cos(q6)*sin(q5)*q1dot*cos(q3+q2)-2*Iy6s*cos(q5)^2*q4dot*cos(q6)*cos(q4)*q1dot*sin(q3+q2)+2*Iy6s*cos(q5)^2*q4dot*cos(q6)*sin(q4)*q2dot+Ix6s*cos(q6)*sin(q4)*q3dot*q1dot*cos(q3+q2)+2*Iy6s*cos(q5)^2*q1dot*cos(q3+q2)*cos(q6)*sin(q4)*q3dot-2*Iy6s*cos(q5)^2*q1dot^2*cos(q3+q2)*cos(q6)*cos(q4)*sin(q3+q2)-Iy6s*cos(q5)*q1dot^2*cos(q3+q2)^2*cos(q6)*sin(q5)-2*Iy6s*cos(q5)*q1dot*cos(q3+q2)*cos(q6)*sin(q5)*q4dot+Iy6s*cos(q5)*q1dot^2*cos(q3+q2)*sin(q6)*sin(q4)*sin(q3+q2)+2*Iy6s*cos(q5)^2*q4dot*cos(q6)*sin(q4)*q3dot+2*Iz6s*cos(q5)^2*q1dot^2*cos(q3+q2)*cos(q6)*cos(q4)*sin(q3+q2)+2*Iz6s*cos(q5)*q1dot*cos(q3+q2)*cos(q6)*sin(q5)*q4dot-Iz6s*cos(q5)*q1dot^2*cos(q3+q2)*sin(q6)*sin(q4)*sin(q3+q2)-2*Iz6s*cos(q5)^2*q4dot*cos(q6)*sin(q4)*q2dot-2*Iz6s*cos(q5)^2*q4dot*cos(q6)*sin(q4)*q3dot+2*Iz6s*cos(q5)^2*q4dot*cos(q6)*cos(q4)*q1dot*sin(q3+q2)-Iz6s*sin(q6)*cos(q5)*cos(q4)*q2dot*q1dot*cos(q3+q2)-Iz6s*sin(q6)*cos(q5)*cos(q4)*q3dot*q1dot*cos(q3+q2)+Iy6s*sin(q6)*cos(q5)*cos(q4)*q2dot*q1dot*cos(q3+q2)+Iy6s*sin(q6)*cos(q5)*cos(q4)*q3dot*q1dot*cos(q3+q2)+Iy6s*sin(q6)*cos(q5)*q4dot*sin(q4)*q1dot*sin(q3+q2)-Iz6s*sin(q5)*cos(q4)*q1dot^2*sin(q6)*sin(q4)*cos(q3+q2)^2-Iz6s*sin(q5)*sin(q4)*q3dot^2*sin(q6)*cos(q4)+Iy6s*sin(q6)*cos(q5)*q4dot*cos(q4)*q3dot+Iy6s*sin(q6)*q5dot*cos(q5)*q1dot*cos(q3+q2)+Iy6s*sin(q6)*q5dot*sin(q5)*sin(q4)*q2dot+Iy6s*sin(q6)*q5dot*sin(q5)*sin(q4)*q3dot-Iy6s*sin(q6)*q5dot*sin(q5)*cos(q4)*q1dot*sin(q3+q2)-Iy6s*q6dot*cos(q6)*sin(q5)*q4dot+Iy6s*q6dot*sin(q6)*cos(q4)*q2dot+Iy6s*q6dot*sin(q6)*cos(q4)*q3dot-Iy6s*cos(q5)*q4dot^2*cos(q6)*sin(q5)+Iy6s*q6dot*cos(q6)*cos(q5)*sin(q4)*q2dot+Iy6s*q6dot*cos(q6)*cos(q5)*sin(q4)*q3dot-Iy6s*q6dot*cos(q6)*cos(q5)*cos(q4)*q1dot*sin(q3+q2)-Iy6s*q6dot*cos(q6)*sin(q5)*q1dot*cos(q3+q2)+Iy6s*q6dot*sin(q6)*sin(q4)*q1dot*sin(q3+q2)+Iy6s*sin(q6)*q5dot*cos(q5)*q4dot-2*Iy6s*sin(q5)*sin(q4)*q2dot*cos(q6)*cos(q5)*cos(q4)*q1dot*sin(q3+q2)+Iy6s*sin(q5)*sin(q4)*q2dot^2*sin(q6)*cos(q4)+2*Iy6s*sin(q5)*sin(q4)*q2dot*sin(q6)*cos(q4)*q3dot-2*Iy6s*sin(q5)*sin(q4)*q3dot*cos(q6)*cos(q5)*cos(q4)*q1dot*sin(q3+q2)+Iy6s*sin(q5)*sin(q4)*q3dot^2*sin(q6)*cos(q4)-2*Iy6s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*sin(q6)*q2dot-2*Iy6s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*sin(q6)*q3dot+2*Iy6s*cos(q5)^2*q1dot*cos(q3+q2)*cos(q6)*sin(q4)*q2dot-Iz6s*sin(q6)*cos(q5)*q4dot*sin(q4)*q1dot*sin(q3+q2)-Iz6s*sin(q6)*cos(q5)*q4dot*cos(q4)*q2dot-Iz6s*sin(q6)*cos(q5)*q4dot*cos(q4)*q3dot-Iz6s*sin(q6)*q5dot*cos(q5)*q1dot*cos(q3+q2)-Iz6s*sin(q6)*q5dot*sin(q5)*sin(q4)*q2dot-Iz6s*sin(q6)*q5dot*sin(q5)*sin(q4)*q3dot+Iz6s*sin(q6)*q5dot*sin(q5)*cos(q4)*q1dot*sin(q3+q2)-Iz6s*

45

q6dot*sin(q6)*cos(q4)*q2dot-Iz6s*q6dot*sin(q6)*cos(q4)*q3dot+Iz6s*cos(q5)*q4dot^2*cos(q6)*sin(q5)-Iz6s*q6dot*cos(q6)*cos(q5)*sin(q4)*q2dot-Iz6s*q6dot*cos(q6)*cos(q5)*sin(q4)*q3dot+Iz6s*q6dot*cos(q6)*cos(q5)*cos(q4)*q1dot*sin(q3+q2)-Iz6s*q6dot*sin(q6)*sin(q4)*q1dot*sin(q3+q2)-Iz6s*sin(q6)*q5dot*cos(q5)*q4dot+2*Iz6s*sin(q5)*sin(q4)*q2dot*cos(q6)*cos(q5)*cos(q4)*q1dot*sin(q3+q2)-Iz6s*sin(q5)*sin(q4)*q2dot^2*sin(q6)*cos(q4)-2*Iz6s*sin(q5)*sin(q4)*q2dot*sin(q6)*cos(q4)*q3dot+2*Iz6s*sin(q5)*sin(q4)*q3dot*cos(q6)*cos(q5)*cos(q4)*q1dot*sin(q3+q2)+2*Iz6s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*sin(q6)*q2dot+2*Iz6s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*sin(q6)*q3dot-2*Iz6s*cos(q5)^2*q1dot*cos(q3+q2)*cos(q6)*sin(q4)*q2dot-2*Iz6s*cos(q5)^2*q1dot*cos(q3+q2)*cos(q6)*sin(q4)*q3dot+Ix6s*sin(q6)*cos(q5)*cos(q4)*q2dot*q1dot*cos(q3+q2)+Ix6s*cos(q6)*sin(q4)*q2dot*q1dot*cos(q3+q2)+Ix6s*cos(q6)*q4dot*cos(q4)*q1dot*sin(q3+q2)+Ix6s*sin(q6)*cos(q5)*cos(q4)*q1ddot*sin(q3+q2)+Ix6s*sin(q6)*cos(q5)*cos(q4)*q3dot*q1dot*cos(q3+q2)-Ix6s*sin(q6)*cos(q5)*q4dot*sin(q4)*q1dot*sin(q3+q2)-Ix6s*sin(q6)*cos(q5)*q4dot*cos(q4)*q2dot-Ix6s*sin(q6)*cos(q5)*q4dot*cos(q4)*q3dot-Ix6s*sin(q6)*sin(q5)*q2dot*q1dot*sin(q3+q2)-Ix6s*sin(q6)*sin(q5)*q3dot*q1dot*sin(q3+q2)+Ix6s*sin(q6)*q5dot*cos(q5)*q1dot*cos(q3+q2)+Ix6s*sin(q6)*q5dot*sin(q5)*sin(q4)*q3dot-Ix6s*sin(q6)*q5dot*sin(q5)*cos(q4)*q1dot*sin(q3+q2)+Ix6s*q6dot*cos(q6)*sin(q5)*q4dot-Ix6s*q6dot*sin(q6)*cos(q4)*q2dot-Ix6s*q6dot*cos(q6)*cos(q5)*sin(q4)*q2dot-Ix6s*q6dot*cos(q6)*cos(q5)*sin(q4)*q3dot+Ix6s*q6dot*cos(q6)*cos(q5)*cos(q4)*q1dot*sin(q3+q2)+Ix6s*q6dot*cos(q6)*sin(q5)*q1dot*cos(q3+q2)-Ix6s*q6dot*sin(q6)*sin(q4)*q1dot*sin(q3+q2)-Ix6s*cos(q6)*q4dot*sin(q4)*q2dot-Ix6s*cos(q6)*q4dot*sin(q4)*q3dot-Ix6s*sin(q6)*cos(q5)*sin(q4)*q2ddot-Ix6s*sin(q6)*cos(q5)*sin(q4)*q3ddot+Ix6s*sin(q6)*sin(q5)*q1ddot*cos(q3+q2)+Ix6s*sin(q6)*q5dot*cos(q5)*q4dot+Iy6s*sin(q5)*cos(q4)*q1dot^2*sin(q6)*sin(q4)*cos(q3+q2)^2+2*Iz6s*sin(q5)*q2dot*cos(q6)*cos(q5)*q3dot*cos(q4)^2-Iy6s*sin(q4)*q3dot*cos(q6)*q1dot*cos(q3+q2)-Iz6s*cos(q4)*q1dot*sin(q3+q2)*cos(q6)*q4dot-Iy6s*sin(q4)*q2dot*cos(q6)*q4dot-2*Iy6s*sin(q5)*q2dot*cos(q6)*cos(q5)*q3dot*cos(q4)^2-Iy6s*sin(q4)*q2dot*cos(q6)*q1dot*cos(q3+q2)+Iz6s*sin(q5)*q3dot^2*cos(q6)*cos(q5)*cos(q4)^2-Iy6s*sin(q5)*q2dot^2*cos(q6)*cos(q5)*cos(q4)^2+Iz6s*sin(q4)*q2dot*cos(q6)*q1dot*cos(q3+q2)-Iy6s*sin(q5)*q3dot^2*cos(q6)*cos(q5)*cos(q4)^2+Iz6s*sin(q4)*q2dot*cos(q6)*q4dot+Iy6s*sin(q5)*cos(q4)^2*q1dot^2*cos(q6)*cos(q5)-Iy6s*sin(q5)*cos(q4)^2*q1dot^2*cos(q6)*cos(q5)*cos(q3+q2)^2+Iy6s*cos(q4)*q1dot^2*sin(q3+q2)*cos(q6)*cos(q3+q2)+Iy6s*cos(q4)*q1dot*sin(q3+q2)*cos(q6)*q4dot+Iz6s*q6dot*cos(q6)*sin(q5)*q4dot-Ix6s*q6dot*sin(q6)*q5dot-Iz6s*q6dot*sin(q6)*q5dot+Iy6s*q6dot*sin(q6)*q5dot-Iz6s*cos(q4)*q1dot^2*sin(q3+q2)*cos(q6)*cos(q3+q2)+Iz6s*sin(q4)*q3dot*cos(q6)*q1dot*cos(q3+q2)+Iz6s*sin(q4)*q3dot*cos(q6)*q4dot+Iz6s*sin(q5)*cos(q4)*q1dot^2*sin(q6)*sin(q4)+Iz6s*sin(q5)*q2dot^2*cos(q6)*cos(q5)*cos(q4)^2-Iy6s*sin(q4)*q3dot*cos(q6)*q4dot-Iz6s*sin(q5)*cos(q4)^2*q1dot^2*cos(q6)*cos(q5)+Iz6s*sin(q5)*cos(q4)^2*q1dot^2*cos(q6)*cos(q5)*cos(q3+q2)^2-Iz6s*sin(q5)*q2dot*sin(q6)*q1dot*sin(q3+q2)-Iz6s*sin(q5)*q3dot^2*cos(q6)*cos(q5)-Iz6s*sin(q5)*q3dot*sin(q6)*q1dot*sin(q3+q2)+Iy6s*sin(q5)*q3dot^2*cos(q6)*cos(q5)+Iy6s*sin(q5)*q2dot^2*cos(q6)*cos(q5)+2*Iy6s*sin(q5)*q2dot*cos(q6)*cos(q5)*q3dot+Iy6s*sin(q5)*q2dot*sin(q6)*q1dot*sin(q3+q2)-Iz6s*sin(q5)*q2dot^2*cos(q6)*cos(q5)-2*Iz6s*sin(q5)*q2dot*cos(q6)*cos(q5)*q3dot-Iy6s*sin(q5)*cos(q4)*q1dot^2*sin(q6)*sin(q4) Iy6s*cos(q6)*q5dot*sin(q5)*sin(q4)*q3dot-Iy6s*cos(q6)*q5dot*sin(q5)*cos(q4)*q1dot*sin(q3+q2)-Iy6s*q6dot*sin(q6)*sin(q5)*q4dot-Iy6s*q6dot*cos(q6)*cos(q4)*q2dot+Iz6s*cos(q6)*q5dot*sin(q5)*cos(q4)*q1dot*sin(q3+q2)-Iz6s*sin(q5)*sin(q4)*q2dot^2*cos(q6)*cos(q4)-2*Iz6s*sin(q5)*sin(q4)*q2dot*cos(q6)*cos(q4)*q3dot+2*Ix6s*sin(q5)*sin(q4)*q2dot*cos(q6)*cos(q4)*q3dot+Ix6s*sin(q5)*sin(q4)*q3dot^2*cos(q6)*cos(q4)+Ix6s*q6dot*cos(q6)*cos(q4)*q2dot+Ix6s*q6dot*cos(q6)*cos(q4)*q3dot+Ix6s*cos(q5)*q1dot^2*cos(q3+q2)*cos(q6)*sin(q4)*sin(q3+q2)+2*Ix6s*cos(q5)^2*q1dot^2*cos(q3+q2)*sin(q6)*cos(q4)*sin(q3+q2)-2*Ix6s*cos(q5)^2*q1dot*cos(q3+q2)*sin(q6)*sin(q4)*q2dot-2*Ix6s*cos(q5)^2*q1dot*cos(q3+q2)*sin(q6)*sin(q4)*q3dot-2*Ix6s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*cos(q6)*q2dot-2*Ix6s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*cos(q6)*q3dot+2*Ix6s*sin(q5)*cos(q4)*q1dot*sin(q3+q2)*sin(q6)*cos(q5)*sin(q4)*q2dot-Iz6s*sin(q4)*q3dot*sin(q6)*q1dot*cos(q3+q2)+2*Ix6s*sin(q5)*cos(q4)*q1dot*sin(q3+q2)*sin(q6)*cos(q5)*sin(q4)*q3dot+Ix6s*cos(q5)*q1dot^2*cos(q3+q2)^2*sin(q6)*sin(q5)+2*Ix6s*cos(q5)*q1dot*cos(q3+q2)*sin(q6)*sin(q5)*q4dot+2*Ix6s*cos(q5)^2*q4dot*sin(q6)*cos(q4)*q1dot*sin(q3+q2)-2*Ix6s*cos(q5)^2*q4dot*sin(q6)*sin(q4)*q2dot-2*Ix6s*cos(q5)^2*q4dot*sin(q6)*sin(q4)*q3dot+Ix6s*q6dot*sin(q6)*cos(q5)*cos(q4)*q1dot*sin(q3+q2)-Ix6s*q6dot*sin(q6)*cos(q5)*sin(q4)*q2dot-Ix6s*q6dot*sin(q6)*cos(q5)*sin(q4)*q3dot+Ix6s*q6dot*sin(q6)*sin(q5)*q1dot*cos(q3+q2)+Ix6s*q6dot*cos(q6)*sin(q4)*q1dot*sin(q3+q2)+Ix6s*cos(q5)*q4dot^2*sin(q6)*sin(q5)+Ix6s*cos(q6)*cos(q5)*cos(q4)*q2dot*q1dot*cos(q3+q2)+Ix6s*cos(q6)*cos(q5)*cos(q4)*q3dot*q1dot*cos(q3+q2)-Iz6s*sin(q5)*q3dot*cos(q6)*q1dot*sin(q3+q2)+Iy6s*cos(q6)*q5dot*cos(q5)*q4dot-Iy6s*sin(q6)*cos(q4)*q3ddot-Iz6s*q6dot*

46

cos(q6)*cos(q4)*q3dot-Iz6s*cos(q5)*q1dot^2*cos(q3+q2)*cos(q6)*sin(q4)*sin(q3+q2)-2*Iz6s*cos(q5)^2*q1dot^2*cos(q3+q2)*sin(q6)*cos(q4)*sin(q3+q2)+2*Iz6s*cos(q5)^2*q1dot*cos(q3+q2)*sin(q6)*sin(q4)*q2dot+2*Iz6s*cos(q5)^2*q1dot*cos(q3+q2)*sin(q6)*sin(q4)*q3dot-Iz6s*cos(q5)*q1dot^2*cos(q3+q2)^2*sin(q6)*sin(q5)-2*Iz6s*cos(q5)*q1dot*cos(q3+q2)*sin(q6)*sin(q5)*q4dot-2*Iz6s*cos(q5)^2*q4dot*sin(q6)*cos(q4)*q1dot*sin(q3+q2)+2*Iz6s*cos(q5)^2*q4dot*sin(q6)*sin(q4)*q2dot+2*Iz6s*cos(q5)^2*q4dot*sin(q6)*sin(q4)*q3dot-Iz6s*cos(q5)*q4dot^2*sin(q6)*sin(q5)-Iz6s*q6dot*sin(q6)*cos(q5)*cos(q4)*q1dot*sin(q3+q2)+Iz6s*q6dot*sin(q6)*cos(q5)*sin(q4)*q2dot+Iz6s*q6dot*sin(q6)*cos(q5)*sin(q4)*q3dot-Iz6s*q6dot*sin(q6)*sin(q5)*q1dot*cos(q3+q2)-Iz6s*q6dot*cos(q6)*sin(q4)*q1dot*sin(q3+q2)

Iz6s*cos(q6)*cos(q5)*cos(q4)*q2dot*q1dot*cos(q3+q2)-Iz6s*cos(q6)*cos(q5)*cos(q4)*q3dot*q1dot*cos(q3+q2)-Iz6s*cos(q6)*cos(q5)*q4dot*sin(q4)*q1dot*sin(q3+q2)-Iz6s*cos(q6)*cos(q5)*q4dot*cos(q4)*q2dot-Iz6s*cos(q6)*cos(q5)*q4dot*cos(q4)*q3dot+2*Iz6s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*cos(q6)*q2dot+2*Iz6s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*cos(q6)*q3dot-2*Iz6s*sin(q5)*cos(q4)*q1dot*sin(q3+q2)*sin(q6)*cos(q5)*sin(q4)*q2dot-Iy6s*q6dot*cos(q6)*cos(q4)*q3dot-Iy6s*q6dot*sin(q6)*cos(q5)*cos(q4)*q1dot*sin(q3+q2)+Iy6s*q6dot*sin(q6)*cos(q5)*sin(q4)*q2dot+Iy6s*q6dot*sin(q6)*cos(q5)*sin(q4)*q3dot-Iy6s*q6dot*sin(q6)*sin(q5)*q1dot*cos(q3+q2)-Iy6s*q6dot*cos(q6)*sin(q4)*q1dot*sin(q3+q2)+Iz6s*sin(q5)*cos(q4)^2*q1dot^2*sin(q6)*cos(q5)-Iz6s*sin(q5)*cos(q4)^2*q1dot^2*sin(q6)*cos(q5)*cos(q3+q2)^2+Iz6s*sin(q5)*q3dot^2*sin(q6)*cos(q5)-Iz6s*sin(q5)*q3dot^2*sin(q6)*cos(q5)*cos(q4)^2+2*Iz6s*sin(q5)*q2dot*sin(q6)*cos(q5)*q3dot-2*Iz6s*sin(q5)*q2dot*sin(q6)*cos(q5)*q3dot*cos(q4)^2-Ix6s*sin(q5)*cos(q4)*q1dot^2*cos(q6)*sin(q4)+Ix6s*sin(q5)*cos(q4)*q1dot^2*cos(q6)*sin(q4)*cos(q3+q2)^2+Ix6s*sin(q5)*q2dot^2*sin(q6)*cos(q5)*cos(q4)^2-Ix6s*cos(q4)*q1dot^2*sin(q3+q2)*sin(q6)*cos(q3+q2)-Iz6s*sin(q4)*q3dot*sin(q6)*q4dot-Iz6s*q6dot*cos(q6)*cos(q4)*q2dot-Iy6s*sin(q6)*q5ddot+Ix6s*cos(q6)*cos(q5)*q4dot*sin(q4)*q1dot*sin(q3+q2)+Ix6s*cos(q6)*cos(q5)*q4dot*cos(q4)*q2dot+Ix6s*cos(q6)*cos(q5)*q4dot*cos(q4)*q3dot-Iy6s*sin(q6)*sin(q4)*q3dot*q1dot*cos(q3+q2)-Iy6s*sin(q6)*q4dot*cos(q4)*q1dot*sin(q3+q2)+Iy6s*cos(q6)*cos(q5)*cos(q4)*q1ddot*sin(q3+q2)+Iy6s*cos(q6)*cos(q5)*cos(q4)*q2dot*q1dot*cos(q3+q2)+Iy6s*cos(q6)*cos(q5)*cos(q4)*q3dot*q1dot*cos(q3+q2)-Iy6s*cos(q6)*cos(q5)*q4dot*sin(q4)*q1dot*sin(q3+q2)-Iy6s*cos(q6)*cos(q5)*q4dot*cos(q4)*q2dot-Iy6s*cos(q6)*cos(q5)*q4dot*cos(q4)*q3dot-Iz6s*cos(q6)*q5dot*cos(q5)*q4dot-Iz6s*cos(q6)*q5dot*cos(q5)*q1dot*cos(q3+q2)-Iz6s*cos(q6)*q5dot*sin(q5)*sin(q4)*q2dot-Iz6s*cos(q6)*q5dot*sin(q5)*sin(q4)*q3dot-Iz6s*q6dot*sin(q6)*sin(q5)*q4dot-2*Iz6s*sin(q5)*cos(q4)*q1dot*sin(q3+q2)*sin(q6)*cos(q5)*sin(q4)*q3dot+Iy6s*sin(q6)*q4dot*sin(q4)*q3dot-Iz6s*sin(q5)*sin(q4)*q3dot^2*cos(q6)*cos(q4)-Ix6s*sin(q5)*cos(q4)^2*q1dot^2*sin(q6)*cos(q5)+Ix6s*sin(q5)*cos(q4)^2*q1dot^2*sin(q6)*cos(q5)*cos(q3+q2)^2-Iz6s*sin(q4)*q2dot*sin(q6)*q1dot*cos(q3+q2)-Ix6s*cos(q4)*q1dot*sin(q3+q2)*sin(q6)*q4dot+Iy6s*sin(q6)*q4dot*sin(q4)*q2dot-Iy6s*sin(q6)*sin(q4)*q1ddot*sin(q3+q2)-Iy6s*cos(q6)*cos(q5)*sin(q4)*q3ddot-Iy6s*cos(q6)*cos(q5)*sin(q4)*q2ddot+Ix6s*cos(q6)*q5dot*cos(q5)*q4dot+Ix6s*cos(q6)*q5dot*cos(q5)*q1dot*cos(q3+q2)+Ix6s*cos(q6)*q5dot*sin(q5)*sin(q4)*q2dot+Ix6s*cos(q6)*q5dot*sin(q5)*sin(q4)*q3dot-Ix6s*cos(q6)*q5dot*sin(q5)*cos(q4)*q1dot*sin(q3+q2)+Ix6s*q6dot*sin(q6)*sin(q5)*q4dot+Ix6s*sin(q5)*sin(q4)*q2dot^2*cos(q6)*cos(q4)+Iy6s*cos(q6)*sin(q5)*q1ddot*cos(q3+q2)-Iy6s*sin(q6)*sin(q4)*q2dot*q1dot*cos(q3+q2)-Iy6s*cos(q6)*sin(q5)*q2dot*q1dot*sin(q3+q2)-Iy6s*cos(q6)*sin(q5)*q3dot*q1dot*sin(q3+q2)+Iy6s*cos(q6)*q5dot*cos(q5)*q1dot*cos(q3+q2)+Iy6s*cos(q6)*q5dot*sin(q5)*sin(q4)*q2dot-Iy6s*sin(q6)*cos(q4)*q2ddot+Ix6s*q6dot*cos(q6)*q5dot+Iy6s*cos(q6)*sin(q5)*q4ddot-Iy6s*q6dot*cos(q6)*q5dot-Iz6s*q6dot*cos(q6)*q5dot-Ix6s*sin(q5)*q2dot^2*sin(q6)*cos(q5)-2*Ix6s*sin(q5)*q2dot*sin(q6)*cos(q5)*q3dot+2*Ix6s*sin(q5)*q2dot*sin(q6)*cos(q5)*q3dot*cos(q4)^2+Ix6s*sin(q4)*q2dot*sin(q6)*q1dot*cos(q3+q2)+Iz6s*sin(q5)*q2dot^2*sin(q6)*cos(q5)-Iz6s*sin(q5)*q2dot^2*sin(q6)*cos(q5)*cos(q4)^2-Iz6s*sin(q4)*q2dot*sin(q6)*q4dot-Ix6s*sin(q5)*q3dot^2*sin(q6)*cos(q5)+Ix6s*sin(q5)*q3dot^2*sin(q6)*cos(q5)*cos(q4)^2-Iz6s*sin(q5)*q2dot*cos(q6)*q1dot*sin(q3+q2)+Ix6s*sin(q4)*q2dot*sin(q6)*q4dot+Ix6s*sin(q5)*q3dot*cos(q6)*q1dot*sin(q3+q2)+Ix6s*sin(q4)*q3dot*sin(q6)*q1dot*cos(q3+q2)+Ix6s*sin(q4)*q3dot*sin(q6)*q4dot+Iz6s*sin(q5)*cos(q4)*q1dot^2*cos(q6)*sin(q4)-Iz6s*sin(q5)*cos(q4)*q1dot^2*cos(q6)*sin(q4)*cos(q3+q2)^2+Iz6s*cos(q4)*q1dot*sin(q3+q2)*sin(q6)*q4dot+Ix6s*sin(q5)*q2dot*cos(q6)*q1dot*sin(q3+q2)+Iz6s*cos(q4)*q1dot^2*sin(q3+q2)*sin(q6)*cos(q3+q2)-2*Iy6s*cos(q6)*cos(q5)*sin(q4)*q3dot*sin(q6)*sin(q5)*q1dot*cos(q3+q2)-2*Iy6s*cos(q6)*cos(q5)^2*sin(q4)*q2dot*sin(q6)*cos(q4)*q1dot*sin(q3+q2)-2*Iy6s*cos(q6)*cos(q5)*sin(q4)*q2dot*sin(q6)*sin(q5)*q4dot-2*Iy6s*cos(q6)*cos(q5)*sin(q4)*q3dot*sin(q6)*sin(q5)*q4dot+2*Iy6s*cos(q6)^2*sin(q5)*q1dot*cos(q3+q2)*cos(q4)*q2dot+2*Iy6s*cos(q6)^2*sin(q5)*q1dot^2*cos(q3+q2)*sin(q4)*sin(q3+q2)+2*Iy6s*cos(q6)^2*sin(q5)*q1dot*cos(q3+q2)*cos(q4)*q3dot-Iy6s*sin(q6)*cos(q4)^2*q3dot^2*cos(q6)+Ix6s*sin(q6)*cos(q4)^2*q2dot^2*cos(q6)-2*Ix6s*cos(q6)^2*sin(q5)*q4dot*q5dot+Ix6s*sin(q6)*cos(q4)^2*q3dot^2*cos(q6)-Iy6s*sin(q6)*cos(q4)^2*

47

q2dot^2*cos(q6)+2*Iy6s*cos(q6)^2*sin(q5)*q4dot*q5dot-Iz6s*sin(q5)*cos(q4)*q1ddot*sin(q3+q2)+Iz6s*sin(q5)*q4dot*cos(q4)*q2dot-2*Iy6s*cos(q6)*cos(q5)^2*sin(q4)*q3dot*sin(q6)*cos(q4)*q1dot*sin(q3+q2)+2*Iy6s*cos(q6)^2*sin(q5)*q4dot*sin(q4)*q1dot*sin(q3+q2)+Iz6s*sin(q5)*q4dot*cos(q4)*q3dot-Iz6s*cos(q5)*q2dot*q1dot*sin(q3+q2)-Iz6s*cos(q5)*q3dot*q1dot*sin(q3+q2)-Iz6s*q5dot*sin(q5)*q1dot*cos(q3+q2)+Iz6s*q5dot*cos(q5)*sin(q4)*q2dot+Iz6s*q5dot*cos(q5)*sin(q4)*q3dot-2*Iy6s*sin(q6)*sin(q4)*q1dot*sin(q3+q2)*cos(q6)*q5dot+2*Iy6s*cos(q6)*cos(q5)*cos(q4)*q1dot^2*sin(q3+q2)*sin(q6)*sin(q5)*cos(q3+q2)-Iy6s*sin(q6)*q5dot^2*cos(q6)-2*Ix6s*cos(q6)^2*sin(q5)*q4dot*sin(q4)*q1dot*sin(q3+q2)+2*Ix6s*cos(q6)^2*cos(q5)*sin(q4)*q2dot^2*cos(q4)-2*Ix6s*cos(q6)^2*sin(q5)*q1dot*cos(q3+q2)*cos(q4)*q2dot-Iz6s*sin(q5)*cos(q4)*q2dot*q1dot*cos(q3+q2)-Iz6s*sin(q5)*cos(q4)*q3dot*q1dot*cos(q3+q2)-Ix6s*cos(q6)*q4dot^2*sin(q6)+Iy6s*cos(q6)*q4dot^2*sin(q6)+Ix6s*q5dot*sin(q5)*q4dot+Ix6s*sin(q6)*q1dot^2*cos(q6)+Iz6s*sin(q5)*q4dot*sin(q4)*q1dot*sin(q3+q2)-Iz6s*q5dot*cos(q5)*cos(q4)*q1dot*sin(q3+q2)-2*Ix6s*cos(q6)^2*sin(q5)*q1dot*cos(q3+q2)*cos(q4)*q3dot-2*Ix6s*cos(q6)^2*sin(q5)*q1dot^2*cos(q3+q2)*sin(q4)*sin(q3+q2)+2*Ix6s*sin(q6)*cos(q4)*q3dot*cos(q6)*sin(q4)*q1dot*sin(q3+q2)+2*Ix6s*cos(q6)^2*cos(q5)*sin(q4)*q2dot*q5dot+Ix6s*sin(q6)*q5dot^2*cos(q6)+2*Ix6s*cos(q6)^2*cos(q5)*sin(q4)*q3dot*q5dot-2*Ix6s*cos(q6)^2*sin(q5)*q1dot*cos(q3+q2)*q5dot+2*Ix6s*sin(q6)*cos(q4)*q2dot*cos(q6)*q5dot+2*Ix6s*sin(q6)*cos(q4)^2*q2dot*cos(q6)*q3dot-2*Ix6s*cos(q6)^2*sin(q5)*q4dot*cos(q4)*q3dot-2*Ix6s*cos(q6)^2*sin(q5)*q4dot*cos(q4)*q2dot+2*Ix6s*sin(q6)*cos(q4)*q3dot*cos(q6)*q5dot+2*Ix6s*cos(q6)*cos(q5)*sin(q4)*q2dot*sin(q6)*sin(q5)*q1dot*cos(q3+q2)+2*Ix6s*cos(q6)*cos(q5)^2*sin(q4)*q2dot*sin(q6)*cos(q4)*q1dot*sin(q3+q2)+2*Ix6s*cos(q6)*cos(q5)*sin(q4)*q2dot*sin(q6)*sin(q5)*q4dot+2*Ix6s*cos(q6)*cos(q5)*sin(q4)*q3dot*sin(q6)*sin(q5)*q4dot+2*Ix6s*sin(q6)*cos(q4)*q2dot*cos(q6)*sin(q4)*q1dot*sin(q3+q2)+Iz6s*q6ddot+2*Ix6s*cos(q6)^2*cos(q5)*sin(q4)*q3dot^2*cos(q4)+2*Ix6s*cos(q6)*cos(q5)^2*sin(q4)*q3dot*sin(q6)*cos(q4)*q1dot*sin(q3+q2)+Iz6s*sin(q5)*sin(q4)*q2ddot-2*Ix6s*cos(q6)*cos(q5)*cos(q4)*q1dot^2*sin(q3+q2)*sin(q6)*sin(q5)*cos(q3+q2)-Iz6s*q5dot*sin(q5)*q4dot+2*Ix6s*sin(q6)*sin(q4)*q1dot*sin(q3+q2)*cos(q6)*q5dot+2*Ix6s*cos(q6)*cos(q5)*sin(q4)*q3dot*sin(q6)*sin(q5)*q1dot*cos(q3+q2)-2*Ix6s*cos(q6)*cos(q5)*cos(q4)*q1dot*sin(q3+q2)*sin(q6)*sin(q5)*q4dot+Iz6s*cos(q5)*q1ddot*cos(q3+q2)+4*Ix6s*cos(q6)^2*cos(q5)*sin(q4)*q2dot*cos(q4)*q3dot+2*Iy6s*cos(q6)*cos(q5)*cos(q4)*q1dot*sin(q3+q2)*sin(q6)*sin(q5)*q4dot+Iz6s*sin(q5)*sin(q4)*q3ddot-2*Iy6s*sin(q6)*cos(q4)*q3dot*cos(q6)*q5dot-4*Iy6s*cos(q6)^2*cos(q5)*sin(q4)*q2dot*cos(q4)*q3dot-2*Iy6s*sin(q6)*cos(q4)*q2dot*cos(q6)*q5dot-2*Ix6s*cos(q6)^2*cos(q5)*cos(q4)*q1dot*sin(q3+q2)*q5dot-4*Ix6s*cos(q6)^2*cos(q5)*cos(q4)^2*q1dot*sin(q3+q2)*q3dot-4*Ix6s*cos(q6)^2*cos(q5)*cos(q4)^2*q1dot*sin(q3+q2)*q2dot-2*Iy6s*cos(q6)^2*cos(q5)*sin(q4)*q2dot^2*cos(q4)+Iy6s*cos(q4)*q3dot^2*cos(q5)*sin(q4)+Iy6s*cos(q6)*cos(q5)^2*cos(q4)^2*q1dot^2*sin(q6)-Iy6s*cos(q6)*cos(q5)^2*cos(q4)^2*q1dot^2*sin(q6)*cos(q3+q2)^2+Ix6s*cos(q6)*q4dot^2*sin(q6)*cos(q5)^2-2*Ix6s*cos(q6)*cos(q5)^2*q2dot*sin(q6)*q3dot+2*Ix6s*cos(q6)*cos(q5)^2*q2dot*sin(q6)*q3dot*cos(q4)^2-Ix6s*cos(q6)*cos(q5)^2*q2dot^2*sin(q6)-2*Iy6s*cos(q6)^2*cos(q5)*sin(q4)*q2dot*q5dot+2*Iy6s*cos(q6)*q1dot*cos(q3+q2)*sin(q6)*q4dot-2*Iy6s*cos(q6)*q1dot*cos(q3+q2)*sin(q6)*q4dot*cos(q5)^2+Ix6s*cos(q4)*q2dot*sin(q5)*q1dot*cos(q3+q2)+Ix6s*cos(q4)*q2dot*sin(q5)*q4dot-Iy6s*sin(q4)*q1dot^2*sin(q3+q2)*sin(q5)*cos(q3+q2)-Iy6s*sin(q4)*q1dot^2*cos(q5)*cos(q4)-Iy6s*cos(q6)*q4dot^2*sin(q6)*cos(q5)^2+2*Iy6s*cos(q6)^2*cos(q5)*cos(q4)*q1dot*sin(q3+q2)*q5dot-2*Iy6s*cos(q6)^2*cos(q5)*sin(q4)*q3dot^2*cos(q4)+Ix6s*cos(q6)*cos(q5)^2*q2dot^2*sin(q6)*cos(q4)^2-Iy6s*sin(q4)*q1dot*sin(q3+q2)*sin(q5)*q4dot-Iy6s*q5dot*cos(q5)*cos(q4)*q1dot*sin(q3+q2)+Iy6s*sin(q4)*q1dot^2*cos(q5)*cos(q4)*cos(q3+q2)^2+Iy6s*cos(q6)*cos(q5)^2*q2dot^2*sin(q6)-Iy6s*cos(q6)*cos(q5)^2*q2dot^2*sin(q6)*cos(q4)^2+Iy6s*q1dot*sin(q3+q2)*cos(q5)*q2dot+2*Ix6s*cos(q4)^2*q2dot*cos(q5)*q1dot*sin(q3+q2)+Ix6s*sin(q4)*q1dot^2*sin(q3+q2)*sin(q5)*cos(q3+q2)+Iy6s*q5dot*cos(q5)*sin(q4)*q3dot+Ix6s*sin(q4)*q1dot^2*cos(q5)*cos(q4)+Iz6s*cos(q5)*q4ddot-Iy6s*cos(q4)*q2dot*sin(q5)*q1dot*cos(q3+q2)-Ix6s*q1dot*sin(q3+q2)*cos(q5)*q3dot+2*Ix6s*q1dot*sin(q3+q2)*cos(q5)*q3dot*cos(q4)^2+2*Ix6s*q1dot*sin(q3+q2)*cos(q5)*q3dot*cos(q6)^2-Ix6s*cos(q6)*cos(q5)^2*cos(q4)^2*q1dot^2*sin(q6)+Ix6s*cos(q6)*cos(q5)^2*cos(q4)^2*q1dot^2*sin(q6)*cos(q3+q2)^2+2*Iy6s*cos(q4)*q2dot*cos(q5)*sin(q4)*q3dot+Iy6s*q1dot*sin(q3+q2)*cos(q5)*q3dot-2*Ix6s*cos(q6)*q1dot*cos(q3+q2)*sin(q6)*q4dot-Ix6s*sin(q4)*q1dot^2*cos(q5)*cos(q4)*cos(q3+q2)^2-2*Ix6s*sin(q4)*q1dot^2*cos(q5)*cos(q4)*cos(q6)^2+2*Ix6s*sin(q4)*q1dot^2*cos(q5)*cos(q4)*cos(q6)^2*cos(q3+q2)^2+2*Iy6s*cos(q6)^2*cos(q5)*cos(q4)*q1dot^2*sin(q4)-2*Iy6s*cos(q6)^2*cos(q5)*cos(q4)*q1dot^2*sin(q4)*cos(q3+q2)^2-2*Iy6s*cos(q4)^2*q2dot*cos(q5)*q1dot*sin(q3+q2)-Iy6s*cos(q4)*q3dot*sin(q5)*q4dot-2*Ix6s*cos(q6)*q1dot^2*cos(q3+q2)^2*sin(q6)+Ix6s*cos(q6)*q1dot^2*cos(q3+q2)^2*sin(q6)*cos(q5)^2-Iy6s*cos(q4)*q3dot*sin(q5)*q1dot*cos(q3+q2)+2*Ix6s*cos(q6)*q1dot*cos(q3+q2)*sin(q6)*q4dot*cos(q5)^2-2*Iy6s*q1dot*sin(q3+q2)*cos(q5)*q3dot*cos(q6)^2-Ix6s*q5dot*cos(q5)*sin(q4)*q2dot-Iy6s*cos(q4)*q2dot*sin(q5)*q4dot+2*Iy6s*cos(q6)*cos(q5)^2*q2dot*sin(q6)*q3dot-2*Iy6s*cos(q6)*cos(q5)^2*q2dot*sin(q6)*q3dot*

48

cos(q4)^2-Iy6s*q5dot*sin(q5)*q1dot*cos(q3+q2)-2*Iy6s*sin(q6)*cos(q4)^2*q2dot*cos(q6)*q3dot-2*Iy6s*cos(q6)^2*cos(q5)*sin(q4)*q3dot*q5dot+2*Iy6s*cos(q6)^2*sin(q5)*q4dot*cos(q4)*q3dot-Iy6s*sin(q6)*q1dot^2*cos(q6)-Ix6s*sin(q6)*q1dot^2*cos(q6)*cos(q4)^2+Ix6s*sin(q6)*q1dot^2*cos(q6)*cos(q4)^2*cos(q3+q2)^2-2*Iy6s*cos(q4)^2*q3dot*cos(q5)*q1dot*sin(q3+q2)-Ix6s*cos(q4)*q3dot^2*cos(q5)*sin(q4)-2*Ix6s*cos(q4)*q2dot*cos(q5)*sin(q4)*q3dot-Ix6s*q1dot*sin(q3+q2)*cos(q5)*q2dot+2*Ix6s*q1dot*sin(q3+q2)*cos(q5)*q2dot*cos(q6)^2+4*Iy6s*cos(q6)^2*cos(q5)*cos(q4)^2*q1dot*sin(q3+q2)*q2dot-2*Iy6s*q1dot*sin(q3+q2)*cos(q5)*q2dot*cos(q6)^2+Iy6s*cos(q4)*q2dot^2*cos(q5)*sin(q4)-Iy6s*q5dot*sin(q5)*q4dot+2*Iy6s*cos(q6)^2*sin(q5)*q4dot*cos(q4)*q2dot-2*Iy6s*cos(q6)*cos(q5)*sin(q4)*q2dot*sin(q6)*sin(q5)*q1dot*cos(q3+q2)-2*Iy6s*sin(q6)*cos(q4)*q2dot*cos(q6)*sin(q4)*q1dot*sin(q3+q2)-Ix6s*cos(q4)*q2dot^2*cos(q5)*sin(q4)+2*Iy6s*sin(q6)*q1dot^2*cos(q6)*cos(q3+q2)^2+Iy6s*sin(q6)*q1dot^2*cos(q6)*cos(q4)^2-Iy6s*sin(q6)*q1dot^2*cos(q6)*cos(q4)^2*cos(q3+q2)^2-Ix6s*q5dot*cos(q5)*sin(q4)*q3dot+Ix6s*q5dot*sin(q5)*q1dot*cos(q3+q2)-2*Iy6s*sin(q6)*cos(q4)*q3dot*cos(q6)*sin(q4)*q1dot*sin(q3+q2)+4*Iy6s*cos(q6)^2*cos(q5)*cos(q4)^2*q1dot*sin(q3+q2)*q3dot+2*Iy6s*cos(q6)^2*sin(q5)*q1dot*cos(q3+q2)*q5dot+Ix6s*cos(q4)*q3dot*sin(q5)*q4dot+Ix6s*sin(q4)*q1dot*sin(q3+q2)*sin(q5)*q4dot+Ix6s*q5dot*cos(q5)*cos(q4)*q1dot*sin(q3+q2)+Iy6s*q5dot*cos(q5)*sin(q4)*q2dot-Ix6s*cos(q6)*cos(q5)^2*q3dot^2*sin(q6)+Ix6s*cos(q6)*cos(q5)^2*q3dot^2*sin(q6)*cos(q4)^2+Iy6s*cos(q6)*cos(q5)^2*q3dot^2*sin(q6)-Iy6s*cos(q6)*cos(q5)^2*q3dot^2*sin(q6)*cos(q4)^2+Ix6s*cos(q4)*q3dot*sin(q5)*q1dot*cos(q3+q2)-Iy6s*sin(q6)*q1dot^2*cos(q6)*cos(q3+q2)^2*cos(q5)^2

Forţele de legătură ale robotului articulat 6R

f66 = fx7-M6*sin(q6)*sin(q5)*cos(q3)*l2*q2dot^2+M6*sin(q6)*sin(q5)*l3*q1dot^2*cos(q3+q2)^2+M6*cos(q6)*cos(q4)*l3*q1ddot*sin(q3+q2)+M6*cos(q6)*cos(q4)*l2*sin(q2)*q1ddot-M6*cos(q6)*sin(q4)*cos(q3)*q2ddot*l2+M6*cos(q6)*sin(q4)*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3-M6*sin(q6)*cos(q5)*sin(q4)*l2*sin(q2)*q1ddot-M6*sin(q6)*cos(q5)*sin(q4)*l3*q1ddot*sin(q3+q2)+M6*sin(q6)*cos(q5)*cos(q4)*g*sin(q3+q2)+1/2*M6*sin(q6)*sin(q5)*q1dot^2*l2*cos(q3+2*q2)+1/2*M6*cos(q6)*sin(q4)*q1dot^2*l2*sin(q3+2*q2)-M6*sin(q6)*cos(q5)*cos(q4)*cos(q3)*q2ddot*l2-M6*sin(q6)*cos(q5)*cos(q4)*sin(q3)*l2*q2dot^2-M6*sin(q6)*cos(q5)*cos(q4)*l3*q2ddot+M6*cos(q6)*sin(q4)*g*sin(q3+q2)-M6*cos(q6)*sin(q4)*l3*q2ddot+M6*sin(q6)*sin(q5)*g*cos(q3+q2)-M6*cos(q6)*sin(q4)*l3*q3ddot-M6*sin(q6)*sin(q5)*l3*q2dot^2-M6*sin(q6)*sin(q5)*l3*q3dot^2-M6*sin(q6)*sin(q5)*l3*q1dot^2-2*M6*sin(q6)*sin(q5)*l3*q2dot*q3dot-1/2*M6*sin(q6)*cos(q5)*cos(q4)*sin(q3)*l2*q1dot^2-1/2*M6*sin(q6)*sin(q5)*cos(q3)*l2*q1dot^2-1/2*M6*cos(q6)*sin(q4)*sin(q3)*l2*q1dot^2+1/2*M6*sin(q6)*cos(q5)*cos(q4)*q1dot^2*l2*sin(q3+2*q2)+2*M6*cos(q6)*cos(q4)*l3*q3dot*q1dot*cos(q3+q2)+2*M6*cos(q6)*cos(q4)*l3*q2dot*q1dot*cos(q3+q2)+2*M6*cos(q6)*cos(q4)*q2dot*cos(q2)*q1dot*l2-2*M6*sin(q6)*cos(q5)*sin(q4)*q2dot*cos(q2)*q1dot*l2-2*M6*sin(q6)*cos(q5)*sin(q4)*l3*q2dot*q1dot*cos(q3+q2)-2*M6*sin(q6)*cos(q5)*sin(q4)*l3*q3dot*q1dot*cos(q3+q2)-M6*cos(q6)*sin(q4)*sin(q3)*l2*q2dot^2-M6*sin(q6)*cos(q5)*cos(q4)*l3*q3ddot+M6*sin(q6)*cos(q5)*cos(q4)*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3+M6*sin(q6)*sin(q5)*sin(q3)*q2ddot*l2

M6*cos(q6)*cos(q5)*cos(q4)*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3-M6*cos(q6)*cos(q5)*cos(q4)*l3*q2ddot-M6*cos(q6)*cos(q5)*cos(q4)*l3*q3ddot+1/2*M6*sin(q6)*sin(q4)*sin(q3)*l2*q1dot^2-M6*cos(q6)*sin(q5)*l3*q2dot^2+M6*cos(q6)*sin(q5)*g*cos(q3+q2)+M6*sin(q6)*sin(q4)*l3*q2ddot+M6*cos(q6)*sin(q5)*l3*q1dot^2*cos(q3+q2)^2+1/2*M6*cos(q6)*cos(q5)*cos(q4)*q1dot^2*l2*sin(q3+2*q2)+1/2*M6*cos(q6)*sin(q5)*q1dot^2*l2*cos(q3+2*q2)-M6*cos(q6)*sin(q5)*l3*q1dot^2+M6*cos(q6)*cos(q5)*cos(q4)*g*sin(q3+q2)-M6*cos(q6)*cos(q5)*cos(q4)*cos(q3)*q2ddot*l2-M6*cos(q6)*cos(q5)*cos(q4)*sin(q3)*l2*q2dot^2-M6*cos(q6)*sin(q5)*cos(q3)*l2*q2dot^2-M6*cos(q6)*cos(q5)*sin(q4)*l3*q1ddot*sin(q3+q2)+M6*cos(q6)*sin(q5)*sin(q3)*q2ddot*l2-M6*cos(q6)*sin(q5)*l3*q3dot^2+M6*sin(q6)*sin(q4)*sin(q3)*l2*q2dot^2-1/2*M6*cos(q6)*cos(q5)*cos(q4)*sin(q3)*l2*q1dot^2+M6*sin(q6)*sin(q4)*l3*q3ddot-1/2*M6*cos(q6)*sin(q5)*cos(q3)*l2*q1dot^2-1/2*M6*sin(q6)*sin(q4)*q1dot^2*l2*sin(q3+2*q2)+fy7-2*M6*cos(q6)*cos(q5)*sin(q4)*q2dot*cos(q2)*q1dot*l2-2*M6*cos(q6)*cos(q5)*sin(q4)*l3*q3dot*q1dot*cos(q3+q2)-2*M6*cos(q6)*sin(q5)*l3*q2dot*q3dot-2*M6*sin(q6)*cos(q4)*l3*q2dot*q1dot*cos(q3+q2)-2*M6*sin(q6)*cos(q4)*q2dot*cos(q2)*q1dot*l2-2*M6*sin(q6)*cos(q4)*l3*q3dot*q1dot*cos(q3+q2)-M6*sin(q6)*cos(q4)*l3*q1ddot*sin(q3+q2)+

49

M6*sin(q6)*sin(q4)*cos(q3)*q2ddot*l2-M6*sin(q6)*sin(q4)*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3-M6*sin(q6)*sin(q4)*g*sin(q3+q2)-M6*cos(q6)*cos(q5)*sin(q4)*l2*sin(q2)*q1ddot-M6*sin(q6)*cos(q4)*l2*sin(q2)*q1ddot-2*M6*cos(q6)*cos(q5)*sin(q4)*l3*q2dot*q1dot*cos(q3+q2) fz7+M6*sin(q5)*sin(q4)*l2*sin(q2)*q1ddot+2*M6*sin(q5)*sin(q4)*q2dot*cos(q2)*q1dot*l2+M6*sin(q5)*sin(q4)*l3*q1ddot*sin(q3+q2)+2*M6*sin(q5)*sin(q4)*l3*q2dot*q1dot*cos(q3+q2)+2*M6*sin(q5)*sin(q4)*l3*q3dot*q1dot*cos(q3+q2)-M6*sin(q5)*cos(q4)*g*sin(q3+q2)+M6*sin(q5)*cos(q4)*cos(q3)*q2ddot*l2-1/2*M6*sin(q5)*cos(q4)*q1dot^2*l2*sin(q3+2*q2)+1/2*M6*sin(q5)*cos(q4)*sin(q3)*l2*q1dot^2+M6*sin(q5)*cos(q4)*sin(q3)*l2*q2dot^2+M6*sin(q5)*cos(q4)*l3*q2ddot+M6*sin(q5)*cos(q4)*l3*q3ddot-M6*sin(q5)*cos(q4)*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3+M6*cos(q5)*g*cos(q3+q2)+M6*cos(q5)*sin(q3)*q2ddot*l2+1/2*M6*cos(q5)*q1dot^2*l2*cos(q3+2*q2)-1/2*M6*cos(q5)*cos(q3)*l2*q1dot^2-M6*cos(q5)*cos(q3)*l2*q2dot^2-M6*cos(q5)*l3*q1dot^2+M6*cos(q5)*l3*q1dot^2*cos(q3+q2)^2-M6*cos(q5)*l3*q2dot^2-2*M6*cos(q5)*l3*q2dot*q3dot-M6*cos(q5)*l3*q3dot^2 f55 = M6*sin(q4)*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3-1/2*M6*sin(q4)*sin(q3)*l2*q1dot^2+2*M6*cos(q4)*l3*q3dot*q1dot*cos(q3+q2)+M6*cos(q4)*l2*sin(q2)*q1ddot-M6*sin(q4)*cos(q3)*q2ddot*l2+2*M6*cos(q4)*l3*q2dot*q1dot*cos(q3+q2)+2*M6*cos(q4)*q2dot*cos(q2)*q1dot*l2+1/2*M6*sin(q4)*q1dot^2*l2*sin(q3+2*q2)-M6*sin(q4)*sin(q3)*l2*q2dot^2+M6*cos(q4)*l3*q1ddot*sin(q3+q2)+M5*cos(q4)*l2*sin(q2)*q1ddot+2*M5*cos(q4)*q2dot*cos(q2)*q1dot*l2+M5*cos(q4)*l3*q1ddot*sin(q3+q2)+2*M5*cos(q4)*l3*q2dot*q1dot*cos(q3+q2)+2*M5*cos(q4)*l3*q3dot*q1dot*cos(q3+q2)-M5*sin(q4)*cos(q3)*q2ddot*l2+1/2*M5*sin(q4)*q1dot^2*l2*sin(q3+2*q2)-1/2*M5*sin(q4)*sin(q3)*l2*q1dot^2-M5*sin(q4)*sin(q3)*l2*q2dot^2+M5*sin(q4)*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3+M5*sin(q4)*g*sin(q3+q2)-M5*sin(q4)*l3*q2ddot-M5*sin(q4)*l3*q3ddot+cos(q6)*fx7-sin(q6)*fy7-M6*sin(q4)*l3*q2ddot-M6*sin(q4)*l3*q3ddot+M6*sin(q4)*g*sin(q3+q2)

-M6*cos(q5)*sin(q4)*l3*q1ddot*sin(q3+q2)+1/2*M6*sin(q5)*q1dot^2*l2*cos(q3+2*q2)-2*M6*cos(q5)*sin(q4)*l3*q2dot*q1dot*cos(q3+q2)-1/2*M6*cos(q5)*cos(q4)*sin(q3)*l2*q1dot^2-M6*sin(q5)*cos(q3)*l2*q2dot^2-M6*cos(q5)*cos(q4)*l3*q2ddot-1/2*M6*sin(q5)*cos(q3)*l2*q1dot^2+1/2*M6*cos(q5)*cos(q4)*q1dot^2*l2*sin(q3+2*q2)-2*M6*cos(q5)*sin(q4)*q2dot*cos(q2)*q1dot*l22*M6*cos(q5)*sin(q4)*l3*q3dot*q1dot*cos(q3+q2)-M6*cos(q5)*cos(q4)*l3*q3ddot-M6*cos(q5)*sin(q4)*l2*sin(q2)*q1ddot-M6*cos(q5)*cos(q4)*cos(q3)*q2ddot*l2-M6*cos(q5)*cos(q4)*sin(q3)*l2*q2dot^2+M6*sin(q5)*g*cos(q3+q2)+M5*sin(q5)*g*cos(q3+q2)-M5*sin(q5)*l3*q1dot^2-M5*sin(q5)*l3*q2dot^2-M5*sin(q5)*l3*q3dot^2+M6*cos(q5)*cos(q4)*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3+M6*sin(q5)*sin(q3)*q2ddot*l2+M6*sin(q5)*l3*q1dot^2*cos(q3+q2)^2-M5*cos(q5)*sin(q4)*l3*q1ddot*sin(q3+q2)-M5*cos(q5)*sin(q4)*l2*sin(q2)*q1ddot-2*M5*cos(q5)*sin(q4)*q2dot*cos(q2)*q1dot*l2+cos(q6)*fy7-2*M5*cos(q5)*sin(q4)*l3*q2dot*q1dot*cos(q3+q2)-2*M5*cos(q5)*sin(q4)*l3*q3dot*q1dot*cos(q3+q2)+M5*cos(q5)*cos(q4)*g*sin(q3+q2)-M5*cos(q5)*cos(q4)*cos(q3)*q2ddot*l2+1/2*M5*cos(q5)*cos(q4)*q1dot^2*l2*sin(q3+2*q2)-1/2*M5*cos(q5)*cos(q4)*sin(q3)*l2*q1dot^2-M5*cos(q5)*cos(q4)*sin(q3)*l2*q2dot^2-M5*cos(q5)*cos(q4)*l3*q2ddot-M5*cos(q5)*cos(q4)*l3*q3ddot+M5*cos(q5)*cos(q4)*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3+M5*sin(q5)*sin(q3)*q2ddot*l2+1/2*M5*sin(q5)*q1dot^2*l2*cos(q3+2*q2)-1/2*M5*sin(q5)*cos(q3)*l2*q1dot^2-M5*sin(q5)*cos(q3)*l2*q2dot^2+M5*sin(q5)*l3*q1dot^2*cos(q3+q2)^2-2*M5*sin(q5)*l3*q2dot*q3dot+sin(q6)*fx7-2*M6*sin(q5)*l3*q2dot*q3dot+M6*cos(q5)*cos(q4)*g*sin(q3+q2)-M6*sin(q5)*l3*q1dot^2-M6*sin(q5)*l3*q2dot^2-M6*sin(q5)*l3*q3dot^2

1/2*M5*cos(q5)*q1dot^2*l2*cos(q3+2*q2)-1/2*M5*cos(q5)*cos(q3)*l2*q1dot^2-M5*cos(q5)*cos(q3)*l2*q2dot^2+M5*cos(q5)*l3*q1dot^2*cos(q3+q2)^2-2*M5*cos(q5)*l3*q2dot*q3dot+M5*sin(q5)*cos(q4)*sin(q3)*l2*q2dot^2+M5*sin(q5)*cos(q4)*l3*q3ddot-M5*sin(q5)*cos(q4)*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3+M5*cos(q5)*sin(q3)*q2ddot*l2-M6*cos(q5)*l3*q2dot^2+M5*cos(q5)*g*cos(q3+q2)-M5*cos(q5)*l3*q1dot^2-M5*cos(q5)*l3*q2dot^2+fz7+M6*cos(q5)*g*cos(q3+q2)-M6*cos(q5)*l3*q3dot^2+M6*sin(q5)*sin(q4)*l2*sin(q2)*q1ddot+2*M6*sin(q5)*sin(q4)*q2dot*cos(q2)*q1dot*l2+M6*sin(q5)*sin(q4)*l3*q1ddot*sin(q3+q2)+2*M6*sin(q5)*sin(q4)*l3*q2dot*q1dot*cos(q3+q2)+2*M6*sin(q5)*sin(q4)*l3*q3dot*q1dot*cos(q3+q2)-M6*sin(q5)*cos(q4)*g*sin(q3+q2)+M6*sin(q5)*cos(q4)*cos(q3)*q2ddot*l2-1/2*M6*sin(q5)*cos(q4)*q1dot^2*l2*sin(q3+2*q2)+

50

1/2*M6*sin(q5)*cos(q4)*sin(q3)*l2*q1dot^2+M6*sin(q5)*cos(q4)*sin(q3)*l2*q2dot^2+M6*sin(q5)*cos(q4)*l3*q2ddot+M6*sin(q5)*cos(q4)*l3*q3ddot-M6*sin(q5)*cos(q4)*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3+M6*cos(q5)*sin(q3)*q2ddot*l2+1/2*M6*cos(q5)*q1dot^2*l2*cos(q3+2*q2)-M6*cos(q5)*l3*q1dot^2-1/2*M6*cos(q5)*cos(q3)*l2*q1dot^2-M6*cos(q5)*cos(q3)*l2*q2dot^2+M6*cos(q5)*l3*q1dot^2*cos(q3+q2)^2-2*M6*cos(q5)*l3*q2dot*q3dot-M5*cos(q5)*l3*q3dot^2+M5*sin(q5)*sin(q4)*l2*sin(q2)*q1ddot+2*M5*sin(q5)*sin(q4)*q2dot*cos(q2)*q1dot*l2+M5*sin(q5)*sin(q4)*l3*q1ddot*sin(q3+q2)+2*M5*sin(q5)*sin(q4)*l3*q2dot*q1dot*cos(q3+q2)+2*M5*sin(q5)*sin(q4)*l3*q3dot*q1dot*cos(q3+q2)-M5*sin(q5)*cos(q4)*g*sin(q3+q2)+M5*sin(q5)*cos(q4)*cos(q3)*q2ddot*l2-1/2*M5*sin(q5)*cos(q4)*q1dot^2*l2*sin(q3+2*q2)+1/2*M5*sin(q5)*cos(q4)*sin(q3)*l2*q1dot^2+M5*sin(q5)*cos(q4)*l3*q2ddot

f44 = M6*sin(q4)*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3-1/2*M6*sin(q4)*sin(q3)*l2*q1dot^2+2*M6*cos(q4)*l3*q3dot*q1dot*cos(q3+q2)+M6*cos(q4)*l2*sin(q2)*q1ddot-M6*sin(q4)*cos(q3)*q2ddot*l2+2*M6*cos(q4)*l3*q2dot*q1dot*cos(q3+q2)+2*M6*cos(q4)*q2dot*cos(q2)*q1dot*l2+1/2*M6*sin(q4)*q1dot^2*l2*sin(q3+2*q2)-M6*sin(q4)*sin(q3)*l2*q2dot^2+M6*cos(q4)*l3*q1ddot*sin(q3+q2)+M5*cos(q4)*l2*sin(q2)*q1ddot+2*M5*cos(q4)*q2dot*cos(q2)*q1dot*l2+M5*cos(q4)*l3*q1ddot*sin(q3+q2)+2*M5*cos(q4)*l3*q2dot*q1dot*cos(q3+q2)+2*M5*cos(q4)*l3*q3dot*q1dot*cos(q3+q2)-M5*sin(q4)*cos(q3)*q2ddot*l2+1/2*M5*sin(q4)*q1dot^2*l2*sin(q3+2*q2)-1/2*M5*sin(q4)*sin(q3)*l2*q1dot^2-M5*sin(q4)*sin(q3)*l2*q2dot^2+M5*sin(q4)*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3+M5*sin(q4)*g*sin(q3+q2)-M5*sin(q4)*l3*q2ddot-M5*sin(q4)*l3*q3ddot+cos(q6)*fx7-sin(q6)*fy7-M6*sin(q4)*l3*q2ddot-M6*sin(q4)*l3*q3ddot+M6*sin(q4)*g*sin(q3+q2)+M4*cos(q4)*l3*q1ddot*sin(q3+q2)+2*M4*cos(q4)*l3*q2dot*q1dot*cos(q3+q2)+2*M4*cos(q4)*l3*q3dot*q1dot*cos(q3+q2)-M4*sin(q4)*cos(q3)*q2ddot*l2+1/2*M4*sin(q4)*q1dot^2*l2*sin(q3+2*q2)-1/2*M4*sin(q4)*sin(q3)*l2*q1dot^2-M4*sin(q4)*sin(q3)*l2*q2dot^2+M4*sin(q4)*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3+M4*cos(q4)*l2*sin(q2)*q1ddot+2*M4*cos(q4)*q2dot*cos(q2)*q1dot*l2+M4*sin(q4)*g*sin(q3+q2)-M4*sin(q4)*l3*q2ddot-M4*sin(q4)*l3*q3ddot

1/2*M4*cos(q4)*q1dot^2*l2*sin(q3+2*q2)-sin(q5)*fz7+M4*cos(q4)*g*sin(q3+q2)-M5*cos(q4)*cos(q3)*q2ddot*l2+1/2*M5*cos(q4)*q1dot^2*l2*sin(q3+2*q2)-2*M5*sin(q4)*q2dot*cos(q2)*q1dot*l2-M5*sin(q4)*l3*q1ddot*sin(q3+q2)-M6*sin(q4)*l2*sin(q2)*q1ddot+M6*cos(q4)*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3+M5*cos(q4)*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3-M6*sin(q4)*l3*q1ddot*sin(q3+q2)-2*M5*sin(q4)*l3*q2dot*q1dot*cos(q3+q2)+1/2*M6*cos(q4)*q1dot^2*l2*sin(q3+2*q2)+cos(q5)*cos(q6)*fy7+cos(q5)*sin(q6)*fx7-M4*cos(q4)*l3*q2ddot-M4*cos(q4)*l3*q3ddot+M6*cos(q4)*g*sin(q3+q2)+M5*cos(q4)*g*sin(q3+q2)-M6*cos(q4)*l3*q3ddot-M5*cos(q4)*l3*q3ddot-M5*cos(q4)*l3*q2ddot-M6*cos(q4)*l3*q2ddot-M4*sin(q4)*l2*sin(q2)*q1ddot-2*M4*sin(q4)*q2dot*cos(q2)*q1dot*l2-M4*sin(q4)*l3*q1ddot*sin(q3+q2)-2*M4*sin(q4)*l3*q2dot*q1dot*cos(q3+q2)-2*M4*sin(q4)*l3*q3dot*q1dot*cos(q3+q2)-M4*cos(q4)*cos(q3)*q2ddot*l2-1/2*M4*cos(q4)*sin(q3)*l2*q1dot^2-M4*cos(q4)*sin(q3)*l2*q2dot^2+M4*cos(q4)*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3-1/2*M5*cos(q4)*sin(q3)*l2*q1dot^2-M5*cos(q4)*sin(q3)*l2*q2dot^2-2*M6*sin(q4)*l3*q3dot*q1dot*cos(q3+q2)-M6*cos(q4)*sin(q3)*l2*q2dot^2-2*M5*sin(q4)*l3*q3dot*q1dot*cos(q3+q2)-1/2*M6*cos(q4)*sin(q3)*l2*q1dot^2-M6*cos(q4)*cos(q3)*q2ddot*l2-2*M6*sin(q4)*l3*q2dot*q1dot*cos(q3+q2)-2*M6*sin(q4)*q2dot*cos(q2)*q1dot*l2-M5*sin(q4)*l2*sin(q2)*q1ddot M5*g*cos(q3+q2)+M6*g*cos(q3+q2)+M6*sin(q3)*q2ddot*l2+M5*sin(q3)*q2ddot*l2+1/2*M5*q1dot^2*l2*cos(q3+2*q2)-1/2*M6*cos(q3)*l2*q1dot^2-M6*cos(q3)*l2*q2dot^2+1/2*M6*q1dot^2*l2*cos(q3+2*q2)+M6*l3*q1dot^2*cos(q3+q2)^2-M5*cos(q3)*l2*q2dot^2-1/2*M5*cos(q3)*l2*q1dot^2+M5*l3*q1dot^2*cos(q3+q2)^2+cos(q5)*fz7-2*M4*l3*q2dot*q3dot+M4*sin(q3)*q2ddot*l2+1/2*M4*q1dot^2*l2*cos(q3+2*q2)-1/2*M4*cos(q3)*l2*q1dot^2-M4*cos(q3)*l2*q2dot^2+M4*l3*q1dot^2*cos(q3+q2)^2-M5*l3*q1dot^2-M6*l3*q1dot^2-M5*l3*q3dot^2-M6*l3*q2dot^2-M6*l3*q3dot^2-M5*l3*q2dot^2-2*M5*l3*q2dot*q3dot-2*M6*l3*q2dot*q3dot-M4*l3*q1dot^2-M4*l3*q2dot^2-M4*l3*q3dot^2+sin(q5)*sin(q6)*fx7+sin(q5)*cos(q6)*fy7+M4*g*cos(q3+q2) f33 =

51

-sin(q4)*cos(q5)*cos(q6)*fy7+sin(q4)*sin(q5)*fz7+cos(q4)*cos(q6)*fx7-cos(q4)*sin(q6)*fy7+M5*l3*q1ddot*sin(q3+q2)+M4*l2*sin(q2)*q1ddot+M6*l3*q1ddot*sin(q3+q2)+2*M6*l3*q2dot*q1dot*cos(q3+q2)+2*M5*l3*q2dot*q1dot*cos(q3+q2)+2*M3*l2*q2dot*cos(q2)*q1dot+M6*l2*sin(q2)*q1ddot+2*M6*l3*q3dot*q1dot*cos(q3+q2)+2*M5*l3*q3dot*q1dot*cos(q3+q2)+2*M5*q2dot*cos(q2)*q1dot*l2+2*M4*l3*q3dot*q1dot*cos(q3+q2)+2*M6*q2dot*cos(q2)*q1dot*l2+2*M4*q2dot*cos(q2)*q1dot*l2+2*M4*l3*q2dot*q1dot*cos(q3+q2)+M5*l2*sin(q2)*q1ddot+M4*l3*q1ddot*sin(q3+q2)+M3*l2*sin(q2)*q1ddot-sin(q4)*cos(q5)*sin(q6)*fx7

M4*g*sin(q3+q2)-M5*l3*q2ddot-M5*l3*q3ddot-M6*l3*q2ddot-M6*l3*q3ddot-M4*l3*q2ddot-M4*l3*q3ddot-1/2*M6*sin(q3)*l2*q1dot^2-M5*cos(q3)*q2ddot*l2+1/2*M5*q1dot^2*l2*sin(q3+2*q2)-1/2*M5*sin(q3)*l2*q1dot^2-M6*cos(q3)*q2ddot*l2+1/2*M6*q1dot^2*l2*sin(q3+2*q2)-M5*sin(q3)*l2*q2dot^2+sin(q4)*cos(q6)*fx7+M3*g*sin(q3+q2)-sin(q4)*sin(q6)*fy7-cos(q4)*sin(q5)*fz7+M5*g*sin(q3+q2)+cos(q4)*cos(q5)*cos(q6)*fy7+cos(q4)*cos(q5)*sin(q6)*fx7-M3*cos(q3)*q2ddot*l2+1/2*M3*q1dot^2*l2*sin(q3+2*q2)-1/2*M3*sin(q3)*l2*q1dot^2-M3*sin(q3)*l2*q2dot^2-M4*sin(q3)*l2*q2dot^2-M4*cos(q3)*q2ddot*l2+1/2*M4*q1dot^2*l2*sin(q3+2*q2)-1/2*M4*sin(q3)*l2*q1dot^2-M6*sin(q3)*l2*q2dot^2+M6*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3+M5*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3+M4*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3+M6*g*sin(q3+q2) M5*g*cos(q3+q2)+M6*g*cos(q3+q2)+M6*sin(q3)*q2ddot*l2+M5*sin(q3)*q2ddot*l2+1/2*M5*q1dot^2*l2*cos(q3+2*q2)-1/2*M6*cos(q3)*l2*q1dot^2-M6*cos(q3)*l2*q2dot^2+1/2*M6*q1dot^2*l2*cos(q3+2*q2)+M6*l3*q1dot^2*cos(q3+q2)^2-M5*cos(q3)*l2*q2dot^2-1/2*M5*cos(q3)*l2*q1dot^2+M5*l3*q1dot^2*cos(q3+q2)^2+M3*sin(q3)*q2ddot*l2+1/2*M3*q1dot^2*l2*cos(q3+2*q2)-1/2*M3*cos(q3)*l2*q1dot^2-M3*cos(q3)*l2*q2dot^2+cos(q5)*fz7+M3*g*cos(q3+q2)-2*M4*l3*q2dot*q3dot+M4*sin(q3)*q2ddot*l2+1/2*M4*q1dot^2*l2*cos(q3+2*q2)-1/2*M4*cos(q3)*l2*q1dot^2-M4*cos(q3)*l2*q2dot^2+M4*l3*q1dot^2*cos(q3+q2)^2-M5*l3*q1dot^2-M6*l3*q1dot^2-M5*l3*q3dot^2-M6*l3*q2dot^2-M6*l3*q3dot^2-M5*l3*q2dot^2-2*M5*l3*q2dot*q3dot-2*M6*l3*q2dot*q3dot-M4*l3*q1dot^2-M4*l3*q2dot^2-M4*l3*q3dot^2+sin(q5)*sin(q6)*fx7+sin(q5)*cos(q6)*fy7+M4*g*cos(q3+q2) f22 = -sin(q4)*cos(q5)*cos(q6)*fy7+sin(q4)*sin(q5)*fz7+cos(q4)*cos(q6)*fx7-cos(q4)*sin(q6)*fy7+M5*l3*q1ddot*sin(q3+q2)+M4*l2*sin(q2)*q1ddot+M6*l3*q1ddot*sin(q3+q2)+2*M6*l3*q2dot*q1dot*cos(q3+q2)+2*M5*l3*q2dot*q1dot*cos(q3+q2)+2*M3*l2*q2dot*cos(q2)*q1dot+M6*l2*sin(q2)*q1ddot+2*M6*l3*q3dot*q1dot*cos(q3+q2)+2*M5*l3*q3dot*q1dot*cos(q3+q2)+2*M5*q2dot*cos(q2)*q1dot*l2+2*M4*l3*q3dot*q1dot*cos(q3+q2)+2*M6*q2dot*cos(q2)*q1dot*l2+2*M4*q2dot*cos(q2)*q1dot*l2+2*M4*l3*q2dot*q1dot*cos(q3+q2)+M5*l2*sin(q2)*q1ddot+M4*l3*q1ddot*sin(q3+q2)+M3*l2*sin(q2)*q1ddot-sin(q4)*cos(q5)*sin(q6)*fx7

-cos(q3)*M5*l3*q3ddot-cos(q3)*M6*l3*q2ddot-cos(q3)*M6*l3*q3ddot-cos(q3)*M4*l3*q2ddot-cos(q3)*M4*l3*q3ddot+cos(q3)*sin(q4)*cos(q6)*fx7+cos(q3)*M3*g*sin(q3+q2)-cos(q3)*sin(q4)*sin(q6)*fy7-cos(q3)*cos(q4)*sin(q5)*fz7-sin(q3)*M3*g*cos(q3+q2)-sin(q3)*M6*g*cos(q3+q2)-sin(q3)*M5*g*cos(q3+q2)+sin(q3)*M5*l3*q1dot^2+sin(q3)*M6*l3*q1dot^2+1/2*cos(q3)*M4*q1dot^2*l2*sin(q3+2*q2)+cos(q3)*M6*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3+cos(q3)*M5*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3+cos(q3)*M4*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3-1/2*sin(q3)*M3*q1dot^2*l2*cos(q3+2*q2)-1/2*sin(q3)*M4*q1dot^2*l2*cos(q3+2*q2)-sin(q3)*M4*l3*q1dot^2*cos(q3+q2)^2+2*sin(q3)*M4*l3*q2dot*q3dot-sin(q3)*M6*l3*q1dot^2*cos(q3+q2)^2-1/2*sin(q3)*M6*q1dot^2*l2*cos(q3+2*q2)-1/2*sin(q3)*M5*q1dot^2*l2*cos(q3+2*q2)+2*sin(q3)*M5*l3*q2dot*q3dot+2*sin(q3)*M6*l3*q2dot*q3dot-sin(q3)*M5*l3*q1dot^2*cos(q3+q2)^2+1/2*cos(q3)*M5*q1dot^2*l2*sin(q3+2*q2)+1/2*cos(q3)*M6*q1dot^2*l2*sin(q3+2*q2)+cos(q3)*cos(q4)*cos(q5)*cos(q6)*fy7+sin(q3)*M6*l3*q3dot^2+1/2*cos(q3)*M3*q1dot^2*l2*sin(q3+2*q2)+sin(q3)*M5*l3*q3dot^2+cos(q3)*cos(q4)*cos(q5)*sin(q6)*fx7+sin(q3)*M6*l3*q2dot^2+sin(q3)*M5*l3*q2dot^2-sin(q3)*sin(q5)*sin(q6)*fx7+M2*sin(q2)*g-sin(q3)*cos(q5)*fz7-M3*q2ddot*l2-M4*q2ddot*l2-M5*q2ddot*l2-M6*q2ddot*l2-sin(q3)*sin(q5)*cos(q6)*fy7-sin(q3)*M4*g*cos(q3+q2)+sin(q3)*M4*l3*q1dot^2+sin(q3)*M4*l3*q2dot^2+sin(q3)*M4*l3*q3dot^2+cos(q3)*M5*g*sin(q3+q2)+cos(q3)*M4*g*sin(q3+q2)+cos(q3)*M6*g*sin(q3+q2)-cos(q3)*M5*l3*q2ddot

52

cos(q3)*cos(q5)*fz7-M6*l2*q2dot^2-sin(q3)*sin(q4)*sin(q6)*fy7+sin(q3)*M6*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3+sin(q3)*M4*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3+sin(q3)*M5*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3+1/2*sin(q3)*M5*q1dot^2*l2*sin(q3+2*q2)+sin(q3)*sin(q4)*cos(q6)*fx7+sin(q3)*M6*g*sin(q3+q2)+sin(q3)*M5*g*sin(q3+q2)+sin(q3)*M3*g*sin(q3+q2)-sin(q3)*cos(q4)*sin(q5)*fz7-sin(q3)*M5*l3*q3ddot+1/2*sin(q3)*M4*q1dot^2*l2*sin(q3+2*q2)+1/2*sin(q3)*M3*q1dot^2*l2*sin(q3+2*q2)+sin(q3)*M4*g*sin(q3+q2)+sin(q3)*cos(q4)*cos(q5)*sin(q6)*fx7+sin(q3)*cos(q4)*cos(q5)*cos(q6)*fy7+1/2*sin(q3)*M6*q1dot^2*l2*sin(q3+2*q2)-sin(q3)*M4*l3*q2ddot-sin(q3)*M6*l3*q2ddot-sin(q3)*M5*l3*q2ddot+cos(q3)*M4*g*cos(q3+q2)-sin(q3)*M4*l3*q3ddot-sin(q3)*M6*l3*q3ddot+cos(q3)*sin(q5)*cos(q6)*fy7+cos(q3)*sin(q5)*sin(q6)*fx7+cos(q3)*M3*g*cos(q3+q2)+cos(q3)*M6*g*cos(q3+q2)+1/2*cos(q3)*M6*q1dot^2*l2*cos(q3+2*q2)+cos(q3)*M5*l3*q1dot^2*cos(q3+q2)^2+cos(q3)*M4*l3*q1dot^2*cos(q3+q2)^2+1/2*cos(q3)*M5*q1dot^2*l2*cos(q3+2*q2)+cos(q3)*M5*g*cos(q3+q2)+cos(q3)*M6*l3*q1dot^2*cos(q3+q2)^2-2*cos(q3)*M4*l3*q2dot*q3dot-cos(q3)*M5*l3*q1dot^2-cos(q3)*M6*l3*q1dot^2+1/2*cos(q3)*M3*q1dot^2*l2*cos(q3+2*q2)+1/2*cos(q3)*M4*q1dot^2*l2*cos(q3+2*q2)-cos(q3)*M4*l3*q3dot^2-2*cos(q3)*M6*l3*q2dot*q3dot-2*cos(q3)*M5*l3*q2dot*q3dot-cos(q3)*M6*l3*q3dot^2-cos(q3)*M5*l3*q2dot^2-cos(q3)*M6*l3*q2dot^2-cos(q3)*M5*l3*q3dot^2-cos(q3)*M4*l3*q1dot^2-cos(q3)*M4*l3*q2dot^2-1/2*M3*q1dot^2*l2-1/2*M4*q1dot^2*l2-1/2*M6*q1dot^2*l2-1/2*M5*q1dot^2*l2+M2*cos(q2)*g-M5*l2*q2dot^2-M4*l2*q2dot^2-M3*l2*q2dot^2 f11 = -sin(q4)*cos(q5)*cos(q6)*fy7+sin(q4)*sin(q5)*fz7+cos(q4)*cos(q6)*fx7-cos(q4)*sin(q6)*fy7+M5*l3*q1ddot*sin(q3+q2)+M4*l2*sin(q2)*q1ddot+M6*l3*q1ddot*sin(q3+q2)+2*M6*l3*q2dot*q1dot*cos(q3+q2)+2*M5*l3*q2dot*q1dot*cos(q3+q2)+2*M3*l2*q2dot*cos(q2)*q1dot+M6*l2*sin(q2)*q1ddot+2*M6*l3*q3dot*q1dot*cos(q3+q2)+2*M5*l3*q3dot*q1dot*cos(q3+q2)+2*M5*q2dot*cos(q2)*q1dot*l2+2*M4*l3*q3dot*q1dot*cos(q3+q2)+2*M6*q2dot*cos(q2)*q1dot*l2+2*M4*q2dot*cos(q2)*q1dot*l2+2*M4*l3*q2dot*q1dot*cos(q3+q2)+M5*l2*sin(q2)*q1ddot+M4*l3*q1ddot*sin(q3+q2)+M3*l2*sin(q2)*q1ddot-sin(q4)*cos(q5)*sin(q6)*fx7-sin(q2)*sin(q3)*M5*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3+sin(q2)*sin(q3)*sin(q4)*sin(q6)*fy7-cos(q2)*M4*q2ddot*l2-cos(q2)*M5*q2ddot*l2-sin(q2)*sin(q3)*M4*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3-sin(q2)*sin(q3)*M6*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3+sin(q2)*sin(q3)*cos(q4)*sin(q5)*fz7-sin(q2)*sin(q3)*M3*g*sin(q3+q2)-sin(q2)*sin(q3)*M5*g*sin(q3+q2)-sin(q2)*sin(q3)*M6*g*sin(q3+q2)-sin(q2)*sin(q3)*sin(q4)*cos(q6)*fx7-1/2*sin(q2)*sin(q3)*M5*q1dot^2*l2*sin(q3+2*q2)-1/2*sin(q2)*sin(q3)*M6*q1dot^2*l2*sin(q3+2*q2)-sin(q2)*sin(q3)*cos(q4)*cos(q5)*cos(q6)*fy7-sin(q2)*sin(q3)*cos(q4)*cos(q5)*sin(q6)*fx7-sin(q2)*sin(q3)*M4*g*sin(q3+q2)-1/2*sin(q2)*sin(q3)*M3*q1dot^2*l2*sin(q3+2*q2)-1/2*sin(q2)*sin(q3)*M4*q1dot^2*l2*sin(q3+2*q2)+sin(q2)*sin(q3)*M5*l3*q3ddot+sin(q2)*sin(q3)*M5*l3*q2ddot+sin(q2)*sin(q3)*

M6*l3*q2ddot+sin(q2)*sin(q3)*M4*l3*q2ddot-sin(q2)*cos(q3)*cos(q5)*fz7-cos(q2)*sin(q3)*cos(q5)*fz7+sin(q2)*sin(q3)*M6*l3*q3ddot+sin(q2)*sin(q3)*M4*l3*q3ddot-sin(q2)*cos(q3)*M4*g*cos(q3+q2)-sin(q2)*cos(q3)*M6*g*cos(q3+q2)-sin(q2)*cos(q3)*M3*g*cos(q3+q2)-sin(q2)*cos(q3)*sin(q5)*sin(q6)*fx7-sin(q2)*cos(q3)*sin(q5)*cos(q6)*fy7-sin(q2)*cos(q3)*M6*l3*q1dot^2*cos(q3+q2)^2-sin(q2)*cos(q3)*M5*g*cos(q3+q2)-1/2*sin(q2)*cos(q3)*M5*q1dot^2*l2*cos(q3+2*q2)-sin(q2)*cos(q3)*M4*l3*q1dot^2*cos(q3+q2)^2-sin(q2)*cos(q3)*M5*l3*q1dot^2*cos(q3+q2)^2-1/2*sin(q2)*cos(q3)*M6*q1dot^2*l2*cos(q3+2*q2)-1/2*sin(q2)*cos(q3)*M4*q1dot^2*l2*cos(q3+2*q2)-1/2*sin(q2)*cos(q3)*M3*q1dot^2*l2*cos(q3+2*q2)+sin(q2)*cos(q3)*M6*l3*q1dot^2+sin(q2)*cos(q3)*M5*l3*q1dot^2+cos(q2)*cos(q3)*cos(q4)*cos(q5)*sin(q6)*fx7+cos(q2)*cos(q3)*cos(q4)*cos(q5)*cos(q6)*fy7-1/2*cos(q2)*sin(q3)*M3*q1dot^2*l2*cos(q3+2*q2)+1/2*cos(q2)*cos(q3)*M4*q1dot^2*l2*sin(q3+2*q2)-cos(q2)*sin(q3)*M4*l3*q1dot^2*cos(q3+q2)^2-1/2*cos(q2)*sin(q3)*M4*q1dot^2*l2*cos(q3+2*q2)-1/2*cos(q2)*sin(q3)*M6*q1dot^2*l2*cos(q3+2*q2)-cos(q2)*sin(q3)*M6*l3*q1dot^2*cos(q3+q2)^2+2*cos(q2)*sin(q3)*M4*l3*q2dot*q3dot+1/2*cos(q2)*cos(q3)*M6*q1dot^2*l2*sin(q3+2*q2)+1/2*cos(q2)*cos(q3)*M5*q1dot^2*l2*sin(q3+2*q2)-cos(q2)*sin(q3)*M5*l3*q1dot^2*cos(q3+q2)^2+2*cos(q2)*sin(q3)*M6*l3*q2dot*q3dot+2*cos(q2)*sin(q3)*M5*l3*q2dot*q3dot-1/2*cos(q2)*sin(q3)*M5*q1dot^2*l2*cos(q3+2*q2)+1/2*cos(q2)*cos(q3)*M3*q1dot^2*l2*sin(q3+2*q2)+cos(q2)*cos(q3)*sin(q4)*cos(q6)*fx7+cos(q2)*sin(q3)*M6*l3*q3dot^2+cos(q2)*sin(q3)*M6*l3*q1dot^2+cos(q2)*sin(q3)*M5*l3*q1dot^2+cos(q2)*cos(q3)*M3*g*sin(q3+q2)+cos(q2)*sin(q3)*M5*l3*q3dot^2+cos(q2)*sin(q3)*M4*l3*q2dot^2+cos(q2)*sin(q3)*M4*l3*q1dot^2+cos(q2)*sin(q3)*M5*l3*q2dot^2+

53

cos(q2)*sin(q3)*M6*l3*q2dot^2+cos(q2)*cos(q3)*M5*g*sin(q3+q2)+cos(q2)*sin(q3)*M4*l3*q3dot^2+cos(q2)*cos(q3)*M4*g*sin(q3+q2)+cos(q2)*cos(q3)*M6*g*sin(q3+q2)-cos(q2)*cos(q3)*M5*l3*q3ddot-cos(q2)*cos(q3)*M4*l3*q2ddot-cos(q2)*cos(q3)*M6*l3*q3ddot-cos(q2)*cos(q3)*M6*l3*q2ddot-cos(q2)*cos(q3)*M4*l3*q3ddot-cos(q2)*sin(q3)*M3*g*cos(q3+q2)-cos(q2)*cos(q3)*cos(q4)*sin(q5)*fz7-cos(q2)*cos(q3)*sin(q4)*sin(q6)*fy7-cos(q2)*sin(q3)*M6*g*cos(q3+q2)-cos(q2)*sin(q3)*sin(q5)*sin(q6)*fx7-cos(q2)*M6*q2ddot*l2-cos(q2)*sin(q3)*M5*g*cos(q3+q2)-cos(q2)*sin(q3)*sin(q5)*cos(q6)*fy7-cos(q2)*cos(q3)*M5*l3*q2ddot-cos(q2)*sin(q3)*M4*g*cos(q3+q2)+cos(q2)*cos(q3)*M6*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3+cos(q2)*cos(q3)*M5*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3+cos(q2)*cos(q3)*M4*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3+sin(q2)*M6*l2*q2dot^2+1/2*sin(q2)*M3*q1dot^2*l2+1/2*sin(q2)*M4*q1dot^2*l2+1/2*sin(q2)*M6*q1dot^2*l2+1/2*sin(q2)*M5*q1dot^2*l2+sin(q2)*M5*l2*q2dot^2+sin(q2)*M4*l2*q2dot^2+sin(q2)*M3*l2*q2dot^2+2*sin(q2)*cos(q3)*M4*l3*q2dot*q3dot+sin(q2)*cos(q3)*M5*l3*q3dot^2+sin(q2)*cos(q3)*M6*l3*q2dot^2+sin(q2)*cos(q3)*M5*l3*q2dot^2+sin(q2)*cos(q3)*M6*l3*q3dot^2+2*sin(q2)*cos(q3)*M5*l3*q2dot*q3dot+2*sin(q2)*cos(q3)*M6*l3*q2dot*q3dot+sin(q2)*cos(q3)*M4*l3*q3dot^2+sin(q2)*cos(q3)*M4*l3*q2dot^2+sin(q2)*cos(q3)*M4*l3*q1dot^2-cos(q2)*M3*q2ddot*l2

M2*g-cos(q2)*cos(q3)*M5*l3*q1dot^2-cos(q2)*cos(q3)*M6*l3*q2dot^2-cos(q2)*cos(q3)*M5*l3*q2dot^2-cos(q2)*cos(q3)*M6*l3*q3dot^2-2*cos(q2)*cos(q3)*M5*l3*q2dot*q3dot-2*cos(q2)*cos(q3)*M6*l3*q2dot*q3dot-cos(q2)*cos(q3)*M4*l3*q3dot^2-cos(q2)*cos(q3)*M4*l3*q2dot^2-cos(q2)*cos(q3)*M4*l3*q1dot^2-sin(q2)*sin(q3)*sin(q5)*cos(q6)*fy7-sin(q2)*cos(q3)*M5*l3*q2ddot-sin(q2)*sin(q3)*M4*g*cos(q3+q2)+sin(q2)*cos(q3)*M6*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3+sin(q2)*cos(q3)*M5*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3+sin(q2)*cos(q3)*M4*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3+cos(q2)*sin(q3)*M5*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3+cos(q2)*sin(q3)*M4*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3+cos(q2)*sin(q3)*M6*q1dot^2*cos(q3+q2)*sin(q3+q2)*l3-cos(q2)*sin(q3)*sin(q4)*sin(q6)*fy7-cos(q2)*sin(q3)*cos(q4)*sin(q5)*fz7+cos(q2)*sin(q3)*M3*g*sin(q3+q2)+cos(q2)*sin(q3)*M5*g*sin(q3+q2)+cos(q2)*sin(q3)*M6*g*sin(q3+q2)+cos(q2)*sin(q3)*sin(q4)*cos(q6)*fx7+1/2*cos(q2)*sin(q3)*M5*q1dot^2*l2*sin(q3+2*q2)+1/2*cos(q2)*sin(q3)*M6*q1dot^2*l2*sin(q3+2*q2)+cos(q2)*sin(q3)*cos(q4)*cos(q5)*cos(q6)*fy7+cos(q2)*sin(q3)*cos(q4)*cos(q5)*sin(q6)*fx7+cos(q2)*sin(q3)*M4*g*sin(q3+q2)+1/2*cos(q2)*sin(q3)*M3*q1dot^2*l2*sin(q3+2*q2)+1/2*cos(q2)*sin(q3)*M4*q1dot^2*l2*sin(q3+2*q2)-cos(q2)*sin(q3)*M5*l3*q3ddot-cos(q2)*sin(q3)*M5*l3*q2ddot-cos(q2)*sin(q3)*M6*l3*q2ddot-cos(q2)*sin(q3)*M4*l3*q2ddot-cos(q2)*sin(q3)*M6*l3*q3ddot-cos(q2)*sin(q3)*M4*l3*q3ddot+cos(q2)*cos(q3)*M4*g*cos(q3+q2)+cos(q2)*cos(q3)*M6*g*cos(q3+q2)+cos(q2)*cos(q3)*M3*g*cos(q3+q2)+cos(q2)*cos(q3)*sin(q5)*sin(q6)*fx7+cos(q2)*cos(q3)*sin(q5)*cos(q6)*fy7+cos(q2)*cos(q3)*M6*l3*q1dot^2*cos(q3+q2)^2+cos(q2)*cos(q3)*M5*g*cos(q3+q2)+1/2*cos(q2)*cos(q3)*M5*q1dot^2*l2*cos(q3+2*q2)+cos(q2)*cos(q3)*M4*l3*q1dot^2*cos(q3+q2)^2+cos(q2)*cos(q3)*M5*l3*q1dot^2*cos(q3+q2)^2+1/2*cos(q2)*cos(q3)*M6*q1dot^2*l2*cos(q3+2*q2)+1/2*cos(q2)*cos(q3)*M4*q1dot^2*l2*cos(q3+2*q2)+1/2*cos(q2)*cos(q3)*M3*q1dot^2*l2*cos(q3+2*q2)-cos(q2)*cos(q3)*M6*l3*q1dot^2-2*cos(q2)*cos(q3)*M4*l3*q2dot*q3dot-cos(q2)*cos(q3)*M5*l3*q3dot^2+sin(q2)*cos(q3)*cos(q4)*cos(q5)*sin(q6)*fx7+sin(q2)*cos(q3)*cos(q4)*cos(q5)*cos(q6)*fy7-1/2*sin(q2)*sin(q3)*M3*q1dot^2*l2*cos(q3+2*q2)+1/2*sin(q2)*cos(q3)*M4*q1dot^2*l2*sin(q3+2*q2)-sin(q2)*sin(q3)*M4*l3*q1dot^2*cos(q3+q2)^2-1/2*sin(q2)*sin(q3)*M4*q1dot^2*l2*cos(q3+2*q2)-1/2*sin(q2)*sin(q3)*M6*q1dot^2*l2*cos(q3+2*q2)-sin(q2)*sin(q3)*M6*l3*q1dot^2*cos(q3+q2)^2+2*sin(q2)*sin(q3)*M4*l3*q2dot*q3dot+1/2*sin(q2)*cos(q3)*M6*q1dot^2*l2*sin(q3+2*q2)+M1*g-cos(q2)*M5*l2*q2dot^2-1/2*cos(q2)*M3*q1dot^2*l2-sin(q2)*sin(q3)*cos(q5)*fz7-sin(q2)*M3*q2ddot*l2-sin(q2)*M4*q2ddot*l2-sin(q2)*M5*q2ddot*l2-sin(q2)*M6*q2ddot*l2+cos(q2)*cos(q3)*cos(q5)*fz7-cos(q2)*M6*l2*q2dot^2-cos(q2)*M4*l2*q2dot^2-cos(q2)*M3*l2*q2dot^2-1/2*cos(q2)*M6*q1dot^2*l2-1/2*cos(q2)*M5*q1dot^2*l2-1/2*cos(q2)*M4*q1dot^2*l2+1/2*sin(q2)*cos(q3)*M5*q1dot^2*l2*sin(q3+2*q2)-sin(q2)*sin(q3)*M5*l3*q1dot^2*cos(q3+q2)^2+2*sin(q2)*sin(q3)*M6*l3*q2dot*q3dot+2*sin(q2)*sin(q3)*M5*l3*q2dot*q3dot-1/2*sin(q2)*sin(q3)*M5*q1dot^2*l2*cos(q3+2*q2)+1/2*sin(q2)*cos(q3)*M3*q1dot^2*l2*sin(q3+2*q2)+sin(q2)*cos(q3)*sin(q4)*cos(q6)*fx7+sin(q2)*sin(q3)*M6*l3*q3dot^2+sin(q2)*sin(q3)*M6*l3*q1dot^2+sin(q2)*sin(q3)*M5*l3*q1dot^2+sin(q2)*cos(q3)*M3*g*sin(q3+q2)+sin(q2)*sin(q3)*M5*l3*q3dot^2+sin(q2)*sin(q3)*M4*l3*q2dot^2+sin(q2)*sin(q3)*M4*l3*q1dot^2+sin(q2)*sin(q3)*M5*l3*q2dot^2+sin(q2)*sin(q3)*M6*l3*q2dot^2+sin(q2)*cos(q3)*M5*g*sin(q3+q2)+sin(q2)*sin(q3)*M4*l3*q3dot^2+sin(q2)*cos(q3)*M4*g*sin(q3+q2)+sin(q2)*cos(q3)*M6*g*sin(q3+q2)-sin(q2)*cos(q3)*M5*l3*q3ddot-sin(q2)*cos(q3)*M4*l3*q2ddot-sin(q2)*cos(q3)*M6*l3*q3ddot-sin(q2)*cos(q3)*M6*l3*q2ddot-sin(q2)*cos(q3)*M4*l3*q3ddot-sin(q2)*sin(q3)*M3*g*cos(q3+q2)-sin(q2)*cos(q3)*cos(q4)*sin(q5)*fz7-sin(q2)*cos(q3)*sin(q4)*sin(q6)*fy7-sin(q2)*sin(q3)*M6*g*

54

cos(q3+q2)-sin(q2)*sin(q3)*sin(q5)*sin(q6)*fx7-sin(q2)*sin(q3)*M5*g*cos(q3+q2)

Momentele forţelor de legătură ale robotului articulat 6R

n66 = -Ix6s*q6dot*sin(q6)*q5dot+Ix6s*sin(q6)*sin(q5)*q4ddot+Ix6s*cos(q6)*cos(q4)*q2ddot+Ix6s*cos(q6)*cos(q4)*q3ddot-Iz6s*q6dot*sin(q6)*q5dot+Iy6s*q6dot*sin(q6)*q5dot-Ix6s*sin(q6)*cos(q5)*q4dot*cos(q4)*q2dot-Iz6s*sin(q6)*q5dot*sin(q5)*sin(q4)*q2dot+Iz6s*q6dot*cos(q6)*cos(q5)*cos(q4)*q1dot*sin(q3+q2)-Iz6s*q6dot*sin(q6)*sin(q4)*q1dot*sin(q3+q2)-l4*fy7+nx7-Ix6s*sin(q6)*cos(q5)*q4dot*cos(q4)*q3dot-Ix6s*sin(q6)*sin(q5)*q2dot*q1dot*sin(q3+q2)+Ix6s*cos(q6)*q5ddot-Ix6s*sin(q6)*sin(q5)*q3dot*q1dot*sin(q3+q2)+Ix6s*sin(q6)*q5dot*cos(q5)*q1dot*cos(q3+q2)+Ix6s*sin(q6)*q5dot*sin(q5)*sin(q4)*q3dot-Ix6s*sin(q6)*q5dot*sin(q5)*cos(q4)*q1dot*sin(q3+q2)+Ix6s*q6dot*cos(q6)*sin(q5)*q4dot-Ix6s*q6dot*sin(q6)*cos(q4)*q2dot-2*Iy6s*cos(q5)^2*q4dot*cos(q6)*cos(q4)*q1dot*sin(q3+q2)+2*Iy6s*cos(q5)^2*q4dot*cos(q6)*sin(q4)*q2dot+2*Iy6s*cos(q5)^2*q1dot*cos(q3+q2)*cos(q6)*sin(q4)*q3dot-2*Iy6s*cos(q5)^2*q1dot^2*cos(q3+q2)*cos(q6)*cos(q4)*sin(q3+q2)-2*Iy6s*cos(q5)*q1dot*cos(q3+q2)*cos(q6)*sin(q5)*q4dot+2*Iy6s*cos(q5)^2*q4dot*cos(q6)*sin(q4)*q3dot+2*Iz6s*cos(q5)^2*q1dot^2*cos(q3+q2)*cos(q6)*cos(q4)*sin(q3+q2)+2*Iz6s*cos(q5)*q1dot*cos(q3+q2)*cos(q6)*sin(q5)*q4dot-2*Iz6s*cos(q5)^2*q4dot*cos(q6)*sin(q4)*q2dot-2*Iz6s*cos(q5)^2*q4dot*cos(q6)*sin(q4)*q3dot+2*Iz6s*cos(q5)^2*q4dot*cos(q6)*cos(q4)*q1dot*sin(q3+q2)-2*Iy6s*sin(q5)*sin(q4)*q2dot*cos(q6)*cos(q5)*cos(q4)*q1dot*sin(q3+q2)+2*Iy6s*sin(q5)*sin(q4)*q2dot*sin(q6)*cos(q4)*q3dot-2*Iy6s*sin(q5)*sin(q4)*q3dot*cos(q6)*cos(q5)*cos(q4)*q1dot*sin(q3+q2)-2*Iy6s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*sin(q6)*q2dot-2*Iy6s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*sin(q6)*q3dot+2*Iy6s*cos(q5)^2*q1dot*cos(q3+q2)*cos(q6)*sin(q4)*q2dot+2*Iz6s*sin(q5)*sin(q4)*q2dot*cos(q6)*cos(q5)*cos(q4)*q1dot*sin(q3+q2)-2*Iz6s*sin(q5)*sin(q4)*q2dot*sin(q6)*cos(q4)*q3dot+2*Iz6s*sin(q5)*sin(q4)*q3dot*cos(q6)*cos(q5)*cos(q4)*q1dot*sin(q3+q2)+2*Iz6s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*sin(q6)*q2dot+2*Iz6s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*sin(q6)*q3dot-2*Iz6s*cos(q5)^2*q1dot*cos(q3+q2)*cos(q6)*sin(q4)*q2dot-2*Iz6s*cos(q5)^2*q1dot*cos(q3+q2)*cos(q6)*sin(q4)*q3dot+2*Iz6s*sin(q5)*q2dot*cos(q6)*cos(q5)*q3dot*cos(q4)^2-2*Iy6s*sin(q5)*q2dot*cos(q6)*cos(q5)*q3dot*cos(q4)^2+2*Iy6s*sin(q5)*q2dot*cos(q6)*cos(q5)*q3dot-2*Iz6s*sin(q5)*q2dot*cos(q6)*cos(q5)*q3dot-Ix6s*q6dot*cos(q6)*cos(q5)*sin(q4)*q2dot-Ix6s*q6dot*cos(q6)*cos(q5)*sin(q4)*q3dot+Ix6s*q6dot*cos(q6)*cos(q5)*cos(q4)*q1dot*sin(q3+q2)+Ix6s*q6dot*cos(q6)*sin(q5)*q1dot*cos(q3+q2)-Ix6s*q6dot*sin(q6)*sin(q4)*q1dot*sin(q3+q2)-Iz6s*cos(q4)*q1dot*sin(q3+q2)*cos(q6)*q4dot-Iy6s*sin(q4)*q2dot*cos(q6)*q4dot-Iy6s*sin(q4)*q2dot*cos(q6)*q1dot*cos(q3+q2)+Iz6s*sin(q5)*q3dot^2*cos(q6)*cos(q5)*cos(q4)^2-Iy6s*sin(q5)*q2dot^2*cos(q6)*cos(q5)*cos(q4)^2+Iz6s*sin(q4)*q2dot*cos(q6)*q1dot*cos(q3+q2)-Ix6s*cos(q6)*q4dot*sin(q4)*q2dot-Ix6s*cos(q6)*q4dot*sin(q4)*q3dot-Ix6s*sin(q6)*cos(q5)*sin(q4)*q2ddot-Ix6s*sin(q6)*cos(q5)*sin(q4)*q3ddot+Ix6s*sin(q6)*sin(q5)*q1ddot*cos(q3+q2)+Ix6s*sin(q6)*q5dot*cos(q5)*q4dot+Iy6s*sin(q5)*cos(q4)*q1dot^2*sin(q6)*sin(q4)*cos(q3+q2)^2-Iy6s*sin(q4)*q3dot*cos(q6)*q1dot*cos(q3+q2)-Iy6s*sin(q5)*cos(q4)^2*q1dot^2*cos(q6)*cos(q5)*cos(q3+q2)^2+Iy6s*cos(q4)*q1dot^2*sin(q3+q2)*cos(q6)*cos(q3+q2)+Iy6s*cos(q4)*q1dot*sin(q3+q2)*cos(q6)*q4dot+Iz6s*q6dot*cos(q6)*sin(q5)*q4dot-Iz6s*cos(q4)*q1dot^2*sin(q3+q2)*cos(q6)*cos(q3+q2)+Iz6s*sin(q4)*q3dot*cos(q6)*q1dot*cos(q3+q2)+Iz6s*sin(q4)*q3dot*cos(q6)*q4dot+Iz6s*sin(q5)*cos(q4)*q1dot^2*sin(q6)*sin(q4)+Iz6s*sin(q5)*q2dot^2*cos(q6)*cos(q5)*cos(q4)^2-Iy6s*sin(q4)*q3dot*cos(q6)*q4dot-Iz6s*sin(q5)*cos(q4)^2*q1dot^2*cos(q6)*cos(q5)+Iz6s*sin(q5)*cos(q4)^2*q1dot^2*cos(q6)*cos(q5)*cos(q3+q2)^2-Iz6s*sin(q5)*q2dot*sin(q6)*q1dot*sin(q3+q2)-Iz6s*sin(q5)*q3dot^2*cos(q6)*cos(q5)-Iz6s*sin(q5)*q3dot*sin(q6)*q1dot*sin(q3+q2)+Iy6s*sin(q5)*q3dot^2*cos(q6)*cos(q5)+Iy6s*sin(q5)*q2dot^2*cos(q6)*cos(q5)+Iy6s*sin(q5)*q2dot*sin(q6)*q1dot*sin(q3+q2)-Iz6s*sin(q5)*q2dot^2*cos(q6)*cos(q5)-Iz6s*sin(q6)*q5dot*cos(q5)*q4dot-Iz6s*sin(q5)*sin(q4)*q2dot^2*sin(q6)*cos(q4)-Iy6s*sin(q5)*q3dot^2*cos(q6)*cos(q5)*cos(q4)^2+Iz6s*sin(q4)*q2dot*cos(q6)*q4dot+Iy6s*sin(q5)*cos(q4)^2*q1dot^2*cos(q6)*cos(q5)+Ix6s*sin(q6)*q5dot*sin(q5)*sin(q4)*q2dot-Ix6s*q6dot*sin(q6)*cos(q4)*q3dot+Iy6s*sin(q5)*q3dot*sin(q6)*q1dot*sin(q3+q2)+Iz6s*cos(q5)*q1dot^2*cos(q3+q2)^2*cos(q6)*sin(q5)+Iz6s*q6dot*cos(q6)*sin(q5)*q1dot*cos(q3+q2)-Iz6s*sin(q5)*cos(q4)*q1dot^2*sin(q6)*sin(q4)*cos(q3+q2)^2-Iz6s*sin(q5)*sin(q4)*q3dot^2*sin(q6)*

55

cos(q4)+Iy6s*sin(q6)*cos(q5)*q4dot*cos(q4)*q3dot+Iy6s*q6dot*cos(q6)*cos(q5)*sin(q4)*q2dot+Iy6s*q6dot*cos(q6)*cos(q5)*sin(q4)*q3dot-Iy6s*q6dot*cos(q6)*cos(q5)*cos(q4)*q1dot*sin(q3+q2)-Iy6s*q6dot*cos(q6)*sin(q5)*q1dot*cos(q3+q2)+Iy6s*sin(q6)*cos(q5)*q4dot*sin(q4)*q1dot*sin(q3+q2)-Iz6s*cos(q5)*q1dot^2*cos(q3+q2)*sin(q6)*sin(q4)*sin(q3+q2)-Iz6s*sin(q6)*cos(q5)*cos(q4)*q2dot*q1dot*cos(q3+q2)-Iz6s*sin(q6)*cos(q5)*cos(q4)*q3dot*q1dot*cos(q3+q2)+Iy6s*sin(q6)*cos(q5)*cos(q4)*q3dot*q1dot*cos(q3+q2)+Iy6s*sin(q6)*cos(q5)*cos(q4)*q2dot*q1dot*cos(q3+q2)+Iy6s*q6dot*sin(q6)*sin(q4)*q1dot*sin(q3+q2)-Iy6s*sin(q5)*cos(q4)*q1dot^2*sin(q6)*sin(q4)+Iy6s*sin(q6)*cos(q5)*q4dot*cos(q4)*q2dot+Ix6s*cos(q6)*sin(q4)*q3dot*q1dot*cos(q3+q2)-Iy6s*cos(q5)*q1dot^2*cos(q3+q2)^2*cos(q6)*sin(q5)+Iy6s*cos(q5)*q1dot^2*cos(q3+q2)*sin(q6)*sin(q4)*sin(q3+q2)+Ix6s*cos(q6)*sin(q4)*q1ddot*sin(q3+q2)+Iy6s*sin(q6)*q5dot*cos(q5)*q4dot+Iy6s*sin(q6)*q5dot*cos(q5)*q1dot*cos(q3+q2)+Iy6s*sin(q6)*q5dot*sin(q5)*sin(q4)*q2dot+Iy6s*sin(q6)*q5dot*sin(q5)*sin(q4)*q3dot-Iy6s*sin(q6)*q5dot*sin(q5)*cos(q4)*q1dot*sin(q3+q2)-Iy6s*q6dot*cos(q6)*sin(q5)*q4dot+Iy6s*q6dot*sin(q6)*cos(q4)*q2dot-Iz6s*q6dot*cos(q6)*cos(q5)*sin(q4)*q2dot+Iy6s*q6dot*sin(q6)*cos(q4)*q3dot-Iy6s*cos(q5)*q4dot^2*cos(q6)*sin(q5)+Iy6s*sin(q5)*sin(q4)*q3dot^2*sin(q6)*cos(q4)-Iz6s*sin(q6)*q5dot*sin(q5)*sin(q4)*q3dot+Iz6s*sin(q6)*q5dot*sin(q5)*cos(q4)*q1dot*sin(q3+q2)-Iz6s*q6dot*sin(q6)*cos(q4)*q2dot-Iz6s*q6dot*sin(q6)*cos(q4)*q3dot+Iz6s*cos(q5)*q4dot^2*cos(q6)*sin(q5)+Iy6s*sin(q5)*sin(q4)*q2dot^2*sin(q6)*cos(q4)-Iz6s*q6dot*cos(q6)*cos(q5)*sin(q4)*q3dot+Ix6s*sin(q6)*cos(q5)*cos(q4)*q2dot*q1dot*cos(q3+q2)+Ix6s*cos(q6)*sin(q4)*q2dot*q1dot*cos(q3+q2)+Ix6s*cos(q6)*q4dot*cos(q4)*q1dot*sin(q3+q2)+Ix6s*sin(q6)*cos(q5)*cos(q4)*q1ddot*sin(q3+q2)+Ix6s*sin(q6)*cos(q5)*cos(q4)*q3dot*q1dot*cos(q3+q2)-Ix6s*sin(q6)*cos(q5)*q4dot*sin(q4)*q1dot*sin(q3+q2)-Iz6s*sin(q6)*cos(q5)*q4dot*sin(q4)*q1dot*sin(q3+q2)-Iz6s*sin(q6)*cos(q5)*q4dot*cos(q4)*q2dot-Iz6s*sin(q6)*cos(q5)*q4dot*cos(q4)*q3dot-Iz6s*sin(q6)*q5dot*cos(q5)*q1dot*cos(q3+q2) -Iy6s*q6dot*cos(q6)*q5dot+Iy6s*cos(q6)*sin(q5)*q4ddot+Ix6s*q6dot*cos(q6)*q5dot-Iz6s*q6dot*cos(q6)*q5dot-Iy6s*sin(q6)*cos(q4)*q2ddot-Iy6s*sin(q6)*cos(q4)*q3ddot-Iy6s*sin(q6)*q5ddot+Iy6s*cos(q6)*cos(q5)*cos(q4)*q2dot*q1dot*cos(q3+q2)+Iy6s*cos(q6)*cos(q5)*cos(q4)*q3dot*q1dot*cos(q3+q2)-Iy6s*cos(q6)*cos(q5)*q4dot*sin(q4)*q1dot*sin(q3+q2)-Iz6s*cos(q5)*q4dot^2*sin(q6)*sin(q5)-Iz6s*q6dot*sin(q6)*cos(q5)*cos(q4)*q1dot*sin(q3+q2)+Iz6s*q6dot*sin(q6)*cos(q5)*sin(q4)*q2dot+Iz6s*q6dot*sin(q6)*cos(q5)*sin(q4)*q3dot-Iz6s*q6dot*sin(q6)*sin(q5)*q1dot*cos(q3+q2)-Iz6s*q6dot*cos(q6)*sin(q4)*q1dot*sin(q3+q2)-Iz6s*cos(q6)*cos(q5)*cos(q4)*q2dot*q1dot*cos(q3+q2)-Iz6s*cos(q6)*cos(q5)*cos(q4)*q3dot*q1dot*cos(q3+q2)-Iz6s*cos(q6)*cos(q5)*q4dot*sin(q4)*q1dot*sin(q3+q2)-Iy6s*q6dot*cos(q6)*cos(q4)*q2dot+Iz6s*cos(q6)*q5dot*sin(q5)*cos(q4)*q1dot*sin(q3+q2)-Iz6s*sin(q5)*sin(q4)*q2dot^2*cos(q6)*cos(q4)+Ix6s*q6dot*cos(q6)*cos(q4)*q3dot+Ix6s*sin(q5)*sin(q4)*q3dot^2*cos(q6)*cos(q4)+Ix6s*q6dot*cos(q6)*cos(q4)*q2dot+Ix6s*cos(q5)*q1dot^2*cos(q3+q2)*cos(q6)*sin(q4)*sin(q3+q2)+Ix6s*cos(q6)*q5dot*cos(q5)*q4dot+Ix6s*cos(q6)*q5dot*cos(q5)*q1dot*cos(q3+q2)+Ix6s*cos(q6)*q5dot*sin(q5)*sin(q4)*q2dot-Iz6s*cos(q6)*cos(q5)*q4dot*cos(q4)*q2dot-Iy6s*sin(q6)*sin(q4)*q3dot*q1dot*cos(q3+q2)-Iy6s*sin(q6)*q4dot*cos(q4)*q1dot*sin(q3+q2)+Iy6s*cos(q6)*cos(q5)*cos(q4)*q1ddot*sin(q3+q2)-Iy6s*q6dot*cos(q6)*cos(q4)*q3dot-Iy6s*q6dot*sin(q6)*cos(q5)*cos(q4)*q1dot*sin(q3+q2)+Iy6s*q6dot*sin(q6)*cos(q5)*sin(q4)*q2dot+Iy6s*q6dot*sin(q6)*cos(q5)*sin(q4)*q3dot-Iy6s*q6dot*sin(q6)*sin(q5)*q1dot*cos(q3+q2)-Iy6s*q6dot*cos(q6)*sin(q4)*q1dot*sin(q3+q2)+Iz6s*sin(q5)*cos(q4)^2*q1dot^2*sin(q6)*cos(q5)+Iz6s*sin(q5)*q3dot^2*sin(q6)*cos(q5)-Iz6s*cos(q5)*q1dot^2*cos(q3+q2)*cos(q6)*sin(q4)*sin(q3+q2)-Iz6s*cos(q6)*cos(q5)*q4dot*cos(q4)*q3dot+Ix6s*cos(q6)*q5dot*sin(q5)*sin(q4)*q3dot-Ix6s*cos(q6)*q5dot*sin(q5)*cos(q4)*q1dot*sin(q3+q2)-Iy6s*sin(q6)*sin(q4)*q2dot*q1dot*cos(q3+q2)-Iy6s*cos(q6)*sin(q5)*q2dot*q1dot*sin(q3+q2)-Iy6s*cos(q6)*sin(q5)*q3dot*q1dot*sin(q3+q2)+Iy6s*cos(q6)*q5dot*cos(q5)*q1dot*cos(q3+q2)-Iz6s*sin(q5)*cos(q4)^2*q1dot^2*sin(q6)*cos(q5)*cos(q3+q2)^2-Iz6s*sin(q5)*q3dot^2*sin(q6)*cos(q5)*cos(q4)^2-Ix6s*sin(q5)*cos(q4)*q1dot^2*cos(q6)*sin(q4)+Iy6s*cos(q6)*q5dot*sin(q5)*sin(q4)*q2dot-Ix6s*sin(q5)*q2dot^2*sin(q6)*cos(q5)+Ix6s*sin(q4)*q2dot*sin(q6)*q1dot*cos(q3+q2)+Iz6s*sin(q5)*q2dot^2*sin(q6)*cos(q5)-Iz6s*sin(q5)*q2dot^2*sin(q6)*cos(q5)*cos(q4)^2+l4*fx7+ny7-Iz6s*sin(q4)*q2dot*sin(q6)*q4dot-Ix6s*sin(q5)*q3dot^2*sin(q6)*cos(q5)+Ix6s*sin(q5)*q3dot^2*sin(q6)*cos(q5)*cos(q4)^2+Ix6s*sin(q5)*q3dot*cos(q6)*q1dot*sin(q3+q2)+Ix6s*sin(q4)*q3dot*sin(q6)*q1dot*cos(q3+q2)+Ix6s*sin(q4)*q3dot*sin(q6)*q4dot+Iz6s*sin(q5)*cos(q4)*q1dot^2*cos(q6)*sin(q4)+Ix6s*q6dot*sin(q6)*sin(q5)*q4dot+Ix6s*sin(q5)*sin(q4)*q2dot^2*cos(q6)*cos(q4)+Iy6s*cos(q6)*sin(q5)*q1ddot*cos(q3+q2)+Ix6s*cos(q6)*cos(q5)*q4dot*sin(q4)*q1dot*sin(q3+q2)+Ix6s*cos(q6)*cos(q5)*q4dot*cos(q4)*q2dot+Ix6s*cos(q6)*cos(q5)*q4dot*cos(q4)*q3dot-2*Iz6s*sin(q5)*sin(q4)*q2dot*cos(q6)*

56

cos(q4)*q3dot+2*Ix6s*sin(q5)*sin(q4)*q2dot*cos(q6)*cos(q4)*q3dot+2*Ix6s*cos(q5)^2*q1dot^2*cos(q3+q2)*sin(q6)*cos(q4)*sin(q3+q2)-2*Ix6s*cos(q5)^2*q1dot*cos(q3+q2)*sin(q6)*sin(q4)*q2dot-2*Ix6s*cos(q5)^2*q1dot*cos(q3+q2)*sin(q6)*sin(q4)*q3dot-2*Ix6s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*cos(q6)*q2dot-2*Ix6s*cos(q5)^2*q4dot*sin(q6)*sin(q4)*q2dot-2*Ix6s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*cos(q6)*q3dot+2*Ix6s*sin(q5)*cos(q4)*q1dot*sin(q3+q2)*sin(q6)*cos(q5)*sin(q4)*q2dot+2*Ix6s*sin(q5)*cos(q4)*q1dot*sin(q3+q2)*sin(q6)*cos(q5)*sin(q4)*q3dot+2*Ix6s*cos(q5)*q1dot*cos(q3+q2)*sin(q6)*sin(q5)*q4dot+2*Ix6s*cos(q5)^2*q4dot*sin(q6)*cos(q4)*q1dot*sin(q3+q2)-2*Ix6s*cos(q5)^2*q4dot*sin(q6)*sin(q4)*q3dot-2*Iz6s*cos(q5)^2*q1dot^2*cos(q3+q2)*sin(q6)*cos(q4)*sin(q3+q2)+2*Iz6s*cos(q5)^2*q1dot*cos(q3+q2)*sin(q6)*sin(q4)*q2dot+2*Iz6s*cos(q5)^2*q1dot*cos(q3+q2)*sin(q6)*sin(q4)*q3dot-2*Iz6s*cos(q5)*q1dot*cos(q3+q2)*sin(q6)*sin(q5)*q4dot-2*Iz6s*cos(q5)^2*q4dot*sin(q6)*cos(q4)*q1dot*sin(q3+q2)+2*Iz6s*cos(q5)^2*q4dot*sin(q6)*sin(q4)*q2dot+2*Iz6s*cos(q5)^2*q4dot*sin(q6)*sin(q4)*q3dot-Iz6s*sin(q5)*cos(q4)*q1dot^2*cos(q6)*sin(q4)*cos(q3+q2)^2+Iz6s*cos(q4)*q1dot*sin(q3+q2)*sin(q6)*q4dot+Ix6s*sin(q5)*q2dot*cos(q6)*q1dot*sin(q3+q2)-Iy6s*cos(q6)*cos(q5)*q4dot*cos(q4)*q3dot-Iz6s*cos(q6)*q5dot*cos(q5)*q4dot-Iz6s*cos(q6)*q5dot*cos(q5)*q1dot*cos(q3+q2)+Ix6s*cos(q5)*q1dot^2*cos(q3+q2)^2*sin(q6)*sin(q5)-Ix6s*q6dot*sin(q6)*cos(q5)*sin(q4)*q2dot+Iz6s*cos(q4)*q1dot^2*sin(q3+q2)*sin(q6)*cos(q3+q2)+Ix6s*q6dot*sin(q6)*cos(q5)*cos(q4)*q1dot*sin(q3+q2)-Ix6s*q6dot*sin(q6)*cos(q5)*sin(q4)*q3dot-Iz6s*cos(q5)*q1dot^2*cos(q3+q2)^2*sin(q6)*sin(q5)+Ix6s*q6dot*sin(q6)*sin(q5)*q1dot*cos(q3+q2)+Ix6s*q6dot*cos(q6)*sin(q4)*q1dot*sin(q3+q2)+Ix6s*cos(q5)*q4dot^2*sin(q6)*sin(q5)+Ix6s*cos(q6)*cos(q5)*cos(q4)*q2dot*q1dot*cos(q3+q2)+Ix6s*cos(q6)*cos(q5)*cos(q4)*q3dot*q1dot*cos(q3+q2)+2*Iz6s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*cos(q6)*q2dot+2*Iz6s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*cos(q6)*q3dot-2*Iz6s*sin(q5)*cos(q4)*q1dot*sin(q3+q2)*sin(q6)*cos(q5)*sin(q4)*q2dot+2*Iz6s*sin(q5)*q2dot*sin(q6)*cos(q5)*q3dot-2*Iz6s*sin(q5)*q2dot*sin(q6)*cos(q5)*q3dot*cos(q4)^2-2*Iz6s*sin(q5)*cos(q4)*q1dot*sin(q3+q2)*sin(q6)*cos(q5)*sin(q4)*q3dot+Ix6s*sin(q5)*cos(q4)*q1dot^2*cos(q6)*sin(q4)*cos(q3+q2)^2+Ix6s*sin(q5)*q2dot^2*sin(q6)*cos(q5)*cos(q4)^2-Ix6s*cos(q4)*q1dot^2*sin(q3+q2)*sin(q6)*cos(q3+q2)-Iz6s*sin(q4)*q3dot*sin(q6)*q4dot-Iz6s*q6dot*cos(q6)*cos(q4)*q2dot-Iy6s*cos(q6)*cos(q5)*q4dot*cos(q4)*q2dot-Iz6s*sin(q4)*q3dot*sin(q6)*q1dot*cos(q3+q2)-Iz6s*sin(q5)*q2dot*cos(q6)*q1dot*sin(q3+q2)+Ix6s*sin(q4)*q2dot*sin(q6)*q4dot-2*Ix6s*sin(q5)*q2dot*sin(q6)*cos(q5)*q3dot+2*Ix6s*sin(q5)*q2dot*sin(q6)*cos(q5)*q3dot*cos(q4)^2-Iz6s*sin(q5)*q3dot*cos(q6)*q1dot*sin(q3+q2)+Iy6s*cos(q6)*q5dot*sin(q5)*sin(q4)*q3dot-Iy6s*cos(q6)*q5dot*sin(q5)*cos(q4)*q1dot*sin(q3+q2)-Iy6s*q6dot*sin(q6)*sin(q5)*q4dot-Iz6s*cos(q6)*q5dot*sin(q5)*sin(q4)*q2dot-Iz6s*cos(q6)*q5dot*sin(q5)*sin(q4)*q3dot-Iz6s*q6dot*sin(q6)*sin(q5)*q4dot+Iy6s*sin(q6)*q4dot*sin(q4)*q3dot-Iz6s*sin(q5)*sin(q4)*q3dot^2*cos(q6)*cos(q4)-Ix6s*sin(q5)*cos(q4)^2*q1dot^2*sin(q6)*cos(q5)+Ix6s*sin(q5)*cos(q4)^2*q1dot^2*sin(q6)*cos(q5)*cos(q3+q2)^2-Iz6s*sin(q4)*q2dot*sin(q6)*q1dot*cos(q3+q2)-Ix6s*cos(q4)*q1dot*sin(q3+q2)*sin(q6)*q4dot+Iy6s*sin(q6)*q4dot*sin(q4)*q2dot-Iy6s*sin(q6)*sin(q4)*q1ddot*sin(q3+q2)-Iy6s*cos(q6)*cos(q5)*sin(q4)*q3ddot-Iy6s*cos(q6)*cos(q5)*sin(q4)*q2ddot+Iy6s*cos(q6)*q5dot*cos(q5)*q4dot-Iz6s*q6dot*cos(q6)*cos(q4)*q3dot nz7+2*Iy6s*cos(q6)^2*sin(q5)*q1dot^2*cos(q3+q2)*sin(q4)*sin(q3+q2)+2*Iy6s*cos(q6)^2*sin(q5)*q1dot*cos(q3+q2)*cos(q4)*q3dot-2*Ix6s*cos(q6)^2*sin(q5)*q4dot*q5dot+2*Iy6s*cos(q6)^2*sin(q5)*q4dot*q5dot-Ix6s*sin(q6)*q1dot^2*cos(q6)*cos(q4)^2+Ix6s*sin(q6)*q1dot^2*cos(q6)*cos(q4)^2*cos(q3+q2)^2-Ix6s*cos(q4)*q3dot^2*cos(q5)*sin(q4)-2*Iy6s*cos(q6)*cos(q5)^2*sin(q4)*q3dot*sin(q6)*cos(q4)*q1dot*sin(q3+q2)+2*Iy6s*cos(q6)^2*sin(q5)*q4dot*sin(q4)*q1dot*sin(q3+q2)-2*Iy6s*sin(q6)*sin(q4)*q1dot*sin(q3+q2)*cos(q6)*q5dot+2*Iy6s*cos(q6)*cos(q5)*cos(q4)*q1dot^2*sin(q3+q2)*sin(q6)*sin(q5)*cos(q3+q2)-2*Ix6s*cos(q6)^2*sin(q5)*q4dot*sin(q4)*q1dot*sin(q3+q2)+Ix6s*sin(q6)*cos(q4)^2*q3dot^2*cos(q6)-Iy6s*sin(q6)*cos(q4)^2*q3dot^2*cos(q6)+Ix6s*sin(q6)*cos(q4)^2*q2dot^2*cos(q6)+Iz6s*sin(q5)*q4dot*cos(q4)*q2dot-Iy6s*sin(q6)*cos(q4)^2*q2dot^2*cos(q6)-Iz6s*sin(q5)*cos(q4)*q1ddot*sin(q3+q2)-Iz6s*q5dot*sin(q5)*q1dot*cos(q3+q2)+2*Ix6s*cos(q6)^2*cos(q5)*sin(q4)*q2dot^2*cos(q4)-2*Ix6s*cos(q6)^2*sin(q5)*q1dot*cos(q3+q2)*cos(q4)*q2dot+Iy6s*sin(q6)*q1dot^2*cos(q6)*cos(q4)^2-Iy6s*sin(q6)*q1dot^2*cos(q6)*cos(q4)^2*cos(q3+q2)^2-Ix6s*q5dot*cos(q5)*sin(q4)*q3dot+Ix6s*q5dot*sin(q5)*q1dot*cos(q3+q2)-2*Iy6s*cos(q6)^2*cos(q5)*sin(q4)*q2dot*q5dot+2*Iy6s*cos(q6)*q1dot*cos(q3+q2)*sin(q6)*q4dot-2*Iy6s*cos(q6)*q1dot*cos(q3+q2)*sin(q6)*q4dot*cos(q5)^2+2*Iy6s*cos(q6)^2*cos(q5)*cos(q4)*q1dot*sin(q3+q2)*q5dot-2*Iy6s*cos(q6)^2*cos(q5)*sin(q4)*q3dot^2*cos(q4)+2*Ix6s*cos(q4)^2*q2dot*cos(q5)*q1dot*sin(q3+q2)+2*Ix6s*q1dot*sin(q3+q2)*cos(q5)*q3dot*cos(q4)^2+Iz6s*q6ddot-2*Ix6s*cos(q6)^2*sin(q5)*q1dot*cos(q3+q2)*cos(q4)*q3dot-2*Ix6s*cos(q6)^2*sin(q5)*q1dot^2*cos(q3+q2)*sin(q4)*sin(q3+q2)+2*Ix6s*sin(q6)*cos(q4)*q3dot*cos(q6)*sin(q4)*q1dot*sin(q3+q2)-Iy6s*sin(q6)*q5dot^2*cos(q6)-Iy6s*sin(q4)*q1dot^2*cos(q5)*cos(q4)-Iy6s*cos(q6)*q4dot^2*sin(q6)*cos(q5)^2-2*Ix6s*

57

cos(q6)*q1dot^2*cos(q3+q2)^2*sin(q6)+2*Ix6s*cos(q6)*q1dot*cos(q3+q2)*sin(q6)*q4dot*cos(q5)^2-2*Iy6s*q1dot*sin(q3+q2)*cos(q5)*q3dot*cos(q6)^2+2*Iy6s*cos(q6)*cos(q5)^2*q2dot*sin(q6)*q3dot-2*Iy6s*cos(q6)*cos(q5)^2*q2dot*sin(q6)*q3dot*cos(q4)^2-2*Iy6s*sin(q6)*cos(q4)^2*q2dot*cos(q6)*q3dot-2*Iy6s*cos(q6)^2*cos(q5)*sin(q4)*q3dot*q5dot+2*Iy6s*cos(q6)^2*sin(q5)*q4dot*cos(q4)*q3dot-2*Iy6s*cos(q4)^2*q3dot*cos(q5)*q1dot*sin(q3+q2)-2*Ix6s*cos(q4)*q2dot*cos(q5)*sin(q4)*q3dot+2*Ix6s*cos(q6)^2*cos(q5)*sin(q4)*q2dot*q5dot+2*Ix6s*cos(q6)^2*cos(q5)*sin(q4)*q3dot*q5dot-2*Ix6s*cos(q6)^2*sin(q5)*q1dot*cos(q3+q2)*q5dot+2*Ix6s*sin(q6)*cos(q4)*q2dot*cos(q6)*q5dot+2*Ix6s*sin(q6)*cos(q4)^2*q2dot*cos(q6)*q3dot-2*Ix6s*cos(q6)^2*sin(q5)*q4dot*cos(q4)*q3dot-2*Ix6s*cos(q6)^2*sin(q5)*q4dot*cos(q4)*q2dot+2*Ix6s*sin(q6)*cos(q4)*q3dot*cos(q6)*q5dot+2*Ix6s*cos(q6)*cos(q5)*sin(q4)*q2dot*sin(q6)*sin(q5)*q1dot*cos(q3+q2)+2*Ix6s*cos(q6)*cos(q5)^2*sin(q4)*q2dot*sin(q6)*cos(q4)*q1dot*sin(q3+q2)+2*Ix6s*cos(q6)*cos(q5)*sin(q4)*q2dot*sin(q6)*sin(q5)*q4dot+2*Ix6s*cos(q6)*cos(q5)*sin(q4)*q3dot*sin(q6)*sin(q5)*q4dot+2*Ix6s*sin(q6)*cos(q4)*q2dot*cos(q6)*sin(q4)*q1dot*sin(q3+q2)-Ix6s*cos(q6)*cos(q5)^2*q2dot^2*sin(q6)-Iz6s*cos(q5)*q3dot*q1dot*sin(q3+q2)-Iz6s*sin(q5)*cos(q4)*q2dot*q1dot*cos(q3+q2)+Iz6s*q5dot*cos(q5)*sin(q4)*q2dot+Iz6s*q5dot*cos(q5)*sin(q4)*q3dot+Iz6s*sin(q5)*q4dot*cos(q4)*q3dot-Iz6s*cos(q5)*q2dot*q1dot*sin(q3+q2)-Ix6s*cos(q6)*q4dot^2*sin(q6)+Iy6s*cos(q6)*q4dot^2*sin(q6)+Ix6s*q5dot*sin(q5)*q4dot+Ix6s*sin(q6)*q1dot^2*cos(q6)+Ix6s*sin(q6)*q5dot^2*cos(q6)+Iz6s*sin(q5)*sin(q4)*q2ddot-Iz6s*q5dot*sin(q5)*q4dot+Iz6s*sin(q5)*sin(q4)*q3ddot-Iy6s*sin(q6)*q1dot^2*cos(q6)-Iy6s*q5dot*sin(q5)*q4dot+Iz6s*cos(q5)*q1ddot*cos(q3+q2)+2*Ix6s*q1dot*sin(q3+q2)*cos(q5)*q3dot*cos(q6)^2+2*Iy6s*cos(q4)*q2dot*cos(q5)*sin(q4)*q3dot-2*Ix6s*cos(q6)*q1dot*cos(q3+q2)*sin(q6)*q4dot-2*Ix6s*sin(q4)*q1dot^2*cos(q5)*cos(q4)*cos(q6)^2+2*Ix6s*sin(q4)*q1dot^2*cos(q5)*cos(q4)*cos(q6)^2*cos(q3+q2)^2+2*Iy6s*cos(q6)^2*cos(q5)*cos(q4)*q1dot^2*sin(q4)-2*Iy6s*cos(q6)^2*cos(q5)*cos(q4)*q1dot^2*sin(q4)*cos(q3+q2)^2-2*Iy6s*cos(q4)^2*q2dot*cos(q5)*q1dot*sin(q3+q2)+Iy6s*cos(q4)*q3dot^2*cos(q5)*sin(q4)+2*Ix6s*cos(q6)^2*cos(q5)*sin(q4)*q3dot^2*cos(q4)+2*Ix6s*cos(q6)*cos(q5)^2*sin(q4)*q3dot*sin(q6)*cos(q4)*q1dot*sin(q3+q2)+2*Ix6s*sin(q6)*sin(q4)*q1dot*sin(q3+q2)*cos(q6)*q5dot-2*Ix6s*cos(q6)*cos(q5)*cos(q4)*q1dot^2*sin(q3+q2)*sin(q6)*sin(q5)*cos(q3+q2)+2*Ix6s*cos(q6)*cos(q5)*sin(q4)*q3dot*sin(q6)*sin(q5)*q1dot*cos(q3+q2)-2*Ix6s*cos(q6)*cos(q5)*cos(q4)*q1dot*sin(q3+q2)*sin(q6)*sin(q5)*q4dot+4*Ix6s*cos(q6)^2*cos(q5)*sin(q4)*q2dot*cos(q4)*q3dot+2*Iy6s*cos(q6)*cos(q5)*cos(q4)*q1dot*sin(q3+q2)*sin(q6)*sin(q5)*q4dot-2*Iy6s*sin(q6)*cos(q4)*q3dot*cos(q6)*q5dot-4*Iy6s*cos(q6)^2*cos(q5)*sin(q4)*q2dot*cos(q4)*q3dot-2*Iy6s*sin(q6)*cos(q4)*q2dot*cos(q6)*q5dot-2*Ix6s*cos(q6)^2*cos(q5)*cos(q4)*q1dot*sin(q3+q2)*q5dot-4*Ix6s*cos(q6)^2*cos(q5)*cos(q4)^2*q1dot*sin(q3+q2)*q3dot-4*Ix6s*cos(q6)^2*cos(q5)*cos(q4)^2*q1dot*sin(q3+q2)*q2dot-2*Iy6s*cos(q6)^2*cos(q5)*sin(q4)*q2dot^2*cos(q4)-2*Ix6s*cos(q6)*cos(q5)^2*q2dot*sin(q6)*q3dot+2*Ix6s*cos(q6)*cos(q5)^2*q2dot*sin(q6)*q3dot*cos(q4)^2-Ix6s*q1dot*sin(q3+q2)*cos(q5)*q2dot+Iy6s*cos(q4)*q2dot^2*cos(q5)*sin(q4)-Ix6s*cos(q4)*q2dot^2*cos(q5)*sin(q4)-Iz6s*sin(q5)*cos(q4)*q3dot*q1dot*cos(q3+q2)+Iz6s*sin(q5)*q4dot*sin(q4)*q1dot*sin(q3+q2)+2*Ix6s*q1dot*sin(q3+q2)*cos(q5)*q2dot*cos(q6)^2+4*Iy6s*cos(q6)^2*cos(q5)*cos(q4)^2*q1dot*sin(q3+q2)*q2dot-2*Iy6s*q1dot*sin(q3+q2)*cos(q5)*q2dot*cos(q6)^2+2*Iy6s*cos(q6)^2*sin(q5)*q4dot*cos(q4)*q2dot-2*Iy6s*cos(q6)*cos(q5)*sin(q4)*q2dot*sin(q6)*sin(q5)*q1dot*cos(q3+q2)-2*Iy6s*sin(q6)*cos(q4)*q2dot*cos(q6)*sin(q4)*q1dot*sin(q3+q2)+2*Iy6s*sin(q6)*q1dot^2*cos(q6)*cos(q3+q2)^2-2*Iy6s*sin(q6)*cos(q4)*q3dot*cos(q6)*sin(q4)*q1dot*sin(q3+q2)+Ix6s*cos(q6)*cos(q5)^2*q2dot^2*sin(q6)*cos(q4)^2-Iy6s*sin(q4)*q1dot*sin(q3+q2)*sin(q5)*q4dot-Iy6s*q5dot*cos(q5)*cos(q4)*q1dot*sin(q3+q2)+Iy6s*sin(q4)*q1dot^2*cos(q5)*cos(q4)*cos(q3+q2)^2-Iz6s*q5dot*cos(q5)*cos(q4)*q1dot*sin(q3+q2)+Iy6s*cos(q6)*cos(q5)^2*q2dot^2*sin(q6)-Iy6s*cos(q6)*cos(q5)^2*q2dot^2*sin(q6)*cos(q4)^2+Iy6s*q1dot*sin(q3+q2)*cos(q5)*q2dot+Ix6s*sin(q4)*q1dot^2*sin(q3+q2)*sin(q5)*cos(q3+q2)+Iy6s*q5dot*cos(q5)*sin(q4)*q3dot+Ix6s*sin(q4)*q1dot^2*cos(q5)*cos(q4)+4*Iy6s*cos(q6)^2*cos(q5)*cos(q4)^2*q1dot*sin(q3+q2)*q3dot+2*Iy6s*cos(q6)^2*sin(q5)*q1dot*cos(q3+q2)*q5dot+Ix6s*cos(q4)*q3dot*sin(q5)*q4dot+Ix6s*sin(q4)*q1dot*sin(q3+q2)*sin(q5)*q4dot+Ix6s*q5dot*cos(q5)*cos(q4)*q1dot*sin(q3+q2)+Iy6s*q5dot*cos(q5)*sin(q4)*q2dot+Iy6s*cos(q6)*cos(q5)^2*cos(q4)^2*q1dot^2*sin(q6)+Ix6s*cos(q6)*q4dot^2*sin(q6)*cos(q5)^2-Iy6s*cos(q6)*cos(q5)^2*cos(q4)^2*q1dot^2*sin(q6)*cos(q3+q2)^2-Ix6s*sin(q4)*q1dot^2*cos(q5)*cos(q4)*cos(q3+q2)^2+Ix6s*cos(q4)*q2dot*sin(q5)*q1dot*cos(q3+q2)+Ix6s*cos(q4)*q2dot*sin(q5)*q4dot-Iy6s*sin(q4)*q1dot^2*sin(q3+q2)*sin(q5)*cos(q3+q2)-Ix6s*cos(q6)*cos(q5)^2*q3dot^2*sin(q6)+Ix6s*cos(q6)*cos(q5)^2*q3dot^2*sin(q6)*cos(q4)^2+Iy6s*cos(q6)*cos(q5)^2*q3dot^2*sin(q6)-Iy6s*cos(q6)*cos(q5)^2*q3dot^2*sin(q6)*cos(q4)^2+Ix6s*cos(q4)*q3dot*sin(q5)*q1dot*cos(q3+q2)-Iy6s*sin(q6)*q1dot^2*cos(q6)*cos(q3+q2)^2*cos(q5)^2-Iy6s*cos(q4)*q2dot*sin(q5)*q1dot*cos(q3+q2)-Ix6s*q1dot*sin(q3+q2)*cos(q5)*q3dot-Ix6s*cos(q6)*cos(q5)^2*cos(q4)^2*q1dot^2*sin(q6)+Ix6s*cos(q6)*cos(q5)^2*cos(q4)^2*q1dot^2*sin(q6)*cos(q3+q2)^2+Iy6s*q1dot*sin(q3+q2)*

58

cos(q5)*q3dot-Iy6s*cos(q4)*q3dot*sin(q5)*q4dot+Ix6s*cos(q6)*q1dot^2*cos(q3+q2)^2*sin(q6)*cos(q5)^2-Iy6s*cos(q4)*q3dot*sin(q5)*q1dot*cos(q3+q2)-Ix6s*q5dot*cos(q5)*sin(q4)*q2dot-Iy6s*cos(q4)*q2dot*sin(q5)*q4dot-Iy6s*q5dot*sin(q5)*q1dot*cos(q3+q2)-2*Iy6s*cos(q6)*cos(q5)*sin(q4)*q3dot*sin(q6)*sin(q5)*q1dot*cos(q3+q2)-2*Iy6s*cos(q6)*cos(q5)^2*sin(q4)*q2dot*sin(q6)*cos(q4)*q1dot*sin(q3+q2)+Iz6s*cos(q5)*q4ddot-2*Iy6s*cos(q6)*cos(q5)*sin(q4)*q2dot*sin(q6)*sin(q5)*q4dot-2*Iy6s*cos(q6)*cos(q5)*sin(q4)*q3dot*sin(q6)*sin(q5)*q4dot+2*Iy6s*cos(q6)^2*sin(q5)*q1dot*cos(q3+q2)*cos(q4)*q2dot n55 = Iy6s*cos(q4)*q3ddot+Iy6s*cos(q4)*q2ddot-Iy6s*q5ddot*cos(q6)^2+Iy6s*q5ddot+Iy5s*sin(q5)*cos(q4)^2*q1dot^2*cos(q5)+Iz5s*sin(q4)*q2dot*q1dot*cos(q3+q2)-Iy5s*sin(q5)*cos(q4)^2*q1dot^2*cos(q5)*cos(q3+q2)^2+2*Iy5s*sin(q5)*q2dot*cos(q5)*q3dot+2*Iy5s*cos(q5)^2*q4dot*sin(q4)*q3dot+2*Iy5s*cos(q5)^2*q4dot*sin(q4)*q2dot+2*Iz5s*cos(q5)*q1dot*cos(q3+q2)*sin(q5)*q4dot-Iy5s*sin(q5)*q3dot^2*cos(q5)*cos(q4)^2-2*Iz5s*sin(q5)*q2dot*cos(q5)*q3dot+cos(q6)*Iy6s*sin(q5)*sin(q4)*q2dot^2*sin(q6)*cos(q4)+cos(q6)*Ix6s*sin(q6)*cos(q5)*cos(q4)*q1ddot*sin(q3+q2)-cos(q6)*Ix6s*sin(q6)*cos(q5)*sin(q4)*q2ddot-cos(q6)*Ix6s*sin(q6)*cos(q5)*sin(q4)*q3ddot-2*Iy5s*sin(q5)*cos(q4)*q1dot*sin(q3+q2)*cos(q5)*sin(q4)*q2dot+Ix6s*cos(q6)^2*q5ddot-Ix5s*q4dot*sin(q4)*q3dot-Ix5s*q4dot*sin(q4)*q2dot+Ix5s*sin(q4)*q1ddot*sin(q3+q2)-2*Iy5s*sin(q5)*cos(q4)*q1dot*sin(q3+q2)*cos(q5)*sin(q4)*q3dot+2*Iz5s*sin(q5)*q2dot*cos(q5)*q3dot*cos(q4)^2-2*Iy5s*sin(q5)*q2dot*cos(q5)*q3dot*cos(q4)^2-Iy5s*sin(q4)*q2dot*q4dot-2*Iy5s*cos(q5)*q1dot*cos(q3+q2)*sin(q5)*q4dot-2*Iz5s*cos(q5)^2*q4dot*sin(q4)*q2dot+2*Iy5s*cos(q5)^2*q1dot*cos(q3+q2)*sin(q4)*q3dot+Iz5s*sin(q4)*q3dot*q4dot+Iy5s*sin(q5)*q3dot^2*cos(q5)-2*Iy5s*cos(q5)^2*q1dot^2*cos(q3+q2)*cos(q4)*sin(q3+q2)+2*Iy5s*cos(q5)^2*q1dot*cos(q3+q2)*sin(q4)*q2dot-2*Iy5s*cos(q5)^2*q4dot*cos(q4)*q1dot*sin(q3+q2)-Iy5s*cos(q5)*q4dot^2*sin(q5)+Iz5s*cos(q5)*q4dot^2*sin(q5)+cos(q6)*Iy6s*sin(q5)*cos(q4)*q1dot^2*sin(q6)*sin(q4)*cos(q3+q2)^2+2*Iz5s*cos(q5)^2*q1dot^2*cos(q3+q2)*cos(q4)*sin(q3+q2)+2*Iz5s*cos(q5)^2*q4dot*cos(q4)*q1dot*sin(q3+q2)-2*sin(q6)*Ix6s*q6dot*cos(q6)*cos(q4)*q3dot+sin(q6)*Iy6s*cos(q6)*cos(q5)*sin(q4)*q2ddot+Iz5s*sin(q4)*q2dot*q4dot+Iy5s*sin(q5)*q2dot^2*cos(q5)+Iy5s*cos(q4)*q1dot^2*sin(q3+q2)*cos(q3+q2)+2*Iz5s*sin(q5)*cos(q4)*q1dot*sin(q3+q2)*cos(q5)*sin(q4)*q3dot-2*Iz5s*cos(q5)^2*q4dot*sin(q4)*q3dot+2*Iz5s*sin(q5)*cos(q4)*q1dot*sin(q3+q2)*cos(q5)*sin(q4)*q2dot-Iy5s*sin(q5)*q2dot^2*cos(q5)*cos(q4)^2-Ix6s*q6dot*cos(q5)*cos(q4)*q1dot*sin(q3+q2)-Iy5s*sin(q4)*q3dot*q4dot+2*sin(q6)*Iy6s*cos(q6)*sin(q5)*q2dot*q1dot*sin(q3+q2)+2*sin(q6)*Iy6s*cos(q6)*sin(q5)*q3dot*q1dot*sin(q3+q2)-cos(q6)*l4*fy7+2*Iy6s*cos(q5)^2*q4dot*cos(q6)^2*sin(q4)*q3dot+2*Iy6s*sin(q5)*q2dot*cos(q6)^2*cos(q5)*q3dot-Iy6s*sin(q5)*q3dot^2*cos(q6)^2*cos(q5)*cos(q4)^2+Iz6s*q6dot*cos(q5)*cos(q4)*q1dot*sin(q3+q2)+Ix6s*cos(q4)*q1dot^2*sin(q3+q2)*cos(q3+q2)-Ix6s*cos(q4)*q1dot^2*sin(q3+q2)*cos(q3+q2)*cos(q6)^2+Iz6s*sin(q4)*q3dot*q1dot*cos(q3+q2)+Iy6s*sin(q4)*q3dot*q1dot*cos(q3+q2)-Ix6s*sin(q5)*q2dot^2*cos(q5)*cos(q6)^2+Iz6s*sin(q5)*q2dot^2*cos(q5)*cos(q4)^2+Iz6s*sin(q4)*q2dot*q1dot*cos(q3+q2)+Ix6s*cos(q4)*q1dot*sin(q3+q2)*q4dot-Iy6s*sin(q4)*q1ddot*sin(q3+q2)*cos(q6)^2-Iz6s*cos(q4)*q1dot^2*sin(q3+q2)*cos(q3+q2)-2*Ix6s*cos(q5)^2*q1dot^2*cos(q3+q2)*cos(q4)*sin(q3+q2)+2*Ix6s*cos(q5)^2*q1dot^2*cos(q3+q2)*cos(q4)*sin(q3+q2)*cos(q6)^2-2*Ix6s*sin(q5)*cos(q4)*q1dot*sin(q3+q2)*cos(q5)*sin(q4)*q3dot+2*Ix6s*sin(q5)*cos(q4)*q1dot*sin(q3+q2)*cos(q5)*sin(q4)*q3dot*cos(q6)^2+Iz6s*sin(q5)*q3dot^2*cos(q5)*cos(q4)^2+Ix6s*cos(q5)*q4dot^2*sin(q5)*cos(q6)^2+2*Ix6s*cos(q5)^2*q1dot*cos(q3+q2)*sin(q4)*q2dot+2*Ix6s*sin(q5)*q2dot*cos(q5)*q3dot-2*Iz5s*cos(q5)^2*q1dot*cos(q3+q2)*sin(q4)*q2dot-2*Iz5s*cos(q5)^2*q1dot*cos(q3+q2)*sin(q4)*q3dot+Iy5s*cos(q4)*q1dot*sin(q3+q2)*q4dot+cos(q6)*Iy6s*cos(q5)*q1dot^2*cos(q3+q2)*sin(q6)*sin(q4)*sin(q3+q2)-2*sin(q6)*Ix6s*sin(q5)*q2dot*cos(q6)*q1dot*sin(q3+q2)+2*Ix6s*cos(q6)^2*sin(q4)*q3dot*q1dot*cos(q3+q2)+Ix6s*cos(q6)^2*sin(q4)*q1ddot*sin(q3+q2)+Iy6s*sin(q5)*q3dot^2*cos(q6)^2*cos(q5)+Iy6s*sin(q5)*q2dot^2*cos(q6)^2*cos(q5)+Ix5s*cos(q4)*q2ddot+Iz5s*sin(q5)*q2dot^2*cos(q5)*cos(q4)^2+Iz5s*sin(q4)*q3dot*q1dot*cos(q3+q2)-Iz5s*sin(q5)*cos(q4)^2*q1dot^2*cos(q5)+Iz5s*sin(q5)*cos(q4)^2*q1dot^2*cos(q5)*cos(q3+q2)^2-Iy5s*sin(q4)*q2dot*q1dot*cos(q3+q2)-Iy5s*sin(q4)*q3dot*q1dot*cos(q3+q2)-Iz5s*cos(q4)*q1dot*sin(q3+q2)*q4dot+Ix5s*sin(q4)*q3dot*q1dot*cos(q3+q2)+Iy6s*sin(q5)*cos(q4)^2*q1dot^2*cos(q6)^2*cos(q5)+2*Ix6s*q6dot*cos(q6)^2*sin(q5)*q4dot-2*Iy6s*sin(q5)*sin(q4)*q2dot*cos(q6)^2*cos(q5)*cos(q4)*q1dot*sin(q3+q2)-2*Iy6s*sin(q5)*sin(q4)*q3dot*cos(q6)^2*cos(q5)*cos(q4)*q1dot*sin(q3+q2)+2*Iy6s*cos(q5)^2*q1dot*cos(q3+q2)*cos(q6)^2*sin(q4)*q2dot+2*Iy6s*q6dot*cos(q6)^2*cos(q5)*sin(q4)*q2dot+2*Iy6s*q6dot*cos(q6)^2*

59

cos(q5)*sin(q4)*q3dot+Ix5s*sin(q4)*q2dot*q1dot*cos(q3+q2)+Ix5s*q4dot*cos(q4)*q1dot*sin(q3+q2)-sin(q6)*l4*fx7-cos(q6)*Iy6s*sin(q5)*cos(q4)*q1dot^2*sin(q6)*sin(q4)+2*sin(q6)*Iy6s*q6dot*cos(q6)*cos(q4)*q2dot-2*sin(q6)*Ix6s*sin(q5)*q3dot*cos(q6)*q1dot*sin(q3+q2)-sin(q6)*Ix6s*sin(q5)*sin(q4)*q2dot^2*cos(q6)*cos(q4)+2*sin(q6)*Iy6s*q6dot*cos(q6)*cos(q4)*q3dot+2*sin(q6)*Iy6s*q6dot*cos(q6)*sin(q4)*q1dot*sin(q3+q2)-sin(q6)*Iy6s*cos(q6)*sin(q5)*q4ddot+Iz5s*sin(q5)*q3dot^2*cos(q5)*cos(q4)^2-Iz5s*cos(q4)*q1dot^2*sin(q3+q2)*cos(q3+q2)+Ix5s*cos(q4)*q3ddot-Iy5s*cos(q5)*q1dot^2*cos(q3+q2)^2*sin(q5)-2*Iy6s*q6dot*cos(q6)^2*cos(q5)*cos(q4)*q1dot*sin(q3+q2)-2*Iy6s*q6dot*cos(q6)^2*sin(q5)*q1dot*cos(q3+q2)+2*Iy6s*cos(q5)^2*q1dot*cos(q3+q2)*cos(q6)^2*sin(q4)*q3dot-2*Iy6s*cos(q5)^2*q1dot^2*cos(q3+q2)*cos(q6)^2*cos(q4)*sin(q3+q2)-2*sin(q6)*Ix6s*cos(q6)*cos(q5)*q4dot*cos(q4)*q2dot-2*sin(q6)*Ix6s*cos(q6)*cos(q5)*q4dot*cos(q4)*q3dot+2*sin(q6)*Ix6s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*cos(q6)*q2dot+2*sin(q6)*Ix6s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*cos(q6)*q3dot+sin(q6)*Ix6s*sin(q5)*cos(q4)*q1dot^2*cos(q6)*sin(q4)+2*sin(q6)*Iy6s*cos(q6)*cos(q5)*q4dot*cos(q4)*q3dot-2*sin(q6)*Ix6s*sin(q5)*sin(q4)*q2dot*cos(q6)*cos(q4)*q3dot-sin(q6)*ny7+2*Iz6s*sin(q5)*cos(q4)*q1dot*sin(q3+q2)*cos(q5)*sin(q4)*q2dot-Ix6s*sin(q4)*q2dot*q1dot*cos(q3+q2)-Ix6s*sin(q5)*q3dot^2*cos(q5)*cos(q6)^2+Iz6s*sin(q5)*cos(q4)^2*q1dot^2*cos(q5)*cos(q3+q2)^2+Ix6s*q6dot*cos(q5)*sin(q4)*q2dot+Iz5s*cos(q5)*q1dot^2*cos(q3+q2)^2*sin(q5)+Ix5s*q5ddot-2*sin(q6)*Ix6s*q6dot*cos(q6)*q5dot+2*sin(q6)*Iy6s*q6dot*cos(q6)*q5dot-Iy6s*cos(q5)*q1dot^2*cos(q3+q2)^2*cos(q6)^2*sin(q5)+2*Iz6s*cos(q5)^2*q4dot*cos(q4)*q1dot*sin(q3+q2)-2*Iz6s*cos(q5)^2*q1dot*cos(q3+q2)*sin(q4)*q2dot-2*Ix6s*sin(q5)*cos(q4)*q1dot*sin(q3+q2)*cos(q5)*sin(q4)*q2dot+2*Ix6s*sin(q5)*cos(q4)*q1dot*sin(q3+q2)*cos(q5)*sin(q4)*q2dot*cos(q6)^2-2*Iz6s*sin(q5)*q2dot*cos(q5)*q3dot-2*Iz6s*cos(q5)^2*q4dot*sin(q4)*q2dot+2*Iz6s*cos(q5)^2*q1dot^2*cos(q3+q2)*cos(q4)*sin(q3+q2)+Ix6s*sin(q5)*cos(q4)^2*q1dot^2*cos(q5)-Ix6s*sin(q5)*cos(q4)^2*q1dot^2*cos(q5)*cos(q6)^2+Iy6s*q6dot*cos(q5)*cos(q4)*q1dot*sin(q3+q2)+Iy6s*q6dot*sin(q5)*q1dot*cos(q3+q2)-Iz6s*q6dot*cos(q5)*sin(q4)*q3dot-2*Iy6s*q6dot*cos(q6)^2*sin(q5)*q4dot-Iy6s*cos(q5)*q4dot^2*cos(q6)^2*sin(q5)+2*Ix6s*q6dot*cos(q6)^2*cos(q5)*cos(q4)*q1dot*sin(q3+q2)+2*Ix6s*q6dot*cos(q6)^2*sin(q5)*q1dot*cos(q3+q2)+Iy6s*cos(q4)*q1dot^2*sin(q3+q2)*cos(q6)^2*cos(q3+q2)+cos(q6)*Ix6s*sin(q6)*sin(q5)*q1ddot*cos(q3+q2)+cos(q6)*Ix6s*sin(q6)*sin(q5)*q4ddot+cos(q6)*Iy6s*sin(q5)*sin(q4)*q3dot^2*sin(q6)*cos(q4)+2*sin(q6)*Iy6s*cos(q6)*cos(q5)*q4dot*cos(q4)*q2dot+sin(q6)*Iy6s*cos(q6)*cos(q5)*sin(q4)*q3ddot-2*sin(q6)*Ix6s*q6dot*cos(q6)*sin(q4)*q1dot*sin(q3+q2)-2*Iy6s*cos(q5)^2*q4dot*cos(q6)^2*cos(q4)*q1dot*sin(q3+q2)+2*Iy6s*cos(q5)^2*q4dot*cos(q6)^2*sin(q4)*q2dot+2*cos(q6)*Iy6s*sin(q5)*sin(q4)*q2dot*sin(q6)*cos(q4)*q3dot-sin(q6)*Ix6s*sin(q5)*cos(q4)*q1dot^2*cos(q6)*sin(q4)*cos(q3+q2)^2+2*sin(q6)*Iy6s*cos(q6)*cos(q5)*q4dot*sin(q4)*q1dot*sin(q3+q2)-2*Iy6s*cos(q5)*q1dot*cos(q3+q2)*cos(q6)^2*sin(q5)*q4dot+Ix6s*cos(q6)^2*cos(q4)*q2ddot+Ix6s*cos(q6)^2*cos(q4)*q3ddot+cos(q6)*nx7-Iz5s*sin(q5)*q2dot^2*cos(q5)+Iz6s*cos(q5)*q4dot^2*sin(q5)+Ix6s*sin(q5)*q2dot^2*cos(q5)+Iz6s*sin(q4)*q3dot*q4dot+Iy6s*sin(q4)*q1ddot*sin(q3+q2)-Ix6s*cos(q5)*q4dot^2*sin(q5)+Ix6s*sin(q5)*q3dot^2*cos(q5)-Iy6s*cos(q4)*q2ddot*cos(q6)^2-Iy6s*cos(q4)*q3ddot*cos(q6)^2-Ix6s*sin(q4)*q3dot*q4dot-Ix6s*q6dot*sin(q5)*q4dot-Iz6s*sin(q5)*q2dot^2*cos(q5)-2*cos(q6)*Iy6s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*sin(q6)*q2dot-2*cos(q6)*Iy6s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*sin(q6)*q3dot-2*Iy6s*sin(q5)*q2dot*cos(q6)^2*cos(q5)*q3dot*cos(q4)^2-2*Ix6s*q6dot*cos(q6)^2*cos(q5)*sin(q4)*q2dot-2*Ix6s*q6dot*cos(q6)^2*cos(q5)*sin(q4)*q3dot-Ix6s*sin(q5)*q3dot^2*cos(q5)*cos(q4)^2+Ix6s*sin(q5)*q3dot^2*cos(q5)*cos(q4)^2*cos(q6)^2-Ix6s*cos(q5)*q1dot^2*cos(q3+q2)^2*sin(q5)+Ix6s*cos(q5)*q1dot^2*cos(q3+q2)^2*sin(q5)*cos(q6)^2-Ix6s*q6dot*sin(q5)*q1dot*cos(q3+q2)-2*Ix6s*cos(q5)^2*q4dot*cos(q4)*q1dot*sin(q3+q2)+2*Ix6s*cos(q5)^2*q4dot*cos(q4)*q1dot*sin(q3+q2)*cos(q6)^2+2*Ix6s*cos(q5)^2*q4dot*sin(q4)*q3dot-2*Ix6s*cos(q5)^2*q4dot*sin(q4)*q3dot*cos(q6)^2-Iy6s*q6dot*cos(q5)*sin(q4)*q2dot-Iy6s*q6dot*cos(q5)*sin(q4)*q3dot-Iz6s*sin(q5)*cos(q4)^2*q1dot^2*cos(q5)-Iz6s*q6dot*cos(q5)*sin(q4)*q2dot-Iy6s*q4dot*sin(q4)*q2dot+Iz6s*sin(q4)*q2dot*q4dot-Iz6s*sin(q5)*q3dot^2*cos(q5)-Iy6s*q4dot*sin(q4)*q3dot+Iy6s*q6dot*sin(q5)*q4dot+Iz6s*q6dot*sin(q5)*q4dot-Iz5s*sin(q5)*q3dot^2*cos(q5)+2*Iz6s*sin(q5)*cos(q4)*q1dot*sin(q3+q2)*cos(q5)*sin(q4)*q3dot-Iz6s*cos(q4)*q1dot*sin(q3+q2)*q4dot-2*Ix6s*cos(q5)^2*q1dot*cos(q3+q2)*sin(q4)*q2dot*cos(q6)^2-2*Ix6s*sin(q5)*q2dot*cos(q5)*q3dot*cos(q6)^2-2*Ix6s*sin(q5)*q2dot*cos(q5)*q3dot*cos(q4)^2+2*Ix6s*sin(q5)*q2dot*cos(q5)*q3dot*cos(q4)^2*cos(q6)^2+2*Iz6s*sin(q5)*q2dot*cos(q5)*q3dot*cos(q4)^2-Ix6s*sin(q4)*q3dot*q1dot*cos(q3+q2)+2*Ix6s*cos(q5)^2*q1dot*cos(q3+q2)*sin(q4)*q3dot-2*Ix6s*cos(q5)^2*q1dot*cos(q3+q2)*sin(q4)*q3dot*cos(q6)^2+2*Ix6s*cos(q6)^2*sin(q4)*q2dot*q1dot*cos(q3+q2)-2*Iy6s*sin(q4)*q2dot*cos(q6)^2*q1dot*cos(q3+q2)-Iy6s*sin(q5)*q2dot^2*cos(q6)^2*cos(q5)*cos(q4)^2-2*Iy6s*sin(q4)*q3dot*cos(q6)^2*q1dot*cos(q3+q2)-Iy6s*sin(q5)*cos(q4)^2*q1dot^2*cos(q6)^2*cos(q5)*

60

cos(q3+q2)^2-sin(q6)*Ix6s*sin(q5)*sin(q4)*q3dot^2*cos(q6)*cos(q4)-2*sin(q6)*Ix6s*q6dot*cos(q6)*cos(q4)*q2dot-sin(q6)*Ix6s*cos(q5)*q1dot^2*cos(q3+q2)*cos(q6)*sin(q4)*sin(q3+q2)-sin(q6)*Iy6s*cos(q6)*cos(q5)*cos(q4)*q1ddot*sin(q3+q2)-sin(q6)*Iy6s*cos(q6)*sin(q5)*q1ddot*cos(q3+q2)-2*sin(q6)*Ix6s*cos(q6)*cos(q5)*q4dot*sin(q4)*q1dot*sin(q3+q2)+Ix6s*q6dot*cos(q5)*sin(q4)*q3dot+Iz6s*cos(q5)*q1dot^2*cos(q3+q2)^2*sin(q5)+2*Ix6s*cos(q5)^2*q4dot*sin(q4)*q2dot-2*Ix6s*cos(q5)^2*q4dot*sin(q4)*q2dot*cos(q6)^2-2*Iz6s*cos(q5)^2*q1dot*cos(q3+q2)*sin(q4)*q3dot-Ix6s*sin(q5)*q2dot^2*cos(q5)*cos(q4)^2+Ix6s*sin(q5)*q2dot^2*cos(q5)*cos(q4)^2*cos(q6)^2-2*Ix6s*cos(q5)*q1dotcos(q3+q2)*sin(q5)*q4dot+2*Ix6s*cos(q5)*q1dot*cos(q3+q2)*sin(q5)*q4dot*cos(q6)^2-2*Iz6s*cos(q5)^2*q4dot*sin(q4)*q3dot+Iz6s*q6dot*sin(q5)*q1dot*cos(q3+q2)+Iy6s*q4dot*cos(q4)*q1dot*sin(q3+q2)+Iy6s*sin(q4)*q2dot*q1dot*cos(q3+q2)-Ix6s*sin(q5)*cos(q4)^2*q1dot^2*cos(q5)*cos(q3+q2)^2+Ix6s*sin(q5)*cos(q4)^2*q1dot^2*cos(q5)*cos(q3+q2)^2*cos(q6)^2+2*Iz6s*cos(q5)*q1dot*cos(q3+q2)*sin(q5)*q4dot-Ix6s*sin(q4)*q2dot*q4dot 2*Iy6s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*q2dot*cos(q6)^2+Ix6s*sin(q5)*q4ddot+2*Iy6s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*q3dot*cos(q6)^2-2*Iy6s*sin(q5)*sin(q4)*q2dot*cos(q4)*q3dot*cos(q6)^2-Iy6s*cos(q5)*q1dot^2*cos(q3+q2)*sin(q4)*sin(q3+q2)*cos(q6)^2-Iy6s*sin(q5)*cos(q4)*q1dot^2*sin(q4)*cos(q3+q2)^2*cos(q6)^2-Iz5s*sin(q5)*cos(q4)*q1dot^2*sin(q4)*cos(q3+q2)^2-Iy5s*cos(q5)*sin(q4)*q3ddot-Iz5s*q5dot*cos(q5)*q4dot+Ix5s*q5dot*cos(q5)*q4dot+Iy5s*q5dot*cos(q5)*q4dot-Iy5s*cos(q5)*sin(q4)*q2ddot+Iy5s*cos(q5)*cos(q4)*q3dot*q1dot*cos(q3+q2)-Iz5s*q5dot*sin(q5)*sin(q4)*q3dot-Iy5s*cos(q5)*q4dot*sin(q4)*q1dot*sin(q3+q2)-Iy5s*cos(q5)*q4dot*cos(q4)*q2dot+Iy5s*q5dot*sin(q5)*sin(q4)*q2dot+Iy5s*q5dot*sin(q5)*sin(q4)*q3dot-Iy5s*cos(q5)*q4dot*cos(q4)*q3dot+Iz5s*q5dot*sin(q5)*cos(q4)*q1dot*sin(q3+q2)+Iy5s*cos(q5)*cos(q4)*q2dot*q1dot*cos(q3+q2)+Iy5s*sin(q5)*q1ddot*cos(q3+q2)+Ix6s*cos(q5)*sin(q4)*q3ddot*cos(q6)^2+Ix6s*cos(q5)*sin(q4)*q2ddot*cos(q6)^2-Ix6s*sin(q5)*q1ddot*cos(q3+q2)*cos(q6)^2+Iy6s*q6dot*q5dot-Iz6s*q6dot*q5dot+Ix6s*q5dot*sin(q5)*sin(q4)*q3dot+Iy6s*q5dot*cos(q5)*q1dot*cos(q3+q2)+Iy6s*q5dot*sin(q5)*sin(q4)*q2dot+Ix6s*q5dot*cos(q5)*q1dot*cos(q3+q2)+Iy6s*cos(q5)*q4dot*cos(q4)*q2dot+Iy6s*q6dot*sin(q4)*q1dot*sin(q3+q2)+Iy6s*sin(q5)*sin(q4)*q3dot^2*cos(q4)+Iy6s*q5dot*sin(q5)*sin(q4)*q3dot+Iy6s*sin(q5)*q2dot*q1dot*sin(q3+q2)-Ix6s*q6dot*q5dot-Ix6s*q6dot*cos(q4)*q2dot+Iz6s*sin(q5)*cos(q4)*q1dot^2*sin(q4)+Iy6s*sin(q5)*sin(q4)*q2dot^2*cos(q4)+Ix6s*sin(q5)*q1ddot*cos(q3+q2)-Iz6s*q5dot*cos(q5)*q4dot-2*cos(q6)*Iy6s*q6dot*sin(q6)*sin(q5)*q1dot*cos(q3+q2)-2*cos(q6)*Iy6s*sin(q6)*sin(q4)*q2dot*q1dot*cos(q3+q2)+2*cos(q6)*Ix6s*sin(q4)*q2dot*sin(q6)*q1dot*cos(q3+q2)+cos(q6)*Ix6s*sin(q5)*cos(q4)^2*q1dot^2*sin(q6)*cos(q5)*cos(q3+q2)^2+cos(q6)*Ix6s*sin(q5)*q3dot^2*sin(q6)*cos(q5)*cos(q4)^2+2*cos(q6)*Ix6s*sin(q4)*q3dot*sin(q6)*q1dot*cos(q3+q2)+2*cos(q6)*Ix6s*q6dot*sin(q6)*sin(q5)*q4dot-2*sin(q6)*Iy6s*sin(q5)*q2dot*cos(q6)*cos(q5)*q3dot*cos(q4)^2+2*sin(q6)*Iy6s*sin(q5)*q2dot*cos(q6)*cos(q5)*q3dot+sin(q6)*Iy6s*sin(q5)*cos(q4)^2*q1dot^2*cos(q6)*cos(q5)-Iz6s*q6dot*sin(q4)*q1dot*sin(q3+q2)-Iz6s*q6dot*cos(q4)*q2dot-Ix6s*q6dot*cos(q4)*q3dot+Iy6s*q5dot*cos(q5)*q4dot+Iy6s*cos(q5)*q4dot*cos(q4)*q3dot-Ix6s*cos(q5)*sin(q4)*q2ddot-Iz6s*q6dot*cos(q4)*q3dot+Iy6s*sin(q5)*q3dot*q1dot*sin(q3+q2)-Iz6s*q5dot*sin(q5)*sin(q4)*q2dot+Ix6s*q5dot*sin(q5)*sin(q4)*q2dot-Ix6s*sin(q5)*q2dot*q1dot*sin(q3+q2)-Ix6s*cos(q5)*q4dot*cos(q4)*q3dot-Iy6s*sin(q5)*cos(q4)*q1dot^2*sin(q4)+Ix6s*cos(q5)*cos(q4)*q1ddot*sin(q3+q2)-sin(q6)*l4*fy7-Iz6s*sin(q5)*q2dot*q1dot*sin(q3+q2)-Iz6s*sin(q5)*sin(q4)*q3dot^2*cos(q4)-Ix6s*cos(q5)*q4dot*cos(q4)*q2dot-Iz6s*sin(q5)*q3dot*q1dot*sin(q3+q2)-Iz6s*q5dot*sin(q5)*sin(q4)*q3dot-Ix6s*sin(q5)*q3dot*q1dot*sin(q3+q2)-Iz6s*q5dot*cos(q5)*q1dot*cos(q3+q2)-Ix6s*cos(q5)*sin(q4)*q3ddot+Iy6s*q6dot*cos(q4)*q2dot-Iz6s*cos(q5)*q4dot*cos(q4)*q3dot-Iz6s*cos(q5)*q4dot*cos(q4)*q2dot-Ix6s*q6dot*sin(q4)*q1dot*sin(q3+q2)-Iz6s*sin(q5)*sin(q4)*q2dot^2*cos(q4)+Iy6s*q6dot*cos(q4)*q3dot+Ix6s*q5dot*cos(q5)*q4dot+Iy6s*cos(q5)*q4dot*sin(q4)*q1dot*sin(q3+q2)+Iy6s*sin(q5)*cos(q4)*q1dot^2*sin(q4)*cos(q3+q2)^2+Ix6s*cos(q5)*cos(q4)*q2dot*q1dot*cos(q3+q2)+Ix6s*cos(q5)*cos(q4)*q3dot*q1dot*cos(q3+q2)+2*Iz6s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*q2dot-Ix6s*q5dot*sin(q5)*cos(q4)*q1dot*sin(q3+q2)+2*Iy6s*sin(q5)*sin(q4)*q2dot*cos(q4)*q3dot-Iz6s*sin(q5)*cos(q4)*q1dot^2*sin(q4)*cos(q3+q2)^2-Iz6s*cos(q5)*cos(q4)*q2dot*q1dot*cos(q3+q2)-Iz6s*cos(q5)*q1dot^2*cos(q3+q2)*sin(q4)*sin(q3+q2)-Iz6s*cos(q5)*cos(q4)*q3dot*q1dot*cos(q3+q2)-Iy6s*q5dot*sin(q5)*cos(q4)*q1dot*sin(q3+q2)+Iy6s*cos(q5)*q1dot^2*cos(q3+q2)*sin(q4)*sin(q3+q2)-2*Iy6s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*q2dot-2*Iz6s*sin(q5)*sin(q4)*q2dot*cos(q4)*q3dot-2*Iy6s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*q3dot-Iz6s*cos(q5)*q4dot*sin(q4)*q1dot*sin(q3+q2)-Ix6s*cos(q5)*q4dot*sin(q4)*q1dot*sin(q3+q2)+2*Iz6s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*q3dot-Iy6s*sin(q5)*sin(q4)*q3dot^2*cos(q4)*cos(q6)^2-sin(q6)*Iy6s*sin(q5)*q2dot^2*cos(q6)*cos(q5)*cos(q4)^2-sin(q6)*Iy6s*cos(q5)*q1dot^2*cos(q3+q2)^2*cos(q6)*sin(q5)-sin(q6)*Iy6s*cos(q5)*

61

q4dot^2*cos(q6)*sin(q5)+sin(q6)*Ix6s*cos(q6)*sin(q4)*q1ddot*sin(q3+q2)-2*sin(q6)*Iy6s*cos(q5)^2*q4dot*cos(q6)*cos(q4)*q1dot*sin(q3+q2)+2*sin(q6)*Iy6s*cos(q5)^2*q4dot*cos(q6)*sin(q4)*q2dot+2*sin(q6)*Iy6s*cos(q5)^2*q1dot*cos(q3+q2)*cos(q6)*sin(q4)*q3dot-2*sin(q6)*Iy6s*cos(q5)^2*q1dot^2*cos(q3+q2)*cos(q6)*cos(q4)*sin(q3+q2)-2*sin(q6)*Iy6s*cos(q5)*q1dot*cos(q3+q2)*cos(q6)*sin(q5)*q4dot+2*sin(q6)*Iy6s*cos(q5)^2*q4dot*cos(q6)*sin(q4)*q3dot+2*cos(q6)*Ix6s*cos(q5)*q1dot*cos(q3+q2)*sin(q6)*sin(q5)*q4dot+2*cos(q6)*Ix6s*cos(q5)^2*q4dot*sin(q6)*cos(q4)*q1dot*sin(q3+q2)-2*cos(q6)*Ix6s*cos(q5)^2*q4dot*sin(q6)*sin(q4)*q3dot-2*cos(q6)*Ix6s*q6dot*sin(q6)*cos(q5)*sin(q4)*q2dot-2*cos(q6)*Ix6s*q6dot*sin(q6)*cos(q5)*sin(q4)*q3dot-cos(q6)*Ix6s*cos(q4)*q1dot^2*sin(q3+q2)*sin(q6)*cos(q3+q2)+Ix6s*cos(q5)*q1dot^2*cos(q3+q2)*cos(q6)^2*sin(q4)*sin(q3+q2)+Iy6s*cos(q6)^2*cos(q5)*cos(q4)*q1ddot*sin(q3+q2)-Ix6s*cos(q5)*cos(q4)*q1ddot*sin(q3+q2)*cos(q6)^2-Iy6s*sin(q5)*sin(q4)*q2dot^2*cos(q4)*cos(q6)^2+Iy6s*sin(q5)*cos(q4)*q1dot^2*sin(q4)*cos(q6)^2-Iy5s*q5dot*sin(q5)*cos(q4)*q1dot*sin(q3+q2)+Ix5s*cos(q5)*q4dot*cos(q4)*q2dot+Iy5s*cos(q5)*cos(q4)*q1ddot*sin(q3+q2)+Ix5s*cos(q5)*q4dot*cos(q4)*q3dot+Ix5s*cos(q5)*cos(q4)*q3dot*q1dot*cos(q3+q2)-2*Iy6s*q6dot*cos(q6)^2*q5dot+Ix5s*q5dot*cos(q5)*q1dot*cos(q3+q2)+Ix5s*cos(q5)*q4dot*sin(q4)*q1dot*sin(q3+q2)-Iz5s*cos(q5)*q4dot*cos(q4)*q3dot-Iz5s*cos(q5)*cos(q4)*q3dot*q1dot*cos(q3+q2)+Ix5s*sin(q5)*sin(q4)*q2dot^2*cos(q4)+Ix5s*cos(q5)*cos(q4)*q2dot*q1dot*cos(q3+q2)+Ix5s*q5dot*sin(q5)*sin(q4)*q3dot+Ix5s*cos(q5)*q1dot^2*cos(q3+q2)*sin(q4)*sin(q3+q2)-Ix6s*sin(q5)*q4ddot*cos(q6)^2-Iz5s*sin(q5)*sin(q4)*q3dot^2*cos(q4)+Ix5s*sin(q5)*sin(q4)*q3dot^2*cos(q4)-Ix5s*q5dot*sin(q5)*cos(q4)*q1dot*sin(q3+q2)-Iz5s*cos(q5)*cos(q4)*q2dot*q1dot*cos(q3+q2)-Iz5s*cos(q5)*q1dot^2*cos(q3+q2)*sin(q4)*sin(q3+q2)+2*Ix6s*sin(q5)*q2dot*cos(q6)^2*q1dot*sin(q3+q2)+2*Ix6s*q6dot*cos(q6)^2*sin(q4)*q1dot*sin(q3+q2)+Ix6s*sin(q5)*cos(q4)*q1dot^2*cos(q6)^2*sin(q4)*cos(q3+q2)^2-Iy6s*cos(q6)^2*cos(q5)*sin(q4)*q2ddot-2*Iy6s*cos(q6)^2*cos(q5)*q4dot*sin(q4)*q1dot*sin(q3+q2)-2*Iy6s*q6dot*cos(q6)^2*cos(q4)*q2dot-2*Iy6s*q6dot*cos(q6)^2*cos(q4)*q3dot-2*Iy6s*q6dot*cos(q6)^2*sin(q4)*q1dot*sin(q3+q2)-2*Iy6s*cos(q6)^2*sin(q5)*q2dot*q1dot*sin(q3+q2)-2*sin(q6)*Iy6s*sin(q5)*sin(q4)*q2dot*cos(q6)*cos(q5)*cos(q4)*q1dot*sin(q3+q2)+sin(q6)*Ix6s*cos(q6)*cos(q4)*q2ddot+sin(q6)*Ix6s*cos(q6)*cos(q4)*q3ddot+sin(q6)*Iy6s*cos(q4)*q1dot^2*sin(q3+q2)*cos(q6)*cos(q3+q2)+sin(q6)*Iy6s*sin(q5)*q3dot^2*cos(q6)*cos(q5)-sin(q6)*Iy6s*sin(q5)*cos(q4)^2*q1dot^2*cos(q6)*cos(q5)*cos(q3+q2)^2-sin(q6)*Iy6s*sin(q5)*q3dot^2*cos(q6)*cos(q5)*cos(q4)^2-2*Iy6s*cos(q6)^2*sin(q5)*q3dot*q1dot*sin(q3+q2)+2*Ix6s*sin(q5)*q3dot*cos(q6)^2*q1dot*sin(q3+q2)+Ix6s*sin(q5)*sin(q4)*q2dot^2*cos(q6)^2*cos(q4)+Iy6s*cos(q6)^2*sin(q5)*q1ddot*cos(q3+q2)+2*Ix6s*cos(q6)^2*cos(q5)*q4dot*sin(q4)*q1dot*sin(q3+q2)+2*Ix6s*cos(q6)^2*cos(q5)*q4dot*cos(q4)*q2dot+2*Ix6s*cos(q6)^2*cos(q5)*q4dot*cos(q4)*q3dot-cos(q6)*Ix6s*sin(q5)*q3dot^2*sin(q6)*cos(q5)+2*cos(q6)*Ix6s*cos(q5)^2*q1dot^2*cos(q3+q2)*sin(q6)*cos(q4)*sin(q3+q2)-2*cos(q6)*Ix6s*cos(q5)^2*q1dot*cos(q3+q2)*sin(q6)*sin(q4)*q2dot-2*cos(q6)*Ix6s*cos(q5)^2*q1dot*cos(q3+q2)*sin(q6)*sin(q4)*q3dot-2*cos(q6)*Ix6s*cos(q5)^2*q4dot*sin(q6)*sin(q4)*q2dot+2*cos(q6)*Ix6s*sin(q5)*cos(q4)*q1dot*sin(q3+q2)*sin(q6)*cos(q5)*sin(q4)*q2dot+2*cos(q6)*Ix6s*sin(q5)*cos(q4)*q1dot*sin(q3+q2)*sin(q6)*cos(q5)*sin(q4)*q3dot-Iz5s*cos(q5)*q4dot*sin(q4)*q1dot*sin(q3+q2)+Ix5s*q5dot*sin(q5)*sin(q4)*q2dot-Iy5s*sin(q5)*q3dot*q1dot*sin(q3+q2)+Iy5s*q5dot*cos(q5)*q1dot*cos(q3+q2)-Iz5s*q5dot*sin(q5)*sin(q4)*q2dot-Iz5s*q5dot*cos(q5)*q1dot*cos(q3+q2)-Iz5s*sin(q5)*sin(q4)*q2dot^2*cos(q4)-cos(q6)*Iy6s*sin(q6)*cos(q4)*q2ddot+cos(q6)*l4*fx7+Iy6s*cos(q5)*cos(q4)*q2dot*q1dot*cos(q3+q2)+sin(q6)*Iy6s*sin(q5)*q2dot^2*cos(q6)*cos(q5)-2*sin(q6)*Iy6s*sin(q5)*sin(q4)*q3dot*cos(q6)*cos(q5)*cos(q4)*q1dot*sin(q3+q2)+2*sin(q6)*Iy6s*cos(q5)^2*q1dot*cos(q3+q2)*cos(q6)*sin(q4)*q2dot-2*Iy6s*cos(q6)^2*cos(q5)*q4dot*cos(q4)*q2dot-Iy6s*cos(q6)^2*cos(q5)*sin(q4)*q3ddot-2*Iy6s*cos(q6)^2*cos(q5)*q4dot*cos(q4)*q3dot+Iy6s*cos(q6)^2*sin(q5)*q4ddot+2*Ix6s*q6dot*cos(q6)^2*q5dot-cos(q6)*Iy6s*sin(q6)*q5ddot+sin(q6)*Ix6s*cos(q6)*q5ddot-Iy5s*sin(q5)*q2dot*q1dot*sin(q3+q2)-Iz5s*cos(q5)*q4dot*cos(q4)*q2dot+2*Ix5s*sin(q5)*sin(q4)*q2dot*cos(q4)*q3dot-2*Iz5s*sin(q5)*sin(q4)*q2dot*cos(q4)*q3dot+Iy6s*cos(q5)*cos(q4)*q3dot*q1dot*cos(q3+q2)+Iz6s*q5dot*sin(q5)*cos(q4)*q1dot*sin(q3+q2)+2*Ix6s*q6dot*cos(q6)^2*cos(q4)*q3dot+Ix6s*sin(q5)*sin(q4)*q3dot^2*cos(q6)^2*cos(q4)+2*Ix6s*q6dot*cos(q6)^2*cos(q4)*q2dot-cos(q6)*Ix6s*sin(q5)*q2dot^2*sin(q6)*cos(q5)-2*cos(q6)*Ix6s*sin(q5)*q2dot*sin(q6)*cos(q5)*q3dot+2*cos(q6)*Ix6s*sin(q5)*q2dot*sin(q6)*cos(q5)*q3dot*cos(q4)^2-2*cos(q6)*Iy6s*q6dot*sin(q6)*sin(q5)*q4dot-cos(q6)*Ix6s*sin(q5)*cos(q4)^2*q1dot^2*sin(q6)*cos(q5)-cos(q6)*Iy6s*sin(q6)*sin(q4)*q1ddot*sin(q3+q2)+2*cos(q6)*Iy6s*q6dot*sin(q6)*cos(q5)*sin(q4)*q2dot+2*cos(q6)*Iy6s*q6dot*sin(q6)*cos(q5)*sin(q4)*q3dot+cos(q6)*Ix6s*cos(q5)*q1dot^2*cos(q3+q2)^2*sin(q6)*sin(q5)+2*cos(q6)*Ix6s*q6dot*sin(q6)*cos(q5)*cos(q4)*q1dot*sin(q3+q2)+2*cos(q6)*Ix6s*q6dot*sin(q6)*sin(q5)*q1dot*cos(q3+q2)+cos(q6)*Ix6s*cos(q5)*q4dot^2*sin(q6)*sin(q5)+cos(q6)*Ix6s*sin(q5)*

62

q2dot^2*sin(q6)*cos(q5)*cos(q4)^2-2*cos(q6)*Iy6s*sin(q6)*sin(q4)*q3dot*q1dot*cos(q3+q2)-2*cos(q6)*Iy6s*q6dot*sin(q6)*cos(q5)*cos(q4)*q1dot*sin(q3+q2)-cos(q6)*Iy6s*sin(q6)*cos(q4)*q3ddot+2*Iz5s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*q2dot-2*Ix5s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*q3dot-2*Ix5s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*q2dot+Ix5s*sin(q5)*cos(q4)*q1dot^2*sin(q4)*cos(q3+q2)^2+2*Iz5s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*q3dot+Iz5s*sin(q5)*cos(q4)*q1dot^2*sin(q4)-Ix5s*sin(q5)*cos(q4)*q1dot^2*sin(q4)-Iz5s*sin(q5)*q2dot*q1dot*sin(q3+q2)+Ix5s*sin(q5)*q3dot*q1dot*sin(q3+q2)-Iz5s*sin(q5)*q3dot*q1dot*sin(q3+q2)+Ix5s*sin(q5)*q2dot*q1dot*sin(q3+q2)+Iy5s*sin(q5)*q4ddot+sin(q6)*nx7+cos(q6)*ny7-Ix6s*sin(q5)*cos(q4)*q1dot^2*cos(q6)^2*sin(q4)+2*Ix6s*sin(q5)*sin(q4)*q2dot*cos(q6)^2*cos(q4)*q3dot-2*Ix6s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*cos(q6)^2*q2dot-2*Ix6s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*cos(q6)^2*q3dot -Ix5s*sin(q5)*q4dot*cos(q4)*q3dot-2*Ix5s*cos(q5)*cos(q4)^2*q1dot*sin(q3+q2)*q3dot-Ix5s*q5dot*sin(q5)*q1dot*cos(q3+q2)+Ix5s*q5dot*cos(q5)*sin(q4)*q2dot+Iz5s*sin(q5)*sin(q4)*q2ddot+Iz5s*sin(q5)*sin(q4)*q3ddot-Iy5s*cos(q5)*q2dot*q1dot*sin(q3+q2)+Iz5s*cos(q5)*q1ddot*cos(q3+q2)+Ix5s*cos(q5)*sin(q4)*q2dot^2*cos(q4)+Ix5s*cos(q5)*sin(q4)*q3dot^2*cos(q4)+Ix5s*q5dot*cos(q5)*sin(q4)*q3dot+Ix5s*cos(q5)*q2dot*q1dot*sin(q3+q2)-Iz5s*q5dot*sin(q5)*q4dot-Iy5s*cos(q5)*q3dot*q1dot*sin(q3+q2)+Ix5s*cos(q5)*cos(q4)*q1dot^2*sin(q4)*cos(q3+q2)^2+Ix5s*cos(q5)*q3dot*q1dot*sin(q3+q2)+Iy5s*cos(q5)*cos(q4)*q1dot^2*sin(q4)-Iy5s*cos(q5)*cos(q4)*q1dot^2*sin(q4)*cos(q3+q2)^2-Ix5s*cos(q5)*cos(q4)*q1dot^2*sin(q4)+Iy5s*q5dot*sin(q5)*q4dot-Ix5s*q5dot*sin(q5)*q4dot+nz7+2*Iy5s*cos(q5)*cos(q4)^2*q1dot*sin(q3+q2)*q2dot+Iy5s*sin(q5)*cos(q4)*q3dot*q1dot*cos(q3+q2)+Iy5s*q5dot*cos(q5)*cos(q4)*q1dot*sin(q3+q2)+2*Iy6s*cos(q6)^2*sin(q5)*q1dot^2*cos(q3+q2)*sin(q4)*sin(q3+q2)+2*Iy6s*cos(q6)^2*sin(q5)*q1dot*cos(q3+q2)*cos(q4)*q3dot-2*Ix6s*cos(q6)^2*sin(q5)*q4dot*q5dot+2*Iy6s*cos(q6)^2*sin(q5)*q4dot*q5dot-Ix6s*sin(q6)*q1dot^2*cos(q6)*cos(q4)^2+Ix6s*sin(q6)*q1dot^2*cos(q6)*cos(q4)^2*cos(q3+q2)^2-Ix6s*cos(q4)*q3dot^2*cos(q5)*sin(q4)-2*Iy6s*cos(q6)*cos(q5)^2*sin(q4)*q3dot*sin(q6)*cos(q4)*q1dot*sin(q3+q2)+2*Iy6s*cos(q6)^2*sin(q5)*q4dot*sin(q4)*q1dot*sin(q3+q2)-2*Iy6s*sin(q6)*sin(q4)*q1dot*sin(q3+q2)*cos(q6)*q5dot+2*Iy6s*cos(q6)*cos(q5)*cos(q4)*q1dot^2*sin(q3+q2)*sin(q6)*sin(q5)*cos(q3+q2)-2*Ix6s*cos(q6)^2*sin(q5)*q4dot*sin(q4)*q1dot*sin(q3+q2)+Ix6s*sin(q6)*cos(q4)^2*q3dot^2*cos(q6)-Iy6s*sin(q6)*cos(q4)^2*q3dot^2*cos(q6)+Ix6s*sin(q6)*cos(q4)^2*q2dot^2*cos(q6)+Iz6s*sin(q5)*q4dot*cos(q4)*q2dot-Iy6s*sin(q6)*cos(q4)^2*q2dot^2*cos(q6)-Iz6s*sin(q5)*cos(q4)*q1ddot*sin(q3+q2)-Iz6s*q5dot*sin(q5)*q1dot*cos(q3+q2)+2*Ix6s*cos(q6)^2*cos(q5)*sin(q4)*q2dot^2*cos(q4)-2*Ix6s*cos(q6)^2*sin(q5)*q1dot*cos(q3+q2)*cos(q4)*q2dot+Iy6s*sin(q6)*q1dot^2*cos(q6)*cos(q4)^2-Iy6s*sin(q6)*q1dot^2*cos(q6)*cos(q4)^2*cos(q3+q2)^2-Ix6s*q5dot*cos(q5)*sin(q4)*q3dot+Ix6s*q5dot*sin(q5)*q1dot*cos(q3+q2)-2*Iy6s*cos(q6)^2*cos(q5)*sin(q4)*q2dot*q5dot+2*Iy6s*cos(q6)*q1dot*cos(q3+q2)*sin(q6)*q4dot-2*Iy6s*cos(q6)*q1dot*cos(q3+q2)*sin(q6)*q4dot*cos(q5)^2+2*Iy6s*cos(q6)^2*cos(q5)*cos(q4)*q1dot*sin(q3+q2)*q5dot-2*Iy6s*cos(q6)^2*cos(q5)*sin(q4)*q3dot^2*cos(q4)+2*Ix6s*cos(q4)^2*q2dot*cos(q5)*q1dot*sin(q3+q2)+2*Ix6s*q1dot*sin(q3+q2)*cos(q5)*q3dot*cos(q4)^2+Iz6s*q6ddot+Iy5s*q5dot*sin(q5)*q1dot*cos(q3+q2)-2*Ix6s*cos(q6)^2*sin(q5)*q1dot*cos(q3+q2)*cos(q4)*q3dot-2*Ix6s*cos(q6)^2*sin(q5)*q1dot^2*cos(q3+q2)*sin(q4)*sin(q3+q2)+2*Ix6s*sin(q6)*cos(q4)*q3dot*cos(q6)*sin(q4)*q1dot*sin(q3+q2)-Iy5s*q5dot*cos(q5)*sin(q4)*q2dot-Iy5s*q5dot*cos(q5)*sin(q4)*q3dot+Iy5s*sin(q5)*q4dot*sin(q4)*q1dot*sin(q3+q2)+2*Iy5s*cos(q5)*cos(q4)^2*q1dot*sin(q3+q2)*q3dot-2*Iy5s*cos(q5)*sin(q4)*q2dot*cos(q4)*q3dot-Iz5s*cos(q5)*q3dot*q1dot*sin(q3+q2)+Iz5s*sin(q5)*q4dot*cos(q4)*q3dot-Iz5s*cos(q5)*q2dot*q1dot*sin(q3+q2)-Iy6s*sin(q6)*q5dot^2*cos(q6)-Iy6s*sin(q4)*q1dot^2*cos(q5)*cos(q4)-Iy6s*cos(q6)*q4dot^2*sin(q6)*cos(q5)^2-2*Ix6s*cos(q6)*q1dot^2*cos(q3+q2)^2*sin(q6)+2*Ix6s*cos(q6)*q1dot*cos(q3+q2)*sin(q6)*q4dot*cos(q5)^2-2*Iy6s*q1dot*sin(q3+q2)*cos(q5)*q3dot*cos(q6)^2+2*Iy6s*cos(q6)*cos(q5)^2*q2dot*sin(q6)*q3dot-2*Iy6s*cos(q6)*cos(q5)^2*q2dot*sin(q6)*q3dot*cos(q4)^2-2*Iy6s*sin(q6)*cos(q4)^2*q2dot*cos(q6)*q3dot-2*Iy6s*cos(q6)^2*cos(q5)*sin(q4)*q3dot*q5dot+2*Iy6s*cos(q6)^2*sin(q5)*q4dot*cos(q4)*q3dot-2*Iy6s*cos(q4)^2*q3dot*cos(q5)*q1dot*sin(q3+q2)-2*Ix6s*cos(q4)*q2dot*cos(q5)*sin(q4)*q3dot+Iz5s*sin(q5)*q4dot*cos(q4)*q2dot+Iz5s*sin(q5)*q4dot*sin(q4)*q1dot*sin(q3+q2)+2*Ix5s*cos(q5)*sin(q4)*q2dot*cos(q4)*q3dot+2*Ix6s*cos(q6)^2*cos(q5)*sin(q4)*q2dot*q5dot+2*Ix6s*cos(q6)^2*cos(q5)*sin(q4)*q3dot*q5dot-2*Ix6s*cos(q6)^2*sin(q5)*q1dot*cos(q3+q2)*q5dot+2*Ix6s*sin(q6)*cos(q4)*q2dot*cos(q6)*q5dot+2*Ix6s*sin(q6)*cos(q4)^2*q2dot*cos(q6)*q3dot-2*Ix6s*cos(q6)^2*sin(q5)*q4dot*cos(q4)*q3dot-2*Ix6s*cos(q6)^2*sin(q5)*q4dot*cos(q4)*q2dot+2*Ix6s*sin(q6)*cos(q4)*q3dot*cos(q6)*q5dot+2*Ix6s*cos(q6)*cos(q5)*sin(q4)*q2dot*sin(q6)*sin(q5)*q1dot*cos(q3+q2)+2*Ix6s*cos(q6)*cos(q5)^2*sin(q4)*q2dot*sin(q6)*cos(q4)*q1dot*sin(q3+q2)+2*Ix6s*cos(q6)*cos(q5)*sin(q4)*q2dot*sin(q6)*

63

sin(q5)*q4dot+2*Ix6s*cos(q6)*cos(q5)*sin(q4)*q3dot*sin(q6)*sin(q5)*q4dot+2*Ix6s*sin(q6)*cos(q4)*q2dot*cos(q6)*sin(q4)*q1dot*sin(q3+q2)-Ix6s*cos(q6)*cos(q5)^2*q2dot^2*sin(q6)-Iz6s*cos(q5)*q3dot*q1dot*sin(q3+q2)-Iz6s*sin(q5)*cos(q4)*q2dot*q1dot*cos(q3+q2)+Iz6s*q5dot*cos(q5)*sin(q4)*q2dot+Iz6s*q5dot*cos(q5)*sin(q4)*q3dot+Iz6s*sin(q5)*q4dot*cos(q4)*q3dot-Iz6s*cos(q5)*q2dot*q1dot*sin(q3+q2)-Ix6s*cos(q6)*q4dot^2*sin(q6)+Iy6s*cos(q6)*q4dot^2*sin(q6)+Ix6s*q5dot*sin(q5)*q4dot+Ix6s*sin(q6)*q1dot^2*cos(q6)+Ix6s*sin(q6)*q5dot^2*cos(q6)+Iz6s*sin(q5)*sin(q4)*q2ddot-Iz6s*q5dot*sin(q5)*q4dot+Iz6s*sin(q5)*sin(q4)*q3ddot-Iy6s*sin(q6)*q1dot^2*cos(q6)-Iy6s*q5dot*sin(q5)*q4dot+Iz6s*cos(q5)*q1ddot*cos(q3+q2)-Iz5s*q5dot*cos(q5)*cos(q4)*q1dot*sin(q3+q2)-Ix5s*q5dot*cos(q5)*cos(q4)*q1dot*sin(q3+q2)+2*Ix6s*q1dot*sin(q3+q2)*cos(q5)*q3dot*cos(q6)^2+2*Iy6s*cos(q4)*q2dot*cos(q5)*sin(q4)*q3dot-2*Ix6s*cos(q6)*q1dot*cos(q3+q2)*sin(q6)*q4dot-2*Ix6s*sin(q4)*q1dot^2*cos(q5)*cos(q4)*cos(q6)^2+2*Ix6s*sin(q4)*q1dot^2*cos(q5)*cos(q4)*cos(q6)^2*cos(q3+q2)^2+2*Iy6s*cos(q6)^2*cos(q5)*cos(q4)*q1dot^2*sin(q4)-2*Iy6s*cos(q6)^2*cos(q5)*cos(q4)*q1dot^2*sin(q4)*cos(q3+q2)^2-2*Iy6s*cos(q4)^2*q2dot*cos(q5)*q1dot*sin(q3+q2)+Iy6s*cos(q4)*q3dot^2*cos(q5)*sin(q4)+2*Ix6s*cos(q6)^2*cos(q5)*sin(q4)*q3dot^2*cos(q4)+2*Ix6s*cos(q6)*cos(q5)^2*sin(q4)*q3dot*sin(q6)*cos(q4)*q1dot*sin(q3+q2)+2*Ix6s*sin(q6)*sin(q4)*q1dot*sin(q3+q2)*cos(q6)*q5dot-2*Ix5s*cos(q5)*cos(q4)^2*q1dot*sin(q3+q2)*q2dot-Ix5s*sin(q5)*q1dot^2*cos(q3+q2)*sin(q4)*sin(q3+q2)-Ix5s*sin(q5)*cos(q4)*q3dot*q1dot*cos(q3+q2)-Iz5s*q5dot*sin(q5)*q1dot*cos(q3+q2)+Iz5s*q5dot*cos(q5)*sin(q4)*q2dot+Iy5s*sin(q5)*q4dot*cos(q4)*q2dot-Iy5s*cos(q5)*sin(q4)*q3dot^2*cos(q4)+Iy5s*sin(q5)*cos(q4)*q2dot*q1dot*cos(q3+q2)-2*Ix6s*cos(q6)*cos(q5)*cos(q4)*q1dot^2*sin(q3+q2)*sin(q6)*sin(q5)*cos(q3+q2)+2*Ix6s*cos(q6)*cos(q5)*sin(q4)*q3dot*sin(q6)*sin(q5)*q1dot*cos(q3+q2)-2*Ix6s*cos(q6)*cos(q5)*cos(q4)*q1dot*sin(q3+q2)*sin(q6)*sin(q5)*q4dot+4*Ix6s*cos(q6)^2*cos(q5)*sin(q4)*q2dot*cos(q4)*q3dot+2*Iy6s*cos(q6)*cos(q5)*cos(q4)*q1dot*sin(q3+q2)*sin(q6)*sin(q5)*q4dot-2*Iy6s*sin(q6)*cos(q4)*q3dot*cos(q6)*q5dot-4*Iy6s*cos(q6)^2*cos(q5)*sin(q4)*q2dot*cos(q4)*q3dot-2*Iy6s*sin(q6)*cos(q4)*q2dot*cos(q6)*q5dot-2*Ix6s*cos(q6)^2*cos(q5)*cos(q4)*q1dot*sin(q3+q2)*q5dot-4*Ix6s*cos(q6)^2*cos(q5)*cos(q4)^2*q1dot*sin(q3+q2)*q3dot-4*Ix6s*cos(q6)^2*cos(q5)*cos(q4)^2*q1dot*sin(q3+q2)*q2dot-2*Iy6s*cos(q6)^2*cos(q5)*sin(q4)*q2dot^2*cos(q4)-2*Ix6s*cos(q6)*cos(q5)^2*q2dot*sin(q6)*q3dot+2*Ix6s*cos(q6)*cos(q5)^2*q2dot*sin(q6)*q3dot*cos(q4)^2-Ix6s*q1dot*sin(q3+q2)*cos(q5)*q2dot+Iy6s*cos(q4)*q2dot^2*cos(q5)*sin(q4)-Ix6s*cos(q4)*q2dot^2*cos(q5)*sin(q4)-Iz6s*sin(q5)*cos(q4)*q3dot*q1dot*cos(q3+q2)+Iz6s*sin(q5)*q4dot*sin(q4)*q1dot*sin(q3+q2)+2*Ix6s*q1dot*sin(q3+q2)*cos(q5)*q2dot*cos(q6)^2+4*Iy6s*cos(q6)^2*cos(q5)*cos(q4)^2*q1dot*sin(q3+q2)*q2dot-2*Iy6s*q1dot*sin(q3+q2)*cos(q5)*q2dot*cos(q6)^2+2*Iy6s*cos(q6)^2*sin(q5)*q4dot*cos(q4)*q2dot-2*Iy6s*cos(q6)*cos(q5)*sin(q4)*q2dot*sin(q6)*sin(q5)*q1dot*cos(q3+q2)-2*Iy6s*sin(q6)*cos(q4)*q2dot*cos(q6)*sin(q4)*q1dot*sin(q3+q2)+2*Iy6s*sin(q6)*q1dot^2*cos(q6)*cos(q3+q2)^2-2*Iy6s*sin(q6)*cos(q4)*q3dot*cos(q6)*sin(q4)*q1dot*sin(q3+q2)+Ix6s*cos(q6)*cos(q5)^2*q2dot^2*sin(q6)*cos(q4)^2-Iy6s*sin(q4)*q1dot*sin(q3+q2)*sin(q5)*q4dot-Iy6s*q5dot*cos(q5)*cos(q4)*q1dot*sin(q3+q2)+Iy6s*sin(q4)*q1dot^2*cos(q5)*cos(q4)*cos(q3+q2)^2-Iy5s*cos(q5)*sin(q4)*q2dot^2*cos(q4)-Iz6s*q5dot*cos(q5)*cos(q4)*q1dot*sin(q3+q2)+Iz5s*q5dot*cos(q5)*sin(q4)*q3dot-Ix5s*sin(q5)*cos(q4)*q2dot*q1dot*cos(q3+q2)-Iz5s*sin(q5)*cos(q4)*q3dot*q1dot*cos(q3+q2)+Iy6s*cos(q6)*cos(q5)^2*q2dot^2*sin(q6)-Iy6s*cos(q6)*cos(q5)^2*q2dot^2*sin(q6)*cos(q4)^2+Iy6s*q1dot*sin(q3+q2)*cos(q5)*q2dot+Ix6s*sin(q4)*q1dot^2*sin(q3+q2)*sin(q5)*cos(q3+q2)+Iy6s*q5dot*cos(q5)*sin(q4)*q3dot+Ix6s*sin(q4)*q1dot^2*cos(q5)*cos(q4)+4*Iy6s*cos(q6)^2*cos(q5)*cos(q4)^2*q1dot*sin(q3+q2)*q3dot+2*Iy6s*cos(q6)^2*sin(q5)*q1dot*cos(q3+q2)*q5dot+Ix6s*cos(q4)*q3dot*sin(q5)*q4dot+Ix6s*sin(q4)*q1dot*sin(q3+q2)*sin(q5)*q4dot+Ix6s*q5dot*cos(q5)*cos(q4)*q1dot*sin(q3+q2)+Iy6s*q5dot*cos(q5)*sin(q4)*q2dot+Iy6s*cos(q6)*cos(q5)^2*cos(q4)^2*q1dot^2*sin(q6)+Ix6s*cos(q6)*q4dot^2*sin(q6)*cos(q5)^2-Iy6s*cos(q6)*cos(q5)^2*cos(q4)^2*q1dot^2*sin(q6)*cos(q3+q2)^2-Ix6s*sin(q4)*q1dot^2*cos(q5)*cos(q4)*cos(q3+q2)^2+Ix6s*cos(q4)*q2dot*sin(q5)*q1dot*cos(q3+q2)+Ix6s*cos(q4)*q2dot*sin(q5)*q4dot-Iy6s*sin(q4)*q1dot^2*sin(q3+q2)*sin(q5)*cos(q3+q2)-Ix6s*cos(q6)*cos(q5)^2*q3dot^2*sin(q6)+Ix6s*cos(q6)*cos(q5)^2*q3dot^2*sin(q6)*cos(q4)^2+Iy6s*cos(q6)*cos(q5)^2*q3dot^2*sin(q6)-Iy6s*cos(q6)*cos(q5)^2*q3dot^2*sin(q6)*cos(q4)^2+Ix6s*cos(q4)*q3dot*sin(q5)*q1dot*cos(q3+q2)-Iy6s*sin(q6)*q1dot^2*cos(q6)*cos(q3+q2)^2*cos(q5)^2-Iy6s*cos(q4)*q2dot*sin(q5)*q1dot*cos(q3+q2)-Ix6s*q1dot*sin(q3+q2)*cos(q5)*q3dot-Ix6s*cos(q6)*cos(q5)^2*cos(q4)^2*q1dot^2*sin(q6)+Ix6s*cos(q6)*cos(q5)^2*cos(q4)^2*q1dot^2*sin(q6)*cos(q3+q2)^2+Iy6s*q1dot*sin(q3+q2)*cos(q5)*q3dot-Iy6s*cos(q4)*q3dot*sin(q5)*q4dot+Ix6s*cos(q6)*q1dot^2*cos(q3+q2)^2*sin(q6)*cos(q5)^2-Iy6s*cos(q4)*q3dot*sin(q5)*q1dot*cos(q3+q2)-Ix6s*q5dot*cos(q5)*sin(q4)*q2dot-Iy6s*cos(q4)*q2dot*sin(q5)*q4dot-Iy6s*q5dot*sin(q5)*q1dot*cos(q3+q2)-2*Iy6s*cos(q6)*cos(q5)*sin(q4)*q3dot*sin(q6)*sin(q5)*q1dot*cos(q3+q2)-2*Iy6s*cos(q6)*cos(q5)^2*sin(q4)*q2dot*sin(q6)*cos(q4)*q1dot*

64

sin(q3+q2)+Iy5s*sin(q5)*q4dot*cos(q4)*q3dot+Iy5s*sin(q5)*q1dot^2*cos(q3+q2)*sin(q4)*sin(q3+q2)-Iz5s*sin(q5)*cos(q4)*q1ddot*sin(q3+q2)-Iz5s*sin(q5)*cos(q4)*q2dot*q1dot*cos(q3+q2)-Ix5s*sin(q5)*q4dot*sin(q4)*q1dot*sin(q3+q2)-Ix5s*sin(q5)*q4dot*cos(q4)*q2dot+Iz6s*cos(q5)*q4ddot+Iz5s*cos(q5)*q4ddot-2*Iy6s*cos(q6)*cos(q5)*sin(q4)*q2dot*sin(q6)*sin(q5)*q4dot-2*Iy6s*cos(q6)*cos(q5)*sin(q4)*q3dot*sin(q6)*sin(q5)*q4dot+2*Iy6s*cos(q6)^2*sin(q5)*q1dot*cos(q3+q2)*cos(q4)*q2dot n44 = Iy6s*cos(q4)*q3ddot+Iy6s*cos(q4)*q2ddot-Iy6s*q5ddot*cos(q6)^2+Iy6s*q5ddot+Iy5s*sin(q5)*cos(q4)^2*q1dot^2*cos(q5)+Iz5s*sin(q4)*q2dot*q1dot*cos(q3+q2)-Iy5s*sin(q5)*cos(q4)^2*q1dot^2*cos(q5)*cos(q3+q2)^2+2*Iy5s*sin(q5)*q2dot*cos(q5)*q3dot+2*Iy5s*cos(q5)^2*q4dot*sin(q4)*q3dot+2*Iy5s*cos(q5)^2*q4dot*sin(q4)*q2dot+2*Iz5s*cos(q5)*q1dot*cos(q3+q2)*sin(q5)*q4dot-Iy5s*sin(q5)*q3dot^2*cos(q5)*cos(q4)^2-2*Iz5s*sin(q5)*q2dot*cos(q5)*q3dot+cos(q6)*Iy6s*sin(q5)*sin(q4)*q2dot^2*sin(q6)*cos(q4)+cos(q6)*Ix6s*sin(q6)*cos(q5)*cos(q4)*q1ddot*sin(q3+q2)-cos(q6)*Ix6s*sin(q6)*cos(q5)*sin(q4)*q2ddot-cos(q6)*Ix6s*sin(q6)*cos(q5)*sin(q4)*q3ddot-2*Iy5s*sin(q5)*cos(q4)*q1dot*sin(q3+q2)*cos(q5)*sin(q4)*q2dot+Ix6s*cos(q6)^2*q5ddot-Ix5s*q4dot*sin(q4)*q3dot-Ix5s*q4dot*sin(q4)*q2dot+Ix4s*sin(q4)*q2dot*q1dot*cos(q3+q2)+Ix4s*sin(q4)*q3dot*q1dot*cos(q3+q2)+Ix5s*sin(q4)*q1ddot*sin(q3+q2)-2*Iy5s*sin(q5)*cos(q4)*q1dot*sin(q3+q2)*cos(q5)*sin(q4)*q3dot+2*Iz5s*sin(q5)*q2dot*cos(q5)*q3dot*cos(q4)^2-2*Iy5s*sin(q5)*q2dot*cos(q5)*q3dot*cos(q4)^2-Iy5s*sin(q4)*q2dot*q4dot-2*Iy5s*cos(q5)*q1dot*cos(q3+q2)*sin(q5)*q4dot-2*Iz5s*cos(q5)^2*q4dot*sin(q4)*q2dot+2*Iy5s*cos(q5)^2*q1dot*cos(q3+q2)*sin(q4)*q3dot+Iz5s*sin(q4)*q3dot*q4dot+Iy5s*sin(q5)*q3dot^2*cos(q5)-2*Iy5s*cos(q5)^2*q1dot^2*cos(q3+q2)*cos(q4)*sin(q3+q2)+2*Iy5s*cos(q5)^2*q1dot*cos(q3+q2)*sin(q4)*q2dot-2*Iy5s*cos(q5)^2*q4dot*cos(q4)*q1dot*sin(q3+q2)-Iy5s*cos(q5)*q4dot^2*sin(q5)+Iz5s*cos(q5)*q4dot^2*sin(q5)+cos(q6)*Iy6s*sin(q5)*cos(q4)*q1dot^2*sin(q6)*sin(q4)*cos(q3+q2)^2+2*Iz5s*cos(q5)^2*q1dot^2*cos(q3+q2)*cos(q4)*sin(q3+q2)+2*Iz5s*cos(q5)^2*q4dot*cos(q4)*q1dot*sin(q3+q2)-2*sin(q6)*Ix6s*q6dot*cos(q6)*cos(q4)*q3dot+sin(q6)*Iy6s*cos(q6)*cos(q5)*sin(q4)*q2ddot+Iz5s*sin(q4)*q2dot*q4dot+Iy5s*sin(q5)*q2dot^2*cos(q5)+Iy5s*cos(q4)*q1dot^2*sin(q3+q2)*cos(q3+q2)+2*Iz5s*sin(q5)*cos(q4)*q1dot*sin(q3+q2)*cos(q5)*sin(q4)*q3dot-2*Iz5s*cos(q5)^2*q4dot*sin(q4)*q3dot+2*Iz5s*sin(q5)*cos(q4)*q1dot*sin(q3+q2)*cos(q5)*sin(q4)*q2dot-Iy5s*sin(q5)*q2dot^2*cos(q5)*cos(q4)^2+Iy4s*q4dot*sin(q4)*q3dot-Iz4s*q4dot*sin(q4)*q2dot-Iz4s*q4dot*sin(q4)*q3dot-Ix6s*q6dot*cos(q5)*cos(q4)*q1dot*sin(q3+q2)+Ix4s*q4dot*cos(q4)*q1dot*sin(q3+q2)-Iy4s*q1dot^2*cos(q3+q2)*cos(q4)*sin(q3+q2)+Iy4s*sin(q4)*q2dot*q1dot*cos(q3+q2)+Iy4s*sin(q4)*q3dot*q1dot*cos(q3+q2)-Iy4s*q4dot*cos(q4)*q1dot*sin(q3+q2)+Iy4s*q4dot*sin(q4)*q2dot-Iy5s*sin(q4)*q3dot*q4dot+Iz4s*q1dot^2*cos(q3+q2)*cos(q4)*sin(q3+q2)+2*sin(q6)*Iy6s*cos(q6)*sin(q5)*q2dot*q1dot*sin(q3+q2)+2*sin(q6)*Iy6s*cos(q6)*sin(q5)*q3dot*q1dot*sin(q3+q2)+Iz4s*q4dot*cos(q4)*q1dot*sin(q3+q2)-Iz4s*sin(q4)*q2dot*q1dot*cos(q3+q2)-Iz4s*sin(q4)*q3dot*q1dot*cos(q3+q2)-cos(q6)*l4*fy7+2*Iy6s*cos(q5)^2*q4dot*cos(q6)^2*sin(q4)*q3dot+2*Iy6s*sin(q5)*q2dot*cos(q6)^2*cos(q5)*q3dot-Iy6s*sin(q5)*q3dot^2*cos(q6)^2*cos(q5)*cos(q4)^2+Iz6s*q6dot*cos(q5)*cos(q4)*q1dot*sin(q3+q2)+Ix6s*cos(q4)*q1dot^2*sin(q3+q2)*cos(q3+q2)-Ix6s*cos(q4)*q1dot^2*sin(q3+q2)*cos(q3+q2)*cos(q6)^2+Iz6s*sin(q4)*q3dot*q1dot*cos(q3+q2)+Iy6s*sin(q4)*q3dot*q1dot*cos(q3+q2)-Ix6s*sin(q5)*q2dot^2*cos(q5)*cos(q6)^2+Iz6s*sin(q5)*q2dot^2*cos(q5)*cos(q4)^2+Iz6s*sin(q4)*q2dot*q1dot*cos(q3+q2)+Ix6s*cos(q4)*q1dot*sin(q3+q2)*q4dot-Iy6s*sin(q4)*q1ddot*sin(q3+q2)*cos(q6)^2-Iz6s*cos(q4)*q1dot^2*sin(q3+q2)*cos(q3+q2)-2*Ix6s*cos(q5)^2*q1dot^2*cos(q3+q2)*cos(q4)*sin(q3+q2)+2*Ix6s*cos(q5)^2*q1dot^2*cos(q3+q2)*cos(q4)*sin(q3+q2)*cos(q6)^2-2*Ix6s*sin(q5)*cos(q4)*q1dot*sin(q3+q2)*cos(q5)*sin(q4)*q3dot+2*Ix6s*sin(q5)*cos(q4)*q1dot*sin(q3+q2)*cos(q5)*sin(q4)*q3dot*cos(q6)^2+Iz6s*sin(q5)*q3dot^2*cos(q5)*cos(q4)^2+Ix6s*cos(q5)*q4dot^2*sin(q5)*cos(q6)^2+2*Ix6s*cos(q5)^2*q1dot*cos(q3+q2)*sin(q4)*q2dot+2*Ix6s*sin(q5)*q2dot*cos(q5)*q3dot-2*Iz5s*cos(q5)^2*q1dot*cos(q3+q2)*sin(q4)*q2dot-2*Iz5s*cos(q5)^2*q1dot*cos(q3+q2)*sin(q4)*q3dot+Iy5s*cos(q4)*q1dot*sin(q3+q2)*q4dot+cos(q6)*Iy6s*cos(q5)*q1dot^2*cos(q3+q2)*sin(q6)*sin(q4)*sin(q3+q2)-2*sin(q6)*Ix6s*sin(q5)*q2dot*cos(q6)*q1dot*sin(q3+q2)+2*Ix6s*cos(q6)^2*sin(q4)*q3dot*q1dot*cos(q3+q2)+Ix6s*cos(q6)^2*sin(q4)*q1ddot*sin(q3+q2)+Iy6s*sin(q5)*q3dot^2*cos(q6)^2*cos(q5)+Iy6s*sin(q5)*q2dot^2*cos(q6)^2*cos(q5)+Ix5s*cos(q4)*q2ddot+Iz5s*sin(q5)*q2dot^2*cos(q5)*cos(q4)^2+Iz5s*sin(q4)*q3dot*q1dot*cos(q3+q2)-Iz5s*sin(q5)*cos(q4)^2*q1dot^2*cos(q5)+Iz5s*sin(q5)*cos(q4)^2*q1dot^2*cos(q5)*cos(q3+q2)^2-Iy5s*sin(q4)*q2dot*q1dot*cos(q3+q2)-Iy5s*sin(q4)*q3dot*q1dot*cos(q3+q2)-Iz5s*cos(q4)*q1dot*sin(q3+q2)*q4dot+Ix5s*sin(q4)*q3dot*q1dot*cos(q3+q2)+

65

Iy6s*sin(q5)*cos(q4)^2*q1dot^2*cos(q6)^2*cos(q5)+2*Ix6s*q6dot*cos(q6)^2*sin(q5)*q4dot-2*Iy6s*sin(q5)*sin(q4)*q2dot*cos(q6)^2*cos(q5)*cos(q4)*q1dot*sin(q3+q2)-2*Iy6s*sin(q5)*sin(q4)*q3dot*cos(q6)^2*cos(q5)*cos(q4)*q1dot*sin(q3+q2)+2*Iy6s*cos(q5)^2*q1dot*cos(q3+q2)*cos(q6)^2*sin(q4)*q2dot+2*Iy6s*q6dot*cos(q6)^2*cos(q5)*sin(q4)*q2dot+2*Iy6s*q6dot*cos(q6)^2*cos(q5)*sin(q4)*q3dot+Ix5s*sin(q4)*q2dot*q1dot*cos(q3+q2)+Ix5s*q4dot*cos(q4)*q1dot*sin(q3+q2)-sin(q6)*l4*fx7-cos(q6)*Iy6s*sin(q5)*cos(q4)*q1dot^2*sin(q6)*sin(q4)+2*sin(q6)*Iy6s*q6dot*cos(q6)*cos(q4)*q2dot-2*sin(q6)*Ix6s*sin(q5)*q3dot*cos(q6)*q1dot*sin(q3+q2)-sin(q6)*Ix6s*sin(q5)*sin(q4)*q2dot^2*cos(q6)*cos(q4)+2*sin(q6)*Iy6s*q6dot*cos(q6)*cos(q4)*q3dot+2*sin(q6)*Iy6s*q6dot*cos(q6)*sin(q4)*q1dot*sin(q3+q2)-sin(q6)*Iy6s*cos(q6)*sin(q5)*q4ddot+Iz5s*sin(q5)*q3dot^2*cos(q5)*cos(q4)^2-Iz5s*cos(q4)*q1dot^2*sin(q3+q2)*cos(q3+q2)+Ix5s*cos(q4)*q3ddot-Iy5s*cos(q5)*q1dot^2*cos(q3+q2)^2*sin(q5)-2*Iy6s*q6dot*cos(q6)^2*cos(q5)*cos(q4)*q1dot*sin(q3+q2)-2*Iy6s*q6dot*cos(q6)^2*sin(q5)*q1dot*cos(q3+q2)+2*Iy6s*cos(q5)^2*q1dot*cos(q3+q2)*cos(q6)^2*sin(q4)*q3dot-2*Iy6s*cos(q5)^2*q1dot^2*cos(q3+q2)*cos(q6)^2*cos(q4)*sin(q3+q2)-2*sin(q6)*Ix6s*cos(q6)*cos(q5)*q4dot*cos(q4)*q2dot-2*sin(q6)*Ix6s*cos(q6)*cos(q5)*q4dot*cos(q4)*q3dot+2*sin(q6)*Ix6s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*cos(q6)*q2dot+2*sin(q6)*Ix6s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*cos(q6)*q3dot+sin(q6)*Ix6s*sin(q5)*cos(q4)*q1dot^2*cos(q6)*sin(q4)+2*sin(q6)*Iy6s*cos(q6)*cos(q5)*q4dot*cos(q4)*q3dot-2*sin(q6)*Ix6s*sin(q5)*sin(q4)*q2dot*cos(q6)*cos(q4)*q3dot-sin(q6)*ny7+2*Iz6s*sin(q5)*cos(q4)*q1dot*sin(q3+q2)*cos(q5)*sin(q4)*q2dot-Ix6s*sin(q4)*q2dot*q1dot*cos(q3+q2)-Ix6s*sin(q5)*q3dot^2*cos(q5)*cos(q6)^2+Iz6s*sin(q5)*cos(q4)^2*q1dot^2*cos(q5)*cos(q3+q2)^2+Ix6s*q6dot*cos(q5)*sin(q4)*q2dot+Iz5s*cos(q5)*q1dot^2*cos(q3+q2)^2*sin(q5)+Ix5s*q5ddot-2*sin(q6)*Ix6s*q6dot*cos(q6)*q5dot+2*sin(q6)*+Iy6s*q6dot*cos(q6)*q5dot-Iy6s*cos(q5)*q1dot^2*cos(q3+q2)^2*cos(q6)^2*sin(q5)+2*Iz6s*cos(q5)^2*q4dot*cos(q4)*q1dot*sin(q3+q2)-2*Iz6s*cos(q5)^2*q1dot*cos(q3+q2)*sin(q4)*q2dot-2*Ix6s*sin(q5)*cos(q4)*q1dot*sin(q3+q2)*cos(q5)*sin(q4)*q2dot+2*Ix6s*sin(q5)*cos(q4)*q1dot*sin(q3+q2)*cos(q5)*sin(q4)*q2dot*cos(q6)^2-2*Iz6s*sin(q5)*q2dot*cos(q5)*q3dot-2*Iz6s*cos(q5)^2*q4dot*sin(q4)*q2dot+2*Iz6s*cos(q5)^2*q1dot^2*cos(q3+q2)*cos(q4)*sin(q3+q2)+Ix6s*sin(q5)*cos(q4)^2*q1dot^2*cos(q5)-Ix6s*sin(q5)*cos(q4)^2*q1dot^2*cos(q5)*cos(q6)^2+Iy6s*q6dot*cos(q5)*cos(q4)*q1dot*sin(q3+q2)+Iy6s*q6dot*sin(q5)*q1dot*cos(q3+q2)-Iz6s*q6dot*cos(q5)*sin(q4)*q3dot-2*Iy6s*q6dot*cos(q6)^2*sin(q5)*q4dot-Iy6s*cos(q5)*q4dot^2*cos(q6)^2*sin(q5)+2*Ix6s*q6dot*cos(q6)^2*cos(q5)*cos(q4)*q1dot*sin(q3+q2)+2*Ix6s*q6dot*cos(q6)^2*sin(q5)*q1dot*cos(q3+q2)+Iy6s*cos(q4)*q1dot^2*sin(q3+q2)*cos(q6)^2*cos(q3+q2)+cos(q6)*Ix6s*sin(q6)*sin(q5)*q1ddot*cos(q3+q2)+cos(q6)*Ix6s*sin(q6)*sin(q5)*q4ddot+cos(q6)*Iy6s*sin(q5)*sin(q4)*q3dot^2*sin(q6)*cos(q4)+2*sin(q6)*Iy6s*cos(q6)*cos(q5)*q4dot*cos(q4)*q2dot+sin(q6)*Iy6s*cos(q6)*cos(q5)*sin(q4)*q3ddot-2*sin(q6)*Ix6s*q6dot*cos(q6)*sin(q4)*q1dot*sin(q3+q2)-2*Iy6s*cos(q5)^2*q4dot*cos(q6)^2*cos(q4)*q1dot*sin(q3+q2)+2*Iy6s*cos(q5)^2*q4dot*cos(q6)^2*sin(q4)*q2dot+2*cos(q6)*Iy6s*sin(q5)*sin(q4)*q2dot*sin(q6)*cos(q4)*q3dot-sin(q6)*Ix6s*sin(q5)*cos(q4)*q1dot^2*cos(q6)*sin(q4)*cos(q3+q2)^2+2*sin(q6)*Iy6s*cos(q6)*cos(q5)*q4dot*sin(q4)*q1dot*sin(q3+q2)-2*Iy6s*cos(q5)*q1dot*cos(q3+q2)*cos(q6)^2*sin(q5)*q4dot+Ix6s*cos(q6)^2*cos(q4)*q2ddot+Ix6s*cos(q6)^2*cos(q4)*q3ddot+cos(q6)*nx7-Iz5s*sin(q5)*q2dot^2*cos(q5)+Ix4s*cos(q4)*q3ddot+Iz6s*cos(q5)*q4dot^2*sin(q5)+Ix6s*sin(q5)*q2dot^2*cos(q5)+Iz6s*sin(q4)*q3dot*q4dot+Iy6s*sin(q4)*q1ddot*sin(q3+q2)-Ix6s*cos(q5)*q4dot^2*sin(q5)+Ix6s*sin(q5)*q3dot^2*cos(q5)-Iy6s*cos(q4)*q2ddot*cos(q6)^2-Iy6s*cos(q4)*q3ddot*cos(q6)^2-Ix6s*sin(q4)*q3dot*q4dot-Ix6s*q6dot*sin(q5)*q4dot-Iz6s*sin(q5)*q2dot^2*cos(q5)-2*cos(q6)*Iy6s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*sin(q6)*q2dot-2*cos(q6)*Iy6s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*sin(q6)*q3dot-2*Iy6s*sin(q5)*q2dot*cos(q6)^2*cos(q5)*q3dot*cos(q4)^2-2*Ix6s*q6dot*cos(q6)^2*cos(q5)*sin(q4)*q2dot-2*Ix6s*q6dot*cos(q6)^2*cos(q5)*sin(q4)*q3dot-Ix6s*sin(q5)*q3dot^2*cos(q5)*cos(q4)^2+Ix6s*sin(q5)*q3dot^2*cos(q5)*cos(q4)^2*cos(q6)^2-Ix6s*cos(q5)*q1dot^2*cos(q3+q2)^2*sin(q5)+Ix6s*cos(q5)*q1dot^2*cos(q3+q2)^2*sin(q5)*cos(q6)^2-Ix6s*q6dot*sin(q5)*q1dot*cos(q3+q2)-2*Ix6s*cos(q5)^2*q4dot*cos(q4)*q1dot*sin(q3+q2)+2*Ix6s*cos(q5)^2*q4dot*cos(q4)*q1dot*sin(q3+q2)*cos(q6)^2+2*Ix6s*cos(q5)^2*q4dot*sin(q4)*q3dot-2*Ix6s*cos(q5)^2*q4dot*sin(q4)*q3dot*cos(q6)^2-Iy6s*q6dot*cos(q5)*sin(q4)*q2dot-Iy6s*q6dot*cos(q5)*sin(q4)*q3dot-Iz6s*sin(q5)*cos(q4)^2*q1dot^2*cos(q5)-Iz6s*q6dot*cos(q5)*sin(q4)*q2dot-Iy6s*q4dot*sin(q4)*q2dot+Iz6s*sin(q4)*q2dot*q4dot-Iz6s*sin(q5)*q3dot^2*cos(q5)-Iy6s*q4dot*sin(q4)*q3dot+Iy6s*q6dot*sin(q5)*q4dot+Iz6s*q6dot*sin(q5)*q4dot-Iz5s*sin(q5)*q3dot^2*cos(q5)+2*Iz6s*sin(q5)*cos(q4)*q1dot*sin(q3+q2)*cos(q5)*sin(q4)*q3dot-Iz6s*cos(q4)*q1dot*sin(q3+q2)*q4dot-2*Ix6s*cos(q5)^2*q1dot*cos(q3+q2)*sin(q4)*q2dot*cos(q6)^2-2*Ix6s*sin(q5)*q2dot*cos(q5)*q3dot*cos(q6)^2-2*Ix6s*sin(q5)*q2dot*cos(q5)*q3dot*cos(q4)^2+2*Ix6s*

66

sin(q5)*q2dot*cos(q5)*q3dot*cos(q4)^2*cos(q6)^2+2*Iz6s*sin(q5)*q2dot*cos(q5)*q3dot*cos(q4)^2-Ix6s*sin(q4)*q3dot*q1dot*cos(q3+q2)+2*Ix6s*cos(q5)^2*q1dot*cos(q3+q2)*sin(q4)*q3dot-2*Ix6s*cos(q5)^2*q1dot*cos(q3+q2)*sin(q4)*q3dot*cos(q6)^2+2*Ix6s*cos(q6)^2*sin(q4)*q2dot*q1dot*cos(q3+q2)-2*Iy6s*sin(q4)*q2dot*cos(q6)^2*q1dot*cos(q3+q2)-Iy6s*sin(q5)*q2dot^2*cos(q6)^2*cos(q5)*cos(q4)^2-2*Iy6s*sin(q4)*q3dot*cos(q6)^2*q1dot*cos(q3+q2)-Iy6s*sin(q5)*cos(q4)^2*q1dot^2*cos(q6)^2*cos(q5)*cos(q3+q2)^2-sin(q6)*Ix6s*sin(q5)*sin(q4)*q3dot^2*cos(q6)*cos(q4)-2*sin(q6)*Ix6s*q6dot*cos(q6)*cos(q4)*q2dot-sin(q6)*Ix6s*cos(q5)*q1dot^2*cos(q3+q2)*cos(q6)*sin(q4)*sin(q3+q2)-sin(q6)*Iy6s*cos(q6)*cos(q5)*cos(q4)*q1ddot*sin(q3+q2)-sin(q6)*Iy6s*cos(q6)*sin(q5)*q1ddot*cos(q3+q2)-2*sin(q6)*Ix6s*cos(q6)*cos(q5)*q4dot*sin(q4)*q1dot*sin(q3+q2)+Ix6s*q6dot*cos(q5)*sin(q4)*q3dot+Iz6s*cos(q5)*q1dot^2*cos(q3+q2)^2*sin(q5)+2*Ix6s*cos(q5)^2*q4dot*sin(q4)*q2dot-2*Ix6s*cos(q5)^2*q4dot*sin(q4)*q2dot*cos(q6)^2-2*Iz6s*cos(q5)^2*q1dot*cos(q3+q2)*sin(q4)*q3dot-Ix6s*sin(q5)*q2dot^2*cos(q5)*cos(q4)^2+Ix6s*sin(q5)*q2dot^2*cos(q5)*cos(q4)^2*cos(q6)^2-2*Ix6s*cos(q5)*q1dot*cos(q3+q2)*sin(q5)*q4dot+2*Ix6s*cos(q5)*q1dot*cos(q3+q2)*sin(q5)*q4dot*cos(q6)^2-2*Iz6s*cos(q5)^2*q4dot*sin(q4)*q3dot+Iz6s*q6dot*sin(q5)*q1dot*cos(q3+q2)+Iy6s*q4dot*cos(q4)*q1dot*sin(q3+q2)+Iy6s*sin(q4)*q2dot*q1dot*cos(q3+q2)-Ix6s*sin(q5)*cos(q4)^2*q1dot^2*cos(q5)*cos(q3+q2)^2+Ix6s*sin(q5)*cos(q4)^2*q1dot^2*cos(q5)*cos(q3+q2)^2*cos(q6)^2+2*Iz6s*cos(q5)*q1dot*cos(q3+q2)*sin(q5)*q4dot-Ix6s*sin(q4)*q2dot*q4dot+Ix4s*sin(q4)*q1ddot*sin(q3+q2)-Ix4s*q4dot*sin(q4)*q2dot-Ix4s*q4dot*sin(q4)*q3dot+Ix4s*cos(q4)*q2ddot -sin(q5)*nz7+Iy4s*cos(q4)*q1ddot*sin(q3+q2)-Iy4s*q4dot*cos(q4)*q2dot-Iy4s*q4dot*cos(q4)*q3dot+Ix4s*q4dot*cos(q4)*q2dot-Iy4s*sin(q4)*q2ddot-sin(q5)*Iz6s*cos(q5)*q1ddot*cos(q3+q2)+sin(q5)*Iy6s*sin(q6)*q5dot^2*cos(q6)-sin(q5)*Ix6s*sin(q4)*q1dot^2*cos(q5)*cos(q4)-sin(q5)*Ix6s*sin(q6)*q1dot^2*cos(q6)*cos(q4)^2*cos(q3+q2)^2-sin(q5)*Ix6s*sin(q6)*cos(q4)^2*q2dot^2*cos(q6)-sin(q5)*Iy6s*sin(q6)*q1dot^2*cos(q6)*cos(q4)^2-sin(q5)*Ix6s*sin(q6)*cos(q4)^2*q3dot^2*cos(q6)-sin(q5)*Iz5s*cos(q5)*q1ddot*cos(q3+q2)-Iy4s*sin(q4)*q3ddot+Ix5s*q5dot*q4dot-Iy4s*q4dot*sin(q4)*q1dot*sin(q3+q2)+Ix4s*cos(q4)*q2dot*q1dot*cos(q3+q2)+Ix4s*cos(q4)*q3dot*q1dot*cos(q3+q2)-Iz5s*cos(q4)*q1ddot*sin(q3+q2)*cos(q5)^2+Iz5s*cos(q4)*q2dot*q1dot*cos(q3+q2)-sin(q5)*Iy6s*cos(q6)*q4dot^2*sin(q6)-sin(q5)*Ix6s*sin(q6)*q1dot^2*cos(q6)-sin(q5)*Ix6s*sin(q6)*q5dot^2*cos(q6)-sin(q5)*Iy5s*cos(q5)*cos(q4)*q1dot^2*sin(q4)+2*sin(q5)*Iy6s*sin(q6)*sin(q4)*q1dot*sin(q3+q2)*cos(q6)*q5dot+sin(q5)*Iy6s*sin(q6)*cos(q4)^2*q3dot^2*cos(q6)+sin(q5)*Iy6s*sin(q6)*cos(q4)^2*q2dot^2*cos(q6)+sin(q5)*Iy6s*sin(q6)*q1dot^2*cos(q6)*cos(q4)^2*cos(q3+q2)^2+2*sin(q5)*Iy6s*cos(q6)^2*cos(q5)*sin(q4)*q2dot*q5dot-2*sin(q5)*Iy6s*cos(q6)*q1dot*cos(q3+q2)*sin(q6)*q4dot+sin(q5)*Iy5s*cos(q5)*cos(q4)*q1dot^2*sin(q4)*cos(q3+q2)^2-2*sin(q5)*Iy5s*cos(q5)*cos(q4)^2*q1dot*sin(q3+q2)*q2dot+sin(q5)*Ix6s*sin(q6)*q1dot^2*cos(q6)*cos(q4)^2+sin(q5)*Ix6s*cos(q4)*q3dot^2*cos(q5)*sin(q4)-2*sin(q5)*Iy6s*cos(q6)^2*cos(q5)*cos(q4)*q1dot*sin(q3+q2)*q5dot-2*sin(q5)*Ix6s*cos(q4)^2*q2dot*cos(q5)*q1dot*sin(q3+q2)-2*sin(q5)*Ix6s*q1dot*sin(q3+q2)*cos(q5)*q3dot*cos(q4)^2-2*sin(q5)*Ix6s*sin(q6)*cos(q4)*q3dot*cos(q6)*sin(q4)*q1dot*sin(q3+q2)+Iy4s*cos(q4)*q2dot*q1dot*cos(q3+q2)+Iy4s*cos(q4)*q3dot*q1dot*cos(q3+q2)+Ix4s*q4dot*sin(q4)*q1dot*sin(q3+q2)-Iz4s*q1dot^2*cos(q3+q2)*sin(q4)*sin(q3+q2)-Iz4s*q4dot*sin(q4)*q1dot*sin(q3+q2)+cos(q5)*cos(q6)*ny7+cos(q5)*sin(q6)*nx7-sin(q5)*Iz6s*q6ddot+2*cos(q5)*Iy5s*q5dot*sin(q5)*sin(q4)*q2dot+2*cos(q5)*Iy5s*q5dot*sin(q5)*sin(q4)*q3dot+2*cos(q5)*Iz5s*q5dot*sin(q5)*cos(q4)*q1dot*sin(q3+q2)-2*cos(q5)*Iy6s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*q2dot*cos(q6)^2-2*cos(q5)*Iy6s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*q3dot*cos(q6)^2+2*cos(q5)*Iy6s*sin(q5)*sin(q4)*q2dot*cos(q4)*q3dot*cos(q6)^2+cos(q5)*cos(q6)*Ix6s*cos(q4)*q1dot^2*sin(q3+q2)*sin(q6)*cos(q3+q2)-2*cos(q5)*Iz6s*sin(q5)*sin(q4)*q2dot*cos(q4)*q3dot+2*cos(q5)*Iz6s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*q3dot+cos(q5)*Iy6s*sin(q5)*sin(q4)*q3dot^2*cos(q4)*cos(q6)^2-cos(q5)*Ix6s*sin(q5)*sin(q4)*q3dot^2*cos(q6)^2*cos(q4)+cos(q5)*Iz5s*sin(q5)*cos(q4)*q1dot^2*sin(q4)-cos(q5)*Ix6s*q6dot*cos(q4)*q2dot-cos(q5)*Iz6s*q6dot*cos(q4)*q2dot-cos(q5)*Ix6s*q6dot*cos(q4)*q3dot+Ix4s*q1dot^2*cos(q3+q2)*sin(q4)*sin(q3+q2)+Ix4s*q4dot*cos(q4)*q3dot-Iz4s*q4dot*cos(q4)*q2dot-Iz4s*q4dot*cos(q4)*q3dot-Iz6s*cos(q5)^2*q1dot^2*cos(q3+q2)*sin(q4)*sin(q3+q2)-Iz4s*cos(q4)*q2dot*q1dot*cos(q3+q2)-Iz4s*cos(q4)*q3dot*q1dot*cos(q3+q2)-sin(q5)*Iz6s*cos(q5)*q4ddot-sin(q5)*Iz5s*cos(q5)*q4ddot-cos(q5)*Iz6s*q6dot*cos(q4)*q3dot-2*cos(q5)*Iy6s*q6dot*cos(q6)^2*q5dot-cos(q5)*Ix6s*sin(q5)*q4ddot*cos(q6)^2+2*cos(q5)*Ix6s*q6dot*cos(q6)^2*q5dot-cos(q5)*cos(q6)*Iy6s*sin(q6)*q5ddot-cos(q5)*Ix6s*sin(q5)*cos(q4)*

67

q1dot^2*cos(q6)^2*sin(q4)*cos(q3+q2)^2-cos(q5)*Iz6s*sin(q5)*sin(q4)*q3dot^2*cos(q4)-2*cos(q5)*Iz6s*q5dot*sin(q5)*sin(q4)*q3dot-cos(q5)*Ix6s*q6dot*sin(q4)*q1dot*sin(q3+q2)-cos(q5)*Iz6s*sin(q5)*sin(q4)*q2dot^2*cos(q4)+2*cos(q5)*Iz6s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*q2dot-2*cos(q5)*Ix6s*q5dot*sin(q5)*cos(q4)*q1dot*sin(q3+q2)-cos(q5)*Iz6s*sin(q5)*cos(q4)*q1dot^2*sin(q4)*cos(q3+q2)^2-2*cos(q5)*Iz6s*q5dot*sin(q5)*sin(q4)*q2dot+cos(q5)*Iy6s*sin(q5)*cos(q4)*q1dot^2*sin(q4)*cos(q3+q2)^2*cos(q6)^2-cos(q5)*Iz5s*sin(q5)*cos(q4)*q1dot^2*sin(q4)*cos(q3+q2)^2-Ix6s*cos(q5)^2*q1dot^2*cos(q3+q2)*cos(q6)^2*sin(q4)*sin(q3+q2)+2*Iy5s*q5dot*cos(q5)^2*q1dot*cos(q3+q2)+Iy6s*cos(q6)^2*cos(q5)^2*cos(q4)*q1ddot*sin(q3+q2)+Iy5s*cos(q5)^2*cos(q4)*q1ddot*sin(q3+q2)-cos(q5)*Iz6s*q6dot*sin(q4)*q1dot*sin(q3+q2)+cos(q5)*Iy6s*sin(q5)*sin(q4)*q2dot^2*cos(q4)*cos(q6)^2-2*cos(q5)*Iy5s*q5dot*sin(q5)*cos(q4)*q1dot*sin(q3+q2)-cos(q5)*Iz5s*sin(q5)*sin(q4)*q3dot^2*cos(q4)+2*cos(q5)*Ix6s*q6dot*cos(q6)^2*sin(q4)*q1dot*sin(q3+q2)-2*cos(q5)*Iy6s*q6dot*cos(q6)^2*cos(q4)*q2dot-2*cos(q5)*Iy6s*q6dot*cos(q6)^2*cos(q4)*q3dot+cos(q5)*Iy6s*q6dot*sin(q4)*q1dot*sin(q3+q2)+cos(q5)*Iz6s*sin(q5)*cos(q4)*q1dot^2*sin(q4)cos(q5)*Ix6s*sin(q5)*sin(q4)*q2dot^2*cos(q6)^2*cos(q4)+2*cos(q5)*Ix6s*q5dot*sin(q5)*sin(q4)*q2dot-2*cos(q5)*Iz5s*q5dot*sin(q5)*sin(q4)*q3dot-cos(q5)*Ix6s*sin(q5)*q1ddot*cos(q3+q2)*cos(q6)^2-2*cos(q5)*Iz5s*sin(q5)*sin(q4)*q2dot*cos(q4)*q3dot+2*cos(q5)*Ix6s*q6dot*cos(q6)^2*cos(q4)*q3dot+2*cos(q5)*Ix6s*q6dot*cos(q6)^2*cos(q4)*q2dot-cos(q5)*cos(q6)*Iy6s*sin(q6)*cos(q4)*q3ddot-2*cos(q5)*Ix6s*sin(q5)*sin(q4)*q2dot*cos(q6)^2*cos(q4)*q3dot+2*cos(q5)*Ix6s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*cos(q6)^2*q2dot+2*cos(q5)*Ix6s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*cos(q6)^2*q3dot+cos(q5)*Ix6s*sin(q5)*cos(q4)*q1dot^2*cos(q6)^2*sin(q4)+cos(q5)*sin(q6)*Ix6s*cos(q6)*cos(q4)*q2ddot+cos(q5)*sin(q6)*Ix6s*cos(q6)*cos(q4)*q3ddot-cos(q5)*sin(q6)*Iy6s*cos(q4)*q1dot^2*sin(q3+q2)*cos(q6)*cos(q3+q2)+2*cos(q5)*Ix6s*q5dot*sin(q5)*sin(q4)*q3dot-2*cos(q5)*Iy6s*q6dot*cos(q6)^2*sin(q4)*q1dot*sin(q3+q2)-2*Iz6s*q5dot*cos(q5)^2*q1dot*cos(q3+q2)-2*Iz6s*cos(q5)^2*cos(q4)*q2dot*q1dot*cos(q3+q2)+Iy6s*cos(q5)^2*q1dot^2*cos(q3+q2)*sin(q4)*sin(q3+q2)*cos(q6)^2+2*Iy5s*cos(q5)^2*cos(q4)*q3dot*q1dot*cos(q3+q2)+2*Iy5s*cos(q5)^2*cos(q4)*q2dot*q1dot*cos(q3+q2)+Ix6s*cos(q5)^2*sin(q4)*q3ddot*cos(q6)^2+Ix6s*cos(q5)^2*sin(q4)*q2ddot*cos(q6)^2-2*sin(q5)*Iy5s*cos(q5)*cos(q4)^2*q1dot*sin(q3+q2)*q3dot+2*sin(q5)*Iy5s*cos(q5)*sin(q4)*q2dot*cos(q4)*q3dot+2*sin(q5)*Ix6s*cos(q6)*q1dot^2*cos(q3+q2)^2*sin(q6)+2*sin(q5)*Iy6s*cos(q6)^2*cos(q5)*sin(q4)*q3dot*q5dot+2*sin(q5)*Ix6s*cos(q4)*q2dot*cos(q5)*sin(q4)*q3dot-2*sin(q5)*Ix6s*cos(q6)^2*cos(q5)*sin(q4)*q2dot*q5dot-2*sin(q5)*Ix6s*cos(q6)^2*cos(q5)*sin(q4)*q3dot*q5dot-2*sin(q5)*Ix6s*sin(q6)*cos(q4)*q2dot*cos(q6)*q5dot+sin(q5)*Ix6s*cos(q6)*q4dot^2*sin(q6)-2*sin(q5)*Ix6s*sin(q6)*cos(q4)^2*q2dot*cos(q6)*q3dot-2*sin(q5)*Ix6s*sin(q6)*cos(q4)*q3dot*cos(q6)*q5dot+2*sin(q5)*Iy6s*sin(q6)*cos(q4)^2*q2dot*cos(q6)*q3dot+2*sin(q5)*Ix6s*cos(q6)*q1dot*cos(q3+q2)*sin(q6)*q4dot-2*sin(q5)*Ix6s*sin(q6)*sin(q4)*q1dot*sin(q3+q2)*cos(q6)*q5dot-2*sin(q5)*Ix6s*sin(q6)*cos(q4)*q2dot*cos(q6)*sin(q4)*q1dot*sin(q3+q2)+sin(q5)*Iy6s*sin(q6)*q1dot^2*cos(q6)+2*sin(q5)*Iy6s*sin(q6)*cos(q4)*q3dot*cos(q6)*q5dot+2*sin(q5)*Iy6s*sin(q6)*cos(q4)*q2dot*cos(q6)*q5dot+2*sin(q5)*Ix6s*cos(q6)^2*cos(q5)*cos(q4)*q1dot*sin(q3+q2)*q5dot+sin(q5)*Ix6s*cos(q4)*q2dot^2*cos(q5)*sin(q4)+sin(q5)*Iy5s*cos(q5)*sin(q4)*q3dot^2*cos(q4)+sin(q5)*Ix6s*sin(q4)*q1dot^2*cos(q5)*cos(q4)*cos(q3+q2)^2+2*sin(q5)*Iy6s*sin(q6)*cos(q4)*q2dot*cos(q6)*sin(q4)*q1dot*sin(q3+q2)-2*sin(q5)*Iy6s*sin(q6)*q1dot^2*cos(q6)*cos(q3+q2)^2+2*sin(q5)*Iy6s*sin(q6)*cos(q4)*q3dot*cos(q6)*sin(q4)*q1dot*sin(q3+q2)-2*cos(q6)*Ix6s*q6dot*sin(q6)*cos(q5)^2*sin(q4)*q2dot-2*cos(q6)*Ix6s*q6dot*sin(q6)*cos(q5)^2*sin(q4)*q3dot-Ix6s*cos(q5)^2*cos(q4)*q1ddot*sin(q3+q2)*cos(q6)^2-2*Iz6s*cos(q5)^2*cos(q4)*q3dot*q1dot*cos(q3+q2)-2*Iz5s*cos(q5)^2*cos(q4)*q3dot*q1dot*cos(q3+q2)-2*Iz5s*cos(q5)^2*cos(q4)*q2dot*q1dot*cos(q3+q2)-Iz5s*cos(q5)^2*q1dot^2*cos(q3+q2)*sin(q4)*sin(q3+q2)-Iy6s*cos(q6)^2*cos(q5)^2*sin(q4)*q2ddot-2*Iz5s*q5dot*cos(q5)^2*q1dot*cos(q3+q2)+2*cos(q6)*Iy6s*q6dot*sin(q6)*cos(q5)^2*sin(q4)*q2dot+2*cos(q6)*Iy6s*q6dot*sin(q6)*cos(q5)^2*sin(q4)*q3dot+2*cos(q6)*Ix6s*q6dot*sin(q6)*cos(q5)^2*cos(q4)*q1dot*sin(q3+q2)-2*cos(q6)*Iy6s*q6dot*sin(q6)*cos(q5)^2*cos(q4)*q1dot*sin(q3+q2)+Ix6s*cos(q5)^2*cos(q4)*q1ddot*sin(q3+q2)+2*Ix6s*cos(q5)^2*cos(q4)*q2dot*q1dot*cos(q3+q2)+sin(q5)*Iy5s*cos(q5)*sin(q4)*q2dot^2*cos(q4)-Ix6s*cos(q5)^2*sin(q4)*q3ddot+cos(q5)*Iy6s*q6dot*q5dot-cos(q5)*Iz6s*q6dot*q5dot-cos(q5)*Ix6s*q6dot*q5dot+cos(q5)*Ix6s*sin(q5)*q4ddot+cos(q5)*cos(q6)*l4*fx7+cos(q5)*Iy5s*sin(q5)*q4ddot-cos(q5)*sin(q6)*l4*fy7-Iz5s*sin(q4)*q2ddot-Iz5s*sin(q4)*q3ddot-Iz6s*sin(q4)*q2ddot-Iz6s*sin(q4)*q3ddot-2*cos(q5)*Iz5s*q5dot*sin(q5)*sin(q4)*q2dot-2*cos(q5)*cos(q6)*Iy6s*q6dot*sin(q6)*sin(q5)*q4dot-cos(q5)*cos(q6)*Iy6s*sin(q6)*sin(q4)*q1ddot*sin(q3+q2)+2*cos(q5)*cos(q6)*Ix6s*q6dot*sin(q6)*sin(q5)*q1dot*cos(q3+q2)+2*cos(q5)*Iz5s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*q2dot+2*cos(q5)*Iz5s*sin(q5)*cos(q4)^2*q1dot*sin(q3+q2)*q3dot-cos(q5)*Iz5s*sin(q5)*sin(q4)*q2dot^2*cos(q4)-cos(q5)*cos(q6)*Iy6s*sin(q6)*cos(q4)*q2ddot-2*cos(q5)*cos(q6)*Iy6s*q6dot*sin(q6)*sin(q5)*q1dot*cos(q3+q2)+

68

2*cos(q5)*cos(q6)*Ix6s*q6dot*sin(q6)*sin(q5)*q4dot-2*Iz6s*q5dot*cos(q5)^2*q4dot+2*Iy5s*q5dot*cos(q5)^2*q4dot+2*Ix6s*q5dot*cos(q5)^2*q4dot-Iy5s*cos(q5)^2*sin(q4)*q3ddot-2*Iz5s*q5dot*cos(q5)^2*q4dot-Iy5s*cos(q5)^2*sin(q4)*q2ddot-Ix6s*cos(q5)^2*sin(q4)*q2ddot+2*Ix6s*cos(q5)^2*cos(q4)*q3dot*q1dot*cos(q3+q2)+cos(q5)*Iy6s*cos(q6)^2*sin(q5)*q1ddot*cos(q3+q2)+cos(q5)*sin(q6)*Ix6s*cos(q6)*sin(q4)*q1ddot*sin(q3+q2)+cos(q5)*Iy5s*sin(q5)*q1ddot*cos(q3+q2)+cos(q5)*Ix6s*sin(q5)*q1ddot*cos(q3+q2)+cos(q5)*Iy6s*q6dot*cos(q4)*q2dot+cos(q5)*Iy6s*q6dot*cos(q4)*q3dot+cos(q5)*Iy6s*cos(q6)^2*sin(q5)*q4ddot+cos(q5)*sin(q6)*Ix6s*cos(q6)*q5ddot+2*cos(q5)*Iz6s*q5dot*sin(q5)*cos(q4)*q1dot*sin(q3+q2)-cos(q5)*Iy6s*sin(q5)*cos(q4)*q1dot^2*sin(q4)*cos(q6)^2-Iy6s*cos(q6)^2*cos(q5)^2*sin(q4)*q3ddot+2*Ix6s*q5dot*cos(q5)^2*q1dot*cos(q3+q2)+2*Ix6s*cos(q6)*cos(q5)*cos(q4)*q1dot*sin(q3+q2)*sin(q6)*q4dot-Iz6s*q4dot*cos(q4)*q2dot-Ix6s*cos(q4)*q3dot*q4dot+Ix5s*q4dot*cos(q4)*q2dot+Iy6s*q5dot*q1dot*cos(q3+q2)+Ix5s*q4dot*cos(q4)*q3dot+Ix5s*q5dot*q1dot*cos(q3+q2)+Iz5s*cos(q4)*q1ddot*sin(q3+q2)+Iy6s*cos(q4)*q3dot*q4dot+Iz5s*q5dot*q1dot*cos(q3+q2)+Iy6s*cos(q4)*q2dot*q4dot+Iz6s*cos(q4)*q1ddot*sin(q3+q2)+Iz6s*q5dot*q1dot*cos(q3+q2)-Ix6s*q5dot*q1dot*cos(q3+q2)+2*Ix6s*cos(q6)^2*q4dot*q5dot-2*Iy6s*cos(q6)^2*q4dot*q5dot-Iy5s*q5dot*q1dot*cos(q3+q2)-Iz5s*q4dot*cos(q4)*q3dot-Iz5s*q4dot*cos(q4)*q2dot-Iz6s*q4dot*cos(q4)*q3dot-Iy5s*q4dot*cos(q4)*q2dot-Ix6s*cos(q4)*q2dot*q4dot-Iy5s*q4dot*cos(q4)*q3dot+Iz5s*sin(q4)*q2ddot*cos(q5)^2+Iz5s*sin(q4)*q3ddot*cos(q5)^2+Iz6s*sin(q4)*q2ddot*cos(q5)^2+Iz6s*sin(q4)*q3ddot*cos(q5)^2-Ix6s*sin(q4)*q1dot^2*sin(q3+q2)*cos(q3+q2)+Iy6s*sin(q4)*q1dot*sin(q3+q2)*q4dot-2*Ix6s*cos(q6)*cos(q5)*sin(q4)*q3dot*sin(q6)*q4dot+Ix5s*q4dot*sin(q4)*q1dot*sin(q3+q2)+Iy6s*sin(q4)*q1dot^2*sin(q3+q2)*cos(q3+q2)+Iy6s*cos(q4)*q2dot*q1dot*cos(q3+q2)+Iy6s*cos(q4)*q3dot*q1dot*cos(q3+q2)+Iz6s*cos(q4)*q3dot*q1dot*cos(q3+q2)+Ix5s*q1dot^2*cos(q3+q2)*sin(q4)*sin(q3+q2)+Iz6s*cos(q4)*q2dot*q1dot*cos(q3+q2)+Ix5s*cos(q4)*q3dot*q1dot*cos(q3+q2)+Ix5s*cos(q4)*q2dot*q1dot*cos(q3+q2)+Iz5s*cos(q4)*q3dot*q1dot*cos(q3+q2)-Iz6s*cos(q4)*q1ddot*sin(q3+q2)*cos(q5)^2+2*Ix6s*cos(q6)^2*q4dot*cos(q4)*q2dot+2*Ix6s*cos(q6)^2*q1dot*cos(q3+q2)*cos(q4)*q2dot-2*Ix6s*cos(q6)^2*q1dot*cos(q3+q2)*cos(q4)*q2dot*cos(q5)^2-2*Iy6s*cos(q6)^2*q1dot^2*cos(q3+q2)*sin(q4)*sin(q3+q2)-2*Iy6s*cos(q6)^2*q1dot*cos(q3+q2)*cos(q4)*q3dot+2*Iy6s*cos(q6)^2*q1dot*cos(q3+q2)*cos(q4)*q3dot*cos(q5)^2-2*Ix6s*cos(q6)^2*q4dot*q5dot*cos(q5)^2+2*Iy6s*cos(q6)^2*q4dot*q5dot*cos(q5)^2+2*Ix6s*cos(q6)^2*q1dot*cos(q3+q2)*cos(q4)*q3dot-2*Ix6s*cos(q6)^2*q1dot*cos(q3+q2)*cos(q4)*q3dot*cos(q5)^2+2*Ix6s*cos(q6)^2*q1dot^2*cos(q3+q2)*sin(q4)*sin(q3+q2)-2*Iy6s*cos(q6)^2*q4dot*cos(q4)*q3dot+2*Ix6s*cos(q6)^2*q1dot*cos(q3+q2)*q5dot-2*Ix6s*cos(q6)^2*q1dot*cos(q3+q2)*q5dot*cos(q5)^2+2*Iy6s*cos(q6)*cos(q5)*sin(q4)*q2dot*sin(q6)*q4dot+2*Iy6s*cos(q6)*cos(q5)*sin(q4)*q3dot*sin(q6)*q4dot-2*Iy6s*cos(q6)^2*q1dot*cos(q3+q2)*cos(q4)*q2dot+2*Iy6s*cos(q6)^2*q1dot*cos(q3+q2)*cos(q4)*q2dot*cos(q5)^2-2*Iy6s*cos(q6)^2*q1dot*cos(q3+q2)*q5dot+2*Iy6s*cos(q6)^2*q1dot*cos(q3+q2)*q5dot*cos(q5)^2-2*Iy6s*cos(q6)^2*q4dot*cos(q4)*q2dot+2*Ix6s*cos(q6)^2*q4dot*cos(q4)*q3dot-2*Ix6s*cos(q6)*cos(q5)*sin(q4)*q2dot*sin(q6)*q4dot-2*Iy6s*cos(q6)*cos(q5)*cos(q4)*q1dot*sin(q3+q2)*sin(q6)*q4dot-Iy5s*cos(q4)*q3dot*q1dot*cos(q3+q2)-Iy5s*q4dot*sin(q4)*q1dot*sin(q3+q2)-Iz5s*q4dot*sin(q4)*q1dot*sin(q3+q2)-Iy5s*cos(q4)*q2dot*q1dot*cos(q3+q2)-Iz6s*q4dot*sin(q4)*q1dot*sin(q3+q2)+Ix6s*sin(q4)*q1dot^2*sin(q3+q2)*cos(q3+q2)*cos(q5)^2-Ix6s*sin(q4)*q1dot*sin(q3+q2)*q4dot-Ix6s*cos(q4)*q2dot*q1dot*cos(q3+q2)-Ix6s*cos(q4)*q3dot*q1dot*cos(q3+q2)-Iy5s*q1dot^2*cos(q3+q2)*sin(q4)*sin(q3+q2)+Iy5s*q1dot^2*cos(q3+q2)*sin(q4)*sin(q3+q2)*cos(q5)^2-2*Iy6s*cos(q6)^2*q4dot*sin(q4)*q1dot*sin(q3+q2)+2*Ix6s*cos(q6)^2*q4dot*sin(q4)*q1dot*sin(q3+q2)+Iy6s*q5dot*q4dot+Iz6s*q5dot*q4dot+Iz5s*q5dot*q4dot-Iy5s*q5dot*q4dot-Ix6s*q5dot*q4dot sin(q5)*sin(q6)*nx7+sin(q5)*cos(q6)*ny7+Iz5s*cos(q5)^2*q4ddot+Iz6s*cos(q5)^2*q4ddot+cos(q5)*Iz6s*q6ddot-Ix6s*q4ddot*cos(q6)^2+Iy5s*q1ddot*cos(q3+q2)+Iy6s*cos(q6)^2*q4ddot+Ix6s*q1ddot*cos(q3+q2)-Iy5s*q4ddot*cos(q5)^2-Ix6s*q4ddot*cos(q5)^2+Ix6s*q4ddot+Iy5s*q4ddot+sin(q5)*Iy6s*cos(q5)*q1dot^2*cos(q3+q2)*sin(q4)*sin(q3+q2)*cos(q6)^2-2*sin(q5)*cos(q6)*Iy6s*sin(q6)*sin(q4)*q2dot*q1dot*cos(q3+q2)+2*sin(q5)*cos(q6)*Ix6s*sin(q4)*q2dot*sin(q6)*q1dot*cos(q3+q2)+2*Iz6s*sin(q4)*q2dot*cos(q4)*q3dot*cos(q5)^2+Iz6s*sin(q4)*q3dot^2*cos(q4)*cos(q5)^2-2*Iy6s*cos(q6)^2*q3dot*q1dot*sin(q3+q2)+2*Iz6s*cos(q4)^2*q1dot*sin(q3+q2)*q3dot-2*Iz6s*cos(q4)^2*q1dot*sin(q3+q2)*q3dot*cos(q5)^2-Iy5s*q5dot*cos(q4)*q1dot*sin(q3+q2)-cos(q6)*Ix6s*q2dot^2*sin(q6)*cos(q5)+Iz5s*sin(q4)*q3dot^2*cos(q4)*cos(q5)^2-Iz6s*cos(q4)*q1dot^2*sin(q4)*cos(q5)^2+Ix6s*q1ddot*cos(q3+q2)*cos(q6)^2*cos(q5)^2-2*cos(q6)*Iy6s*q6dot*sin(q6)*q1dot*cos(q3+q2)+2*cos(q6)*Iy6s*q6dot*sin(q6)*q1dot*cos(q3+q2)*cos(q5)^2-Iy6s*sin(q4)*q2dot^2*cos(q4)*cos(q6)^2+sin(q6)*Iy6s*q2dot^2*cos(q6)*cos(q5)-Iy6s*cos(q6)^2*

69

q1ddot*cos(q3+q2)*cos(q5)^2+sin(q6)*Iy6s*q3dot^2*cos(q6)*cos(q5)+2*sin(q5)*cos(q6)*Ix6s*sin(q4)*q3dot*sin(q6)*q1dot*cos(q3+q2)-2*sin(q5)*cos(q6)*Iy6s*sin(q6)*sin(q4)*q3dot*q1dot*cos(q3+q2)-2*sin(q5)*cos(q6)*Iy6s*q6dot*sin(q6)*cos(q5)*cos(q4)*q1dot*sin(q3+q2)-sin(q5)*cos(q6)*Iy6s*sin(q6)*cos(q4)*q3ddot-sin(q5)*Iz6s*q6dot*sin(q4)*q1dot*sin(q3+q2)-2*sin(q5)*Iz6s*q5dot*cos(q5)*q1dot*cos(q3+q2)+2*sin(q5)*Iy5s*q5dot*cos(q5)*q4dot-2*cos(q5)*Ix6s*cos(q6)^2*sin(q5)*q1dot*cos(q3+q2)*cos(q4)*q3dot-cos(q5)*Iy6s*sin(q6)*q5dot^2*cos(q6)-2*Ix6s*cos(q4)*q2dot*cos(q5)^2*sin(q4)*q3dot+2*Ix6s*cos(q6)^2*cos(q5)^2*sin(q4)*q2dot*q5dot+2*Ix6s*cos(q6)^2*cos(q5)^2*sin(q4)*q3dot*q5dot+2*Ix6s*cos(q6)^2*cos(q5)^2*sin(q4)*q2dot*cos(q4)*q3dot-2*Iy6s*cos(q6)^2*cos(q5)^2*sin(q4)*q2dot*cos(q4)*q3dot-2*Ix6s*cos(q6)^2*cos(q5)^2*cos(q4)*q1dot*sin(q3+q2)*q5dot-2*Ix6s*cos(q6)^2*cos(q5)^2*cos(q4)^2*q1dot*sin(q3+q2)*q3dot-sin(q5)*Ix6s*q6dot*sin(q4)*q1dot*sin(q3+q2)-2*sin(q5)*Iz6s*cos(q5)*cos(q4)*q2dot*q1dot*cos(q3+q2)-sin(q5)*Iz6s*cos(q5)*q1dot^2*cos(q3+q2)*sin(q4)*sin(q3+q2)-2*sin(q5)*Iz6s*cos(q5)*cos(q4)*q3dot*q1dot*cos(q3+q2)+sin(q5)*Iy6s*q6dot*cos(q4)*q3dot+2*cos(q5)*Ix6s*sin(q6)*sin(q4)*q1dot*sin(q3+q2)*cos(q6)*q5dot-2*cos(q5)*Iy6s*sin(q6)*cos(q4)^2*q3dot^2*cos(q6)-2*cos(q5)*Iy6s*sin(q6)*cos(q4)^2*q2dot^2*cos(q6)-cos(q5)*Iz6s*sin(q5)*cos(q4)*q1ddot*sin(q3+q2)-2*cos(q5)*Ix6s*cos(q6)^2*sin(q5)*q1dot*cos(q3+q2)*cos(q4)*q2dot-2*cos(q5)*Iy6s*sin(q6)*q1dot^2*cos(q6)*cos(q4)^2*cos(q3+q2)^2-2*Ix6s*cos(q6)^2*cos(q5)^2*cos(q4)^2*q1dot*sin(q3+q2)*q2dot-Iy6s*cos(q6)^2*cos(q5)^2*sin(q4)*q2dot^2*cos(q4)-Ix6s*cos(q4)*q2dot^2*cos(q5)^2*sin(q4)+2*Iy6s*cos(q6)^2*cos(q5)^2*cos(q4)^2*q1dot*sin(q3+q2)*q2dot+sin(q5)*cos(q6)*l4*fx7-Iy5s*cos(q5)^2*sin(q4)*q2dot^2*cos(q4)-2*Iz6s*q5dot*cos(q5)^2*cos(q4)*q1dot*sin(q3+q2)+2*Iy6s*cos(q6)^2*cos(q5)^2*cos(q4)^2*q1dot*sin(q3+q2)*q3dot+Ix6s*sin(q4)*q1dot^2*cos(q5)^2*cos(q4)+2*Ix6s*q5dot*cos(q5)^2*cos(q4)*q1dot*sin(q3+q2)-2*Iz5s*q5dot*cos(q5)^2*cos(q4)*q1dot*sin(q3+q2)+sin(q5)*Iy6s*q6dot*q5dot-sin(q5)*Iz6s*q6dot*q5dot-sin(q5)*Ix6s*q6dot*q5dot+Iz5s*cos(q5)^2*q1ddot*cos(q3+q2)+Iz6s*cos(q5)^2*q1ddot*cos(q3+q2)-Ix6s*sin(q4)*q1dot^2*cos(q5)^2*cos(q4)*cos(q6)^2+Ix6s*sin(q4)*q1dot^2*cos(q5)^2*cos(q4)*cos(q6)^2*cos(q3+q2)^2+Iy6s*cos(q6)^2*cos(q5)^2*cos(q4)*q1dot^2*sin(q4)-Iy6s*cos(q6)^2*cos(q5)^2*cos(q4)*q1dot^2*sin(q4)*cos(q3+q2)^2+Ix6s*cos(q6)^2*cos(q5)^2*sin(q4)*q3dot^2*cos(q4)+Ix6s*cos(q6)^2*cos(q5)^2*sin(q4)*q2dot^2*cos(q4)-2*Ix6s*q5dot*cos(q5)^2*sin(q4)*q3dot-2*Iy6s*cos(q6)^2*cos(q5)^2*sin(q4)*q2dot*q5dot+2*Iy6s*cos(q6)^2*cos(q5)^2*cos(q4)*q1dot*sin(q3+q2)*q5dot-Iy6s*cos(q6)^2*cos(q5)^2*sin(q4)*q3dot^2*cos(q4)+2*Ix6s*cos(q4)^2*q2dot*cos(q5)^2*q1dot*sin(q3+q2)-cos(q5)*Iz5s*sin(q5)*cos(q4)*q1ddot*sin(q3+q2)+2*cos(q5)*Iy6s*cos(q6)^2*sin(q5)*q1dot*cos(q3+q2)*cos(q4)*q2dot+4*cos(q5)*Ix6s*sin(q6)*cos(q4)*q3dot*cos(q6)*sin(q4)*q1dot*sin(q3+q2)-cos(q5)*Ix6s*cos(q6)*q1dot^2*cos(q3+q2)^2*sin(q6)-4*cos(q5)*Iy6s*sin(q6)*cos(q4)^2*q2dot*cos(q6)*q3dot+cos(q5)*Iz5s*sin(q5)*sin(q4)*q3ddot+sin(q5)*Iy6s*q6dot*cos(q4)*q2dot-2*sin(q5)*cos(q6)*Ix6s*q6dot*sin(q6)*cos(q5)*sin(q4)*q2dot-2*sin(q5)*cos(q6)*Ix6s*q6dot*sin(q6)*cos(q5)*sin(q4)*q3dot-sin(q5)*cos(q6)*Ix6s*cos(q4)*q1dot^2*sin(q3+q2)*sin(q6)*cos(q3+q2)+2*Ix6s*q1dot*sin(q3+q2)*cos(q5)^2*q3dot*cos(q4)^2-Ix6s*sin(q4)*q1dot^2*cos(q5)^2*cos(q4)*cos(q3+q2)^2-2*Ix6s*q5dot*cos(q5)^2*sin(q4)*q2dot+2*Iz6s*q5dot*cos(q5)^2*sin(q4)*q2dot+2*Iz6s*q5dot*cos(q5)^2*sin(q4)*q3dot-2*Iy5s*q5dot*cos(q5)^2*sin(q4)*q2dot-2*Iy5s*q5dot*cos(q5)^2*sin(q4)*q3dot-sin(q5)*Ix6s*cos(q5)*cos(q4)*q1ddot*sin(q3+q2)*cos(q6)^2-2*sin(q5)*Iz5s*cos(q5)*cos(q4)*q3dot*q1dot*cos(q3+q2)+2*sin(q5)*Iy5s*cos(q5)*cos(q4)*q3dot*q1dot*cos(q3+q2)+2*sin(q5)*Ix6s*q5dot*cos(q5)*q4dot+2*sin(q5)*Iy5s*cos(q5)*cos(q4)*q2dot*q1dot*cos(q3+q2)+sin(q5)*Ix6s*cos(q5)*sin(q4)*q3ddot*cos(q6)^2+sin(q5)*Ix6s*cos(q5)*sin(q4)*q2ddot*cos(q6)^2+2*sin(q5)*Ix6s*q5dot*cos(q5)*q1dot*cos(q3+q2)+sin(q5)*Iy6s*q6dot*sin(q4)*q1dot*sin(q3+q2)-2*sin(q5)*Iz5s*cos(q5)*cos(q4)*q2dot*q1dot*cos(q3+q2)-sin(q5)*Iz5s*cos(q5)*q1dot^2*cos(q3+q2)*sin(q4)*sin(q3+q2)+2*sin(q5)*Ix6s*q6dot*cos(q6)^2*sin(q4)*q1dot*sin(q3+q2)-Iy5s*cos(q5)^2*cos(q4)*q1dot^2*sin(q4)*cos(q3+q2)^2+2*Iy5s*cos(q5)^2*cos(q4)^2*q1dot*sin(q3+q2)*q2dot-Ix6s*cos(q4)*q3dot^2*cos(q5)^2*sin(q4)-2*cos(q5)*Ix6s*cos(q6)^2*sin(q5)*q1dot*cos(q3+q2)*q5dot+2*cos(q5)*Ix6s*sin(q6)*cos(q4)*q2dot*cos(q6)*q5dot+2*cos(q5)*Ix6s*sin(q6)*q1dot^2*cos(q6)*cos(q4)^2*cos(q3+q2)^2+2*cos(q5)*Ix6s*sin(q6)*cos(q4)^2*q3dot^2*cos(q6)+2*cos(q5)*Ix6s*sin(q6)*cos(q4)^2*q2dot^2*cos(q6)+2*cos(q5)*Iy6s*sin(q6)*q1dot^2*cos(q6)*cos(q4)^2+cos(q5)*Ix6s*sin(q6)*q1dot^2*cos(q6)+cos(q5)*Ix6s*sin(q6)*q5dot^2*cos(q6)+cos(q5)*Iz6s*sin(q5)*sin(q4)*q2ddot+cos(q5)*Iz6s*sin(q5)*sin(q4)*q3ddot+2*cos(q5)*Iy6s*cos(q6)^2*sin(q5)*q1dot*cos(q3+q2)*cos(q4)*q3dot-2*cos(q5)*Ix6s*cos(q6)^2*sin(q5)*q4dot*q5dot+2*cos(q5)*Iy6s*cos(q6)^2*sin(q5)*q4dot*q5dot-2*cos(q5)*Ix6s*sin(q6)*q1dot^2*cos(q6)*cos(q4)^2+2*Iz5s*q5dot*cos(q5)^2*sin(q4)*q3dot+Iy5s*cos(q5)

70

^2*cos(q4)*q1dot^2*sin(q4)+2*Iy5s*q5dot*cos(q5)^2*cos(q4)*q1dot*sin(q3+q2)+2*Iz5s*q5dot*cos(q5)^2*sin(q4)*q2dot-sin(q5)*Iy6s*cos(q6)^2*cos(q5)*sin(q4)*q2ddot-2*sin(q5)*Iy6s*q6dot*cos(q6)^2*cos(q4)*q2dot+2*Iy5s*cos(q5)^2*cos(q4)^2*q1dot*sin(q3+q2)*q3dot-2*Iy5s*cos(q5)^2*sin(q4)*q2dot*cos(q4)*q3dot-2*Iy6s*cos(q6)^2*cos(q5)^2*sin(q4)*q3dot*q5dot-Iy5s*cos(q5)^2*sin(q4)*q3dot^2*cos(q4)+cos(q5)*Iz5s*sin(q5)*sin(q4)*q2ddot+cos(q5)*nz7-Iy4s*q3dot*q1dot*sin(q3+q2)+Ix4s*q3dot*q1dot*sin(q3+q2)-Iz4s*q2dot*q1dot*sin(q3+q2)+2*sin(q6)*Iy6s*q2dot*cos(q6)*cos(q5)*q3dot-2*Iy6s*cos(q4)^2*q1dot*sin(q3+q2)*q3dot-2*Iz5s*sin(q4)*q2dot*cos(q4)*q3dot+2*Iz5s*sin(q4)*q2dot*cos(q4)*q3dot*cos(q5)^2-2*cos(q6)*Iy6s*q6dot*sin(q6)*q4dot+2*cos(q6)*Iy6s*q6dot*sin(q6)*q4dot*cos(q5)^2+Iy5s*q5dot*sin(q4)*q2dot+Iy5s*q5dot*sin(q4)*q3dot-2*sin(q5)*Iy6s*q6dot*cos(q6)^2*cos(q4)*q3dot-2*sin(q5)*Iy6s*q6dot*cos(q6)^2*sin(q4)*q1dot*sin(q3+q2)+sin(q5)*Ix6s*cos(q5)*cos(q4)*q1ddot*sin(q3+q2)+2*sin(q5)*Ix6s*cos(q5)*cos(q4)*q2dot*q1dot*cos(q3+q2)+2*sin(q5)*Ix6s*cos(q5)*cos(q4)*q3dot*q1dot*cos(q3+q2)+sin(q5)*sin(q6)*Ix6s*cos(q6)*q5ddot+sin(q5)*sin(q6)*Ix6s*cos(q6)*sin(q4)*q1ddot*sin(q3+q2)-2*sin(q5)*Iz5s*q5dot*cos(q5)*q1dot*cos(q3+q2)-sin(q5)*cos(q6)*Iy6s*sin(q6)*cos(q4)*q2ddot-sin(q5)*Iy6s*cos(q6)^2*cos(q5)*sin(q4)*q3ddot-sin(q5)*Ix6s*cos(q5)*q1dot^2*cos(q3+q2)*cos(q6)^2*sin(q4)*sin(q3+q2)+sin(q5)*Iy6s*cos(q6)^2*cos(q5)*cos(q4)*q1ddot*sin(q3+q2)+sin(q5)*Iy5s*cos(q5)*cos(q4)*q1ddot*sin(q3+q2)-sin(q5)*Iy5s*cos(q5)*sin(q4)*q3ddot-2*sin(q5)*Iz5s*q5dot*cos(q5)*q4dot-sin(q5)*Iy5s*cos(q5)*sin(q4)*q2ddot-sin(q5)*Ix6s*q6dot*cos(q4)*q2dot-2*sin(q5)*Iz6s*q5dot*cos(q5)*q4dot-Iz4s*q3dot*q1dot*sin(q3+q2)+Ix4s*q2dot*q1dot*sin(q3+q2)+Ix4s*sin(q4)*q2dot^2*cos(q4)+Ix4s*sin(q4)*q3dot^2*cos(q4)-Iy4s*sin(q4)*q2dot^2*cos(q4)-Iy4s*sin(q4)*q3dot^2*cos(q4)-Ix4s*cos(q4)*q1dot^2*sin(q4)+Iz4s*q1ddot*cos(q3+q2)+2*Ix4s*sin(q4)*q2dot*cos(q4)*q3dot-Ix6s*cos(q4)*q1dot^2*cos(q6)^2*sin(q4)-2*Iy6s*sin(q4)*q2dot*cos(q4)*q3dot*cos(q6)^2+Ix6s*sin(q4)*q2dot^2*cos(q6)^2*cos(q4)+Iz6s*q5dot*cos(q4)*q1dot*sin(q3+q2)+Ix6s*sin(q4)*q3dot^2*cos(q6)^2*cos(q4)-cos(q6)*Ix6s*q3dot^2*sin(q6)*cos(q5)+2*Ix6s*q2dot*cos(q6)^2*q1dot*sin(q3+q2)-2*cos(q6)*Ix6s*q2dot*sin(q6)*cos(q5)*q3dot+Iz4s*q4ddot-2*Ix4s*cos(q4)^2*q1dot*sin(q3+q2)*q2dot-2*Ix4s*cos(q4)^2*q1dot*sin(q3+q2)*q3dot+2*Iy4s*cos(q4)^2*q1dot*sin(q3+q2)*q2dot-2*Iy4s*sin(q4)*q2dot*cos(q4)*q3dot+2*Iy4s*cos(q4)^2*q1dot*sin(q3+q2)*q3dot+Ix4s*cos(q4)*q1dot^2*sin(q4)*cos(q3+q2)^2+Iy4s*cos(q4)*q1dot^2*sin(q4)-Iy4s*q2dot*q1dot*sin(q3+q2)-sin(q5)*sin(q6)*l4*fy7+Iy6s*sin(q4)*q2dot^2*cos(q4)+Ix5s*sin(q4)*q2dot^2*cos(q4)+Ix5s*q5dot*sin(q4)*q3dot+Iy6s*sin(q4)*q3dot^2*cos(q4)+Iz5s*cos(q4)*q1dot^2*sin(q4)+Iy6s*q2dot*q1dot*sin(q3+q2)-Iy6s*cos(q4)*q1dot^2*sin(q4)-Iz6s*q2dot*q1dot*sin(q3+q2)-Iz5s*q5dot*sin(q4)*q2dot-Iz5s*sin(q4)*q2dot^2*cos(q4)-Iy5s*q2dot*q1dot*sin(q3+q2)-Iy4s*cos(q4)*q1dot^2*sin(q4)*cos(q3+q2)^2+2*Ix6s*sin(q4)*q2dot*cos(q6)^2*cos(q4)*q3dot+2*Iy6s*cos(q4)^2*q1dot*sin(q3+q2)*q2dot*cos(q6)^2+2*Iy6s*cos(q4)^2*q1dot*sin(q3+q2)*q3dot*cos(q6)^2-sin(q5)*Iz6s*q6dot*cos(q4)*q2dot-sin(q5)*Ix6s*q6dot*cos(q4)*q3dot-sin(q5)*Ix6s*cos(q5)*sin(q4)*q2ddot-sin(q5)*Iz6s*q6dot*cos(q4)*q3dot-sin(q5)*Ix6s*cos(q5)*sin(q4)*q3ddot-2*sin(q5)*Iy6s*q6dot*cos(q6)^2*q5dot+2*sin(q5)*Ix6s*q6dot*cos(q6)^2*q5dot-sin(q5)*cos(q6)*Iy6s*sin(q6)*q5ddot+sin(q5)*sin(q6)*Ix6s*cos(q6)*cos(q4)*q2ddot+sin(q5)*sin(q6)*Ix6s*cos(q6)*cos(q4)*q3ddot+4*cos(q5)*Ix6s*sin(q6)*cos(q4)^2*q2dot*cos(q6)*q3dot-2*cos(q5)*Iy6s*sin(q6)*sin(q4)*q1dot*sin(q3+q2)*cos(q6)*q5dot+cos(q5)*Iy5s*sin(q5)*q1dot^2*cos(q3+q2)*sin(q4)*sin(q3+q2)-2*cos(q5)*Iy6s*sin(q6)*cos(q4)*q3dot*cos(q6)*q5dot-2*cos(q5)*Iy6s*sin(q6)*cos(q4)*q2dot*cos(q6)*q5dot+2*cos(q5)*Ix6s*sin(q6)*cos(q4)*q3dot*cos(q6)*q5dot+4*cos(q5)*Ix6s*sin(q6)*cos(q4)*q2dot*cos(q6)*sin(q4)*q1dot*sin(q3+q2)-2*Ix6s*cos(q4)^2*q1dot*sin(q3+q2)*cos(q6)^2*q2dot+Ix5s*cos(q4)*q1dot^2*sin(q4)*cos(q3+q2)^2-Iz5s*cos(q4)*q1dot^2*sin(q4)*cos(q5)^2+Iy6s*cos(q4)*q1dot^2*sin(q4)*cos(q6)^2-2*Iz6s*sin(q4)*q2dot*cos(q4)*q3dot-2*Iy6s*cos(q6)^2*q2dot*q1dot*sin(q3+q2)+Iz5s*sin(q4)*q2dot^2*cos(q4)*cos(q5)^2+2*Iy6s*sin(q4)*q2dot*cos(q4)*q3dot-Ix5s*q5dot*cos(q4)*q1dot*sin(q3+q2)-Iz6s*cos(q4)*q1dot^2*sin(q4)*cos(q3+q2)^2+Iz6s*cos(q4)*q1dot^2*sin(q4)*cos(q3+q2)^2*cos(q5)^2-Iy6s*q5dot*cos(q4)*q1dot*sin(q3+q2)-2*Ix6s*cos(q4)^2*q1dot*sin(q3+q2)*cos(q6)^2*q3dot+2*Iz5s*cos(q4)^2*q1dot*sin(q3+q2)*q2dot-2*Iz5s*cos(q4)^2*q1dot*sin(q3+q2)*q2dot*cos(q5)^2+2*cos(q6)*Ix6s*q6dot*sin(q6)*q1dot*cos(q3+q2)-2*cos(q6)*Ix6s*q6dot*sin(q6)*q1dot*cos(q3+q2)*cos(q5)^2-Ix5s*cos(q4)*q1dot^2*sin(q4)-Iz5s*q5dot*sin(q4)*q3dot-Ix6s*q3dot*q1dot*sin(q3+q2)+Iy6s*q3dot*q1dot*sin(q3+q2)+Ix6s*q5dot*sin(q4)*q2dot+Ix5s*sin(q4)*q3dot^2*cos(q4)+Ix6s*q5dot*sin(q4)*q3dot+Ix5s*q2dot*q1dot*sin(q3+q2)+Ix5s*q3dot*q1dot*sin(q3+q2)+Iy6s*q5dot*sin(q4)*q2dot-Iz6s*sin(q4)*q2dot^2*cos(q4)-Iz5s*q2dot*q1dot*sin(q3+q2)-Iz5s*q3dot*q1dot*sin(q3+q2)+2*Iz6s*cos(q4)^2*q1dot*sin(q3+q2)*q2dot-2*Iz6s*cos(q4)^2*q1dot*sin(q3+q2)*q2dot*cos(q5)^2-Ix6s*q5dot*cos(q4)*q1dot*sin(q3+q2)+2*Ix6s*q3dot*cos(q6)^2*q1dot*sin(q3+q2)+Iz5s*q5dot*cos(q4)*q1dot*sin(q3+q2)+Iy6s*cos(q4)*q1dot^2*sin(q4)*cos(q3+q2)^2-2*Ix5s*cos(q4)^2*q1dot*sin(q3+q2)*q2dot+2*Iz5s*cos(q4)^2*q1dot*sin(q3+q2)*q3dot-2*Iz5s*cos(q4)^2*

71

q1dot*sin(q3+q2)*q3dot*cos(q5)^2-2*Ix5s*cos(q4)^2*q1dot*sin(q3+q2)*q3dot-Iy6s*sin(q4)*q3dot^2*cos(q4)*cos(q6)^2+Iz6s*sin(q4)*q2dot^2*cos(q4)*cos(q5)^2+2*Ix5s*sin(q4)*q2dot*cos(q4)*q3dot-2*Iy6s*cos(q4)^2*q1dot*sin(q3+q2)*q2dot+2*cos(q6)*Ix6s*q6dot*sin(q6)*q4dot-2*cos(q6)*Ix6s*q6dot*sin(q6)*q4dot*cos(q5)^2+Ix6s*cos(q4)*q1dot^2*cos(q6)^2*sin(q4)*cos(q3+q2)^2-Iy6s*cos(q4)*q1dot^2*sin(q4)*cos(q3+q2)^2*cos(q6)^2-Iz5s*cos(q4)*q1dot^2*sin(q4)*cos(q3+q2)^2+Iz5s*cos(q4)*q1dot^2*sin(q4)*cos(q3+q2)^2*cos(q5)^2-Iz6s*sin(q4)*q3dot^2*cos(q4)-Iz6s*q3dot*q1dot*sin(q3+q2)-Iz6s*q5dot*sin(q4)*q3dot-Iz5s*sin(q4)*q3dot^2*cos(q4)+Iz6s*cos(q4)*q1dot^2*sin(q4)-Ix6s*q1ddot*cos(q3+q2)*cos(q6)^2-Iz6s*q5dot*sin(q4)*q2dot+Iy6s*cos(q6)^2*q1ddot*cos(q3+q2)-Iy5s*q3dot*q1dot*sin(q3+q2)+Iy6s*q5dot*sin(q4)*q3dot+Ix5s*q5dot*sin(q4)*q2dot-Ix6s*q2dot*q1dot*sin(q3+q2)-Ix6s*q1ddot*cos(q3+q2)*cos(q5)^2-Iy6s*cos(q6)^2*q4ddot*cos(q5)^2-Iy5s*q1ddot*cos(q3+q2)*cos(q5)^2+Ix6s*q4ddot*cos(q6)^2*cos(q5)^2-cos(q5)*Iy6s*sin(q6)*q1dot^2*cos(q6)-4*cos(q5)*Iy6s*sin(q6)*cos(q4)*q2dot*cos(q6)*sin(q4)*q1dot*sin(q3+q2)+cos(q5)*Iy6s*sin(q6)*q1dot^2*cos(q6)*cos(q3+q2)^2-4*cos(q5)*Iy6s*sin(q6)*cos(q4)*q3dot*cos(q6)*sin(q4)*q1dot*sin(q3+q2)+2*cos(q5)*Iy6s*cos(q6)^2*sin(q5)*q1dot*cos(q3+q2)*q5dot+cos(q5)*Ix6s*sin(q4)*q1dot^2*sin(q3+q2)*sin(q5)*cos(q3+q2)+sin(q5)*sin(q6)*Iy6s*cos(q4)*q1dot^2*sin(q3+q2)*cos(q6)*cos(q3+q2)+2*sin(q5)*Ix6s*q6dot*cos(q6)^2*cos(q4)*q3dot+2*sin(q5)*Ix6s*q6dot*cos(q6)^2*cos(q4)*q2dot-sin(q5)*cos(q6)*Iy6s*sin(q6)*sin(q4)*q1ddot*sin(q3+q2)+2*sin(q5)*cos(q6)*Iy6s*q6dot*sin(q6)*cos(q5)*sin(q4)*q2dot+2*sin(q5)*cos(q6)*Iy6s*q6dot*sin(q6)*cos(q5)*sin(q4)*q3dot+2*sin(q5)*cos(q6)*Ix6s*q6dot*sin(q6)*cos(q5)*cos(q4)*q1dot*sin(q3+q2)+2*sin(q5)*Iy5s*q5dot*cos(q5)*q1dot*cos(q3+q2)

Investeşte în oameni !FONDUL SOCIAL EUROPEAN

72

Proiect cofinanţat din Fondul Social European prin Programul Operaţional Sectorial pentru Dezvoltarea Resurselor Umane 2007 – 2013 Axa prioritară 1: „Educaţia şi formarea profesională în sprijinul creşterii economice şi dezvoltării societăţii bazate pe cunoaştere”Domeniul major de intervenţie 1.5 "Programe doctorale şi post-doctorale în sprijinul cercetării"Titlul proiectului: „Q-DOC- Creşterea calităşii studiilor doctorale în ştiinţe inginereşti pentru sprijinirea dezvoltării societăţii bazate pe cunoaştere”Contract : POSDRU/107/1.5/S/78534Beneficiar: Universitatea Tehnică din Cluj-Napoca

RAPORT II DE CERCETARE ŞTIINŢIFICĂ

Titlul raportului de cercetare ştiinţifică:

Doctorand: Ing. Florin BUGNARConducător ştiinţific: Prof.dr.ing. Viorel ISPASDomeniu: Inginerie Industrială

CENTRALIZATOR

Număr pagini Număr de figuri Număr de tabele Număr de relaţii Număr de exemplare

Raport

0 0 0 0

31 - Biblioteca şcolii doctorale,1 - Conducător doctorat,1 – Doctorand.

Conducător ştiinţific, Doctorand, Prof.dr.ing. Viorel ISPAS Florin BUGNAR

73