Plotting 3D Vector in Matplotlib using a Position and a Quaternion

user_9957175 0
Viewed 3508

I have the following information:

  • A position point (x,y,z)
  • An quaternion orientation (w, i, j, k)

I want to plot this as a vector in 3D in Matplotlib. I have seen many questions (example question) on plotting a 3D vector in Matplotlib. For example, I know I can plot a vector in Matplotlib given two points, a start (x, y, z) and an end (x, y, z) as shown below.

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

My question is: how can I either convert my position and quaternion into a start and end point so that I can plot it as a vector, or how can I directly plot my position and quaternion?

Note: I have the code which plots my position in 3D, I just am unsure how to get the orientation:

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

quaternions matplotlib python
1 Answers

I found a partial solution using ROS. In ROS there is a way to convert a quaternion into a Euler angle using:

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

Using this I then could plot the yaw using:

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

I think there should be a way to do this in pure Python and there should be a way to get all the angles, however I decided to put this up as a partial solution incase anyone runs into the same problem and no one else answers here.

user_99571750
Related