class TestEulerRot90_column():
angle = np.pi / 2
b1 = np.array([1, 0, 0])
b2 = np.array([0, 1, 0])
b3 = np.array([0, 0, 1])
R1 = attitude.rot1(angle, 'c')
R2 = attitude.rot2(angle, 'c')
R3 = attitude.rot3(angle, 'c')
R1_row = attitude.rot1(angle, 'r')
R2_row = attitude.rot2(angle, 'r')
R3_row = attitude.rot3(angle, 'r')
def test_rot1_transpose(self):
np.testing.assert_allclose(self.R1_row, self.R1.T)
def test_rot2_transpose(self):
np.testing.assert_allclose(self.R2_row, self.R2.T)
def test_rot3_transpose(self):
np.testing.assert_allclose(self.R3_row, self.R3.T)
def test_rot1_90_b1(self):
np.testing.assert_array_almost_equal(self.R1.dot(self.b1), self.b1)
def test_rot1_90_b2(self):
np.testing.assert_array_almost_equal(self.R1.dot(self.b2), self.b3)
def test_rot1_90_b3(self):
np.testing.assert_array_almost_equal(self.R1.dot(self.b3), -self.b2)
def test_rot2_90_b1(self):
np.testing.assert_array_almost_equal(self.R2.dot(self.b1), -self.b3)
def test_rot2_90_b2(self):
np.testing.assert_array_almost_equal(self.R2.dot(self.b2), self.b2)
def test_rot2_90_b3(self):
np.testing.assert_array_almost_equal(self.R2.dot(self.b3), self.b1)
def test_rot3_90_b1(self):
np.testing.assert_array_almost_equal(self.R3.dot(self.b1), self.b2)
def test_rot3_90_b2(self):
np.testing.assert_array_almost_equal(self.R3.dot(self.b2), -self.b1)
def test_rot3_90_b3(self):
np.testing.assert_array_almost_equal(self.R3.dot(self.b3), self.b3)
class TestInertialTransform():
inertial_pos = np.array([1, 1, 1])
inertial_vel = np.random.rand(3)
R_sc2int = att.rot2(np.pi / 2)
body_ang_vel = np.random.rand(3)
time = np.array([0])
ast = asteroid.Asteroid(name='castalia', num_faces=64)
dum = dumbbell.Dumbbell()
input_state = np.hstack(
(inertial_pos, inertial_vel, R_sc2int.reshape(9), body_ang_vel))
inertial_state = transform.eoms_inertial_to_inertial(
time, input_state, ast, dum)
def test_eoms_inertial_to_inertial_scalar_pos(self):
np.testing.assert_almost_equal(self.inertial_pos,
self.inertial_state[0:3])
def test_eoms_inertial_to_inertial_scalar_vel(self):
np.testing.assert_almost_equal(self.inertial_vel,
self.inertial_state[3:6])
def test_eoms_inertial_to_inertial_scalar_att(self):
np.testing.assert_almost_equal(self.R_sc2int.reshape(9),
self.inertial_state[6:15])
def test_eoms_inertial_to_inertial_scalar_ang_vel(self):
np.testing.assert_almost_equal(self.R_sc2int.dot(self.body_ang_vel),
self.inertial_state[15:18])
def test_expm_rot2_derivative(self):
axis = np.array([0, 1, 0])
R_dot = self.alpha * attitude.hat_map(axis).dot(
scipy.linalg.expm(self.alpha * self.t * attitude.hat_map(axis)))
R2_dot = attitude.rot2(self.alpha * self.t).dot(
attitude.hat_map(self.alpha * axis))
np.testing.assert_almost_equal(R_dot, R2_dot)
def test_dcm_loop(self):
latitude = np.linspace(-np.pi / 2, np.pi / 2, 10)
longitude = np.linspace(-np.pi, np.pi, 10)
for lat, lon in zip(latitude, longitude):
dcm_expected = rot2(np.pi / 2 - lat, 'r').dot(rot3(lon, 'r'))
dcm = transform.dcm_ecef2sez(lat, lon)
np.testing.assert_allclose(dcm, dcm_expected)
def test_dcm_loop(self):
latitude = np.linspace(-np.pi/2, np.pi/2, 10)
longitude = np.linspace(-np.pi, np.pi, 10)
for lat, lon in zip(latitude, longitude):
dcm_expected = rot2(np.pi/2 - lat, 'r').dot(rot3(lon, 'r'))
dcm = transform.dcm_ecef2sez(lat, lon)
np.testing.assert_allclose(dcm, dcm_expected)
class TestEulerRotInvalid():
R1 = attitude.rot1(0, 'x')
R2 = attitude.rot2(0, 'x')
R3 = attitude.rot3(0, 'x')
invalid = 1
def test_rot1_invalid(self):
np.testing.assert_allclose(self.R1, self.invalid)
def test_rot2_invalid(self):
np.testing.assert_allclose(self.R2, self.invalid)
def test_rot3_invalid(self):
np.testing.assert_allclose(self.R3, self.invalid)
def rhoazel(sat_eci, site_eci, site_lat, site_lst):
"""
This function calculates the topcentric range,azimuth and elevation from
the site vector and satellite position vector.
Author: C2C Shankar Kulumani USAFA/CS-19 719-333-4741
Inputs:
sat_eci - satellite ECI position vector (km)
site_eci - site ECI position vector (km)
site_lat - site geodetic latitude (radians)
site_lst - site local sidereal time (radians)
Outputs:
rho - range (km)
az - asimuth (radians)
el - elevation (radians)
Globals: None
Constants: None
Coupling:
References:
Astro 321 Predict LSN 22
"""
site2sat_eci = sat_eci - site_eci
site2sat_sez = attitude.rot3(-site_lst).dot(site2sat_eci)
site2sat_sez = attitude.rot2(-(np.pi / 2 - site_lat)).dot(site2sat_sez)
rho = np.linalg.norm(site2sat_sez)
el = np.arcsin(site2sat_sez[2] / rho)
az = attitude.normalize(np.arctan2(site2sat_sez[1], -site2sat_sez[0]), 0,
2 * np.pi)[0]
return rho, az, el
def rhoazel(sat_eci, site_eci, site_lat, site_lst):
"""
This function calculates the topcentric range,azimuth and elevation from
the site vector and satellite position vector.
Author: C2C Shankar Kulumani USAFA/CS-19 719-333-4741
Inputs:
sat_eci - satellite ECI position vector (km)
site_eci - site ECI position vector (km)
site_lat - site geodetic latitude (radians)
site_lst - site local sidereal time (radians)
Outputs:
rho - range (km)
az - asimuth (radians)
el - elevation (radians)
Globals: None
Constants: None
Coupling:
References:
Astro 321 Predict LSN 22
"""
site2sat_eci = sat_eci - site_eci
site2sat_sez = attitude.rot3(-site_lst).dot(site2sat_eci)
site2sat_sez = attitude.rot2(-(np.pi / 2 - site_lat)).dot(site2sat_sez)
rho = np.linalg.norm(site2sat_sez)
el = np.arcsin(site2sat_sez[2] / rho)
az = attitude.normalize(np.arctan2(site2sat_sez[1], -site2sat_sez[0]), 0,
2 * np.pi)[0]
return rho, az, el
class TestHamiltonRelativeTransform():
time = np.array([0])
ast = asteroid.Asteroid(name='castalia', num_faces=64)
dum = dumbbell.Dumbbell()
inertial_pos = np.array([1, 1, 1])
inertial_vel = np.random.rand(3) + att.hat_map(
ast.omega * np.array([0, 0, 1])).dot(inertial_pos)
R_sc2int = att.rot2(np.pi / 2)
R_ast2int = att.rot3(time * ast.omega)
body_ang_vel = np.random.rand(3)
initial_lin_mom = (dum.m1 + dum.m2) * (inertial_vel)
initial_ang_mom = R_sc2int.dot(dum.J).dot(body_ang_vel)
initial_ham_state = np.hstack(
(inertial_pos, initial_lin_mom, R_ast2int.dot(R_sc2int).reshape(9),
initial_ang_mom))
inertial_state = transform.eoms_hamilton_relative_to_inertial(
time, initial_ham_state, ast, dum)
def test_eoms_hamilton_relative_to_inertial_scalar_pos(self):
np.testing.assert_almost_equal(self.inertial_pos,
self.inertial_state[0:3])
def test_eoms_hamilton_relative_to_inertial_scalar_vel(self):
np.testing.assert_almost_equal(self.inertial_vel,
self.inertial_state[3:6])
def test_eoms_hamilton_relative_to_inertial_scalar_att(self):
np.testing.assert_almost_equal(self.R_sc2int.reshape(9),
self.inertial_state[6:15])
def test_eoms_hamilton_relative_to_inertial_scalar_ang_vel(self):
np.testing.assert_almost_equal(self.R_sc2int.dot(self.body_ang_vel),
self.inertial_state[15:18])
choices=[0, 1],
help=
"Choose which relative energy mode to run. 0 - relative energy, 1 - \Delta E behavior"
)
args = parser.parse_args()
print("Starting the simulation...")
# instantiate the asteroid and dumbbell objects
ast = asteroid.Asteroid(args.ast_name, args.num_faces)
dum = dumbbell.Dumbbell(m1=500, m2=500, l=0.003)
# initialize simulation parameters
time = np.linspace(0, args.tf, args.num_steps)
initial_pos = periodic_pos
initial_R = attitude.rot2(0).reshape(9)
initial_w = np.array([0.01, 0.01, 0.01])
initial_lin_mom = (dum.m1 + dum.m2) * (periodic_vel + attitude.hat_map(
ast.omega * np.array([0, 0, 1])).dot(initial_pos))
initial_ang_mom = initial_R.reshape((3, 3)).dot(dum.J).dot(initial_w)
initial_ham_state = np.hstack(
(initial_pos, initial_lin_mom, initial_R, initial_ang_mom))
print("This will run a long simulation with the following parameters!")
print(" ast_name - %s" % args.ast_name)
print(" num_faces - %s" % args.num_faces)
print(" tf - %s" % args.tf)
print(" num_steps - %s" % args.num_steps)
print(" file_name - %s" % args.file_name)
print("")
"Choose which inertial energy mode to run. 0 - inertial energy, 1 - \Delta E behavior"
)
args = parser.parse_args()
print("Starting the simulation...")
# instantiate the asteroid and dumbbell objects
ast = asteroid.Asteroid(args.ast_name, args.num_faces)
dum = dumbbell.Dumbbell(m1=500, m2=500, l=0.003)
# initialize simulation parameters
time = np.linspace(0, args.tf, args.num_steps)
initial_pos = periodic_pos # km for center of mass in body frame
initial_vel = periodic_vel + attitude.hat_map(
ast.omega * np.array([0, 0, 1])).dot(initial_pos)
initial_R = attitude.rot2(0).reshape(
9) # transforms from dumbbell body frame to the inertial frame
initial_w = np.array([0.01, 0.01, 0.01])
initial_state = np.hstack((initial_pos, initial_vel, initial_R, initial_w))
# echo back all the input parameters
print("This will run a long simulation with the following parameters!")
print(" ast_name - %s" % args.ast_name)
print(" num_faces - %s" % args.num_faces)
print(" tf - %s" % args.tf)
print(" num_steps - %s" % args.num_steps)
print(" file_name - %s" % args.file_name)
print("")
if args.mode == 0:
(_, inertial_state, asteroid_state,
body_state) = inertial_eoms_driver(initial_state, time, ast, dum)
def dcm_ecef2sez(latgd, lon, alt=0):
# dcm = attitude.rot2(np.pi/2 - latgd, 'r').dot(attitude.rot3(lon, 'r'))
dcm = attitude.rot2(latgd - np.pi/2).dot(attitude.rot3(-lon))
return dcm
def plot_keyframe_original_lissajous(
time,
state,
R_bcam2a,
save_fig=False,
fwidth=1,
filename='/tmp/estimate.eps',
kf_path='./data/asteroid_circumnavigate/lissajous.txt'):
"""Plot keyframe trajectory and scale to match truth
The asteroid is assumed to be not rotating. Therefore the input state is
actually already in the asteroid frame
"""
# convert inertial position into asteriod fixed frame
asteroid_pos = state[:, 0:3]
# first determine the scale of the keyframe translations
kf_data = np.loadtxt(kf_path)
kf_time = kf_data[:, 0].astype(
dtype='int') # time of keyframe, matches image/time vector
kf_traj = kf_data[:, 1:4] # postiion of each frame relative to the first
kf_quat = kf_data[:, 4:8] # rotation from first keyframe to current
Rcam2ast = attitude.rot2(np.deg2rad(0)).dot(
attitude.rot3(np.deg2rad(-90)).dot(
np.array([[1, 0, 0], [0, 0, 1], [0, 1, 0]])))
kf_traj = Rcam2ast.dot(kf_traj.T).T
# need R at time of first frame and then the asteroid to inertial - dumbbell to ast needed
R0_s2i = state[kf_time[0], 6:15].reshape((3, 3))
R0_s2a = (R0_s2i)
kf_traj = R0_s2a.dot(kf_traj.T).T
kf_R_first2cur = np.zeros((len(kf_time), 9))
# transform each quaternion to a rotation matrix
for ii, q in enumerate(kf_quat):
kf_R_first2cur[ii, :] = R0_s2a.dot(Rcam2ast.dot(
attitude.quattodcm(q))).reshape(-1)
# determine scale of translation between keyframe points
kf_diff = np.diff(kf_traj, axis=0)
kf_scale = np.sqrt(np.sum(kf_diff**2, axis=1))
# find true positions at the same time as keyframes
kf_traj_true = asteroid_pos[kf_time[0]:kf_time[-1], :]
kf_scale_true = np.sqrt(np.sum(kf_traj_true**2, axis=1))
scale = kf_scale_true[0]
Rb2a = R_bcam2a[kf_time[0], :].reshape((3, 3))
# scale and translate
kf_traj = scale * kf_traj + asteroid_pos[kf_time[0], :]
# translate kf_traj
difference = kf_traj[0, :] - kf_traj_true[0, :]
kf_traj = kf_traj - difference
# plot keyframe motion without any modifications
kf_orig_fig = plt.figure()
kf_orig_ax = axes3d.Axes3D(kf_orig_fig)
kf_orig_ax.set_zlim3d(-5, 5)
kf_orig_ax.set_xlim3d(-5, 5)
kf_orig_ax.set_ylim3d(-5, 5)
kf_orig_ax.plot(kf_traj[:, 0], kf_traj[:, 1], kf_traj[:, 2], 'b-*')
# plot the viewing direction
length = 0.3
for ii, R in enumerate(kf_R_first2cur):
view_axis = R.reshape((3, 3))[:, 2]
kf_orig_ax.plot(
[kf_traj[ii, 0], kf_traj[ii, 0] + length * view_axis[0]],
[kf_traj[ii, 1], kf_traj[ii, 1] + length * view_axis[1]],
[kf_traj[ii, 2], kf_traj[ii, 2] + length * view_axis[2]], 'r')
kf_orig_ax.plot(kf_traj_true[:, 0], kf_traj_true[:, 1], kf_traj_true[:, 2],
'r')
kf_orig_ax.set_title('Keyframe Original')
kf_orig_ax.set_xlabel('X')
kf_orig_ax.set_ylabel('Y')
kf_orig_ax.set_zlabel('Z')
# plot the components
kf_comp_fig, kf_comp_ax = plt.subplots(3,
1,
figsize=plotting.figsize(1),
sharex=True)
kf_comp_ax[0].plot(kf_time, kf_traj[:, 0], 'b-*', label='Estimate')
kf_comp_ax[0].plot(time[kf_time[0]:kf_time[-1]],
kf_traj_true[:, 0],
'r-',
label='True')
kf_comp_ax[0].set_ylim(-5, 5)
kf_comp_ax[0].set_ylabel(r'$X$ (km)')
kf_comp_ax[0].grid()
kf_comp_ax[1].plot(kf_time, kf_traj[:, 1], 'b-*', label='Estimate')
kf_comp_ax[1].plot(time[kf_time[0]:kf_time[-1]],
kf_traj_true[:, 1],
'r-',
label='True')
kf_comp_ax[1].set_ylim(-5, 5)
kf_comp_ax[1].set_ylabel(r'$Y$ (km)')
kf_comp_ax[1].grid()
kf_comp_ax[2].plot(kf_time, kf_traj[:, 2], 'b-*', label='Estimate')
kf_comp_ax[2].plot(time[kf_time[0]:kf_time[-1]],
kf_traj_true[:, 2],
'r-',
label='True')
kf_comp_ax[2].set_ylim(-5, 5)
kf_comp_ax[2].set_ylabel(r'$Z$ (km)')
kf_comp_ax[2].grid()
kf_comp_ax[2].set_xlabel('Time (sec)')
plt.legend(loc='best')
# compute the difference in the components
traj_diff = np.absolute(asteroid_pos[kf_time, :] - kf_traj)
kf_diff_fig, kf_diff_ax = plt.subplots(3,
1,
figsize=plotting.figsize(1),
sharex=True)
kf_diff_ax[0].plot(kf_time, traj_diff[:, 0], 'b-*')
kf_diff_ax[0].set_ylabel(r'$X$ (km)')
kf_diff_ax[0].grid()
kf_diff_ax[1].plot(kf_time, traj_diff[:, 1], 'b-*')
kf_diff_ax[1].set_ylabel(r'$Y$ (km)')
kf_diff_ax[1].grid()
kf_diff_ax[2].plot(kf_time, traj_diff[:, 2], 'b-*')
kf_diff_ax[2].set_ylabel(r'$Z$ (km)')
kf_diff_ax[2].grid()
kf_diff_ax[2].set_xlabel(r'Time (sec)')
kf_diff_ax[0].set_title('Difference between ORBSLAM estimate and truth')
if save_fig:
plt.figure(kf_comp_fig.number)
plt.savefig(filename)
plt.show()
def test_rot2_determinant_1_row(self):
mat = attitude.rot2(self.angle, 'r')
np.testing.assert_allclose(np.linalg.det(mat), 1)
def test_rot2_orthogonal_row(self):
mat = attitude.rot2(self.angle, 'r')
np.testing.assert_array_almost_equal(mat.T.dot(mat), np.eye(3, 3))
lon = np.linspace(-180, 180, den) * np.pi/180
lat = np.linspace(-90, 90, den) * np.pi/180
X, Y = np.meshgrid(lon, lat)
sen_array = np.zeros((3, den**2))
er_attract_array = np.zeros_like(sen_array)
psi_avoid_array = np.zeros_like(X)
psi_attract_array = np.zeros_like(X)
psi_total_array = np.zeros_like(X)
# now loop over all the angles and compute the error function
for ii in range(lon.shape[0]):
for jj in range(lat.shape[0]):
# rotate the body vector to the inertial frame
R_b2i = attitude.rot2(lat[jj]).T.dot(attitude.rot3(lon[ii]))
sen_inertial = R_b2i.dot(sen)
sen_array[:, ii*den + 0] = sen_inertial
psi_attract = 1/2 * np.trace(G.dot(np.eye(3, 3) - R_des.T.dot(R_b2i)))
if np.dot(sen_inertial, con) < con_angle:
psi_avoid = -1 / alpha * np.log(- (np.dot(sen_inertial, con) - con_angle) / (1 + con_angle)) + 1
else:
psi_avoid = 11
psi_total = psi_attract * psi_avoid
er = 1/2 * attitude.vee_map(G.dot(R_des.T).dot(R_b2i) - R_b2i.T.dot(R_des).dot(G))
def rv2rhoazel(r_sat_eci, v_sat_eci, lat, lon, alt, jd):
"""
This function calculates the topcentric range,azimuth and elevation from
the site vector and satellite position vector.
Author: C2C Shankar Kulumani USAFA/CS-19 719-333-4741
Inputs:
sat_eci - satellite ECI position vector (km)
site_eci - site ECI position vector (km)
site_lat - site geodetic latitude (radians)
site_lst - site local sidereal time (radians)
Outputs:
rho - range (km)
az - asimuth (radians)
el - elevation (radians)
Globals: None
Constants: None
Coupling:
References:
Astro 321 Predict LSN 22
Vallado Algorithm 27
"""
small = constants.small
halfpi = constants.halfpi
# get site in ECEF
r_site_ecef = lla2ecef(lat, lon, alt)
# convert sat and site to ecef
dcm_eci2ecef = transform.dcm_eci2ecef(jd)
omega_earth = np.array([0, 0, constants.earth.omega])
r_sat_ecef = dcm_eci2ecef.dot(r_sat_eci)
v_sat_ecef = dcm_eci2ecef.dot(
v_sat_eci) - np.cross(omega_earth, r_sat_ecef)
# find relative vector in ecef frame
rho_ecef = r_sat_ecef - r_site_ecef
drho_ecef = v_sat_ecef - np.zeros_like(v_sat_ecef) # site isn't moving
rho = np.linalg.norm(rho_ecef)
# convert to SEZ coordinate frame
dcm_ecef2sez = attitude.rot2(
np.pi / 2 - lat, 'r').dot(attitude.rot3(lon, 'r'))
rho_sez = dcm_ecef2sez.dot(rho_ecef)
drho_sez = dcm_ecef2sez.dot(drho_ecef)
# find azimuth and eleveation
temp = np.sqrt(rho_sez[0]**2 + rho_sez[1]**2)
if temp < small: # directly over the north pole
el = np.sign(rho_sez[2]) * halfpi # \pm 90 deg
else:
mag_rho_sez = np.linalg.norm(rho_sez)
el = np.arcsin(rho_sez[2] / mag_rho_sez)
if temp < small:
az = np.arctan2(drho_sez[1], - drho_sez[0])
else:
az = np.arctan2(rho_sez[1] / temp, -rho_sez[0] / temp)
# range, azimuth and elevation rates
drho = np.dot(rho_sez, drho_sez) / rho
if np.absolute(temp**2) > small:
daz = (drho_sez[0] * rho_sez[1] - drho_sez[1] * rho_sez[0]) / temp**2
else:
daz = 0
if np.absolute(temp) > small:
dele = (drho_sez[2] - drho * np.sin(el)) / temp
else:
dele = 0
az = attitude.normalize(az, 0, constants.twopi)
return rho, az, el, drho, daz, dele
def test_expm_rot2_equivalent(self):
R = scipy.linalg.expm(self.angle *
attitude.hat_map(np.array([0, 1, 0])))
R2 = attitude.rot2(self.angle)
np.testing.assert_almost_equal(R.T.dot(R2), np.eye(3, 3))
def rv2rhoazel(r_sat_eci, v_sat_eci, lat, lon, alt, jd):
"""
This function calculates the topcentric range,azimuth and elevation from
the site vector and satellite position vector.
Author: C2C Shankar Kulumani USAFA/CS-19 719-333-4741
Inputs:
sat_eci - satellite ECI position vector (km)
site_eci - site ECI position vector (km)
site_lat - site geodetic latitude (radians)
site_lst - site local sidereal time (radians)
Outputs:
rho - range (km)
az - asimuth (radians)
el - elevation (radians)
Globals: None
Constants: None
Coupling:
References:
Astro 321 Predict LSN 22
Vallado Algorithm 27
"""
small = constants.small
halfpi = constants.halfpi
# get site in ECEF
r_site_ecef = lla2ecef(lat, lon, alt)
# convert sat and site to ecef
dcm_eci2ecef = transform.dcm_eci2ecef(jd)
omega_earth = np.array([0, 0, constants.earth.omega])
r_sat_ecef = dcm_eci2ecef.dot(r_sat_eci)
v_sat_ecef = dcm_eci2ecef.dot(v_sat_eci) - np.cross(
omega_earth, r_sat_ecef)
# find relative vector in ecef frame
rho_ecef = r_sat_ecef - r_site_ecef
drho_ecef = v_sat_ecef - np.zeros_like(v_sat_ecef) # site isn't moving
rho = np.linalg.norm(rho_ecef)
# convert to SEZ coordinate frame
dcm_ecef2sez = attitude.rot2(np.pi / 2 - lat,
'r').dot(attitude.rot3(lon, 'r'))
rho_sez = dcm_ecef2sez.dot(rho_ecef)
drho_sez = dcm_ecef2sez.dot(drho_ecef)
# find azimuth and eleveation
temp = np.sqrt(rho_sez[0]**2 + rho_sez[1]**2)
if temp < small: # directly over the north pole
el = np.sign(rho_sez[2]) * halfpi # \pm 90 deg
else:
mag_rho_sez = np.linalg.norm(rho_sez)
el = np.arcsin(rho_sez[2] / mag_rho_sez)
if temp < small:
az = np.arctan2(drho_sez[1], -drho_sez[0])
else:
az = np.arctan2(rho_sez[1] / temp, -rho_sez[0] / temp)
# range, azimuth and elevation rates
drho = np.dot(rho_sez, drho_sez) / rho
if np.absolute(temp**2) > small:
daz = (drho_sez[0] * rho_sez[1] - drho_sez[1] * rho_sez[0]) / temp**2
else:
daz = 0
if np.absolute(temp) > small:
dele = (drho_sez[2] - drho * np.sin(el)) / temp
else:
dele = 0
az = attitude.normalize(az, 0, constants.twopi)
return rho, az, el, drho, daz, dele
class TestInertialandRelativeEOMS():
"""Compare the inertial and relative eoms against one another
"""
RelTol = 1e-9
AbsTol = 1e-9
ast_name = 'castalia'
num_faces = 64
tf = 1e2
num_steps = 1e2
time = np.linspace(0, tf, num_steps)
periodic_pos = np.array(
[1.495746722510590, 0.000001002669660, 0.006129720493607])
periodic_vel = np.array(
[0.000000302161724, -0.000899607989820, -0.000000013286327])
ast = asteroid.Asteroid(ast_name, num_faces)
dum = dumbbell.Dumbbell(m1=500, m2=500, l=0.003)
# set initial state for inertial EOMs
initial_pos = periodic_pos # km for center of mass in body frame
initial_vel = periodic_vel + att.hat_map(
ast.omega * np.array([0, 0, 1])).dot(initial_pos)
initial_R = att.rot2(np.pi / 2).reshape(
9) # transforms from dumbbell body frame to the inertial frame
initial_w = np.array([0.01, 0.01, 0.01])
initial_state = np.hstack((initial_pos, initial_vel, initial_R, initial_w))
i_state = integrate.odeint(dum.eoms_inertial,
initial_state,
time,
args=(ast, ),
atol=AbsTol,
rtol=RelTol)
# (i_time, i_state) = idriver.eom_inertial_driver(initial_state, time, ast, dum, AbsTol=1e-9, RelTol=1e-9)
initial_lin_mom = (dum.m1 + dum.m2) * (periodic_vel + att.hat_map(
ast.omega * np.array([0, 0, 1])).dot(initial_pos))
initial_ang_mom = initial_R.reshape((3, 3)).dot(dum.J).dot(initial_w)
initial_ham_state = np.hstack(
(initial_pos, initial_lin_mom, initial_R, initial_ang_mom))
rh_state = integrate.odeint(dum.eoms_hamilton_relative,
initial_ham_state,
time,
args=(ast, ),
atol=AbsTol,
rtol=RelTol)
# now convert both into the inertial frame
istate_ham = transform.eoms_hamilton_relative_to_inertial(
time, rh_state, ast, dum)
istate_int = transform.eoms_inertial_to_inertial(time, i_state, ast, dum)
# also convert both into the asteroid frame and compare
astate_ham = transform.eoms_hamilton_relative_to_asteroid(
time, rh_state, ast, dum)
astate_int = transform.eoms_inertial_to_asteroid(time, i_state, ast, dum)
def test_inertial_frame_comparison_pos(self):
np.testing.assert_array_almost_equal(self.istate_ham[:, 0:3],
self.istate_int[:, 0:3])
def test_inertial_frame_comparison_vel(self):
np.testing.assert_array_almost_equal(self.istate_ham[:, 3:6],
self.istate_int[:, 3:6])
def test_inertial_frame_comparison_att(self):
np.testing.assert_array_almost_equal(self.istate_ham[:, 6:15],
self.istate_int[:, 6:15])
def test_inertial_frame_comparison_ang_vel(self):
np.testing.assert_array_almost_equal(self.istate_ham[:, 15:18],
self.istate_int[:, 15:18])
def test_asteroid_frame_comparison_pos(self):
np.testing.assert_array_almost_equal(self.astate_ham[:, 0:3],
self.astate_int[:, 0:3])
def test_asteroid_frame_comparison_vel(self):
np.testing.assert_array_almost_equal(self.astate_ham[:, 3:6],
self.astate_int[:, 3:6])
def test_asteroid_frame_comparison_att(self):
np.testing.assert_array_almost_equal(self.astate_ham[:, 6:15],
self.astate_int[:, 6:15])
def test_asteroid_frame_comparison_ang_vel(self):
np.testing.assert_array_almost_equal(self.astate_ham[:, 15:18],
self.astate_int[:, 15:18])
