機器人阻抗控制中末端姿態偏差的表示方法

• 歐拉角：缺點是存在奇異問題，
  案例：KUKA使用的RPY
• 軸角：缺點是有效範圍小，±90°
• 四元數：優點多多，沒有gimbal lock問題，其本身就能代表姿態偏差用於阻抗控制器笛卡爾空間扭矩計算；缺點是不容易理解
  案例：Franka用的四元數

  //Source: cartesian_impedance_example_controller.cpp
  if (orientation_d.coeffs().dot(orientation.coeffs()) < 0.0) 
  {
  	orientation.coeffs() << -orientation.coeffs();
  }
  
  // "difference" quaternion
  Eigen::Quaterniond error_quaternion(orientation.inverse() * orientation_d);
  error.tail(3) << error_quaternion.x(), error_quaternion.y(), error_quaternion.z();
  
  // Transform to base frame
  error.tail(3) << -transform.linear() * error.tail(3);
  
  // compute control
  Eigen::VectorXd tau_task(7), tau_d(7);
  
  // Spring damper system with damping ratio=1
  tau_task << jacobian.transpose() * (-stiffness * error - damping * (jacobian * dq));
  tau_d << tau_task + coriolis;
  

---

參考文獻：

• https://www.zhihu.com/question/336277983