Question

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

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

额外的Unity详细信息：

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

目标是一个单一的GameObject。

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。

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

1 个答案: