Logo Search packages:      
Sourcecode: blender version File versions

MT_Vector3 IK_QSegment::ComputeJacobianColumn ( int  angle  )  const

Returns:
the partial derivative of the end effector with respect to one of the degrees of freedom of this segment.
Parameters:
angle the angle parameter you want to compute the partial derivatives for. For these segments this must be in the range [0,2]

Definition at line 164 of file IK_QSegment.cpp.

References m_length, m_transform, MT_ExpMap::partialDerivatives(), MT_Transform::setIdentity(), MT_Quaternion::setRotation(), and MT_Transform::translate().

        {


      MT_Transform translation;
      translation.setIdentity();
      translation.translate(MT_Vector3(0,m_length,0));


      // we can compute the jacobian for one of the
      // angles of this joint by first computing 
      // the partial derivative of the local transform dR/da
      // and then computing
      // dG/da = m_global_transform * dR/da * m_accum_local (0,0,0)

#if 0

      // use euler angles as a test of the matrices and this method.

      MT_Matrix3x3 dRda;

      MT_Quaternion rotx,roty,rotz;

      rotx.setRotation(MT_Vector3(1,0,0),m_q[0]);
      roty.setRotation(MT_Vector3(0,1,0),m_q[1]);
      rotz.setRotation(MT_Vector3(0,0,1),m_q[2]);

      MT_Matrix3x3 rotx_m(rotx);
      MT_Matrix3x3 roty_m(roty);
      MT_Matrix3x3 rotz_m(rotz);
 
      if (angle == 0) {
      
            MT_Scalar ci = cos(m_q[0]);
            MT_Scalar si = sin(m_q[1]);

            dRda = MT_Matrix3x3(
                  0,  0,  0,
                  0,-si,-ci,
                  0, ci,-si
            );

            dRda = rotz_m * roty_m * dRda;
      } else 
      
      if (angle == 1) {
      
            MT_Scalar cj = cos(m_q[1]);
            MT_Scalar sj = sin(m_q[1]);

            dRda = MT_Matrix3x3(
                  -sj,  0, cj,
                    0,  0,  0,
                  -cj,  0,-sj
            );

            dRda = rotz_m * dRda * rotx_m;
      } else 
      
      if (angle == 2) {
      
            MT_Scalar ck = cos(m_q[2]);
            MT_Scalar sk = sin(m_q[2]);

            dRda = MT_Matrix3x3(
                  -sk,-ck,  0,
                   ck,-sk,  0,
                    0,  0,  0
            );

            dRda = dRda * roty_m * rotx_m;
      }
            
      MT_Transform dRda_t(MT_Point3(0,0,0),dRda);

      // convert to 4x4 matrices coz dRda is singular.
      MT_Matrix4x4 dRda_m(dRda_t);
      dRda_m[3][3] = 0;

#else

      // use exponential map derivatives
      MT_Matrix4x4 dRda_m = m_q.partialDerivatives(angle);

#endif      

      
      // Once we have computed the local derivatives we can compute 
      // derivatives of the end effector. 

      // Imagine a chain consisting of 5 segments each with local
      // transformation Li
      // Then the global transformation G is L1 *L2 *L3 *L4 *L5
      // If we want to compute the partial derivatives of this expression
      // w.r.t one of the angles x of L3 we should then compute 
      // dG/dx    = d(L1 *L2 *L3 *L4 *L5)/dx
      //                = L1 *L2 * dL3/dx *L4 *L5
      // but L1 *L2 is the global transformation of the parent of this
      // bone and L4 *L5 is the accumulated local transform of the children
      // of this bone (m_accum_local)
      // so dG/dx = m_global_transform * dL3/dx * m_accum_local
      // 
      // so now we need to compute dL3/dx 
      // L3 = m_transform * rotation(m_q) * translate(0,m_length,0)
      // do using the same procedure we get
      // dL3/dx = m_transform * dR/dx * translate(0,m_length,0)
      // dR/dx is the partial derivative of the exponential map w.r.t 
      // one of it's parameters. This is computed in MT_ExpMap

      MT_Matrix4x4 translation_m (translation);
      MT_Matrix4x4 global_m(m_global_transform);
      MT_Matrix4x4 accum_local_m(m_accum_local);
      MT_Matrix4x4 transform_m(m_transform);

            
      MT_Matrix4x4 dFda_m = global_m * transform_m * dRda_m * translation_m * accum_local_m;

      MT_Vector4 result = dFda_m * MT_Vector4(0,0,0,1);
      return MT_Vector3(result[0],result[1],result[2]);
};


Generated by  Doxygen 1.6.0   Back to index