Revision history [back]

Found the answer, a problem with my code. So the second part of the jacobian is against rotation axis. Here is the correct code:

void Grasper::debugJacobian(const string linkName) const
{
  //test at random position
  robot_state::RobotState& state=_scene->getCurrentStateNonConst();
  state.setToRandomPositions();
  state.update();

  //compute jacobian in quaternion mode
  vector<string> jointNames;
  Eigen::MatrixXd J=getJacobian(linkName,jointNames);
  if(J.size() == 0)
    return;

  //get current position
  map<string,double> positions;
  for(size_t i=0;i<jointNames.size();i++)
    positions[jointNames[i]]=state.getVariablePosition(jointNames[i]);

  //debug
#define DELTA 1E-8f
  Eigen::Affine3d t0=state.getGlobalLinkTransform(linkName);
  Eigen::Quaterniond q0(t0.rotation());
  for(size_t i=0;i<jointNames.size();i++)
  {
    state.setVariablePosition(jointNames[i],positions[jointNames[i]]+DELTA);
    state.update();

    Eigen::Affine3d t1=state.getGlobalLinkTransform(linkName);
    Eigen::Quaterniond q1(t1.rotation());

    //Jacobian for translation
    Eigen::Vector3d DNT=(t1.translation()-t0.translation())/DELTA;
    Eigen::Vector3d DAT=J.col(i).block<3,1>(0,0);
    ROS_INFO_NOW("Jacobian for translation %ld: %f Err: %f!",i,DAT.norm(),(DNT-DAT).norm())

    //Jacobian for rotation
    Eigen::Matrix3d DNR=(t1.rotation()-t0.rotation())/DELTA;
    Eigen::Matrix3d DAR=cross(J.col(i).block<3,1>(3,0))*t0.rotation();
    ROS_INFO_NOW("Jacobian for rotation: %ld: %f Err: %f!",i,DAR.norm(),(DNR-DAR).norm())

    state.setVariablePosition(jointNames[i],positions[jointNames[i]]);
    state.update();
  }
#undef DELTA
}