title: 7 DOF Inverse Kinematic with Jacobian and Pseudo inverse
body: <p>I'm stuck trying to animate a character's arm with pseudo-inverse that has 7 DOFs.
  The overall FK is like this:</p>
  
  <blockquote>
    <p>(x,y,x) =
    TrootTshoulderRz(θ3)Ry(θ2)Rx(θ1)TelbowRy(θ5)Rx(θ4)TwristRy(θ7)Rz(θ6)
    Pendeffector.</p>
  </blockquote>
  
  <p>I'm not sure what I'm doing wrong. These are what I have:<br />
  This calculates transformations and draw the result if it's a render call:</p>
  
  <pre><code>void BodyPart::DrawE(const Transform&lt;float,3,2,0&gt; *t,bool isCalculating)
  {
      if((isSimOn &amp;&amp; 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 &lt;&lt; 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 &lt; 16; i++) // to send to glLoadMatrixf()
              modelM[i] = pSource[i];
          Render();
      }
  }
  </code></pre>
  
  <p>The IK solver functions:</p>
  
  <pre><code>int IkSimulator::step(double time)
  {
      time = 0.1;
  
      Vector3f err(0.0f,0.0f,0.0f);
      if(INDEX &lt; (spline-&gt;cpCount-1)*100)
      {
  
          targetP = Vector3f(spline-&gt;pts[INDEX][0],spline-&gt;pts[INDEX][1],spline-&gt;pts[INDEX][2]);
  
          do
          {
  
              float thetas[7];
              currentP = ((Bob*)bob)-&gt;getEndEffector(); // in World cs
              err = targetP - currentP;
              Vector3f tP;
              tP = (float)time*err+currentP;
              MatrixXf j3 = ((Bob*)bob)-&gt;getJacobian3();
              float *ts  = solve(j3,tP,thetas);
              ((Bob*)bob)-&gt;setDeltaThetas(ts[0],ts[1],ts[2],ts[3],ts[4],ts[5],ts[6]);
          }while ((err).norm() &gt; 0.1f);
              ((Bob*)bob)-&gt;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;
  }
  </code></pre>
  
  <p>Constructing the Jacobian and updating positions returned from simulation step:</p>
  
  <pre><code>MatrixXf getJacobian3()
      {
          MatrixXf jMat(3,7);
          Vector3f e = bParts[7]-&gt;endEffector; // in Wrist's local cs
  
          Affine3f wrist;
          wrist =  bParts[7]-&gt;lmv;   // lmv is the local transformation
  
          Affine3f elbow;
          elbow =  bParts[6]-&gt;lmv;
  
          Affine3f shoulder;
          shoulder =  bParts[5]-&gt;lmv;
  
          Affine3f root;
          root = bParts[1]-&gt;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&lt;Vector3f&gt; 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&lt;7;i++)
              for(int j=0;j&lt;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]-&gt;Rotation.x += t1;
          bParts[5]-&gt;Rotation.y += t2;
          bParts[5]-&gt;Rotation.z += t3;
  
          bParts[6]-&gt;Rotation.x += t4;
          bParts[6]-&gt;Rotation.y += t5;
  
          bParts[7]-&gt;Rotation.z += t6;
          bParts[7]-&gt;Rotation.y += t7;
  
          bParts[1]-&gt;DrawE(&amp;Affine3f::Identity(),true); // Torso
          bParts[5]-&gt;DrawE(&amp;bParts[1]-&gt;mv,true);  // Shoulder
          bParts[6]-&gt;DrawE(&amp;bParts[5]-&gt;mv,true);  // Elbow
          bParts[7]-&gt;DrawE(&amp;bParts[6]-&gt;mv,true);  // Wrist
      }
  </code></pre>
  
  <p>The utility functions used in making the J:</p>
  
  <pre><code>Matrix3f getDerRotation(int i,char c)
      {
          Matrix3f tMat;
          float coeff = 3.14159265f/180.f;
          //float coeff = 1.f;
          vec3 r = coeff*bParts[i]-&gt;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&lt;float&gt;()/180;
          //float coeff = 1.f;
          vec3 rott = coeff*bParts[i]-&gt;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]-&gt;Position + bParts[i]-&gt;Pivot));
              else
                  trans = Translation3f(BodyPart::convertToEigenVector(-bParts[i]-&gt;Position - bParts[i]-&gt;Pivot));
          else
              trans = Translation3f(BodyPart::convertToEigenVector(bParts[i]-&gt;Position));
  
          return trans;
      }
  </code></pre>
  
  <p>The process doesn't return proper values, the arm goes everywhere except where it should. Also excuse my sporadic use of glm.</p>