7 DOF Inverse Kinematic with Jacobian and Pseudo inverse

I'm stuck trying to animate a character's arm with pseudo-inverse that has 7 DOFs. The overall FK is like this:

(x,y,x) = TrootTshoulderRz(θ3)Ry(θ2)Rx(θ1)TelbowRy(θ5)Rx(θ4)TwristRy(θ7)Rz(θ6) Pendeffector.

I'm not sure what I'm doing wrong. These are what I have:
This calculates transformations and draw the result if it's a render call:

void BodyPart::DrawE(const Transform<float,3,2,0> *t,bool isCalculating)
{
    if((isSimOn && isCalculating) || !isSimOn)
    {
        Affine3f trans;
        trans = Affine3f::Identity();
        mv = *t;
        trans *= Translation3f(BodyPart::convertToEigenVector(Position + Pivot));
        trans *= AngleAxisf(glm::radians(Rotation.z),Vector3f(0,0,1));
        trans *= AngleAxisf(glm::radians(Rotation.y),Vector3f(0,1,0));
        trans *= AngleAxisf(glm::radians(Rotation.x),Vector3f(1,0,0));
        trans *= Translation3f(BodyPart::convertToEigenVector(-Position - Pivot));

        trans *= Translation3f(BodyPart::convertToEigenVector(Position));
        lmv = trans;
        mv = mv * trans;


        if(isWrist)
        {

            Vector3f endf;
            vec3 tef = Position+ 2.1f*Pivot;
            endf << tef.x,tef.y,tef.z;
            Vector3f e = mv * endf;
            gEndEffector = e;  // world ef

            endEffector = trans * endf; // local ef
        }
    }
    if(!isCalculating)
    {
        const float *pSource = mv.data();
        for (int i = 0; i < 16; i++) // to send to glLoadMatrixf()
            modelM[i] = pSource[i];
        Render();
    }
}

The IK solver functions:

int IkSimulator::step(double time)
{
    time = 0.1;

    Vector3f err(0.0f,0.0f,0.0f);
    if(INDEX < (spline->cpCount-1)*100)
    {

        targetP = Vector3f(spline->pts[INDEX][0],spline->pts[INDEX][1],spline->pts[INDEX][2]);

        do
        {

            float thetas[7];
            currentP = ((Bob*)bob)->getEndEffector(); // in World cs
            err = targetP - currentP;
            Vector3f tP;
            tP = (float)time*err+currentP;
            MatrixXf j3 = ((Bob*)bob)->getJacobian3();
            float *ts  = solve(j3,tP,thetas);
            ((Bob*)bob)->setDeltaThetas(ts[0],ts[1],ts[2],ts[3],ts[4],ts[5],ts[6]);
        }while ((err).norm() > 0.1f);
            ((Bob*)bob)->addSplinePoints(vec3(currentP.x(),currentP.y(),currentP.z()));
            INDEX++;
    }
    else
        animTcl::OutputMessage("Reached end of the spline.\n") ;
    return 0;
}
float* IkSimulator::solve(Eigen::MatrixXf J,Vector3f deltaP,float thetas[])
{
    Vector3f JBeta = (J * J.transpose()).inverse() * deltaP;
    Eigen::VectorXf Ts(7);
    Ts = J.transpose() * JBeta;

    thetas[0] = Ts(0);
    thetas[1] = Ts(1);
    thetas[2] = Ts(2);
    thetas[3] = Ts(3);
    thetas[4] = Ts(4);
    thetas[5] = Ts(5);
    thetas[6] = Ts(6);

    return thetas;
}

Constructing the Jacobian and updating positions returned from simulation step:

MatrixXf getJacobian3()
    {
        MatrixXf jMat(3,7);
        Vector3f e = bParts[7]->endEffector; // in Wrist's local cs

        Affine3f wrist;
        wrist =  bParts[7]->lmv;   // lmv is the local transformation

        Affine3f elbow;
        elbow =  bParts[6]->lmv;

        Affine3f shoulder;
        shoulder =  bParts[5]->lmv;

        Affine3f root;
        root = bParts[1]->lmv;

        AngleAxisf dtheta;
        Affine3f amc1,amc2,amc3,amc4,amc5,amc6,amc7;
        amc1 = getAffTranslation(5,true);
        amc1 *= getAffRotation('z',5);
        amc1 *= getAffRotation('y',5);
        amc1 *= dtheta.fromRotationMatrix(getDerRotation(5,'x'));
        amc1 *= getAffTranslation(5,true,true);
        amc1 *= getAffTranslation(5);
        amc1 =  root * amc1 * elbow * wrist;

        amc2 =  getAffTranslation(5,true);
        amc2 *= getAffRotation('z',5);
        amc2 *= dtheta.fromRotationMatrix(getDerRotation(5,'y'));
        amc2 *= getAffRotation('x',5);
        amc2 *= getAffTranslation(5,true,true);
        amc2 *= getAffTranslation(5);
        amc2 = root * amc2 * elbow * wrist;
        //amc2 = root * amc2;

        amc3 =  getAffTranslation(5,true);
        amc3 *= dtheta.fromRotationMatrix(getDerRotation(5,'z'));
        amc3 *= getAffRotation('y',5);
        amc3 *= getAffRotation('x',5);
        amc3 *= getAffTranslation(5,true,true);
        amc3 *= getAffTranslation(5);
        amc3 =  root * amc3 * elbow * wrist;
        //amc3 = root * amc3;

        amc4 =  getAffTranslation(6,true);
        //amc4 *= getAffRotation('z',6);
        amc4 *= getAffRotation('y',6);
        amc4 *= dtheta.fromRotationMatrix(getDerRotation(6,'x'));
        amc4 *= getAffTranslation(6,true,true);
        amc4 *= getAffTranslation(6);
        amc4 =  root * shoulder * amc4 * wrist;

        amc5 =  getAffTranslation(6,true);
        //amc5 *= getAffRotation('z',6);
        amc5 *= dtheta.fromRotationMatrix(getDerRotation(6,'y'));
        amc5 *= getAffRotation('x',6);
        amc5 *= getAffTranslation(6,true,true);
        amc5 *= getAffTranslation(6);
        amc5 = root * shoulder * amc5 * wrist;

        amc6 =  getAffTranslation(7,true);
        amc6 *= getAffRotation('y',7);
        amc6 *= dtheta.fromRotationMatrix(getDerRotation(7,'z'));
        //amc6 *= getAffRotation('x',6);
        amc6 *= getAffTranslation(7,true,true);
        amc6 *= getAffTranslation(7);
        amc6 = root * shoulder * elbow * amc6;

        amc7 =  getAffTranslation(7,true);
        amc7 *= dtheta.fromRotationMatrix(getDerRotation(7,'y'));
        amc7 *= getAffRotation('z',7);
        //amc7 *= getAffRotation('x',6);
        amc7 *= getAffTranslation(7,true,true);
        amc7 *= getAffTranslation(7);
        amc7 = root * shoulder * elbow * amc7;

        Vector3f c1 = amc1 * e;
        Vector3f c2 = amc2 * e;
        Vector3f c3 = amc3 * e;
        Vector3f c4 = amc4 * e;
        Vector3f c5 = amc5 * e;
        Vector3f c6 = amc6 * e;
        Vector3f c7 = amc7 * e;

        vector<Vector3f> cs;
        cs.push_back(c1);
        cs.push_back(c2);
        cs.push_back(c3);
        cs.push_back(c4);
        cs.push_back(c5);
        cs.push_back(c6);
        cs.push_back(c7);

        for(int i=0;i<7;i++)
            for(int j=0;j<3;j++)
                jMat(j,i) = cs[i](j);

                    return jMat;
    }
void setDeltaThetas(float t1,float t2, float t3, float t4, float t5, float t6, float t7)
    {

        bParts[5]->Rotation.x += t1;
        bParts[5]->Rotation.y += t2;
        bParts[5]->Rotation.z += t3;

        bParts[6]->Rotation.x += t4;
        bParts[6]->Rotation.y += t5;

        bParts[7]->Rotation.z += t6;
        bParts[7]->Rotation.y += t7;

        bParts[1]->DrawE(&Affine3f::Identity(),true); // Torso
        bParts[5]->DrawE(&bParts[1]->mv,true);  // Shoulder
        bParts[6]->DrawE(&bParts[5]->mv,true);  // Elbow
        bParts[7]->DrawE(&bParts[6]->mv,true);  // Wrist
    }

The utility functions used in making the J:

Matrix3f getDerRotation(int i,char c)
    {
        Matrix3f tMat;
        float coeff = 3.14159265f/180.f;
        //float coeff = 1.f;
        vec3 r = coeff*bParts[i]->Rotation;
        if(c =='x')
        {
            tMat(0,0) = 0.f;
            tMat(1,0) = 0.f;
            tMat(2,0) = 0.f;

            tMat(0,1) = 0.f;
            tMat(1,1) = -glm::sin(r.x);
            tMat(2,1) = glm::cos(r.x);

            tMat(0,2) = 0.f;
            tMat(1,2) = -glm::cos(r.x);
            tMat(2,2) = -glm::sin(r.x);

        }
        else if(c =='y')
        {
            tMat(0,0) = -glm::sin(r.y);
            tMat(1,0) = 0.f;
            tMat(2,0) = -glm::cos(r.y);

            tMat(0,1) = 0.f;
            tMat(1,1) = 0.f;
            tMat(2,1) = 0.f;

            tMat(0,2) = glm::cos(r.y);
            tMat(1,2) = 0.f;
            tMat(2,2) = -glm::sin(r.y);
        }
        else
        {
            tMat(0,0) = -glm::sin(r.z);
            tMat(1,0) = glm::cos(r.z);
            tMat(2,0) = 0.f;

            tMat(0,1) = -glm::cos(r.z);
            tMat(1,1) = -glm::sin(r.z);
            tMat(2,1) = 0.f;

            tMat(0,2) = 0.f;
            tMat(1,2) = 0.f;
            tMat(2,2) = 0.f;
        }
        return tMat;
    }
    AngleAxisf getAffRotation(char r,int i)
    {
        AngleAxisf rot;
        float coeff = glm::pi<float>()/180;
        //float coeff = 1.f;
        vec3 rott = coeff*bParts[i]->Rotation;
        if(r =='x')
            rot = AngleAxisf(rott.x,Vector3f(1,0,0));
        else if(r=='y')
            rot = AngleAxisf(rott.y,Vector3f(0,1,0));
        else
            rot = AngleAxisf(rott.z,Vector3f(0,0,1));
        return rot;
    }
    Translation3f getAffTranslation(int i,bool doPivot = false,bool pos = false)
    {
        Translation3f trans;
        if(doPivot)
            if(!pos)
                trans = Translation3f(BodyPart::convertToEigenVector(bParts[i]->Position + bParts[i]->Pivot));
            else
                trans = Translation3f(BodyPart::convertToEigenVector(-bParts[i]->Position - bParts[i]->Pivot));
        else
            trans = Translation3f(BodyPart::convertToEigenVector(bParts[i]->Position));

        return trans;
    }

The process doesn't return proper values, the arm goes everywhere except where it should. Also excuse my sporadic use of glm.

Hate to keep answering my own questions, but I had been working on this harder than on anything else so far. The problems were at two places:

• Instead of calling the .inverse() on the J * J.transpose() I used fullPivLu().solve(deltaP) on the J * J.transpose(). (I had overlooked the fact that we weren't suppose to invert the matrix but use LUD in my class notes.)
• The line with tP = (float)time * err+currentP; had to be just tP = (float)time*err;
  With the original version the target point would always be evading the endeffector.

  ---

  Just to be more clear on the first point here's the procedure I used to calculate the J+.
• Jt = J.transpose()
• JJt = (J * Jt)
• JBeta = JJt.solve(deltaP) // Given the deltaX solve by lu decomposition
• Angles = JJt * JBeta