1. 程式人生 > >拉格朗日法建立六軸機器人TX90動力學模型

拉格朗日法建立六軸機器人TX90動力學模型

參考書籍,感謝這本書的作者讓我對雅克比矩陣有了更加深刻的認識: 在這裡插入圖片描述 以下是我建模的相關草稿:

在這裡插入圖片描述 在這裡插入圖片描述 在這裡插入圖片描述 在這裡插入圖片描述 在這裡插入圖片描述 在這裡插入圖片描述 在這裡插入圖片描述 在這裡插入圖片描述 在這裡插入圖片描述 在這裡插入圖片描述 草稿有點凌亂,沒有整理,詳細建模過程可參考以下程式碼,筆者最後反對稱性沒有驗證出來,可能有地方出問題了,相關程式碼如下:

function [ T ] = Trans( alpha, a, theta, d )                            % 變換矩陣
T =[ 
        cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha), a*cos(theta);
    	sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha), a*sin(theta);
              0,          sin(alpha),              cos(alpha),            d;
              0,              0,                        0,                  1];
end

syms theta1 a1 d1 theta2 a2 d2 theta3 a3 d3 theta4 a4 d4 theta5 a5 d5 theta6 a6 d6
syms alpha1 alpha2 alpha3 alpha4 alpha5  alpha6
syms x1 y1 z1 x2 y2 z2 x3 y3 z3 x4 y4 z4 x5 y5 z5 x6 y6 z6  %質心位置
syms m1 m2 m3 m4 m5 m6 g
syms Ixx1 Ixy1 Ixz1 Iyy1 Iyz1 Izz1 Ixx2 Ixy2 Ixz2 Iyy2 Iyz2 Izz2 Ixx3 Ixy3 Ixz3 Iyy3 Iyz3 Izz3 
syms Ixx4 Ixy4 Ixz4 Iyy4 Iyz4 Izz4 Ixx5 Ixy5 Ixz5 Iyy5 Iyz5 Izz5 Ixx6 Ixy6 Ixz6 Iyy6 Iyz6 Izz6

g = sym(9.8);%重力加速度
%各連桿在其質心座標系下的慣性張量
Ic1=[Ixx1, Ixy1, Ixz1; Ixy1, Iyy1, Iyz1;Ixz1, Iyz1, Izz1 ];
Ic2=[Ixx2, Ixy2, Ixz2; Ixy2, Iyy2, Iyz2;Ixz2, Iyz2, Izz2 ];
Ic3=[Ixx3, Ixy3, Ixz3; Ixy3, Iyy3, Iyz3;Ixz3, Iyz3, Izz3 ];
Ic4=[Ixx4, Ixy4, Ixz4; Ixy4, Iyy4, Iyz4;Ixz4, Iyz4, Izz4 ];
Ic5=[Ixx5, Ixy5, Ixz5; Ixy5, Iyy5, Iyz5;Ixz5, Iyz5, Izz5 ];
Ic6=[Ixx6, Ixy6, Ixz6; Ixy6, Iyy6, Iyz6;Ixz6, Iyz6, Izz6 ];
%這樣賦值好化簡
%  theta1 = sym(0);theta2 = sym(0);theta3 = sym(pi/2);theta4 = sym(0);theta5 = sym(pi/4);theta6 = sym(0);
d1 = sym(0);d2 = sym(0);d3 = sym(50);d4 = sym(425);d5 = sym(0);d6 = sym(100);
a1 = sym(50); a2 =sym(425); a3 =sym(0); a4 = sym(0);a5 = sym(0); a6 = sym(0);
alpha1 = sym(-pi/2); alpha2 = sym(0); alpha3 = sym(pi/2); alpha4 = sym(-pi/2); alpha5 = sym(pi/2); alpha6 = sym(0);
A1=Trans(alpha1, a1, theta1,d1 );
A2=Trans(alpha2 , a2, theta2,d2 );
A3=Trans(alpha3, a3, theta3,d3 );
A4=Trans(alpha4, a4, theta4,d4 );
A5=Trans(alpha5, a5, theta5,d5 );
A6=Trans(alpha6, a6, theta6,d6 );

T00 = [1 0 0 0; 0 1 0 0 ; 0 0 1 0; 0 0 0 1];
T01 = A1;
T02 = A1*A2;
T03 = A1*A2*A3;
T04 = A1*A2*A3*A4;
T05 = A1*A2*A3*A4*A5;
T06 = A1*A2*A3*A4*A5*A6;

C1 = [1, 0, 0, x1; 0, 1, 0, y1; 0, 0, 1, z1;0 0 0 1]; %連桿1質心在{1}系下,因為矩陣是右乘的,先轉的T01,再由T1到TC1
T0c1 = T01* C1;

C2 = [1, 0, 0, x2; 0, 1, 0, y2; 0, 0, 1, z2;0 0 0 1];  %連桿2質心在{2}系下
T0c2 = T02* C2;

C3 = [1, 0, 0, x3; 0, 1, 0, y3; 0, 0, 1, z3;0 0 0 1];  %連桿3質心在{3}系下
T0c3 = T03* C3;

C4 = [1, 0, 0, x4; 0, 1, 0, y4; 0, 0, 1, z4;0 0 0 1];  %連桿4質心在{4}系下
T0c4 = T04* C4;

C5 = [1, 0, 0, x5; 0, 1, 0, y5; 0, 0, 1, z5;0 0 0 1];  %連桿5質心在{5}系下
T0c5 = T05* C5;

C6 = [1, 0, 0, x6; 0, 1, 0, y6; 0, 0, 1, z6;0 0 0 1];  %連桿6質心在{6}系下
T0c6 = T06* C6;

%雅克比準備工作
B = [0;0;0]; %零矩陣
Z0 = T00([1 2 3],3);
Z1 = T01([1 2 3],3);
Z2 = T02([1 2 3],3);
Z3 = T03([1 2 3],3);
Z4 = T04([1 2 3],3);
Z5 = T05([1 2 3],3);

O0 = T00([1 2 3],4);
O1 = T01([1 2 3],4);
O2 = T02([1 2 3],4);
O3 = T03([1 2 3],4);
O4 = T04([1 2 3],4);
O5 = T05([1 2 3],4);
O6 = T06([1 2 3],4); 

Oc1 = T0c1([1 2 3],4);
Oc2 = T0c2([1 2 3],4);
Oc3 = T0c3([1 2 3],4);
Oc4 = T0c4([1 2 3],4);
Oc5 = T0c5([1 2 3],4);
Oc6 = T0c6([1 2 3],4);

%質心1的雅克比
JV1_1 = cross(Z0, Oc1-O0);
J1 = [JV1_1, B , B, B, B, B;Z0,B,B,B,B,B];
Jv1 = J1([1 2 3], :);
Jw1 = J1([4 5 6], :);
%質心2的雅克比
JV2_1 = cross(Z0, Oc2-O0);
JV2_2 = cross(Z1, Oc2-O1);
J2 = [JV2_1, JV2_2 , B, B, B, B;Z0,Z1,B,B,B,B];
Jv2 = J2([1 2 3],  :);
Jw2 = J2([4 5 6], :);
%質心3的雅克比
JV3_1 = cross(Z0, Oc3-O0);
JV3_2 = cross(Z1, Oc3-O1);
JV3_3 = cross(Z2, Oc3-O2);
J3 = [JV3_1, JV3_2 , JV3_3, B, B, B;Z0,Z1,Z2,B,B,B];
Jv3 = J3([1 2 3],  :);
Jw3 = J3([4 5 6], :);
%質心4的雅克比
JV4_1 = cross(Z0, Oc4-O0);
JV4_2 = cross(Z1, Oc4-O1);
JV4_3 = cross(Z2, Oc4-O2);
JV4_4 = cross(Z3, Oc4-O3);
J4 = [JV4_1, JV4_2 , JV4_3, JV4_4, B, B;Z0,Z1,Z2,Z3,B,B];
Jv4 = J4([1 2 3],  :);
Jw4 = J4([4 5 6], :);
%質心5的雅克比
JV5_1 = cross(Z0, Oc5-O0);
JV5_2 = cross(Z1, Oc5-O1);
JV5_3 = cross(Z2, Oc5-O2);
JV5_4 = cross(Z3, Oc5-O3);
JV5_5 = cross(Z4, Oc5-O4);
J5 = [JV5_1, JV5_2 , JV5_3, JV5_4, JV5_5, B;Z0,Z1,Z2,Z3,Z4,B];
Jv5 = J5([1 2 3],  :);
Jw5 = J5([4 5 6], :);
%質心6的雅克比
JV6_1 = cross(Z0, Oc6-O0);
JV6_2 = cross(Z1, Oc6-O1);
JV6_3 = cross(Z2, Oc6-O2);
JV6_4 = cross(Z3, Oc6-O3);
JV6_5 = cross(Z4, Oc6-O4);
JV6_6 = cross(Z5, Oc6-O5);
J6 = [JV6_1, JV6_2 , JV6_3, JV6_4, JV6_5, JV6_6;Z0,Z1,Z2,Z3,Z4,Z5];
Jv6 = J6([1 2 3],  :);
Jw6 = J6([4 5 6], :);

%動能定理準備:
R0c1 = T0c1([1 2 3],[1 2 3]);
R0c2 = T0c2([1 2 3],[1 2 3]);
R0c3 = T0c3([1 2 3],[1 2 3]);
R0c4 = T0c4([1 2 3],[1 2 3]);
R0c5 = T0c5([1 2 3],[1 2 3]);
R0c6 = T0c6([1 2 3],[1 2 3]);

%動能定理:
D1 = m1.* (Jv1.' * Jv1)+Jw1.' *(R0c1*Ic1* R0c1.') *Jw1;
D2 = m2.* (Jv2.' * Jv2)+Jw2.' *(R0c2*Ic2* R0c2.') *Jw2;
D3 = m3.* (Jv3.' * Jv3)+Jw3.' *(R0c3*Ic3* R0c3.') *Jw3;
D4 = m4.* (Jv4.' * Jv4)+Jw4.' *(R0c4*Ic4* R0c4.' )*Jw4;
D5 = m5.* (Jv5.' * Jv5)+Jw5.' *(R0c5*Ic5* R0c5.') *Jw5;
D6 = m6.* (Jv6.' * Jv6)+Jw6.' *(R0c6*Ic6* R0c6.' )*Jw6;
%慣性矩陣
D= D1+D2+D3+D4+D5+D6;
%驗證是否對稱


%求重力矩陣
P1 =[x1;y1;z1]; %質心1在{1}的座標
P01 = T01([1 2 3],[1 2 3])*P1; %質心1在{0}系下座標

P2 =[x2;y2;z2]; %質心2在{2}的座標
P02 = T02([1 2 3],[1 2 3])*P2; %質心2在{0}系下座標

P3 =[x3;y3;z3]; %質心3在{3}的座標
P03 = T03([1 2 3],[1 2 3])*P3; %質心3在{0}系下座標

P4 =[x4;y4;z4]; %質心4在{4}的座標
P04 = T04([1 2 3],[1 2 3])*P4; %質心4在{0}系下座標

P5 =[x5;y5;z5]; %質心5在{5}的座標
P05 = T05([1 2 3],[1 2 3])*P5; %質心5在{0}系下座標

P6 =[x1;y1;z1]; %質心6在{6}的座標
P06 = T06([1 2 3],[1 2 3])*P6; %質心6在{0}系下座標

%勢能表示式
P = m1*g*P01(3,1)+m2*g*P02(3,1)+m3*g*P03(3,1)+m4*g*P04(3,1)+m5*g*P05(3,1)+m6*g*P06(3,1);

%勢能對theta1求偏導
G01 = diff( P,theta1);
%勢能對theta2求偏導
G02 = diff( P,theta2);
%勢能對theta3求偏導
G03 = diff( P,theta3);
%勢能對theta4求偏導
G04 = diff( P,theta4);
%勢能對theta5求偏導
G05 = diff( P,theta5);
%勢能對theta6求偏導
G06 = diff( P,theta6);

%重力矩陣
G = [G01; G02; G03; G04; G05; G06];

%%%離心哥式矩陣求解
%軸1相關
syms c111 c121 c131 c141 c151 c161 c211 c221 c231 c241 c251 c261 c311 c321 c331 c341 c351 c361
syms c411 c421 c431 c441 c451 c461 c511 c521 c531 c541 c551 c561 c611 c621 c631 c641 c651 c661
%軸2相關
syms c112 c122 c132 c142 c152 c162 c212 c222 c232 c242 c252 c262 c312 c322 c332 c342 c352 c362
syms c412 c422 c432 c442 c452 c462 c512 c522 c532 c542 c552 c562 c612 c622 c632 c642 c652 c662
%軸3相關
syms c113 c123 c133 c143 c153 c163 c213 c223 c233 c243 c253 c263 c313 c323 c333 c343 c353 c363
syms c413 c423 c433 c443 c453 c463 c513 c523 c533 c543 c553 c563 c613 c623 c633 c643 c653 c663
%軸4相關
syms c114 c124 c134 c144 c154 c164 c214 c224 c234 c244 c254 c264 c314 c324 c334 c344 c354 c364
syms c414 c424 c434 c444 c454 c464 c514 c524 c534 c544 c554 c564 c614 c624 c634 c644 c654 c664
%軸5相關
syms c115 c125 c135 c145 c155 c165 c215 c225 c235 c245 c255 c265 c315 c325 c335 c345 c355 c365
syms c415 c425 c435 c445 c455 c465 c515 c525 c535 c545 c555 c565 c615 c625 c635 c645 c655 c665
%軸6相關
syms c116 c126 c136 c146 c156 c166 c216 c226 c236 c246 c256 c266 c316 c326 c336 c346 c356 c366
syms c416 c426 c436 c446 c456 c466 c516 c526 c536 c546 c556 c566 c616 c626 c636 c646 c656 c666

%定義角速度角加速度
syms dtheta1 dtheta2 dtheta3 dtheta4 dtheta5 dtheta6 ddtheta1 ddtheta2 ddtheta3 ddtheta4 ddtheta5 ddtheta6
syms C11 C12 C13 C14 C15 C16  C21 C22 C23 C24 C25 C26  C31 C32 C33 C34 C35 C36 
syms C41 C42 C43 C44 C45 C46  C51 C52 C53 C54 C55 C56  C61 C62 C63 C64 C65 C66  
%計算1軸相關引數
c111 = 0.5*(diff(D(1,1), theta1) + diff(D(1,1), theta1)- diff(D(1,1), theta1));
c211 = 0.5*(diff(D(1,1), theta2) + diff(D(1,2), theta1)- diff(D(2,1), theta1));
c311 = 0.5*(diff(D(1,1), theta3) + diff(D(1,3), theta1)- diff(D(3,1), theta1));
c411 = 0.5*(diff(D(1,1), theta4) + diff(D(1,4), theta1)- diff(D(4,1), theta1));
c511 = 0.5*(diff(D(1,1), theta5) + diff(D(1,5), theta1)- diff(D(5,1), theta1));
c611 = 0.5*(diff(D(1,1), theta6) + diff(D(1,6), theta1)- diff(D(6,1), theta1));

c121 = 0.5*(diff(D(1,2), theta1) + diff(D(1,1), theta2)- diff(D(1,2), theta1));
c221 = 0.5*(diff(D(1,2), theta2) + diff(D(1,2), theta2)- diff(D(2,2), theta1));
c321 = 0.5*(diff(D(1,2), theta3) + diff(D(1,3), theta2)- diff(D(3,2), theta1));
c421 = 0.5*(diff(D(1,2), theta4) + diff(D(1,4), theta2)- diff(D(4,2), theta1));
c521 = 0.5*(diff(D(1,2), theta5) + diff(D(1,5), theta2)- diff(D(5,2), theta1));
c621 = 0.5*(diff(D(1,2), theta6) + diff(D(1,6), theta2)- diff(D(6,2), theta1));

c131 = 0.5*(diff(D(1,3), theta1) + diff(D(1,1), theta3)- diff(D(1,3), theta1));
c231 = 0.5*(diff(D(1,3), theta2) + diff(D(1,2), theta3)- diff(D(2,3), theta1));
c331 = 0.5*(diff(D(1,3), theta3) + diff(D(1,3), theta3)- diff(D(3,3), theta1));
c431 = 0.5*(diff(D(1,3), theta4) + diff(D(1,4), theta3)- diff(D(4,3), theta1));
c531 = 0.5*(diff(D(1,3), theta5) + diff(D(1,5), theta3)- diff(D(5,3), theta1));
c631 = 0.5*(diff(D(1,3), theta6) + diff(D(1,6), theta3)- diff(D(6,3), theta1));

c141 = 0.5*(diff(D(1,4), theta1) + diff(D(1,1), theta4)- diff(D(1,4), theta1));
c241 = 0.5*(diff(D(1,4), theta2) + diff(D(1,2), theta4)- diff(D(2,4), theta1));
c341 = 0.5*(diff(D(1,4), theta3) + diff(D(1,3), theta4)- diff(D(3,4), theta1));
c441 = 0.5*(diff(D(1,4), theta4) + diff(D(1,4), theta4)- diff(D(4,4), theta1));
c541 = 0.5*(diff(D(1,4), theta5) + diff(D(1,5), theta4)- diff(D(5,4), theta1));
c641 = 0.5*(diff(D(1,4), theta6) + diff(D(1,6), theta4)- diff(D(6,4), theta1));

c151 = 0.5*(diff(D(1,5), theta1) + diff(D(1,1), theta5)- diff(D(1,5), theta1));
c251 = 0.5*(diff(D(1,5), theta2) + diff(D(1,2), theta5)- diff(D(2,5), theta1));
c351 = 0.5*(diff(D(1,5), theta3) + diff(D(1,3), theta5)- diff(D(3,5), theta1));
c451 = 0.5*(diff(D(1,5), theta4) + diff(D(1,4), theta5)- diff(D(4,5), theta1));
c551 = 0.5*(diff(D(1,5), theta5) + diff(D(1,5), theta5)- diff(D(5,5), theta1));
c651 = 0.5*(diff(D(1,5), theta6) + diff(D(1,6), theta5)- diff(D(6,5), theta1));

c161 = 0.5*(diff(D(1,6), theta1) + diff(D(1,1), theta6)- diff(D(1,6), theta1));
c261 = 0.5*(diff(D(1,6), theta2) + diff(D(1,2), theta6)- diff(D(2,6), theta1));
c361 = 0.5*(diff(D(1,6), theta3) + diff(D(1,3), theta6)- diff(D(3,6), theta1));
c461 = 0.5*(diff(D(1,6), theta4) + diff(D(1,4), theta6)- diff(D(4,6), theta1));
c561 = 0.5*(diff(D(1,6), theta5) + diff(D(1,5), theta6)- diff(D(5,6), theta1));
c661 = 0.5*(diff(D(1,6), theta6) + diff(D(1,6), theta6)- diff(D(6,6), theta1));



%離心哥式項矩陣第一行第一列
C11 =  c111*dtheta1 + c211*dtheta2 + c311*dtheta3 + c411*dtheta4+ c511*dtheta5+ c611*dtheta6 ;

C12 =  c121*dtheta1 + c221*dtheta2 + c321*dtheta3 + c421*dtheta4+ c521*dtheta5+ c621*dtheta6 ;

C13 =  c131*dtheta1 + c231*dtheta2 + c331*dtheta3 + c431*dtheta4+ c531*dtheta5+ c631*dtheta6 ;

C14 =  c141*dtheta1 + c241*dtheta2 + c341*dtheta3 + c441*dtheta4+ c541*dtheta5+ c641*dtheta6 ;

C15 =  c151*dtheta1 + c251*dtheta2 + c351*dtheta3 + c451*dtheta4+ c551*dtheta5+ c651*dtheta6 ;

C16 =  c161*dtheta1 + c261*dtheta2 + c361*dtheta3 + c461*dtheta4+ c561*dtheta5+ c661*dtheta6 ;




%計算2軸相關引數
c112 = 0.5*(diff(D(2,1), theta1) + diff(D(2,1), theta1)- diff(D(1,1), theta2));
c212 = 0.5*(diff(D(2,1), theta2) + diff(D(2,2), theta1)- diff(D(2,1), theta2));
c312 = 0.5*(diff(D(2,1), theta3) + diff(D(2,3), theta1)- diff(D(3,1), theta2));
c412 = 0.5*(diff(D(2,1), theta4) + diff(D(2,4), theta1)- diff(D(4,1), theta2));
c512 = 0.5*(diff(D(2,1), theta5) + diff(D(2,5), theta1)- diff(D(5,1), theta2));
c612 = 0.5*(diff(D(2,1), theta6) + diff(D(2,6), theta1)- diff(D(6,1), theta2));

c122 = 0.5*(diff(D(2,2), theta1) + diff(D(2,1), theta2)- diff(D(1,2), theta2));
c222 = 0.5*(diff(D(2,2), theta2) + diff(D(2,2), theta2)- diff(D(2,2), theta2));
c322 = 0.5*(diff(D(2,2), theta3) + diff(D(2,3), theta2)- diff(D(3,2), theta2));
c422 = 0.5*(diff(D(2,2), theta4) + diff(D(2,4), theta2)- diff(D(4,2), theta2));
c522 = 0.5*(diff(D(2,2), theta5) + diff(D(2,5), theta2)- diff(D(5,2), theta2));
c622 = 0.5*(diff(D(2,2), theta6) + diff(D(2,6), theta2)- diff(D(6,2), theta2));

c132 = 0.5*(diff(D(2,3), theta1) + diff(D(2,1), theta3)- diff(D(1,3), theta2));
c232 = 0.5*(diff(D(2,3), theta2) + diff(D(2,2), theta3)- diff(D(2,3), theta2));
c332 = 0.5*(diff(D(2,3), theta3) + diff(D(2,3), theta3)- diff(D(3,3), theta2));
c432 = 0.5*(diff(D(2,3), theta4) + diff(D(2,4), theta3)- diff(D(4,3), theta2));
c532 = 0.5*(diff(D(2,3), theta5) + diff(D(2,5), theta3)- diff(D(5,3), theta2));
c632 = 0.5*(diff(D(2,3), theta6) + diff(D(2,6), theta3)- diff(D(6,3), theta2));

c142 = 0.5*(diff(D(2,4), theta1) + diff(D(2,1), theta4)- diff(D(1,4), theta2));
c242 = 0.5*(diff(D(2,4), theta2) + diff(D(2,2), theta4)- diff(D(2,4), theta2));
c342 = 0.5*(diff(D(2,4), theta3) + diff(D(2,3), theta4)- diff(D(3,4), theta2));
c442 = 0.5*(diff(D(2,4), theta4) + diff(D(2,4), theta4)- diff(D(4,4), theta2));
c542 = 0.5*(diff(D(2,4), theta5) + diff(D(2,5), theta4)- diff(D(5,4), theta2));
c642 = 0.5*(diff(D(2,4), theta6) + diff(D(2,6), theta4)- diff(D(6,4), theta2));

c152 = 0.5*(diff(D(2,5), theta1) + diff(D(2,1), theta5)- diff(D(1,5), theta2));
c252 = 0.5*(diff(D(2,5), theta2) + diff(D(2,2), theta5)- diff(D(2,5), theta2));
c352 = 0.5*(diff(D(2,5), theta3) + diff(D(2,3), theta5)- diff(D(3,5), theta2));
c452 = 0.5*(diff(D(2,5), theta4) + diff(D(2,4), theta5)- diff(D(4,5), theta2));
c552 = 0.5*(diff(D(2,5), theta5) + diff(D(2,5), theta5)- diff(D(5,5), theta2));
c652 = 0.5*(diff(D(2,5), theta6) + diff(D(2,6), theta5)- diff(D(6,5), theta2));

c162 = 0.5*(diff(D(2,6), theta1) + diff(D(2,1), theta6)- diff(D(1,6), theta2));
c262 = 0.5*(diff(D(2,6), theta2) + diff(D(2,2), theta6)- diff(D(2,6), theta2));
c362 = 0.5*(diff(D(2,6), theta3) + diff(D(2,3), theta6)- diff(D(3,6), theta2));
c462 = 0.5*(diff(D(2,6), theta4) + diff(D(2,4), theta6)- diff(D(4,6), theta2));
c562 = 0.5*(diff(D(2,6), theta5) + diff(D(2,5), theta6)- diff(D(5,6), theta2));
c662 = 0.5*(diff(D(2,6), theta6) + diff(D(2,6), theta6)- diff(D(6,6), theta2));



%離心哥式項矩陣第2行
C21 =  c112*dtheta1 + c212*dtheta2 + c312*dtheta3 + c412*dtheta4+ c512*dtheta5+ c612*dtheta6 ;

C22 =  c122*dtheta1 + c222*dtheta2 + c322*dtheta3 + c422*dtheta4+ c522*dtheta5+ c622*dtheta6 ;

C23 =  c132*dtheta1 + c232*dtheta2 + c332*dtheta3 + c432*dtheta4+ c532*dtheta5+ c632*dtheta6 ;

C24 =  c142*dtheta1 + c242*dtheta2 + c342*dtheta3 + c442*dtheta4+ c542*dtheta5+ c642*dtheta6 ;

C25 =  c152*dtheta1 + c252*dtheta2 + c352*dtheta3 + c452*dtheta4+ c552*dtheta5+ c652*dtheta6 ;

C26 =  c162*dtheta1 + c262*dtheta2 + c362*dtheta3 + c462*dtheta4+ c562*dtheta5+ c662*dtheta6 ;





%計算3軸相關引數
c113 = 0.5*(diff(D(3,1), theta1) + diff(D(3,1), theta1)- diff(D(1,1), theta3));
c213 = 0.5*(diff(D(3,1), theta2) + diff(D(3,2), theta1)- diff(D(2,1), theta3));
c313 = 0.5*(diff(D(3,1), theta3) + diff(D(3,3), theta1)- diff(D(3,1), theta3));
c413 = 0.5*(diff(D(3,1), theta4) + diff(D(3,4), theta1)- diff(D(4,1), theta3));
c513 = 0.5*(diff(D(3,1), theta5) + diff(D(3,5), theta1)- diff(D(5,1), theta3));
c613 = 0.5*(diff(D(3,1), theta6) + diff(D(3,6), theta1)- diff(D(6,1), theta3));

c123 = 0.5*(diff(D(3,2), theta1) + diff(D(3,1), theta2)- diff(D(1,2), theta3));
c223 = 0.5*(diff(D(3,2), theta2) + diff(D(3,2), theta2)- diff(D(2,2), theta3));
c323 = 0.5*(diff(D(3,2), theta3) + diff(D(3,3), theta2)- diff(D(3,2), theta3));
c423 = 0.5*(diff(D(3,2), theta4) + diff(D(3,4), theta2)- diff(D(4,2), theta3));
c523 = 0.5*(diff(D(3,2), theta5) + diff(D(3,5), theta2)- diff(D(5,2), theta3));
c623 = 0.5*(diff(D(3,2), theta6) + diff(D(3,6), theta2)- diff(D(6,2), theta3));

c133 = 0.5*(diff(D(3,3), theta1) + diff(D(3,1), theta3)- diff(D(1,3), theta3));
c233 = 0.5*(diff(D(3,3), theta2) + diff(D(3,2), theta3)- diff(D(2,3), theta3));
c333 = 0.5*(diff(D(3,3), theta3) + diff(D(3,3), theta3)- diff(D(3,3), theta3));
c433 = 0.5*(diff(D(3,3), theta4) + diff(D(3,4), theta3)- diff(D(4,3), theta3));
c533 = 0.5*(diff(D(3,3), theta5) + diff(D(3,5), theta3)- diff(D(5,3), theta3));
c633 = 0.5*(diff(D(3,3), theta6) + diff(D(3,6), theta3)- diff(D(6,3), theta3));

c143 = 0.5*(diff(D(3,4), theta1) + diff(D(3,1), theta4)- diff(D(1,4), theta3));
c243 = 0.5*(diff(D(3,4), theta2) + diff(D(3,2), theta4)- diff(D(2,4), theta3));
c343 = 0.5*(diff(D(3,4), theta3) + diff(D(3,3), theta4)- diff(D(3,4), theta3));
c443 = 0.5*(diff(D(3,4), theta4) + diff(D(3,4), theta4)- diff(D(4,4), theta3));
c543 = 0.5*(diff(D(3,4), theta5) + diff(D(3,5), theta4)- diff(D(5,4), theta3));
c643 = 0.5*(diff(D(3,4), theta6) + diff(D(3,6), theta4)- diff(D(6,4), theta3));

c153 = 0.5*(diff(D(3,5), theta1) + diff(D(3,1), theta5)- diff(D(1,5), theta3));
c253 = 0.5*(diff(D(3,5), theta2) + diff(D(3,2), theta5)- diff(D(2,5), theta3));
c353 = 0.5*(diff(D(3,5), theta3) + diff(D(3,3), theta5)- diff(D(3,5), theta3));
c453 = 0.5*(diff(D(3,5), theta4) + diff(D(3,4), theta5)- diff(D(4,5), theta3));
c553 = 0.5*(diff(D(3,5), theta5) + diff(D(3,5), theta5)- diff(D(5,5), theta3));
c653 = 0.5*(diff(D(3,5), theta6) + diff(D(3,6), theta5)- diff(D(6,5), theta3));

c163 = 0.5*(diff(D(3,6), theta1) + diff(D(3,1), theta6)- diff(D(1,6), theta3));
c263 = 0.5*(diff(D(3,6), theta2) + diff(D(3,2), theta6)- diff(D(2,6), theta3));
c363 = 0.5*(diff(D(3,6), theta3) + diff(D(3,3), theta6)- diff(D(3,6), theta3));
c463 = 0.5*(diff(D(3,6), theta4) + diff(D(3,4), theta6)- diff(D(4,6), theta3));
c563 = 0.5*(diff(D(3,6), theta5) + diff(D(3,5), theta6)- diff(D(5,6), theta3));
c663 = 0.5*(diff(D(3,6), theta6) + diff(D(3,6), theta6)- diff(D(6,6), theta3));



%離心哥式項矩陣第3行
C31 =  c113*dtheta1 + c213*dtheta2 + c313*dtheta3 + c413*dtheta4+ c513*dtheta5+ c613*dtheta6 ;

C32 =  c123*dtheta1 + c223*dtheta2 + c323*dtheta3 + c423*dtheta4+ c523*dtheta5+ c623*dtheta6 ;

C33 =  c133*dtheta1 + c233*dtheta2 + c333*dtheta3 + c433*dtheta4+ c533*dtheta5+ c633*dtheta6 ;

C34 =  c143*dtheta1 + c243*dtheta2 + c343*dtheta3 + c443*dtheta4+ c543*dtheta5+ c643*dtheta6 ;

C35 =  c153*dtheta1 + c253*dtheta2 + c353*dtheta3 + c453*dtheta4+ c553*dtheta5+ c653*dtheta6 ;

C36 =  c163*dtheta1 + c263*dtheta2 + c363*dtheta3 + c463*dtheta4+ c563*dtheta5+ c663*dtheta6 ;



%計算4軸相關引數
c114 = 0.5*(diff(D(4,1), theta1) + diff(D(4,1), theta1)- diff(D(1,1), theta4));
c214 = 0.5*(diff(D(4,1), theta2) + diff(D(4,2), theta1)- diff(D(2,1), theta4));
c314 = 0.5*(diff(D(4,1), theta3) + diff(D(4,3), theta1)- diff(D(3,1), theta4));
c414 = 0.5*(diff(D(4,1), theta4) + diff(D(4,4), theta1)- diff(D(4,1), theta4));
c514 = 0.5*(diff(D(4,1), theta5) + diff(D(4,5), theta1)- diff(D(5,1), theta4));
c614 = 0.5*(diff(D(4,1), theta6) + diff(D(4,6), theta1)- diff(D(6,1), theta4));

c124 = 0.5*(diff(D(4,2), theta1) + diff(D(4,1), theta2)- diff(D(1,2), theta4));
c224 = 0.5*(diff(D(4,2), theta2) + diff(D(4,2), theta2)- diff(D(2,2), theta4));
c324 = 0.5*(diff(D(4,2), theta3) + diff(D(4,3), theta2)- diff(D(3,2), theta4));
c424 = 0.5*(diff(D(4,2), theta4) + diff(D(4,4), theta2)- diff(D(4,2), theta4));
c524 = 0.5*(diff(D(4,2), theta5) + diff(D(4,5), theta2)- diff(D(5,2), theta4));
c624 = 0.5*(diff(D(4,2), theta6) + diff(D(4,6), theta2)- diff(D(6,2), theta4));

c134 = 0.5*(diff(D(4,3), theta1) + diff(D(4,1), theta3)- diff(D(1,3), theta4));
c234 = 0.5*(diff(D(4,3), theta2) + diff(D(4,2), theta3)- diff(D(2,3), theta4));
c334 = 0.5*(diff(D(4,3), theta3) + diff(D(4,3), theta3)- diff(D(3,3), theta4));
c434 = 0.5*(diff(D(4,3), theta4) + diff(D(4,4), theta3)- diff(D(4,3), theta4));
c534 = 0.5*(diff(D(4,3), theta5) + diff(D(4,5), theta3)- diff(D(5,3), theta4));
c634 = 0.5*(diff(D(4,3), theta6) + diff(D(4,6), theta3)- diff(D(6,3), theta4));

c144 = 0.5*(diff(D(4,4), theta1) + diff(D(4,1), theta4)- diff(D(1,4), theta4));
c244 = 0.5*(diff(D(4,4), theta2) + diff(D(4,2), theta4)- diff(D(2,4), theta4));
c344 = 0.5*(diff(D(4,4), theta3) + diff(D(4,3), theta4)- diff(D(3,4), theta4));
c444 = 0.5*(diff(D(4,4), theta4) + diff(D(4,4), theta4)- diff(D(4,4), theta4));
c544 = 0.5*(diff(D(4,4), theta5) + diff(D(4,5), theta4)- diff(D(5,4), theta4));
c644 = 0.5*(diff(D(4,4), theta6) + diff(D(4,6), theta4)- diff(D(6,4), theta4));

c154 = 0.5*(diff(D(4,5), theta1) + diff(D(4,1), theta5)- diff(D(1,5), theta4));
c254 = 0.5*(diff(D(4,5), theta2) + diff(D(4,2), theta5)- diff(D(2,5), theta4));
c354 = 0.5*(diff(D(4,5), theta3) + diff(D(4,3), theta5)- diff(D(3,5), theta4));
c454 = 0.5*(diff(D(4,5), theta4) + diff(D(4,4), theta5)- diff(D(4,5), theta4));
c554 = 0.5*(diff(D(4,5), theta5) + diff(D(4,5), theta5)- diff(D(5,5), theta4));
c654 = 0.5*(diff(D(4,5), theta6) + diff(D(4,6), theta5)- diff(D(6,5), theta4));

c164 = 0.5*(diff(D(4,6), theta1) + diff(D(4,1), theta6)- diff(D(1,6), theta4));
c264 = 0.5*(diff(D(4,6), theta2) + diff(D(4,2), theta6)- diff(D(2,6), theta4));
c364 = 0.5*(diff(D(4,6), theta3) + diff(D(4,3), theta6)- diff(D(3,6), theta4));
c464 = 0.5*(diff(D(4,6), theta4) + diff(D(4,4), theta6)- diff(D(4,6), theta4));
c564 = 0.5*(diff(D(4,6), theta5) + diff(D(4,5), theta6)- diff(D(5,6), theta4));
c664 = 0.5*(diff(D(4,6), theta6) + diff(D(4,6), theta6)- diff(D(6,6), theta4));



%離心哥式項矩陣第4行
C41 =  c114*dtheta1 + c214*dtheta2 + c314*dtheta3 + c414*dtheta4+ c514*dtheta5+ c614*dtheta6 ;

C42 =  c124*dtheta1 + c224*dtheta2 + c324*dtheta3 + c424*dtheta4+ c524*dtheta5+ c624*dtheta6 ;

C43 =  c134*dtheta1 + c234*dtheta2 + c334*dtheta3 + c434*dtheta4+ c534*dtheta5+ c634*dtheta6 ;

C44 =  c144*dtheta1 + c244*dtheta2 + c344*dtheta3 + c444*dtheta4+ c544*dtheta5+ c644*dtheta6 ;

C45 =  c154*dtheta1 + c254*dtheta2 + c354*dtheta3 + c454*dtheta4+ c554*dtheta5+ c654*dtheta6 ;

C46 =  c164*dtheta1 + c264*dtheta2 + c364*dtheta3 + c464*dtheta4+ c564*dtheta5+ c664*dtheta6 ;



%計算5軸相關引數
c115 = 0.5*(diff(D(5,1), theta1) + diff(D(5,1), theta1)- diff(D(1,1), theta5));
c215 = 0.5*(diff(D(5,1), theta2) + diff(D(5,2), theta1)- diff(D(2,1), theta5));
c315 = 0.5*(diff(D(5,1), theta3) + diff(D(5,3), theta1)- diff(D(3,1), theta5));
c415 = 0.5*(diff(D(5,1), theta4) + diff(D(5,4), theta1)- diff(D(4,1), theta5));
c515 = 0.5*(diff(D(5,1), theta5) + diff(D(5,5), theta1)- diff(D(5,1), theta5));
c615 = 0.5*(diff(D(5,1), theta6) + diff(D(5,6), theta1)- diff(D(6,1), theta5));

c125 = 0.5*(diff(D(5,2), theta1) + diff(D(5,1), theta2)- diff(D(1,2), theta5));
c225 = 0.5*(diff(D(5,2), theta2) + diff(D(5,2), theta2)- diff(D(2,2), theta5));
c325 = 0.5*(diff(D(5,2), theta3) + diff(D(5,3), theta2)- diff(D(3,2), theta5));
c425 = 0.5*(diff(D(5,2), theta4) + diff(D(5,4), theta2)- diff(D(4,2), theta5));
c525 = 0.5*(diff(D(5,2), theta5) + diff(D(5,5), theta2)- diff(D(5,2), theta5));
c625 = 0.5*(diff(D(5,2), theta6) + diff(D(5,6), theta2)- diff(D(6,2), theta5));

c135 = 0.5*(diff(D(5,3), theta1) + diff(D(5,1), theta3)- diff(D(1,3), theta5));
c235 = 0.5*(diff(D(5,3), theta2) + diff(D(5,2), theta3)- diff(D(2,3), theta5));
c335 = 0.5*(diff(D(5,3), theta3) + diff(D(5,3), theta3)- diff(D(3,3), theta5));
c435 = 0.5*(diff(D(5,3), theta4) + diff(D(5,4), theta3)- diff(D(4,3), theta5));
c535 = 0.5*(diff(D(5,3), theta5) + diff(D(5,5), theta3)- diff(D(5,3), theta5));
c635 = 0.5*(diff(D(5,3), theta6) + diff(D(5,6), theta3)- diff(D(6,3), theta5));

c145 = 0.5*(diff(D(5,4), theta1) + diff(D(5,1), theta4)- diff(D(1,4), theta5));
c245 = 0.5*(diff(D(5,4), theta2) + diff(D(5,2), theta4)- diff(D(2,4), theta5));
c345 = 0.5*(diff(D(5,4), theta3) + diff(D(5,3), theta4)- diff(D(3,4), theta5));
c445 = 0.5*(diff(D(5,4), theta4) + diff(D(5,4), theta4)- diff(D(4,4), theta5));
c545 = 0.5*(diff(D(5,4), theta5) + diff(D(5,5), theta4)- diff(D(5,4), theta5));
c645 = 0.5*(diff(D(5,4), theta6) + diff(D(5,6), theta4)- diff(D(6,4), theta5));

c155 = 0.5*(diff(D(5,5), theta1) + diff(D(5,1), theta5)- diff(D(1,5), theta5));
c255 = 0.5*(diff(D(5,5), theta2) + diff(D(5,2), theta5)- diff(D(2,5), theta5));
c355 = 0.5*(diff(D(5,5), theta3) + diff(D(5,3), theta5)- diff(D(3,5), theta5));
c455 = 0.5*(diff(D(5,5), theta4) + diff(D(5,4), theta5)- diff(D(4,5), theta5));
c555 = 0.5*(diff(D(5,5), theta5) + diff(D(5,5), theta5)- diff(D(5,5), theta5));
c655 = 0.5*(diff(D(5,5), theta6) + diff(D(5,6), theta5)- diff(D(6,5), theta5));

c165 = 0.5*(diff(D(5,6), theta1) + diff(D(5,1), theta6)- diff(D(1,6), theta5));
c265 = 0.5*(diff(D(5,6), theta2) + diff(D(5,2), theta6)- diff(D(2,6), theta5));
c365 = 0.5*(diff(D(5,6), theta3) + diff(D(5,3), theta6)- diff(D(3,6), theta5));
c465 = 0.5*(diff(D(5,6), theta4) + diff(D(5,4), theta6)- diff(D(4,6), theta5));
c565 = 0.5*(diff(D(5,6), theta5) + diff(D(5,5), theta6)- diff(D(5,6), theta5));
c665 = 0.5*(diff(D(5,6), theta6) + diff(D(5,6), theta6)- diff(D(6,6), theta5));



%離心哥式項矩陣第5行
C51 =  c115*dtheta1 + c215*dtheta2 + c315*dtheta3 + c415*dtheta4+ c515*dtheta5+ c615*dtheta6 ;

C52 =  c125*dtheta1 + c225*dtheta2 + c325*dtheta3 + c425*dtheta4+ c525*dtheta5+ c625*dtheta6 ;

C53 =  c135*dtheta1 + c235*dtheta2 + c335*dtheta3 + c435*dtheta4+ c535*dtheta5+ c635*dtheta6 ;

C54 =  c145*dtheta1 + c245*dtheta2 + c345*dtheta3 + c445*dtheta4+ c545*dtheta5+ c645*dtheta6 ;

C55 =  c155*dtheta1 + c255*dtheta2 + c355*dtheta3 + c455*dtheta4+ c555*dtheta5+ c655*dtheta6 ;

C56 =  c165*dtheta1 + c265*dtheta2 + c365*dtheta3 + c465*dtheta4+ c565*dtheta5+ c665*dtheta6 ;



%計算6軸相關引數
c116 = 0.5*(diff(D(6,1), theta1) + diff(D(6,1), theta1)- diff(D(1,1), theta6));
c216 = 0.5*(diff(D(6,1), theta2) + diff(D(6,2), theta1)- diff(D(2,1), theta6));
c316 = 0.5*(diff(D(6,1), theta3) + diff(D(6,3), theta1)- diff(D(3,1), theta6));
c416 = 0.5*(diff(D(6,1), theta4) + diff(D(6,4), theta1)- diff(D(4,1), theta6));
c516 = 0.5*(diff(D(6,1), theta5) + diff(D(6,5), theta1)- diff(D(5,1), theta6));
c616 = 0.5*(diff(D(6,1), theta6) + diff(D(6,6), theta1)- diff(D(6,1), theta6));

c126 = 0.5*(diff(D(6,2), theta1) + diff(D(6,1), theta2)- diff(D(1,2), theta6));
c226 = 0.5*(diff(D(6,2), theta2) + diff(D(6,2), theta2)- diff(D(2,2), theta6));
c326 = 0.5*(diff(D(6,2), theta3) + diff(D(6,3), theta2)- diff(D(3,2), theta6));
c426 = 0.5*(diff(D(6,2), theta4) + diff(D(6,4), theta2)- diff(D(4,2), theta6));
c526 = 0.5*(diff(D(6,2), theta5) + diff(D(6,5), theta2)- diff(D(5,2), theta6));
c626 = 0.5*(diff(D(6,2), theta6) + diff(D(6,6), theta2)- diff(D(6,2), theta6));

c136 = 0.5*(diff(D(6,3), theta1) + diff(D(6,1), theta3)- diff(D(1,3), theta6));
c236 = 0.5*(diff(D(6,3), theta2) + diff(D(6,2), theta3)- diff(D(2,3), theta6));
c336 = 0.5*(diff(D(6,3), theta3) + diff(D(6,3), theta3)- diff(D(3,3), theta6));
c436 = 0.5*(diff(D(6,3), theta4) + diff(D(6,4), theta3)- diff(D(4,3), theta6));
c536 = 0.5*(diff(D(6,3), theta5) + diff(D(6,5), theta3)- diff(D(5,3), theta6));
c636 = 0.5*(diff(D(6,3), theta6) + diff(D(6,6), theta3)- diff(D(6,3), theta6));

c146 = 0.5*(diff(D(6,4), theta1) + diff(D(6,1), theta4)- diff(D(1,4), theta6));
c246 = 0.5*(diff(D(6,4), theta2) + diff(D(6,2), theta4)- diff(D(2,4), theta6));
c346 = 0.5*(diff(D(6,4), theta3) + diff(D(6,3), theta4)- diff(D(3,4), theta6));
c446 = 0.5*(diff(D(6,4), theta4) + diff(D(6,4), theta4)- diff(D(4,4), theta6));
c546 = 0.5*(diff(D(6,4), theta5) + diff(D(6,5), theta4)- diff(D(5,4), theta6));
c646 = 0.5*(diff(D(6,4), theta6) + diff(D(6,6), theta4)- diff(D(6,4), theta6));

c156 = 0.5*(diff(D(6,5), theta1) + diff(D(6,1), theta5)- diff(D(1,5), theta6));
c256 = 0.5*(diff(D(6,5), theta2) + diff(D(6,2), theta5)- diff(D(2,5), theta6));
c356 = 0.5*(diff(D(6,5), theta3) + diff(D(6,3), theta5)- diff(D(3,5), theta6));
c456 = 0.5*(diff(D(6,5), theta4) + diff(D(6,4), theta5)- diff(D(4,5), theta6));
c556 = 0.5*(diff(D(6,5), theta5) + diff(D(6,5), theta5)- diff(D(5,5), theta6));
c656 = 0.5*(diff(D(6,5), theta6) + diff(D(6,6), theta5)- diff(D(6,5), theta6));

c166 = 0.5*(diff(D(6,6), theta1) + diff(D(6,1), theta6)- diff(D(1,6), theta6));
c266 = 0.5*(diff(D(6,6), theta2) + diff(D(6,2), theta6)- diff(D(2,6), theta6));
c366 = 0.5*(diff(D(6,6), theta3) + diff(D(6,3), theta6)- diff(D(3,6), theta6));
c466 = 0.5*(diff(D(6,6), theta4) + diff(D(6,4), theta6)- diff(D(4,6), theta6));
c566 = 0.5*(diff(D(6,6), theta5) + diff(D(6,5), theta6)- diff(D(5,6), theta6));
c666 = 0.5*(diff(D(6,6), theta6) + diff(D(6,6), theta6)- diff(D(6,6), theta6));



%離心哥式項矩陣第6行
C61 =  c116*dtheta1 + c216*dtheta2 + c316*dtheta3 + c416*dtheta4+ c516*dtheta5+ c616*dtheta6 ;

C62 =  c126*dtheta1 + c226*dtheta2 + c326*dtheta3 + c426*dtheta4+ c526*dtheta5+ c626*dtheta6 ;

C63 =  c136*dtheta1 + c236*dtheta2 + c336*dtheta3 + c436*dtheta4+ c536*dtheta5+ c636*dtheta6 ;

C64 =  c146*dtheta1 + c246*dtheta2 + c346*dtheta3 + c446*dtheta4+ c546*dtheta5+ c646*dtheta6 ;

C65 =  c156*dtheta1 + c256*dtheta2 + c356*dtheta3 + c456*dtheta4+ c556*dtheta5+ c656*dtheta6 ;

C66 =  c166*dtheta1 + c266*dtheta2 + c366*dtheta3 + c466*dtheta4+ c566*dtheta5+ c666*dtheta6 ;


%離心哥式矩陣
C = [C11,C12,C13,C14,C15,C16;
    C21,C22,C23,C24,C25,C26; 
    C31,C32,C33,C34,C35,C36; 
    C41,C42,C43,C44,C45,C46; 
    C51,C52,C53,C54,C55,C56; 
    C61,C62,C63,C64,C65,C66];

%驗證慣性矩陣和離心哥式矩陣之間的一個重要關係,反對稱性!
%慣性矩陣每一項求導
syms dd11 dd12 dd13 dd14 dd15 dd16 dd21 dd22 dd23 dd24 dd25 dd26 dd31 dd32 dd33 dd34 dd35 dd36
syms dd41 dd42 dd43 dd44 dd45 dd46 dd51 dd52 dd53 dd54 dd55 dd56 dd61 dd62 dd63 dd64 dd65 dd66

dd11 = diff(D(1,1),theta1)* dtheta1 +diff(D(1,1),theta2)* dtheta2 +diff(D(1,1),theta3)* dtheta3 +diff(D(1,1),theta4)* dtheta4 +diff(D(1,1),theta5)* dtheta5 +diff(D(1,1),theta6)* dtheta6; 

dd12 = diff(D(1,2),theta1)* dtheta1 +diff(D(1,2),theta2)* dtheta2 +diff(D(1,2),theta3)* dtheta3 +diff(D(1,2),theta4)* dtheta4 +diff(D(1,2),theta5)* dtheta5 +diff(D(1,2),theta6)* dtheta6;

dd13 = diff(D(1,3),theta1)* dtheta1 +diff(D(1,3),theta2)* dtheta2 +diff(D(1,3),theta3)* dtheta3 +diff(D(1,3),theta4)* dtheta4 +diff(D(1,3),theta5)* dtheta5 +diff(D(1,3),theta6)* dtheta6;

dd14 = diff(D(1,4),theta1)* dtheta1 +diff(D(1,4),theta2)* dtheta2 +diff(D(1,4),theta3)* dtheta3 +diff(D(1,4),theta4)* dtheta4 +diff(D(1,4),theta5)* dtheta5 +diff(D(1,4),theta6)* dtheta6;

dd15 = diff(D(1,5),theta1)* dtheta1 +diff(D(1,5),theta2)* dtheta2 +diff(D(1,5),theta3)* dtheta3 +diff(D(1,5),theta4)* dtheta4 +diff(D(1,5),theta5)* dtheta5 +diff(D(1,5),theta6)* dtheta6;

dd16 = diff(D(1,6),theta1)* dtheta1 +diff(D(1,6),theta2)* dtheta2 +diff(D(1,6),theta3)* dtheta3 +diff(D(1,6),theta4)* dtheta4 +diff(D(1,6),theta5)* dtheta5 +diff(D(1,6),theta6)* dtheta6;


dd21 = diff(D(2,1),theta1)* dtheta1 +diff(D(2,1),theta2)* dtheta2 +diff(D(2,1),theta3)* dtheta3 +diff(D(2,1),theta4)* dtheta4 +diff(D(2,1),theta5)* dtheta5 +diff(D(2,1),theta6)* dtheta6; 

dd22 = diff(D(2,2),theta1)* dtheta1 +diff(D(2,2),theta2)* dtheta2 +diff(D(2,2),theta3)* dtheta3 +diff(D(2,2),theta4)* dtheta4 +diff(D(2,2),theta5)* dtheta5 +diff(D(2,2),theta6)* dtheta6;

dd23 = diff(D(2,3),theta1)* dtheta1 +diff(D(2,3),theta2)* dtheta2 +diff(D(2,3),theta3)* dtheta3 +diff(D(2,3),theta4)* dtheta4 +diff(D(2,3),theta5)* dtheta5 +diff(D(2,3),theta6)* dtheta6;

dd24 = diff(D(2,4),theta1)* dtheta1 +diff(D(2,4),theta2)* dtheta2 +diff(D(2,4),theta3)* dtheta3 +diff(D(2,4),theta4)* dtheta4 +diff(D(2,4),theta5)* dtheta5 +diff(D(2,4),theta6)* dtheta6;

dd25 = diff(D(2,5),theta1)* dtheta1 +diff(D(2,5),theta2)* dtheta2 +diff(D(2,5),theta3)* dtheta3 +diff(D(2,5),theta4)* dtheta4 +diff(D(2,5),theta5)* dtheta5 +diff(D(2,5),theta6)* dtheta6;

dd26 = diff(D(2,6),theta1)* dtheta1 +diff(D(2,6),theta2)* dtheta2 +diff(D(2,6),theta3)* dtheta3 +diff(D(2,6),theta4)* dtheta4 +diff(D(2,6),theta5)* dtheta5 +diff(D(2,6),theta6)* dtheta6;


dd31 = diff(D(3,1),theta1)* dtheta1 +diff(D(3,1),theta2)* dtheta2 +diff(D(3,1),theta3)* dtheta3 +diff(D(3,1),theta4)* dtheta4 +diff(D(3,1),theta5)* dtheta5 +diff(D(3,1),theta6)* dtheta6; 

dd32 = diff(D(3,2),theta1)* dtheta1 +diff(D(3,2),theta2)* dtheta2 +diff(D(3,2),theta3)* dtheta3 +diff(D(3,2),theta4)* dtheta4 +diff(D(3,2),theta5)* dtheta5 +diff(D(3,2),theta6)* dtheta6;

dd33 = diff(D(3,3),theta1)* dtheta1 +diff(D(3,3),theta2)* dtheta2 +diff(D(3,3),theta3)* dtheta3 +diff(D(3,3),theta4)* dtheta4 +diff(D(3,3),theta5)* dtheta5 +diff(D(3,3),theta6)* dtheta6;

dd34 = diff(D(3,4),theta1)* dtheta1 +diff(D(3,4),theta2)* dtheta2 +diff(D(3,4),theta3)* dtheta3 +diff(D(3,4),theta4)* dtheta4 +diff(D(3,4),theta5)* dtheta5 +diff(D(3,4),theta6)* dtheta6;

dd35 = diff(D(3,5),theta1)* dtheta1 +diff(D(3,5),theta2)* dtheta2 +diff(D(3,5),theta3)* dtheta3 +diff(D(3,5),theta4)* dtheta4 +diff(D(3,5),theta5)* dtheta5 +diff(D(3,5),theta6)* dtheta6;

dd36 = diff(D(3,6),theta1)* dtheta1 +diff(D(3,6),theta2)* dtheta2 +diff(D(3,6),theta3)* dtheta3 +diff(D(3,6),theta4)* dtheta4 +diff(D(3,6),theta5)* dtheta5 +diff(D(3,6),theta6)* dtheta6;


dd41 = diff(D(4,1),theta1)* dtheta1 +diff(D(4,1),theta2)* dtheta2 +diff(D(4,1),theta3)* dtheta3 +diff(D(4,1),theta4)* dtheta4 +diff(D(4,1),theta5)* dtheta5 +diff(D(4,1),theta6)* dtheta6; 

dd42 = diff(D(4,2),theta1)* dtheta1 +diff(D(4,2),theta2)* dtheta2 +diff(D(4,2),theta3)* dtheta3 +diff(D(4,2),theta4)* dtheta4 +diff(D(4,2),theta5)* dtheta5 +diff(D(4,2),theta6)* dtheta6;

dd43 = diff(D(4,3),theta1)* dtheta1 +diff(D(4,3),theta2)* dtheta2 +diff(D(4,3),theta3)* dtheta3 +diff(D(4,3),theta4)* dtheta4 +diff(D(4,3),theta5)* dtheta5 +diff(D(4,3),theta6)* dtheta6;

dd44 = diff(D(4,4),theta1)* dtheta1 +diff(D(4,4),theta2)* dtheta2 +diff(D(4,4),theta3)* dtheta3 +diff(D(4,4),theta4)* dtheta4 +diff(D(4,4),theta5)* dtheta5 +diff(D(4,4),theta6)* dtheta6;

dd45 = diff(D(4,5),theta1)* dtheta1 +diff(D(4,5),theta2)* dtheta2 +diff(D(4,5),theta3)* dtheta3 +diff(D(4,5),theta4)* dtheta4 +diff(D(4,5),theta5)* dtheta5 +diff(D(4,5),theta6)* dtheta6;

dd46 = diff(D(4,6),theta1)* dtheta1 +diff(D(4,6),theta2)* dtheta2 +diff(D(4,6),theta3)* dtheta3 +diff(D(4,6),theta4)* dtheta4 +diff(D(4,6),theta5)* dtheta5 +diff(D(4,6),theta6)* dtheta6;


dd51 = diff(D(5,1),theta1)* dtheta1 +diff(D(5,1),theta2)* dtheta2 +diff(D(5,1),theta3)* dtheta3 +diff(D(5,1),theta4)* dtheta4 +diff(D(5,1),theta5)* dtheta5 +diff(D(5,1),theta6)* dtheta6; 

dd52 = diff(D(5,2),theta1)* dtheta1 +diff(D(5,2),theta2)* dtheta2 +diff(D(5,2),theta3)* dtheta3 +diff(D(5,2),theta4)* dtheta4 +diff(D(5,2),theta5)* dtheta5 +diff(D(5,2),theta6)* dtheta6;

dd53 = diff(D(5,3),theta1)* dtheta1 +diff(D(5,3),theta2)* dtheta2 +diff(D(5,3),theta3)* dtheta3 +diff(D(5,3),theta4)* dtheta4 +diff(D(5,3),theta5)* dtheta5 +diff(D(5,3),theta6)* dtheta6;

dd54 = diff(D(5,4),theta1)* dtheta1 +diff(D(5,4),theta2)* dtheta2 +diff(D(5,4),theta3)* dtheta3 +diff(D(5,4),theta4)* dtheta4 +diff(D(5,4),theta5)* dtheta5 +diff(D(5,4),theta6)* dtheta6;

dd55 = diff(D(5,5),theta1)* dtheta1 +diff(D(5,5),theta2)* dtheta2 +diff(D(5,5),theta3)* dtheta3 +diff(D(5,5),theta4)* dtheta4 +diff(D(5,5),theta5)* dtheta5 +diff(D(5,5),theta6)* dtheta6;

dd56 = diff(D(5,6),theta1)* dtheta1 +diff(D(5,6),theta2)* dtheta2 +diff(D(5,6),theta3)* dtheta3 +diff(D(5,6),theta4)* dtheta4 +diff(D(5,6),theta5)* dtheta5 +diff(D(5,6),theta6)* dtheta6;


dd61 = diff(D(6,1),theta1)* dtheta1 +diff(D(6,1),theta2)* dtheta2 +diff(D(6,1),theta3)* dtheta3 +diff(D(6,1),theta4)* dtheta4 +diff(D(6,1),theta5)* dtheta5 +diff(D(6,1),theta6)* dtheta6; 

dd62 = diff(D(6,2),theta1)* dtheta1 +diff(D(6,2),theta2)* dtheta2 +diff(D(6,2),theta3)* dtheta3 +diff(D(6,2),theta4)* dtheta4 +diff(D(6,2),theta5)* dtheta5 +diff(D(6,2),theta6)* dtheta6;

dd63 = diff(D(6,3),theta1)* dtheta1 +diff(D(6,3),theta2)* dtheta2 +diff(D(6,3),theta3)* dtheta3 +diff(D(6,3),theta4)* dtheta4 +diff(D(6,3),theta5)* dtheta5 +diff(D(6,3),theta6)* dtheta6;

dd64 = diff(D(6,4),theta1)* dtheta1 +diff(D(6,4),theta2)* dtheta2 +diff(D(6,4),theta3)* dtheta3 +diff(D(6,4),theta4)* dtheta4 +diff(D(6,4),theta5)* dtheta5 +diff(D(6,4),theta6)* dtheta6;

dd65 = diff(D(6,5),theta1)* dtheta1 +diff(D(6,5),theta2)* dtheta2 +diff(D(6,5),theta3)* dtheta3 +diff(D(6,5),theta4)* dtheta4 +diff(D(6,5),theta5)* dtheta5 +diff(D(6,5),theta6)* dtheta6;

dd66 = diff(D(6,6),theta1)* dtheta1 +diff(D(6,6),theta2)* dtheta2 +diff(D(6,6),theta3)* dtheta3 +diff(D(6,6),theta4)* dtheta4 +diff(D(6,6),theta5)* dtheta5 +diff(D(6,6),theta6)* dtheta6;

dD = [dd11, dd12, dd13, dd14, dd15, dd16;
    dd21, dd22, dd23, dd24, dd25, dd26;
    dd31, dd32, dd33, dd34, dd35, dd36;
    dd41, dd42, dd43, dd44, dd45, dd46;
    dd51, dd52, dd53, dd54, dd55, dd56;
    dd61, dd62, dd63, dd64, dd65, dd66];

syms N
N = dD-2*C;
simplify(N(1,2)- N(2,1))