In [2]:
# from xvlib import listAttr
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from sympy.physics.vector import ReferenceFrame
from sympy import symbols, latex
# %matplotlib widget
In [3]:
def plotReferenceFrames(rf, baseRf, scale = 1, axis_len = None, **kwargs):
fig = plt.figure(figsize = (7, 7))
ax = plt.axes(projection='3d', box_aspect = (1, 1, 1))
origin = np.array([[0, 0, 0], [0, 0, 0], [0, 0, 0]])
baseRfDCMbaseRf = scale * np.array(baseRf.dcm(baseRf)).astype('float64')
rfDCMbaseRf = scale * np.array(rf.dcm(baseRf)).astype('float64')
colors = kwargs.get('colors', ['r', 'b'])
if not axis_len:
axis_len = [0, np.amax(baseRfDCMbaseRf)]
ax.set_xlim3d(axis_len[0], axis_len[1])
ax.set_ylim3d(axis_len[0], axis_len[1])
ax.set_zlim3d(axis_len[0], axis_len[1])
ax.plot(*origin, 'ko')
if rf == baseRf:
ax.quiver(*origin, rfDCMbaseRf[:, 0], rfDCMbaseRf[:, 1], rfDCMbaseRf[:, 2], color = colors[0], **kwargs)
ax.text(*rfDCMbaseRf[:, 0] / 2, f'${latex(baseRf.x)}$', color = colors[0])
ax.text(*rfDCMbaseRf[:, 1] / 2, f'${latex(baseRf.y)}$', color = colors[0])
ax.text(*rfDCMbaseRf[:, 2] / 2, f'${latex(baseRf.z)}$', color = colors[0])
else:
ax.quiver(*origin, baseRfDCMbaseRf[:, 0], baseRfDCMbaseRf[:, 1], baseRfDCMbaseRf[:, 2], color = colors[0], **kwargs)
ax.text(*baseRfDCMbaseRf[0] / 2, f'${latex(baseRf.x)}$', color = colors[0])
ax.text(*baseRfDCMbaseRf[1] / 2, f'${latex(baseRf.y)}$', color = colors[0])
ax.text(*baseRfDCMbaseRf[2] / 2, f'${latex(baseRf.z)}$', color = colors[0])
ax.quiver(*origin, rfDCMbaseRf[:, 0], rfDCMbaseRf[:, 1], rfDCMbaseRf[:, 2], color = colors[1], **kwargs)
ax.text(*rfDCMbaseRf[0] / 2, f'${latex(rf.x)}$', color = colors[1])
ax.text(*rfDCMbaseRf[1] / 2, f'${latex(rf.y)}$', color = colors[1])
ax.text(*rfDCMbaseRf[2] / 2, f'${latex(rf.z)}$', color = colors[1])
angle = np.arccos(np.diagonal(rfDCMbaseRf)/scale)[np.nonzero(np.arccos(np.diagonal(rfDCMbaseRf)/scale))][0]
step = 100
angle_text = np.around(np.rad2deg(angle))
if rfDCMbaseRf[2, 2] == scale:
# only xy plane
x_start = 0
y_start = np.pi/2
x_end = x_start + angle
y_end = y_start + angle
x_angle = np.linspace(x_start, x_end , step)
y_angle = np.linspace(y_start, y_end , step)
if rfDCMbaseRf[0, 1] < 0 :
ax.plot((scale/ 1.5) * np.cos(x_angle), -(scale/ 1.5) * np.sin(x_angle), [0]*step, 'k')
ax.plot(-(scale/ 1.5) * np.cos(y_angle), (scale/ 1.5) * np.sin(y_angle), [0]*step, 'k')
ax.text((scale/ 1.5) * np.cos(np.median(x_angle)), -(scale/ 1.5) * np.sin(np.median(x_angle)), 0, angle_text)
ax.text(-(scale/ 1.5) * np.cos(np.median(y_angle)), (scale/ 1.5) * np.sin(np.median(y_angle)), 0, angle_text)
else:
ax.plot((scale/ 1.5) * np.cos(x_angle), (scale/ 1.5) * np.sin(x_angle), [0]*step, 'k')
ax.plot((scale/ 1.5) * np.cos(y_angle), (scale/ 1.5) * np.sin(y_angle), [0]*step, 'k')
ax.text((scale/ 1.5) * np.cos(np.median(x_angle)), (scale/ 1.5) * np.sin(np.median(x_angle)), 0, angle_text)
ax.text((scale/ 1.5) * np.cos(np.median(y_angle)), (scale/ 1.5) * np.sin(np.median(y_angle)), 0, angle_text)
elif rfDCMbaseRf[1, 1] == scale:
# only zx plane
z_start = 0
x_start = np.pi/2
z_end = z_start + angle
x_end = x_start + angle
z_angle = np.linspace(z_start, z_end , step)
x_angle = np.linspace(x_start, x_end , step)
if rfDCMbaseRf[2, 0] < 0:
ax.plot((scale/ 1.5) * np.cos(z_angle), [0]*step, (scale/ 1.5) * np.sin(z_angle), 'k')
ax.plot((scale/ 1.5) * np.cos(x_angle), [0]*step, (scale/ 1.5) * np.sin(x_angle), 'k')
ax.text((scale/ 1.5) * np.cos(np.median(z_angle)), 0, (scale/ 1.5) * np.sin(np.median(z_angle)), angle_text)
ax.text((scale/ 1.5) * np.cos(np.median(x_angle)), 0, (scale/ 1.5) * np.sin(np.median(x_angle)), angle_text)
else:
ax.plot((scale/ 1.5) * np.cos(z_angle), [0]*step, -(scale/ 1.5) * np.sin(z_angle), 'k')
ax.plot(-(scale/ 1.5) * np.cos(x_angle), [0]*step, (scale/ 1.5) * np.sin(x_angle), 'k')
ax.text((scale/ 1.5) * np.cos(np.median(z_angle)), 0, -(scale/ 1.5) * np.sin(np.median(z_angle)), angle_text)
ax.text(-(scale/ 1.5) * np.cos(np.median(x_angle)), 0, (scale/ 1.5) * np.sin(np.median(x_angle)), angle_text)
elif rfDCMbaseRf[0, 0] == scale:
# only yz plane
y_start = 0
z_start = np.pi/2
y_end = y_start + angle
z_end = z_start + angle
y_angle = np.linspace(y_start, y_end , step)
z_angle = np.linspace(z_start, z_end , step)
if rfDCMbaseRf[1, 2] < 0:
ax.plot([0]*step, (scale/ 1.5) * np.cos(y_angle), -(scale/ 1.5) * np.sin(y_angle), 'k')
ax.plot([0]*step, -(scale/ 1.5) * np.cos(z_angle), (scale/ 1.5) * np.sin(z_angle), 'k')
ax.text(0, (scale/ 1.5) * np.cos(np.median(y_angle)), -(scale/ 1.5) * np.sin(np.median(y_angle)), angle_text)
ax.text(0, -(scale/ 1.5) * np.cos(np.median(z_angle)), (scale/ 1.5) * np.sin(np.median(z_angle)), angle_text)
else:
ax.plot([0]*step, (scale/ 1.5) * np.cos(y_angle), (scale/ 1.5) * np.sin(y_angle), 'k')
ax.plot([0]*step, (scale/ 1.5) * np.cos(z_angle), (scale/ 1.5) * np.sin(z_angle), 'k')
ax.text(0, (scale/ 1.5) * np.cos(np.median(y_angle)), (scale/ 1.5) * np.sin(np.median(y_angle)), angle_text)
ax.text(0, (scale/ 1.5) * np.cos(np.median(z_angle)), (scale/ 1.5) * np.sin(np.median(z_angle)), angle_text)
return plt.show()
In [4]:
N1 = ReferenceFrame('N1')
N2 = ReferenceFrame('N2')
theta1 = np.pi /10
N2.orient(N1,'Axis', [theta1, N1.z])
In [5]:
N3 = ReferenceFrame('N3')
theta2 = np.pi /5
N3.orient(N2,'Axis', [theta2, N2.x])
In [6]:
N1.dcm(N3)
Out[6]:
In [7]:
plotReferenceFrames(N2, N1, scale = 10) # showing N2 in N1 reference frame
In [8]:
plotReferenceFrames(N3, N2, scale = 10)
In [9]:
plotReferenceFrames(N3, N1, scale = 10) # showing N3 in N1 reference frame
In [10]:
plotReferenceFrames(N1, N1) #showing N1 in N1 reference frame
In [11]:
plotReferenceFrames(N2, N2) # showing N2 in N2 reference frame
In [ ]:
In [ ]:
In [ ]: