3D Вращение позы вокруг источника

Question

Я хочу преобразовать позу вокруг источника. Поза у меня содержит положение [x, y, z] и ориентацию [x, y, z, w], представленную как кватернион. У меня есть матрица преобразования 4x4, которую я использую для перевода и поворота положения позы вокруг источника.

np.dot(transforma_mat, np.transpose(np.append(pose.position, 1))) 

Итак, как происхождение движется и вращается, начальная точка позы также перемещается и поворачивается. Однако я не уверен, как я могу повернуть ориентацию позы.

Я могу преобразовать кватернион в углы Эйлера и матрицу вращения (и наоборот), используя this library.

История: Объект, который перемещается и вращается. Конечный эффектор робота имеет позу перед тем, как объект движется и вращается. Я хочу обновить позу конечного эффектора таким образом, чтобы он перемещался и вращался вместе с объектом.

Answer 1

Общий способ перевода объекта по отношению к движению другого объекта может быть найден here.

Попробуйте преобразовать вращения в матрицы. Углы Эйлера не работали для меня.

Answer 2

Я не уверен, как это сделать с кватернионами, однако вы можете определить свою ориентацию как два ортогональных вектора (соблюдая правильное или левое правило).

После этого вы можете просто умножить эти векторы в своей системе координат на свои матрицы преобразования, чтобы получить преобразованную ориентацию.