Trying to implement Overview of Jacobian IK example within Unity

Question

I am trying to implement the examples shown here.

But when running the code, and using the debug mode, there are no values returned. I am assuming that I am not using the correct rotation axis.

Extra Unity Details:

Joints variable is an array of GameObjects (consisting of 4).

Target is a singel GameObject.

The Tools.M_Populate, Tools.M_Multiply and Tools.M_Transpose. I have checked and seem to be working, when checking with debug data. They are simple codes that return 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);
    }

I am expecting a vector of angles to apply to each joint

score 1 · Answer 1 · answered Jul 12 '19 at 17:29

After a little trial and error, I beleave I have reached an answer. I was having issues with axis of rotation. Using the current code, it couldnt solve for XY plane (the same plane I had the gameobjects positioned on). I have uploaded to GitHub upload the current version.

To correct the code in the question I changed two key areas first from:

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

to:

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

The second to change the code in the GetJacobianTranspose() method from:

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

to:

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

Using the joints[i].transform.forward is used to determine the axis of rotation that the links will be moving toward the target position, allowing to solve the IK for the XY plane.

Trying to implement Overview of Jacobian IK example within Unity

1 Answers1