UR6机器人正运动学

UR6机器人关节D-H矩阵：

末端关节（未包含a6）：

        /// <summary>
        /// 关节末端的空间姿态-位置向量[px,py,pz]
        /// </summary>        
        /// <param name="Ang">每个关节的角度值</param>
        /// <returns>[px,py,pz]</returns>
        public  float[] R6P(float[] Ang)
        {
            float C1 = (float)Math.Cos(Ang[0]);
            float S1 = (float)Math.Sin(Ang[0]);
            float S2 = (float)Math.Sin(Ang[1]);
            float C2 = (float)Math.Cos(Ang[1]);
            float S23 = (float)Math.Sin(Ang[1] + Ang[2]);
            float C23 = (float)Math.Cos(Ang[1] + Ang[2]);
            float S234 = (float)Math.Sin(Ang[1] + Ang[2] + Ang[3]);
            float C234 = (float)Math.Cos(Ang[1] + Ang[2] + Ang[3]);
            float C5 = (float)Math.Cos(Ang[4]);
            float S5 = (float)Math.Sin(Ang[4]);

            float px = (C1 * C234 * C5 - S1 * S5) * d6 + C1 * S234 * d5 - S1 * d4 + C1 * C23 * a3 + C1 * a2 * S2; ;
            float py = (S1 * C234 * C5 + C1 * S5) * d6 + S1 * S234 * d5 + C1 * d4 + S1 * C23 * a3 + S1 * a2 * S2; ;
            float pz = -S234 * C5 * d6 + C234 * d5 - a3 * S23 + a2 * C2 + d1; ;
            return new float[] { px, py, pz };
        }
        /// <summary>
        /// 关节末端的空间姿态-n向量[nx,ny,nz]
        /// </summary>
        /// <param name="Ang">每个关节的角度值</param>
        /// <returns>[nx,ny,nz]</returns>
        public float[] R6N(float[] Ang)
        {
            float C1 = (float)Math.Cos(Ang[0]);
            float S1 = (float)Math.Sin(Ang[0]);
            float S234 = (float)Math.Sin(Ang[1] + Ang[2] + Ang[3]);
            float C234 = (float)Math.Cos(Ang[1] + Ang[2] + Ang[3]);
            float C5 = (float)Math.Cos(Ang[4]);
            float S5 = (float)Math.Sin(Ang[4]);
            float C6 = (float)Math.Cos(Ang[5]);
            float S6 = (float)Math.Sin(Ang[5]);
            float nx = (C1 * C234 * S5 + S1 * C5) * S6 + C1 * S234 * C6; ;
            float ny = (S1 * C234 * S5 - C1 * C5) * S6 + S1 * S234 * C6; ;
            float nz = (-S234) * S5 * S6 + C234 * C6; ;
           return new float[] { nx, ny, nz };
        }
        /// <summary>
        /// 关节末端的空间姿态-o向量[ox,oy,oz]
        /// </summary>
        /// <param name="Ang">每个关节的角度值</param>
        /// <returns>[ox,oy,oz]</returns>
        public float[] R6O(float[] Ang)
        {
            float C1 = (float)Math.Cos(Ang[0]);
            float S1 = (float)Math.Sin(Ang[0]);
            float S234 = (float)Math.Sin(Ang[1] + Ang[2] + Ang[3]);
            float C234 = (float)Math.Cos(Ang[1] + Ang[2] + Ang[3]);
            float C5 = (float)Math.Cos(Ang[4]);
            float S5 = (float)Math.Sin(Ang[4]);
            float C6 = (float)Math.Cos(Ang[5]);
            float S6 = (float)Math.Sin(Ang[5]);
            float ox = (C1 * C234 * S5 + S1 * C5) * (-C6) + C1 * S234 * S6; ;
            float oy = (S1 * C234 * S5 - C1 * C5) * (-C6) + S1 * S234 * (S6); ;
            float oz = S234 * S5 * C6 + C234 * S6; ;
            return new float[] { ox, oy, oz };
        }
        /// <summary>
        /// 关节末端的空间姿态-a向量[ax,ay,az]
        /// </summary>
        /// <param name="Ang">每个关节的角度值</param>
        /// <returns>[ax,ay,az]</returns>
        public float[] R6A(float[] Ang)
        {
            float C1 = (float)Math.Cos(Ang[0]);
            float S1 = (float)Math.Sin(Ang[0]);
            float S234 = (float)Math.Sin(Ang[1] + Ang[2] + Ang[3]);
            float C234 = (float)Math.Cos(Ang[1] + Ang[2] + Ang[3]);
            float C5 = (float)Math.Cos(Ang[4]);
            float S5 = (float)Math.Sin(Ang[4]);

            float ax = C1 * C234 * C5 - S1 * S5; ;
            float ay = S1 * C234 * C5 + C1 * S5; ;
            float az = -S234 * C5; ;
            return new float[] { ax, ay, az };
        }