JESSICA BARON

    //For 2 joints and observing change in position, Jacobian is a 3x2 matrix (wrapped in a 4x4).

    private Matrix4x4 makeJacobian()

    {

        Matrix4x4 jacobian = Matrix4x4.zero;

        Vector3 temp;

 

        temp = Vector3.Cross(rotAxis, posEnd-pos0);

        jacobian.SetColumn(0, new Vector4(temp.x, temp.y, temp.z));

 

        temp = Vector3.Cross(rotAxis, posEnd-pos1);

        jacobian.SetColumn(1, new Vector4(temp.x, temp.y, temp.z));

 

        return jacobian;

    }

​

    //----------------------------------

    //Pseudoinverse(M) = inverse(transpose(M) * M) * transpose(M)

    private Matrix4x4 pseudoinverse(Matrix4x4 M, ref bool isSingular)

    {

        Matrix4x4 M_pInv;   //Pseudoinverse

        Matrix4x4 transpose, prod, inv;

 

        transpose = Matrix4x4.Transpose(M);

        prod = transpose * M;

        inv = invert2x2(prod, ref isSingular);

        M_pInv = inv * transpose;

 

        return M_pInv;

    }

​

//----------------------------------

    private void rotateJoints()

    {

        Vector3 angles, theta0, theta1;

        Quaternion currJ0Rot, currJ1Rot;

        

        goalVel = goal - posEnd;        //End effector to goal

        Debug.Log(string.Concat("Dist between goal and end effector = ", goalVel.magnitude));

 

        angles = getAngleChanges();

 

        currJ0Rot = joint0.transform.localRotation;

        currJ1Rot = joint1.transform.localRotation;

        theta0 = (rotAxis * angles.x) + currJ0Rot.eulerAngles;   //Build up on the last local rotation.

        theta1 = (rotAxis * angles.y) + currJ1Rot.eulerAngles;

 

        joint0.transform.localRotation = Quaternion.Euler(theta0.x, theta0.y, theta0.z);

        joint1.transform.localRotation = Quaternion.Euler(theta1.x, theta1.y, theta1.z);

 

        getJointPositions();            //Update for next call.

        goalVel = goal - posEnd;

    }

 

    //----------------------------------

    //Returns a vector of the necessary changes in angles per joint (in degrees).

    private Vector3 getAngleChanges()

    {

        Vector3 dTheta = new Vector3();

        Matrix4x4 J, J_pInv;

        bool isSingular = false;

 

        J = makeJacobian();

        J_pInv = pseudoinverse(J, ref isSingular);      //Can be 0 matrix if not invertable (straight limb).  If J is a 3x2 matrix, the pseudoinverse is a 2x3.

 

        if (!isSingular)

        { 

            dTheta = J_pInv.MultiplyPoint3x4(goalVel);

        }

        else   //Use the Jacobian's transpose as the second best bet if the pseudoinverse can't be found.

        { 

            dTheta = Matrix4x4.Transpose(J).MultiplyPoint3x4(goalVel);

        }

 

        Debug.Log(string.Concat("goal vel (x,y,z) = ", goalVel));

        Debug.Log(string.Concat("Jacobian = ", J));

        Debug.Log(string.Concat("Pseudoinverse = ", J_pInv));

        Debug.Log(string.Concat("Transpose(J) = ", Matrix4x4.Transpose(J)));

        Debug.Log(string.Concat("dTheta (multiplyPoint3x4) = ", J_pInv.MultiplyPoint3x4(goalVel)));

 

 

        return dTheta;

    }