// Update is called once per frame
void FixedUpdate()
{
if (!isInControl)
{
if (!isIK)
{
if (ikCoroutine != null)
{
StopCoroutine(ikCoroutine);
ikCoroutine = null;
}
//print($"angles { thetas[0] * Mathf.Rad2Deg}, { thetas[1] * Mathf.Rad2Deg}, { thetas[2] * Mathf.Rad2Deg}, { thetas[3] * Mathf.Rad2Deg}, { thetas[4] * Mathf.Rad2Deg}, { thetas[5] * Mathf.Rad2Deg}");
currentAngles = new List <float>()
{
angle1 *Mathf.Deg2Rad, angle2 *Mathf.Deg2Rad, angle3 *Mathf.Deg2Rad, angle4 *Mathf.Deg2Rad, angle5 *Mathf.Deg2Rad, angle6 *Mathf.Deg2Rad
};
SetAngles(currentAngles);
var readThetas = new List <float>();
foreach (var joint in joints)
{
readThetas.Add(joint.GetPosition());
}
HTMs = hC.GetHTMs(readThetas);
EndHTM = HTMs[HTMs.Count() - 1];
fk.Foreach(HTMs);
}
else
{
if (ikCoroutine == null)
{
ikCoroutine = StartCoroutine(IKAuto());
}
SetAngles(currentAngles);
}
}
}
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 static Vector3 GetEndPosition(List <LinkParam> linkParams, List <float> angles, FKManager fk)
{
return(HomogeneourCoordinate.GetHTMs(linkParams, angles)[~1] * fk.hE);
}
public static void Foreach(List <LinkParam> linkParams, List <float> angles, FKManager fk)
{
fk.Foreach(HomogeneourCoordinate.GetHTMs(linkParams, angles));
}