拉格朗日法建立六軸機器人TX90動力學模型
阿新 • • 發佈：2018-12-20
參考書籍，感謝這本書的作者讓我對雅克比矩陣有了更加深刻的認識：
以下是我建模的相關草稿：
草稿有點凌亂，沒有整理，詳細建模過程可參考以下程式碼，筆者最後反對稱性沒有驗證出來，可能有地方出問題了，相關程式碼如下：
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))