六关节机器人的正运动学计算

  1         /// <summary>
  2         /// 关节末端的空间姿态-位置向量[px,py,pz]
  3         /// </summary>
  4         /// <param name="AD">每个关节的a,d值构成的数组列表，如GSK RB8：{{150,0},{560,0},{155,0},{0,630},{0,0},{0,155}}</param>
  5         /// <param name="Ang">每个关节的角度值</param>
  6         /// <returns>[px,py,pz]</returns>
  7         public static double[] R6P(List<double[]> AD, double[] Ang)
  8         {
  9             double A1 = AD[0][0];
 10             double D1 = AD[0][1];
 11             double A2 = AD[1][0];
 12             double A3 = AD[2][0];
 13             double D4 = AD[3][1];
 14             double A6 = AD[5][0];
 15             double D6 = AD[5][1];
 16             double C1 = Math.Cos(Ang[0]);
 17             double S1 = Math.Sin(Ang[0]);
 18             double C2 = Math.Cos(Ang[1]);
 19             double S2 = Math.Sin(Ang[1]);
 20             double C3 = Math.Cos(Ang[2]);
 21             double S3 = Math.Sin(Ang[2]);
 22             double S23 = Math.Sin(Ang[1] + Ang[2]);
 23             double C23 = Math.Cos(Ang[1] + Ang[2]);
 24             double C4 = Math.Cos(Ang[3]);
 25             double S4 = Math.Sin(Ang[3]);
 26             double C5 = Math.Cos(Ang[4]);
 27             double S5 = Math.Sin(Ang[4]);
 28             double C6 = Math.Cos(Ang[5]);
 29             double S6 = Math.Sin(Ang[5]);
 30 
 31             double px = ((C1 * S23 * C4 + S1 * S4) * C5 + C1 * C23 * S5) * A6 * C6 - (C1 * S23 * S4 - S1 * C4) * A6 * S6 - ((C1 * S23 * C4 + S1 * S4) * S5 - C1 * C23 * C5) * D6 + C1 * C23 * D4 + C1 * S23 * A3 + C1 * A2 * S2 + A1 * C1;
 32             double py = ((S1 * S23 * C4 - C1 * S4) * C5 + S1 * C23 * S5) * A6 * C6 - (S1 * S23 * S4 + C1 * C4) * A6 * S6 + ((-S1 * S23 * C4 + C1 * S4) * S5 + S1 * C23 * C5) * D6 + S1 * C23 * D4 + S1 * S23 * A3 + S1 * A2 * S2 + A1 * S1;
 33             double pz = (C23 * C4 * C5 - S23 * S5) * A6 * C6 - C23 * S4 * A6 * S6 - (C23 * C4 * S5 + S23 * C5) * D6 - S23 * D4 + C23 * A3 + A2 * C2 + D1;
 34             return new double[] { px, py, pz };
 35         }
 36         /// <summary>
 37         /// 关节末端的空间姿态-n向量[nx,ny,nz]
 38         /// </summary>
 39         /// <param name="Ang">每个关节的角度值</param>
 40         /// <returns>[nx,ny,nz]</returns>
 41         public static double[] R6N(double[] Ang)
 42         {
 43             double C1 = Math.Cos(Ang[0]);
 44             double S1 = Math.Sin(Ang[0]);
 47             double C3 = Math.Cos(Ang[2]);
 48             double S3 = Math.Sin(Ang[2]);
 49             double S23 = Math.Sin(Ang[1] + Ang[2]);
 50             double C23 = Math.Cos(Ang[1] + Ang[2]);
 51             double C4 = Math.Cos(Ang[3]);
 52             double S4 = Math.Sin(Ang[3]);
 53             double C5 = Math.Cos(Ang[4]);
 54             double S5 = Math.Sin(Ang[4]);
 55             double C6 = Math.Cos(Ang[5]);
 56             double S6 = Math.Sin(Ang[5]);
 57             double nx = ((C1 * S23 * C4 + S1 * S4) * C5 + C1 * C23 * S5) * C6 - (C1 * S23 * S4 - S1 * C4) * S6;
 58             double ny = ((S1 * S23 * C4 - C1 * S4) * C5 + S1 * C23 * S5) * C6 - (S1 * S23 * S4 + C1 * C4) * S6;
 59             double nz = (C23 * C4 * C5 - S23 * S5) * C6 - C23 * S4 * S6;

 66             return new double[] { nx, ny, nz };
 67         }
 68         /// <summary>
 69         /// 关节末端的空间姿态-o向量[ox,oy,oz]
 70         /// </summary>
 71         /// <param name="Ang">每个关节的角度值</param>
 72         /// <returns>[ox,oy,oz]</returns>
 73         public static double[] R6O(double[] Ang)
 74         {
 75             double C1 = Math.Cos(Ang[0]);
 76             double S1 = Math.Sin(Ang[0]);
 79             double C3 = Math.Cos(Ang[2]);
 80             double S3 = Math.Sin(Ang[2]);
 81             double S23 = Math.Sin(Ang[1] + Ang[2]);
 82             double C23 = Math.Cos(Ang[1] + Ang[2]);
 83             double C4 = Math.Cos(Ang[3]);
 84             double S4 = Math.Sin(Ang[3]);
 85             double C5 = Math.Cos(Ang[4]);
 86             double S5 = Math.Sin(Ang[4]);
 87             double C6 = Math.Cos(Ang[5]);
 88             double S6 = Math.Sin(Ang[5]);
 89             double ox = ((C1 * S23 * C4 + S1 * S4) * C5 + C1 * C23 * S5) * (-S6) - (C1 * S23 * S4 - S1 * C4) * C6;
 90             double oy = ((S1 * S23 * C4 - C1 * S4) * C5 + S1 * C23 * S5) * (-S6) - (S1 * S23 * S4 + C1 * C4) * C6;
 91             double oz = (C23 * C4 * C5 - S23 * S5) * (-S6) - C23 * S4 * C6;

 97             return new double[] { ox, oy, oz };
 98         }
 99         /// <summary>
100         /// 关节末端的空间姿态-a向量[ax,ay,az]
101         /// </summary>
102         /// <param name="Ang">每个关节的角度值</param>
103         /// <returns>[ax,ay,az]</returns>
104         public static double[] R6A(double[] Ang)
105         {
106             double C1 = Math.Cos(Ang[0]);
107             double S1 = Math.Sin(Ang[0]);
108             double C3 = Math.Cos(Ang[2]);
111             double S3 = Math.Sin(Ang[2]);
112             double S23 = Math.Sin(Ang[1] + Ang[2]);
113             double C23 = Math.Cos(Ang[1] + Ang[2]);
114             double C4 = Math.Cos(Ang[3]);
115             double S4 = Math.Sin(Ang[3]);
116             double C5 = Math.Cos(Ang[4]);
117             double S5 = Math.Sin(Ang[4]);
118             double C6 = Math.Cos(Ang[5]);
119             double S6 = Math.Sin(Ang[5]);
120             double ax = (C1 * S23 * C4 + S1 * S4) * (-S5) + C1 * C23 * C5;
121             double ay = (S1 * S23 * C4 - C1 * S4) * (-S5) + S1 * C23 * C5;
122             double az = C23 * C4 * (-S5) - S23 * C5;

128             return new double[] { ax, ay, az };
129         }