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);
}