321型正交位移測量系統的正解（已知位姿求位移傳感器位移）

function H=ODMS_zhengjie(Q,L,T_B_E0,T_P_E)
% Q爲RPY角位姿表達式，6*1向量
% L爲六根直線表達式，6*6矩陣
% T_B_E0及T_P_E爲齊次座標變換矩陣，4*4矩陣
M0=L_for_M(T_B_E0,L);%求零位觸點
T_B_P=zhengjie_for_T(Q);%求齊次位姿
T_B_E=T_B_P*T_P_E;%求被測基準塊在定系中的位姿
M1=L_for_M(T_B_E,L);%求非零位觸點
H=zhengjie_for_H(M0,M1);%程序輸出