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

Question

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

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

额外的Unity详细信息：

关节变量是一个GameObjects数组(由4个组成)。

目标是单个GameObject。

当我检查调试数据时，我已经检查过了，并且似乎正在工作。

   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));
    ...

使用jointsi.transform.forward用于确定链接将朝向目标位置移动的旋转轴，从而可以为XY平面解算IK。