Robotics StackExchange | Archived questions

KDL and Moveit Return Different Jacobians

I'm trying to compute the Jacobian matrix (as fast as possible), and would like to use KDL for this purpose. In comparing the results of Moveit::core::RobotState's getJacobian and KDL::ChainJntToJacSolver, I get vastly different results, and Moveit's is correct.

Relevant pieces of code are here:

if(!kdl_parser::treeFromString(robot_desc_,kdl_tree_)){
    ROS_ERROR_STREAM_NAMED(JointTrajectoryController::name_,"Failed to construct KDL tree!!");
    return 1;
  }

 std::unique_ptr<robot_model_loader::RobotModelLoader> model_loader_ptr_ =
  std::unique_ptr<robot_model_loader::RobotModelLoader>(new robot_model_loader::RobotModelLoader);
  const robot_model::RobotModelPtr& kinematic_model = model_loader_ptr_->getModel();
  joint_model_group_ = kinematic_model->getJointModelGroup(compliance_params_->move_group_name);
  kinematic_state_ = std::make_shared<robot_state::RobotState>(kinematic_model);

Then later on I have:

for (unsigned int i = 0; i < JointTrajectoryController::joints_.size(); ++i){
        //ROS_INFO_STREAM_THROTTLE_NAMED(1,JointTrajectoryController::name_, "j"<<i << " pos: " << JointTrajectoryController::current_state_.position[i]);
        q_.data[i] = JointTrajectoryController::current_state_.position[i];
        jntpos[i] = JointTrajectoryController::current_state_.position[i];
}

jnt_to_jac_solver_->JntToJac(q_,J_); // compute the jacobian matrix at this point.
      Eigen::MatrixXd jacobian = J_.data;
      ROS_ERROR_STREAM_NAMED(JointTrajectoryController::name_,"kdlJac: "<< jacobian);

kinematic_state_->setVariablePositions(jntpos);
      jacobian = kinematic_state_->getJacobian(joint_model_group_);

Any ideas as to the cause of this discrepancy would be much appreciated. I would prefer not to depend on moveit for this.

Asked by jbeck28 on 2020-04-24 16:00:41 UTC

Comments

Answers