public static void fkTest(bool b)
{
float[] x = { 0f, 0f, 0f };
float[] y = { 0f, 0f, 0f };
float[] z = { 0f, 0f, 0f };
float[] length = { 50, 50, 50 };
float[] dest = { 70, 70, 70 };
int[] Xmin = { 0, 0, 0 };
int[] Xmax = { 360, 360, 360 };
int[] Ymin = { 0, 0, 0 };
int[] Ymax = { 360, 360, 360 };
int[] Zmin = { 0, 0, 0 };
int[] Zmax = { 360, 360, 360 };
ForwardKinematics fk = new ForwardKinematics(x, y, z, length, Xmin, Xmax, Ymin, Ymax, Zmin, Zmax);
if (b)
{
ikTest(fk, dest);
}
float[] ep = fk.getEndpoint();
Console.WriteLine(ep[0] + " " + ep[1] + " " + ep[2]);
}
public IK(ForwardKinematics FK, float[] destination)
{
this.FK = FK;
this.destination = destination;
currentPos = FK.getEndpoint();
cost = calculateCost();
}
public void directMove()
{
float scaleFactor = 1500f;
float[,] dAngles = returnPartials();
for (int i = 0; i < FK.armCount; i++)
{
//check if in range
FK.angleX[i] += scaleFactor * dAngles[i, 0];
if (FK.angleX[i] < FK.quatList[i].Xmin)
{
FK.angleX[i] = (float)(FK.quatList[i].Xmin);
}
else if (FK.angleX[i] > FK.quatList[i].Xmax)
{
FK.angleX[i] = (float)(FK.quatList[i].Xmax);
}
FK.angleY[i] += scaleFactor * dAngles[i, 1];
if (FK.angleY[i] < FK.quatList[i].Ymin)
{
FK.angleY[i] = (float)(FK.quatList[i].Ymin);
}
else if (FK.angleY[i] > FK.quatList[i].Ymax)
{
FK.angleY[i] = (float)(FK.quatList[i].Ymax);
}
FK.angleZ[i] += scaleFactor * dAngles[i, 2];
if (FK.angleZ[i] < FK.quatList[i].Zmin)
{
FK.angleZ[i] = (float)(FK.quatList[i].Zmin);
}
else if (FK.angleZ[i] > FK.quatList[i].Zmax)
{
FK.angleZ[i] = (float)(FK.quatList[i].Zmax);
}
}
FK.solve();
currentPos = FK.getEndpoint();
}