C#逆运动学求解时Theta4_1象限错误如何调整以输出0而非180°

我在C#中编写机械臂逆运动学求解代码时其余关节角度计算均正常，但Theta4_1返回值为180°而非预期的0°，出现了象限错误。该求解逻辑在MATLAB中运行结果正确，我查阅了多份文档仍未定位问题，恳请各位提供指导，多谢。相关求解结果已在MATLAB中验证，我明确知晓预期返回值

问题代码 #

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace IK
{
    class Inverse
    {
        static void Main()
        {
            {
                //连杆长度与偏移量 单位：毫米(mm)
                double a2 = 380.0; double a3 = 0.0; double d3 = 0.0; double d4 = 351.0; double L5 = 89.00;
 
                //原点位置末端执行器坐标
                double p1x = 362.50;
                double p1y = 12.50;
                double p1z = -420;

                double p2x = 397.50;
                double p2y = 12.50;
                double p2z = -420;

                double p3x = 362.50;
                double p3y = -12.50;
                double p3z = -420;

                double p4x = 362.50;
                double p4y = 12.50;
                double p4z = -432;

                //工具端点坐标
                double epx = 380.00; double epy = 0.00; double epz = -440.00;
                //double epx = X; double epy = Y; double epz = Z;

                //计算法向量nx ny nz
                double nx = (p2x - p1x) / Math.Sqrt(Math.Pow(p2x - p1x, 2) + Math.Pow(p2y - p1y, 2) + Math.Pow(p2z - p1z, 2));
                //Console.WriteLine("nx = "+ nx);
                double ny = (p2y - p1y) / Math.Sqrt(Math.Pow(p2x - p1x, 2) + Math.Pow(p2y - p1y, 2) + Math.Pow(p2z - p1z, 2));
                //Console.WriteLine("ny = " + ny);
                double nz = (p2z - p1z) / Math.Sqrt(Math.Pow(p2x - p1x, 2) + Math.Pow(p2y - p1y, 2) + Math.Pow(p2z - p1z, 2));
                //Console.WriteLine("nz = " + nz);

                //计算滑动向量sx sy sz
                double sx = (p3x - p1x) / Math.Sqrt(Math.Pow(p3x - p1x, 2) + Math.Pow(p3y - p1y, 2) + Math.Pow(p3z - p1z, 2));
                //Console.WriteLine("sx = " + sx);
                double sy = (p3y - p1y) / Math.Sqrt(Math.Pow(p3x - p1x, 2) + Math.Pow(p3y - p1y, 2) + Math.Pow(p3z - p1z, 2));
                //Console.WriteLine("sy = " + sy);
                double sz = (p3z - p1z) / Math.Sqrt(Math.Pow(p3x - p1x, 2) + Math.Pow(p3y - p1y, 2) + Math.Pow(p3z - p1z, 2));
                //Console.WriteLine("sz = " + sz);

                //计算接近向量ax ay az
                double ax = (p4x - p1x) / Math.Sqrt(Math.Pow(p4x - p1x, 2) + Math.Pow(p4y - p1y, 2) + Math.Pow(p4z - p1z, 2));
                //Console.WriteLine("ax = " + ax);
                double ay = (p4y - p1y) / Math.Sqrt(Math.Pow(p4x - p1x, 2) + Math.Pow(p4y - p1y, 2) + Math.Pow(p4z - p1z, 2));
                //Console.WriteLine("ay = " + ay);
                double az = (p4z - p1z) / Math.Sqrt(Math.Pow(p4x - p1x, 2) + Math.Pow(p4y - p1y, 2) + Math.Pow(p4z - p1z, 2));
                //Console.WriteLine("az = " + az);

                //计算末端坐标
                double px = epx - ax * L5;
                //Console.WriteLine("px = " + px);
                double py = epy - ay * L5;
                //Console.WriteLine("py = " + py);
                double pz = epz - az * L5;
                //Console.WriteLine("pz = " + pz);

                //三角代换 对应论文中theta1的Rho值
                double p1 = Math.Sqrt(Math.Pow(px, 2) + Math.Pow(py, 2)); 
                //Console.WriteLine("p1 = " + p1);
                //三角代换 对应论文中theta3的Rho值
                double p3 = Math.Sqrt(Math.Pow(d4, 2) + Math.Pow(a3, 2)); 
                //Console.WriteLine("p3 = " + p3);

                //求解关节1角度
                double theta1_1 = Math.Atan2(py, px) - Math.Atan2(d3/p1, Math.Sqrt(Math.Pow(1 - (d3/p1),2)));                
                double theta1_2 = Math.Atan2(py, px) - Math.Atan2(d3/p1, -Math.Sqrt(Math.Pow(1 - (d3/p1),2)));

                //求解关节3角度
                double K = Math.Pow(px, 2) + Math.Pow(py, 2) + Math.Pow(pz, 2) - Math.Pow(a3,2) - Math.Pow(d4, 2) 
                         - Math.Pow(a2, 2) - Math.Pow(d3, 2) / (2*a2);
                //Console.WriteLine("K = " + K);

                double theta3_1 = Math.Atan2(a3, d4) - Math.Atan2(K/p3, Math.Sqrt(Math.Pow(1 - (K / p3), 2)));
                double theta3_2 = Math.Atan2(a3, d4) - Math.Atan2(K/p3, -Math.Sqrt(Math.Pow(1 - (K / p3), 2)));

                //求解关节2角度
                // Theta2_1
                double s23_1 = ((-a2 * Math.Cos(theta3_1) - a3) * pz + (a2 * Math.Sin(theta3_1) - d4) * 
                               (px * Math.Cos(theta1_1) + py * Math.Sin(theta1_1))); //公式中Y值
                double c23_1 = ((a2 * Math.Sin(theta3_1) - d4) * pz + (a2 * Math.Cos(theta3_1) + a3) * 
                               (px * Math.Cos(theta1_1) + py * Math.Sin(theta1_1))); //公式中X值
                double theta23_1 = Math.Atan2(s23_1, c23_1);
                double theta2_1 = (theta23_1 - theta3_1);               

                // Theta2_2
                double s23_2 = ((-a2 * Math.Cos(theta3_2) - a3) * pz + (a2 * Math.Sin(theta3_2) - d4) *
                               (px * Math.Cos(theta1_1) + py * Math.Sin(theta1_1))); //公式中Y值
                double c23_2 = ((a2 * Math.Sin(theta3_2) - d4) * pz + (a2 * Math.Cos(theta3_2) + a3) *
                               (px * Math.Cos(theta1_1) + py * Math.Sin(theta1_1))); //公式中X值
                double theta23_2 = Math.Atan2(s23_2, c23_2);
                double theta2_2 = (theta23_2 - theta3_2);               

                // Theta2_3
                double s23_3 = ((-a2 * Math.Cos(theta3_1) - a3) * pz + (a2 * Math.Sin(theta3_1) - d4) *
                               (px * Math.Cos(theta1_2) + py * Math.Sin(theta1_2))); //公式中Y值
                double c23_3 = ((a2 * Math.Sin(theta3_1) - d4) * pz + (a2 * Math.Cos(theta3_1) + a3) *
                               (px * Math.Cos(theta1_2) + py * Math.Sin(theta1_2))); //公式中X值
                double theta23_3 = Math.Atan2(s23_3, c23_3);
                double theta2_3 = (theta23_3 - theta3_1);

                // Theta2_4
                double s23_4 = ((-a2 * Math.Cos(theta3_2) - a3) * pz + (a2 * Math.Sin(theta3_2) - d4) *
                               (px * Math.Cos(theta1_2) + py * Math.Sin(theta1_2))); //公式中Y值
                double c23_4 = ((a2 * Math.Sin(theta3_2) - d4) * pz + (a2 * Math.Cos(theta3_2) + a3) *
                               (px * Math.Cos(theta1_2) + py * Math.Sin(theta1_2))); //公式中X值
                double theta23_4 = Math.Atan2(s23_4, c23_4);
                double theta2_4 = (theta23_4 - theta3_2);

                //求解关节4角度
                // Theta4_1
                double s4_1 = Math.Cos(theta1_1) * ay - Math.Sin(theta1_1) * ax ; //Y
                 //Console.WriteLine($"s4_1 = {s4_1:f64}"); //调试s4_1
                double c4_1 = -Math.Cos(theta1_1) * Math.Cos(theta23_1) * ax - Math.Sin(theta1_1) *
                               Math.Cos(theta23_1) * ay + Math.Sin(theta23_1) * az;//X
                //Console.WriteLine($"c4_1 = {c4_1:f64}"); //调试c4_1
                double theta4_1 = Math.Atan2(s4_1, c4_1); //取反X可获得第二象限值

                // Theta4_2
                double s4_2 = Math.Cos(theta1_1) * ay - Math.Sin(theta1_1) * ax;
                double c4_2 = -Math.Cos(theta1_1) * Math.Cos(theta23_2) * ax - Math.Sin(theta1_1) *
                               Math.Cos(theta23_2) * ay + Math.Sin(theta23_2) * az;
                double theta4_2 = Math.Atan2(s4_2, c4_2);

                // Theta4_3
                double s4_3 = Math.Cos(theta1_2) * ay - Math.Sin(theta1_2) * ax;
                double c4_3 = -Math.Cos(theta1_2) * Math.Cos(theta23_3) * ax - Math.Sin(theta1_2) *
                               Math.Cos(theta23_3) * ay + Math.Sin(theta23_3) * az;
                double theta4_3 = Math.Atan2(s4_3, c4_3);

                // Theta4_4
                double s4_4 = Math.Cos(theta1_2) * ay - Math.Sin(theta1_2) * ax;
                double c4_4 = -Math.Cos(theta1_2) * Math.Cos(theta23_4) * ax - Math.Sin(theta1_2) *
                               Math.Cos(theta23_4) * ay + Math.Sin(theta23_4) * az;
                double theta4_4 = Math.Atan2(s4_4, c4_4);

                //弧度转角度并输出结果
                //theta 1
                Console.WriteLine("theta1_1  = " + theta1_1 * 180 / (Math.PI));
                Console.WriteLine("theta1_2  = " + theta1_2 * 180 / (Math.PI));

                //theta 3
                Console.WriteLine("theta3_1  = " + theta3_1 * 180 / (Math.PI));
                Console.WriteLine("theta3_2  = " + theta3_2 * 180 / (Math.PI));

                //theta 2
                Console.WriteLine("theta2_1  = " + theta2_1 * 180 / (Math.PI));
                Console.WriteLine("theta2_2  = " + theta2_2 * 180 / (Math.PI));
                Console.WriteLine("theta2_3  = " + theta2_3 * 180 / (Math.PI));
                Console.WriteLine("theta2_4  = " + theta2_4 * 180 / (Math.PI));

                //theta 4
                Console.WriteLine("theta4_1  = " + theta4_1 * 180 / (Math.PI));
                Console.WriteLine("theta4_2  = " + theta4_2 * 180 / (Math.PI));
                Console.WriteLine("theta4_3  = " + theta4_3 * 180 / (Math.PI));
                Console.WriteLine("theta4_4  = " + theta4_4 * 180 / (Math.PI));

            }
            Console.ReadLine();
        }
    }
}

内容的提问来源于stack exchange，提问作者康拉德