`m_fk_pos_solver` seems to have a different output compare to tf

[lxk-221]

Hi @stefanscherzinger . I'm using cartesian_compliance_controller for dual ur5 arm in ROS1 Noetic and Ubuntu 20.04, and when i publish /target_pose to /left/cartesian_compliance_controller/targer_pose, the left arm did't move to the target pose. I add some codes in cartesian_motion_controller.hpp and try to find out what cause the problem. I print the pose value of m_current_frame and m_target_frame.

template <class HardwareInterface>
ctrl::Vector6D CartesianMotionController<HardwareInterface>::
computeMotionError()
{
  // Compute motion error wrt robot_base_link
  m_current_frame = Base::m_ik_solver->getEndEffectorPose();

  // Transformation from target -> current corresponds to error = target - current
  KDL::Frame error_kdl;
  error_kdl.M = m_target_frame.M * m_current_frame.M.Inverse();
  error_kdl.p = m_target_frame.p - m_current_frame.p;

  // Use Rodrigues Vector for a compact representation of orientation errors
  // Only for angles within [0,Pi)
  KDL::Vector rot_axis = KDL::Vector::Zero();
  double angle    = error_kdl.M.GetRotAngle(rot_axis);   // rot_axis is normalized
  double distance = error_kdl.p.Normalize();

  // Clamp maximal tolerated error.
  // The remaining error will be handled in the next control cycle.
  // Note that this is also the maximal offset that the
  // cartesian_compliance_controller can use to build up a restoring stiffness
  // wrench.
  const double max_angle = 1.0;
  const double max_distance = 1.0;
  angle    = std::clamp(angle,-max_angle,max_angle);
  distance = std::clamp(distance,-max_distance,max_distance);

  // Scale errors to allowed magnitudes
  rot_axis = rot_axis * angle;
  error_kdl.p = error_kdl.p * distance;

  // Reassign values
  ctrl::Vector6D error;
  error(0) = error_kdl.p.x();
  error(1) = error_kdl.p.y();
  error(2) = error_kdl.p.z();
  error(3) = rot_axis(0);
  error(4) = rot_axis(1);
  error(5) = rot_axis(2);

  //ROS_WARN_STREAM("Failed to load "
  //    << nh.getNamespace() + "/target_frame_topic"
  //    << " from parameter server. Will default to: "
  //    << nh.getNamespace() + '/' + m_target_frame_topic);
  double m_current_frame_x,m_current_frame_y,m_current_frame_z,m_current_frame_w;
  m_current_frame.M.GetQuaternion(m_current_frame_x,m_current_frame_y,m_current_frame_z,m_current_frame_w);
  double m_target_frame_x,m_target_frame_y,m_target_frame_z,m_target_frame_w;
  m_target_frame.M.GetQuaternion(m_target_frame_x,m_target_frame_y,m_target_frame_z,m_target_frame_w);

  std::cout << "current pose is:\n"
    << "trans: " << "(" << m_current_frame.p.x() << ", " << m_current_frame.p.y() << ", " << m_current_frame.p.z() << ")\n"
    << "rot: " << "(" << m_current_frame_x << ", " << m_current_frame_y << ", " << m_current_frame_z << ", " << m_current_frame_w << ")\n"
    << std::endl;
  std::cout << "target pose is:\n"
    << "trans: " << "(" << m_target_frame.p.x() << ", " << m_target_frame.p.y() << ", " << m_target_frame.p.z() << ")\n"
    << "rot: " << "(" << m_target_frame_x << ", " << m_target_frame_y << ", " << m_target_frame_z << ", " << m_target_frame_w << ")\n"
    << std::endl;
  std::cout << "target and current pose error is:\n"
    << "trans: " << "(" << error(0) << ", " << error(1) << ", " << error(2) << ")\n"
    << "rot: " << "(" << error(3) << ", " << error(4) << ", " << error(5) << ")\n"
    << std::endl;
  return error;
}

and i found that the output of m_target_frame is the same as what i published on topic /left/cartesian_compliance_controller/targer_pose. But when it turns to m_current_frame, it has the different output with rosrun tf tf_echo world leftxipan my left_arm_controller_config is

cartesian_compliance_controller:
   type: "position_controllers/CartesianComplianceController"
   end_effector_link: "leftxipan"
   robot_base_link: "world"
   ft_sensor_ref_link: "_left_ft300_sensor"
   compliance_ref_link: "leftxipan"
   target_frame_topic: "target_pose"
   joints: *robot_joints

   stiffness:
      trans_x: 500
      trans_y: 500
      trans_z: 500
      rot_x: 20
      rot_y: 20
      rot_z: 20
      publish_state_feedback: true
   solver:
      error_scale: 0.5

   pd_gains:
      trans_x: {p: 0.05}
      trans_y: {p: 0.05}
      trans_z: {p: 0.05}
      rot_x: {p: 1.50}
      rot_y: {p: 1.50}
      rot_z: {p: 1.50}

the first panel is the output in cartesian_motion_controller.hpp, which is computed by chain. the second panel is the output of my own node, which is computed by tf tree. here is my tf tree I'd appreciate if someone can give me some suggestions.