Reputation: 197
I am trying to plot coordinate frames in matplotlib 3D. In my implementation, I am trying to do two things: Plot the origin of the frame and draw the i, j and k vectors of the frame. This works fine for absolute frames (i.e defined w.r.t to the world frame) but fails for relative frames (i.e defined w.r.t to other non-world frames).
The specifics of the problem are:
World frame: Typical frame. [1,0,0], [0,1,0], [0,0,1] are the i,j and k vectors respectively.
Frame A: Defined w.r.t to world frame. It is translated by [5, 2, 8]
Frame B: Defined w.r.t to frame A. It is translated by [5, 0, 0] and rotated by 30 degrees about the Z axis, all w.r.t to frame A.
I have managed to draw the first two frames successfully. The problem is drawing frame B. The origin appears in the wrong position and I don't know how to get the i,j,k vectors of frame B after doing all of this so I don't know how to plot it.
import numpy as np
import matplotlib.pyplot as plt
import math
%matplotlib widget
# setting up matplotlib
fig = plt.figure()
ax = fig.add_subplot(projection='3d')
ax.set(xlim=(-10,10), ylim=(-10,10), zlim=(-10,10))
ax.set(xticks=range(-10,10,2), yticks=range(-10,10,2), zticks=range(-10,10,2))
# position and rotation is relative to the parent
def draw_coord_frame(plt, parent, position, rotation):
T = position
x = np.array([1,0,0,1])
y = np.array([0,1,0,1])
z = np.array([0,0,1,1])
newFrame = np.identity(4)
newFrame[0:3, 0:3] = rotation
newFrame[0:3, 3] = position
transform = parent @ newFrame
X = transform @ x
Y = transform @ y
Z = transform @ z
X[0:3] /= X[3]
Y[0:3] /= Y[3]
Z[0:3] /= Z[3]
ax.plot3D([T[0], X[0]], [T[1], X[1]], [T[2], X[2]], c = 'red')
ax.plot3D([T[0], Y[0]], [T[1], Y[1]], [T[2], Y[2]], c = 'green')
ax.plot3D([T[0], Z[0]], [T[1], Z[1]], [T[2], Z[2]], c = 'blue')
return newFrame
World = draw_coord_frame(plt, np.identity(4), np.array([0,0,0]), np.identity(3))
Stereo2 = draw_coord_frame(plt, World, np.array([5,8,4]), np.identity(3))
draw_coord_frame(plt, Stereo2, np.array([7,0,0]), np.identity(3))
plt.show()
Upvotes: 0
Views: 57