Построение трехмерного вектора в Matplotlib с использованием позиции и кватерниона - PullRequest
Новая
       19

Построение трехмерного вектора в Matplotlib с использованием позиции и кватерниона

2 голосов
/ 26 февраля 2020

У меня есть следующая информация:

  • Точка положения (x, y, z)
  • Ориентация кватерниона (w, i, j, k)

Я хочу построить это как вектор в 3D в Matplotlib. Я видел много вопросов (пример вопроса) о построении трехмерного вектора в Matplotlib. Например, я знаю, что могу построить вектор в Matplotlib с учетом двух точек: начала (x, y, z) и конца (x, y, z), как показано ниже. 

start = [2 3 5];
end = [4 5 5]; 
quiver3(start(1), start(2), start(3), end(1), end(2), end(3));

Мой вопрос является ли: как я могу либо преобразовать свою позицию и кватернион в начальную и конечную точки, чтобы я мог построить ее как вектор, или как я могу напрямую построить свою позицию и кватернион?

Примечание: У меня есть код, который отображает мою позицию в 3D, я просто не уверен, как получить ориентацию: 

ax.set_title("Pos: (" + str(x) + ", " + str(y) + ", " + str(z) + ")")
ax.set_xlim([-5, 5])
ax.set_ylim([-5, 5])
ax.set_zlim([0, 5])
ax.set_xlabel("X Axis")
ax.set_ylabel("Y Axis")
ax.set_zlabel("Z Axis")
ax.scatter(x, y, z)

Image showing what I have compared to what I need

1 Ответ

1 голос
/ 27 февраля 2020

Я нашел частичное решение, используя ROS . В ROS есть способ преобразовать кватернион в угол Эйлера, используя: 

from tf.transformations import euler_from_quaternion
quaternion = (w,i,j,k)
euler = euler_from_quaternion(quaternion)

Используя это, я затем мог бы построить график рыскания, используя: 

yaw = euler[2]
new_x = sin(yaw)
new_y = cos(yaw)
self.ax.quiver(x, y, z, new_x, new_y, 0, color='b')

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

...