已知RPY角位姿表達式，求齊次座標變換矩陣位姿表達式，也稱RPY角正解

function T=zhengjie_for_T(Q)
%T爲4*4矩陣，Q爲1*6向量
RX=[1 0 0;0 cos(Q(4)) -sin(Q(4));0 sin(Q(4)) cos(Q(4))];
RY=[cos(Q(5)) 0 sin(Q(5));0 1 0;-sin(Q(5)) 0 cos(Q(5))];
RZ=[cos(Q(6)) -sin(Q(6)) 0;sin(Q(6)) cos(Q(6)) 0; 0 0 1];
R=RZ*RY*RX;
T=[R Q(1:3)';
    0 0 0 1];