Управление ориентацией с помощью кватерниона

Question

Для управления манипулятором-манипулятором у меня есть контроллер с 6 размерами (x, y, z position, а также roll, pitch, вращение вокруг оси). Я использую положение (x, y, z) и кватернион (x, y, z, w), чтобы представить желаемое положение и ориентацию захвата робота.

Я использую некоторые библиотеки, с которыми люди могут быть не знакомы (а именно, geometry_msgs из ROS), поэтому вот что я делаю в псевдокоде:

while(true):
  new_position = last_position + (joy.x, joy.y, joy.z)
  new_quaternion = last_quaternion * quaternionFromRPY(joy.roll, joy.pitch, joy.yaw)

  // (Compute inverse kinematics using position and orientation, then send to robot arm)

  last_position = new_position
  last_quaternion = new_quaternion

  delay(dt)

Я могу точно установить x, y и z позиции позы. Технически, я также могу установить кватернион очень хорошо, но моя проблема в том, что управление кватернионом крайне неинтуитивно, поскольку вращение не коммутативно. то есть, если я поверну на 180 градусов вокруг оси x, то управление вращением вокруг оси z станет инвертированным.

Есть ли способ, которым я могу управлять вращением из своего рода "глобальной системы отсчета", если это имеет смысл? Мне бы хотелось, чтобы, если я поверну джойстик по часовой стрелке относительно z, поза будет вращаться соответственно, вместо того, чтобы "иногда вращаться по часовой стрелке, иногда вращаться против часовой стрелки".

Answer 1

Разве это не так просто, как обмен порядка факторов в кватернионном произведении?

Единица кватерниона q преобразует вектор v в локальных координатах к повернутому вектору q*v*q' в глобальных координатах. Модифицированный кватернион a*q*b (a, b также единичные кватернионы) превращает v как

a*(q*(b*v*b')*q')*a', 

где часть b может быть интерпретирована как вращение в локальных координатах, а часть a - как вращение в глобальных координатах.

Таким образом, чтобы применить вращение только в глобальных координатах, установите b=1, то есть оставьте это, и поместите желаемое вращение в a фактор.