Jacobian looks fishy

Computation with gx,gy,gz looks fishy!
 bool Model::computeJacobian(taoDNode const * node,
      double gx, double gy, double gz,
      Eigen::MatrixXd & jacobian) const
  {
    if ( ! node) {
      return false;
    }
    std::map<taoDNode *, std::list<SAncestryEntry> >::const_iterator iae(ancestry_table_.find(const_cast<taoDNode*>(node)));
    if (iae == ancestry_table_.end()) {
      return false;
    }
    std::list<SAncestryEntry> const & alist(iae->second);

#ifdef TAO_DEBUG
    fprintf(stderr, "computeJacobian()\ng: [% 4.2f % 4.2f % 4.2f]\n", gx, gy, gz);
#endif // TAO_DEBUG

    // \todo Implement support for more than one joint per node, and
    //  more than one DOF per joint.
    jacobian = Eigen::MatrixXd::Zero(6, ndof_);
    std::list<SAncestryEntry>::const_iterator ia(alist.begin());
    std::list<SAncestryEntry>::const_iterator iend(alist.end());
    for (/**/; ia != iend; ++ia) {
      deVector6 Jg_col;
      ia->joint->getJgColumns(&Jg_col);
      int const icol(ia->id);

#ifdef TAO_DEBUG
      fprintf(stderr, "iJg[%d]: [ % 4.2f % 4.2f % 4.2f % 4.2f % 4.2f % 4.2f]\n",
          icol,
          Jg_col.elementAt(0), Jg_col.elementAt(1), Jg_col.elementAt(2),
          Jg_col.elementAt(3), Jg_col.elementAt(4), Jg_col.elementAt(5));
#endif // TAO_DEBUG

      for (size_t irow(0); irow < 6; ++irow) {
        jacobian.coeffRef(irow, icol) = Jg_col.elementAt(irow);
      }

      // Add the effect of the joint rotation on the translational
      // velocity at the global point (column-wise cross product with
      // [gx;gy;gz]). Note that Jg_col.elementAt(3) is the
      // contribution to omega_x etc, because the upper 3 elements of
      // Jg_col are v_x etc.  (And don't ask me why we have to
      // subtract the cross product, it probably got inverted
      // somewhere)
      jacobian.coeffRef(0, icol) -= -gz * Jg_col.elementAt(4) + gy * Jg_col.elementAt(5);
      jacobian.coeffRef(1, icol) -=  gz * Jg_col.elementAt(3) - gx * Jg_col.elementAt(5);
      jacobian.coeffRef(2, icol) -= -gy * Jg_col.elementAt(3) + gx * Jg_col.elementAt(4);

#ifdef TAO_DEBUG
      fprintf(stderr, "0Jg[%d]: [ % 4.2f % 4.2f % 4.2f % 4.2f % 4.2f % 4.2f]\n",
          icol,
          jacobian.coeff(0, icol), jacobian.coeff(1, icol), jacobian.coeff(2, icol),
          jacobian.coeff(3, icol), jacobian.coeff(4, icol), jacobian.coeff(5, icol));
#endif // TAO_DEBUG

    }
    return true;
  }
Comments (7)
