public override float[] InversKin(Matrix <float> Tgoal, float[] q0)
{
//float[] q2 = ReadState();
//q = new float[] { 0, 0, 0, 90 * Mathf.PI / 180, 0, 90 * Mathf.PI / 180, 0 };
//q = new float[] { 0, 0, 0, 0, 0, 0, 0 };
//Tgoal[1, 3] = -Tgoal[1, 3];
//Tgoal[0, 3] = -Tgoal[0, 3];
//Tgoal = ForwardKin(q2);
Tgoal = Tgoal * MathOperations.MatrixRy4(Mathf.PI / 2) * MathOperations.MatrixRz4(Mathf.PI / 2);
//Debug.Log(Tgoal.ToString());
Matrix <float> end_effector_matrix = ForwardKin(q);
//Debug.Log(end_effector_matrix.ToString());
float diff_r = 0;
float diif_o = 0;
int c = 0;
do
{
end_effector_matrix = ForwardKin(q);
float[] error = CalcErorr(Tgoal, end_effector_matrix);
diff_r = error[0] * error[0] + error[1] * error[1] + error[2] * error[2];
diif_o = error[3] * error[3] + error[4] * error[4] + error[5] * error[5];
Vector <float> del_q = Vector <float> .Build.DenseOfArray(new float[] { 0, 0, 0, 0, 0, 0, 0 });
float[] del_q2 = new float[] { 0, 0, 0, 0, 0, 0, 0 };
if (diff_r > 1 || diif_o > 0.1F)
{
del_q2 = MoveSNS(q, error);
//del_q2 = MovePINV(q, error);
for (int i = 0; i < 7; i++)
{
//q[i] = (i!=3)? (q[i] + del_q2[i] * 0.05f) : (q[i] - del_q2[i] * 0.05f);
q[i] = q[i] + del_q2[i] * 0.02f;
}
}
if (c > 400)
{
return(new float[] { 0, 0, 0, 90 * Mathf.PI / 180, 0, 90 * Mathf.PI / 180, 0 });
}
else
{
c++;
}
} while ((diff_r > 1 || diif_o > 0.1F));
for (int i = 0; i < 6; i++)
{
q[i] = MathOperations.AngleRound(q[i]);
}//Debug.Log("IIWA IK:" + q.ToString());
return(q);
}
public override float[] ReadState()
{
float[] qr = new float[6];
qr[0] = -link1.transform.localRotation.eulerAngles.z * Mathf.PI / 180;
qr[1] = -(link2.transform.localRotation.eulerAngles.y - 90) * Mathf.PI / 180;
qr[2] = -(link3.transform.localRotation.eulerAngles.y + 90) * Mathf.PI / 180;
qr[3] = link4.transform.localRotation.eulerAngles.x * Mathf.PI / 180;
qr[4] = -link5.transform.localRotation.eulerAngles.y * Mathf.PI / 180;
qr[5] = link6.transform.localRotation.eulerAngles.x * Mathf.PI / 180;
for (int i = 0; i < 6; i++)
{
qr[i] = MathOperations.AngleRound(qr[i]);
}
return(qr);
}
private float[] CalcErorr(Matrix <float> goal, Matrix <float> end_effector)
{
float[] err = new float[6];
err[0] = goal[0, 3] - end_effector[0, 3];
err[1] = goal[1, 3] - end_effector[1, 3];
err[2] = goal[2, 3] - end_effector[2, 3];
float[] t1 = new float[] {
Mathf.Atan2(goal[2, 1], Mathf.Sqrt(1 - goal[2, 1] * goal[2, 1])),
Mathf.Atan2(-goal[0, 1], goal[1, 1]),
Mathf.Atan2(-goal[2, 0], goal[2, 2])
};
float[] t2 = new float[] {
Mathf.Atan2(end_effector[2, 1], Mathf.Sqrt(1 - end_effector[2, 1] * end_effector[2, 1])),
Mathf.Atan2(-end_effector[0, 1], end_effector[1, 1]),
Mathf.Atan2(-end_effector[2, 0], end_effector[2, 2])
};
Matrix <float> R = Matrix <float> .Build.DenseOfArray(new float[, ]
{
{ 0, Mathf.Cos(t1[1]), -Mathf.Sin(t1[1]) * Mathf.Cos(t1[0]) },
{ 0, Mathf.Sin(t1[1]), Mathf.Cos(t1[1]) * Mathf.Cos(t1[0]) },
{ 1, 0, Mathf.Sin(t1[0]) }
});
float d1 = t1[0] - t2[0];
float d2 = t1[1] - t2[1];
float d3 = t1[2] - t2[2];
err[3] = (R[0, 0] * d2 + R[0, 1] * d1 + R[0, 2] * d3);
err[4] = (R[1, 0] * d2 + R[1, 1] * d1 + R[1, 2] * d3);
err[5] = (R[2, 0] * d2 + R[2, 1] * d1 + R[2, 2] * d3);
err[3] = MathOperations.AngleRound(err[3]);
err[4] = MathOperations.AngleRound(err[4]);
err[5] = MathOperations.AngleRound(err[5]);
return(err);
}
public override float[] InversKin(Matrix <float> Tgoal)
{
//Tgoal = Tgoal * MathOperations.MatrixRz4(Mathf.PI);
Matrix <float> T0 = Tgoal * MathOperations.MatrixTx4(80).Inverse();
float q1 = Mathf.Atan2(T0[1, 3], T0[0, 3]);
float a = Mathf.Sqrt(T0[1, 3] * T0[1, 3] + T0[0, 3] * T0[0, 3]) - 25;
float b = T0[2, 3] - 400;
float c3 = (a * a + b * b - 560 * 560 - 516.2f * 516.2f) / (2 * 560 * 516.2f);
float s3 = Mathf.Sqrt(1 - c3 * c3);
float del_q3 = Mathf.Atan2(515, 35);
float q3 = Mathf.Atan2(s3, c3) - del_q3;
float q2 = -Mathf.Atan2(516.2f * Mathf.Sin(q3 + del_q3), (560 + 516.2f * Mathf.Cos(q3 + del_q3))) + Mathf.Atan2(a, b);
Matrix <float> T123 = MathOperations.MatrixTz4(400) * MathOperations.MatrixRz4(q1) * MathOperations.MatrixTx4(25) *
MathOperations.MatrixRy4(q2 - Mathf.PI / 2) * MathOperations.MatrixTx4(560) * MathOperations.MatrixRy4(q3 + Mathf.Atan2(515, 35)) *
MathOperations.MatrixTx4(Mathf.Sqrt(35 * 35 + 515 * 515)) * MathOperations.MatrixRy4(Mathf.PI / 2 - Mathf.Atan2(515, 35));
Matrix <float> T456 = T123.Inverse() * T0;
float q4, q5, q6;
float nx = MathOperations.MyRround(T456[0, 0]);
float ny = MathOperations.MyRround(T456[1, 0]);
float nz = MathOperations.MyRround(T456[2, 0]);
float ax = MathOperations.MyRround(T456[0, 2]);
float sx = MathOperations.MyRround(T456[0, 1]);
if (nx != 1)
{
q4 = Mathf.Atan2(ny, -nz);
q6 = Mathf.Atan2(sx, ax);
if (Mathf.Abs(Mathf.Cos(q6)) > 0.00001f)
{
q5 = Mathf.Atan2(ax / Mathf.Cos(q6), nx);
}
else
{
q5 = Mathf.Atan2(sx / Mathf.Sin(q6), nx);
}
}
else
{
q5 = Mathf.Acos(T456[0, 0]);
q4 = 0;
q6 = Mathf.Atan2(T456[2, 1], T456[1, 1]);
}
q = new float[] { q1, q2, q3, q4, q5, q6 };
foreach (float qc in q)
{
if (float.IsNaN(qc))
{
return(new float[] { 0, 0, 0, 0, 0, 0 });
}
}
for (int i = 0; i < 6; i++)
{
q[i] = MathOperations.AngleRound(q[i]);
}
// Debug.Log("Agilus IK:" + q.ToString());
return(q);
}
