参考书籍，感谢这本书的作者让我对雅克比矩阵有了更加深刻的认识：
以下是我建模的相关草稿：
草稿有点凌乱，没有整理，详细建模过程可参考以下代码，笔者最后反对称性没有验证出来，可能有地方出问题了，相关代码如下：
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))