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

Question

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

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

额外的 Unity 细节:

Joints 变量是一个游戏对象数组(由 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));
    ...

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