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