Simulink逆运动学模块在时变RPY输入下出现不稳定问题

我在使用MATLAB机器人工具箱的Simulink逆运动学模块时遇到以下问题：

• 仅输入平移坐标，设置权重向量为[0 0 0 1 1 1]时，模块运行符合预期；
• 加入横滚（Roll）、俯仰（Pitch）、偏航（Yaw）旋转控制后，模块无法维持原有路径；
• 通过自定义模块生成4×4齐次变换矩阵，当RPY为常量且权重向量设为[1 1 1 1 1 1]时运行正常，但RPY随时间变化时，仿真不再遵循既定路径，出现不连续现象；
• 即使平移向量相同，加入RPY旋转后最终位置会发生偏移；
• 当前仅对俯仰角使用时序双精度输入（横滚和偏航设为0），需要能处理三个角度的通用解决方案。

自定义rpy2tform模块代码如下：

function T = rpy2tform(u)

x = u(1); y = u(2); z = u(3);
roll  = u(4); pitch = u(5); yaw   = u(6);

cr = cos(roll);  sr = sin(roll);
cp = cos(pitch); sp = sin(pitch);
cy = cos(yaw);   sy = sin(yaw);

R = [ cy*cp,  cy*sp*sr - sy*cr,  cy*sp*cr + sy*sr;
      sy*cp,  sy*sp*sr + cy*cr,  sy*sp*cr - cy*sr;
      -sp,    cp*sr,             cp*cr ];

T = eye(4);
T(1:3,1:3) = R;
T(1:3,4)   = [x; y; z];
end

内容的提问来源于stack exchange，提问作者Esteban Ricaurte