public IEnumerator IKAuto(List <float> angles, List <LinkParam> linkParams, FKManager fk)
{
int culcCount = 0;
double[,] q = angles.ToVertical();
double[,] prev_e = new double[6, 1];
while (true /*culcCount < 100000*/)
{
var HTMs = HomogeneourCoordinate.GetHTMs(linkParams, angles);
var J = fk.GetJacobian(HTMs).Matrix;
var J_T = J.Transpose();
var EndHTM = HTMs[HTMs.Count() - 1];
var e_q = Error(fk.GetEndPosition(EndHTM), EndHTM.RotationMatrix());
for (int i = 0; i < 3; i++)
{
W_e[i, i] = w_e_pos;
}
for (int i = 3; i < 6; i++)
{
W_e[i, i] = w_e_rot;
}
var Jt_W_J_plus_Wn__inv = (J_T.Times(W_e).Times(J).Plus(W_n(e_q, W_e))).inverseMatrix();
var delta_q = Jt_W_J_plus_Wn__inv.Times(J_T).Times(W_e).Times(e_q);
q = q.Plus(delta_q);
for (int i = 0; i < q.GetLength(0); i++)
{
angles[i] = (float)q[i, 0];
}
prev_e = e_q;
culcCount++;
if (culcCount % 100 == 0)
{
yield return(null);
}
}
print("loop num: " + culcCount);
yield break;
}
public Vector3 CurrentEndPosition()
{
return(fk.GetEndPosition(EndHTM) + transform.position);
}
