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
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])