function T = fcn（~)
T=[cos(q(6))*cos(q(5)) cos(q(6))*sin(q(5))*sin(q(4))-sin(q(6))*cos(q(4)) sin(q(6))*sin(q(4))+cos(q(6))*sin(q(5))*cos(q(4)) q(1);...
sin(q(6))*cos(q(5)) cos(q(6))*cos(q(4))+sin(q(6))*sin(q(5))*sin(q(4)) sin(q(6))*sin(q(5))*cos(q(4))-cos(q(6))*sin(q(4)) q(2);...
-sin(q(5)) cos(q(5))*sin(q(4)) cos(q(5))*sin(q(4)) q(3);0 0 0 1];