尝试在 Unity 中实现 Jacobian IK 示例概述

Question

我正在尝试实现here 显示的示例。

但是在运行代码并使用调试模式时，没有返回值。我假设我没有使用正确的旋转轴。

额外的统一细节：

Joints 变量是一个 GameObjects 数组（由 4 个组成）。

目标是一个单一的游戏对象。

Tools.M_Populate、Tools.M_Multiply 和 Tools.M_Transpose。在检查调试数据时，我已经检查并且似乎正在工作。它们是返回 float[,] 的简单代码。

   private void Update()
 {
        if (Input.GetKeyDown(KeyCode.F))
        {
            //JacobianIK();
            float angleA = Vector3.Angle(joints[0].transform.up, (joints[1].transform.position - joints[0].transform.position).normalized);
            float angleB = Vector3.Angle((joints[1].transform.position - joints[0].transform.position).normalized, (joints[2].transform.position - joints[1].transform.position).normalized);
            float angleC = Vector3.Angle((joints[2].transform.position - joints[1].transform.position).normalized, (joints[3].transform.position - joints[2].transform.position).normalized);


            Vector3 angles = new Vector3(angleA, angleB, angleC);
            JacobianIK(angles);
        }
 }


private void JacobianIK(Vector3 O) {
        int count = 0;
        Vector3 dO = Vector3.zero;
        while (Mathf.Abs(Vector3.Distance(joints[3].transform.position, target.transform.position)) > EPS && count < 100)
        {
            dO = GetDeltaOrientation();
            O += dO * step;
            // update angles
            updateLinks(new float[] { O.x, O.y, O.z });

            Debug.Log("Angles: " + O.ToString());
            count++;
        }

    }

private Vector3 GetDeltaOrientation() {


        float[,] Jt = GetJacobianTranspose();

        Vector3 V = (target.transform.position - joints[joints.Length - 1].transform.position);

        //dO = Jt * V;
        float[,] dO = Tools.M_Multiply(Jt, new float[,] { { V.x }, { V.y }, { V.z  } });
        return new Vector3(dO[0, 0], dO[1, 0], dO[2, 0]);
    }

   private float[,] GetJacobianTranspose() {
            Vector3 J_A = Vector3.Cross(joints[0].transform.up, (joints[joints.Length - 1].transform.position - joints[0].transform.position));
            Vector3 J_B = Vector3.Cross((joints[1].transform.position - joints[0].transform.position), (joints[joints.Length - 1].transform.position - joints[1].transform.position));
            Vector3 J_C = Vector3.Cross((joints[2].transform.position - joints[1].transform.position), (joints[joints.Length - 1].transform.position - joints[2].transform.position));


        float[,] matrix = new float[3, 3];

        matrix = Tools.M_Populate(matrix, new Vector3[] { J_A, J_B, J_C });


        return Tools.M_Transpose(matrix);
    }

我期望将角度向量应用于每个关节

Answer 1

经过一些尝试和错误，我相信我已经找到了答案。 我遇到了旋转轴的问题。使用当前代码，它无法解决 XY 平面（我将游戏对象放置在同一平面上）。我已经上传到GitHub upload当前版本了。

为了更正问题中的代码，我首先更改了两个关键区域：

float angleA = Vector3.Angle(joints[0].transform.up, (joints[1].transform.position - joints[0].transform.position).normalized);
float angleB = Vector3.Angle((joints[1].transform.position - joints[0].transform.position).normalized, (joints[2].transform.position - joints[1].transform.position).normalized);
float angleC = Vector3.Angle((joints[2].transform.position - joints[1].transform.position).normalized, (joints[3].transform.position - joints[2].transform.position).normalized);

到：

float angleA = calculateAngle(Vector3.up, joints[1].transform.position, joints[0].transform.position);
float angleB = calculateAngle(Vector3.up, joints[2].transform.position, joints[1].transform.position);
float angleC = calculateAngle(Vector3.up, joints[3].transform.position, joints[2].transform.position);
...
}

private float calculateAngle(Vector3 axis, Vector3 pos1, Vector3 pos2)
{
    float value = 0f;
    value = Vector3.Angle(axis, (pos1 - pos2).normalized);
    Vector3 cross = Vector3.Cross(axis, (pos1 - pos2).normalized);
    if (cross.z < 0)
        value = -value;

    return value;
}

第二个将GetJacobianTranspose()方法中的代码从：

private float[,] GetJacobianTranspose() {
    Vector3 J_A = Vector3.Cross(joints[0].transform.up, (joints[joints.Length - 1].transform.position - joints[0].transform.position));
    Vector3 J_B = Vector3.Cross((joints[1].transform.position - joints[0].transform.position), (joints[joints.Length - 1].transform.position - joints[1].transform.position));
    Vector3 J_C = Vector3.Cross((joints[2].transform.position - joints[1].transform.position), (joints[joints.Length - 1].transform.position - joints[2].transform.position));
    ...

到：

private float[,] GetJacobianTranspose() {
    Vector3 J_A = Vector3.Cross(joints[0].transform.forward, (joints[joints.Length - 1].transform.position - joints[0].transform.position));
    Vector3 J_B = Vector3.Cross(joints[1].transform.forward, (joints[joints.Length - 1].transform.position - joints[1].transform.position));
    Vector3 J_C = Vector3.Cross(joints[2].transform.forward, (joints[joints.Length - 1].transform.position - joints[2].transform.position));
    ...

使用关节[i].transform.forward 用于确定链接将向目标位置移动的旋转轴，从而可以求解 XY 平面的 IK。