private HomographyMatrix MakeFrame(double a, double alpha, double d, double theta) { HomographyMatrix Temp = new HomographyMatrix("ZYZE"); alpha = alpha * (Math.PI / 180); theta = theta * (Math.PI/ 180); Temp.data[0, 0] = Math.Cos(theta); Temp.data[0, 1] = -1 * Math.Sin(theta); Temp.data[0, 2] = 0; Temp.data[0, 3] = a; Temp.data[1, 0] = Math.Sin(theta) * Math.Cos(alpha); Temp.data[1, 1] = Math.Cos(theta) * Math.Cos(alpha); Temp.data[1, 2] = -1 * Math.Sin(alpha); Temp.data[1, 3] = -1 * Math.Sin(alpha) * d; Temp.data[2, 0] = Math.Sin(theta) * Math.Sin(alpha); Temp.data[2, 1] = Math.Cos(theta) * Math.Sin(alpha); Temp.data[2, 2] = Math.Cos(alpha); Temp.data[2, 3] = Math.Cos(alpha) * d; Temp.data[3, 0] = 0; Temp.data[3, 1] = 0; Temp.data[3, 2] = 0; Temp.data[3, 3] = 1; return Temp; }
public KinematicsRX60(double angle1, double angle2, double angle3, double angle4, double angle5, double angle6) { T01 = MakeFrame(0, 0 , 0 , angle1); T12 = MakeFrame(0, -90, 0, angle2); T23 = MakeFrame(290, 0, 49, angle3); T34 = MakeFrame(0, 90, 310, angle4); T45 = MakeFrame(0, -90, 0, angle5); T56 = MakeFrame(0, 90, 65, angle6); }