Is this a bug in jacobian computation?

edit

Hi All,

I'm using ROS/moveit to compute jacobian matrix (getJacobian as a function of RobotState class). I tested the jacobian using finite difference but the error is large, is this a bug in ROS or my code is wrong?

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

  //compute jacobian in quaternion mode
  Eigen::MatrixXd J=getJacobian(groupName,linkName,true);
  if(J.size() == 0)
    return;

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

  //get current group names
  const robot_model::JointModelGroup* group=state.getRobotModel()->getJointModelGroup(groupName);
  const vector<string>& gnames=group->getVariableNames();

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

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

    Eigen::VectorXd DN=Eigen::VectorXd::Zero(7),DA=J.col(i);
    DN.block<3,1>(0,0)=(t1.translation()-t0.translation())/DELTA;
    DN.block<4,1>(3,0)=Eigen::Vector4d(q1.w()-q0.w(),q1.x()-q0.x(),q1.y()-q0.y(),q1.z()-q0.z())/DELTA;
    ROS_INFO_NOW("Jacobian col %ld: %f Err: %f!",i,DA.norm(),(DN-DA).norm())
    state.setVariablePosition(gnames[i],positions[gnames[i]]);
    state.update();
  }
#undef DELTA
}

Zherong