Ask Your Question
0

Is this a bug in jacobian computation?

asked 2015-11-06 23:00:44 -0500

DamnItROS gravatar image

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

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2015-11-08 13:25:49 -0500

DamnItROS gravatar image

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
}
edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2015-11-06 23:00:44 -0500

Seen: 532 times

Last updated: Nov 08 '15