def dcm_pqw2eci_coe(raan, inc, arg_p): """Define rotation matrix transforming PQW to ECI given orbital elements """ dcm = attitude.rot3(raan).dot( attitude.rot1(inc)).dot(attitude.rot3(arg_p)) dcm_pqw2eci = attitude.rot3(-raan, 'r').dot(attitude.rot1(-inc, 'r') ).dot(attitude.rot3(-arg_p, 'r')) return dcm
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)
def pqw2eci(raan, argp, inc): r"""Return rotation matrix to go from PQW to ECI Returns the rotation matrix to transform a vector represented in the perifocal frame to the inertial frame. Parameters ---------- raan : float Right ascension of the ascending node in radians argp : float Argument of periapsis in radians inc : float inclination of orbit in radians Returns ------- PQW2ECI : ndarray 3x3 numpy array defining the rotation matrix Author ------ Shankar Kulumani GWU [email protected] """ PQW2ECI = attitude.rot3(raan).dot(attitude.rot1(inc)).dot( attitude.rot3(argp)) print(PQW2ECI) return PQW2ECI
class TestDumbbellRelativeODE(): dum = dumbbell.Dumbbell() angle = (2 * np.pi - 0) * np.random.rand(1) + 0 # generate a random initial state that is outside of the asteroid pos = np.random.rand(3) + np.array([2, 2, 2]) vel = np.random.rand(3) R = attitude.rot1(angle).reshape(9) ang_vel = np.random.rand(3) t = np.random.rand() * 100 state = np.hstack((pos, vel, R, ang_vel)) statedot = dum.eoms_relative_ode(t, state, ast) def test_relative_moment_of_inertia(self): R = self.R.reshape((3, 3)) Jr = R.dot(self.dum.J).dot(R.T) Jdr = R.dot(self.dum.Jd).dot(R.T) np.testing.assert_array_almost_equal( attitude.hat_map(Jr.dot(self.ang_vel)), attitude.hat_map(self.ang_vel).dot(Jdr) + Jdr.dot(attitude.hat_map(self.ang_vel))) def test_eoms_relative_size(self): np.testing.assert_allclose(self.statedot.shape, (18, )) def test_eoms_relative_R_dot(self): np.testing.assert_allclose(self.statedot[6:15].shape, (9, ))
def test_expm_rot1_derivative(self): axis = np.array([1, 0, 0]) R_dot = self.alpha * attitude.hat_map(axis).dot( scipy.linalg.expm(self.alpha * self.t * attitude.hat_map(axis))) R1_dot = attitude.rot1(self.alpha * self.t).dot( attitude.hat_map(self.alpha * axis)) np.testing.assert_almost_equal(R_dot, R1_dot)
class TestController: angle = (2 * np.pi - 0) * np.random.rand(1) + 0 # generate a random initial state that is outside the asteroid pos = np.random.rand(3) + np.array([2, 2, 2]) R = attitude.rot1(angle).reshape(9) vel = np.random.rand(3) ang_vel = np.random.rand(3) t = np.random.rand() * 100 state = np.hstack((pos, vel, R, ang_vel)) def test_body_fixed_pointing_attitude_matches_python(self): from dynamics import controller as controller_python (Rd, Rd_dot, ang_vel_d, ang_vel_d_dot) = controller_python.body_fixed_pointing_attitude( 1, self.state) # now for the C++ version att_cont = controller.AttitudeController() att_cont.body_fixed_pointing_attitude(1, self.state) np.testing.assert_array_almost_equal(att_cont.get_Rd(), Rd) np.testing.assert_allclose(att_cont.get_Rd_dot(), Rd_dot) np.testing.assert_allclose(att_cont.get_ang_vel_d(), ang_vel_d) np.testing.assert_allclose(att_cont.get_ang_vel_d_dot(), ang_vel_d_dot) def test_translation_controller_minimum_uncertainty(self): v, f = wavefront.read_obj('./data/shape_model/CASTALIA/castalia.obj') # create a mesh mesh = mesh_data.MeshData(v, f) rmesh = reconstruct.ReconstructMesh(mesh) tran_cont = controller.TranslationController() tran_cont.minimize_uncertainty(self.state, rmesh) np.testing.assert_allclose(tran_cont.get_posd().shape, (3, )) np.testing.assert_allclose(tran_cont.get_veld(), np.zeros(3)) np.testing.assert_allclose(tran_cont.get_acceld(), np.zeros(3)) def test_complete_controller(self): v, f = wavefront.read_obj('./data/shape_model/CASTALIA/castalia.obj') mesh = mesh_data.MeshData(v, f) rmesh = reconstruct.ReconstructMesh(mesh) cont = controller.Controller() cont.explore_asteroid(self.state, rmesh) print(cont.get_posd())
class TestExpMap(): angle = (np.pi - 0) * np.random.rand(1) + 0 axis = np.array([1, 0, 0]) R = attitude.rot1(angle) def test_axisangletodcm(self): np.testing.assert_array_almost_equal( attitude.rot1(self.angle), attitude.axisangletodcm(self.angle, self.axis)) def test_dcmtoaxisangle(self): angle, axis = attitude.dcmtoaxisangle(self.R) np.testing.assert_array_almost_equal(angle, self.angle) np.testing.assert_array_almost_equal(axis, self.axis)
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)
class TestQuaternion(): angle = (2 * np.pi - 0) * np.random.rand(1) + 0 dcm = attitude.rot1(angle) quat = attitude.dcmtoquat(dcm) dcm_identity = np.eye(3, 3) quat_identity = attitude.dcmtoquat(dcm_identity) def test_dcmtoquaternion_unit_norm(self): np.testing.assert_almost_equal(np.linalg.norm(self.quat), 1) def test_quaternion_back_to_rotation_matrix(self): np.testing.assert_array_almost_equal(attitude.quattodcm(self.quat), self.dcm) def test_identity_quaternion_scalar_one(self): np.testing.assert_equal(self.quat_identity[-1], 1) def test_identity_quaternion_vector_zero(self): np.testing.assert_array_almost_equal(self.quat_identity[0:3], np.zeros(3))
def test_rot1_determinant_1_row(self): mat = attitude.rot1(self.angle, 'r') np.testing.assert_allclose(np.linalg.det(mat), 1)
def test_rot1_orthogonal_row(self): mat = attitude.rot1(self.angle, 'r') np.testing.assert_array_almost_equal(mat.T.dot(mat), np.eye(3, 3))
def test_axisangletodcm(self): np.testing.assert_array_almost_equal( attitude.rot1(self.angle), attitude.axisangletodcm(self.angle, self.axis))
def test_expm_rot1_equivalent(self): R = scipy.linalg.expm(self.angle * attitude.hat_map(np.array([1, 0, 0]))) R1 = attitude.rot1(self.angle) np.testing.assert_almost_equal(R.T.dot(R1), np.eye(3, 3))
def test_up_axis(): sensor_y = raycaster.Lidar(up_axis=np.array([0, 1, 0])) sensor_z = raycaster.Lidar(up_axis=np.array([0, 0, 1])) R = attitude.rot1(np.pi / 2) np.testing.assert_array_almost_equal(sensor_z.lidar_arr, R.dot(sensor_y.lidar_arr.T).T)
from __future__ import absolute_import, division, print_function, unicode_literals from dynamics import dumbbell, asteroid, controller, eoms from kinematics import attitude import numpy as np import pdb angle = (2 * np.pi - 0) * np.random.rand(1) + 0 # generate a random initial state that is outside of the asteroid pos = np.random.rand(3) + np.array([2, 2, 2]) R = attitude.rot1(angle).reshape(9) vel = np.random.rand(3) ang_vel = np.random.rand(3) t = np.random.rand() * 100 state = np.hstack((pos, vel, R, ang_vel)) ast = asteroid.Asteroid('castalia', 32) dum = dumbbell.Dumbbell() def test_eoms_controlled_relative_blender_ode_size(): state_dot = eoms.eoms_controlled_relative_blender_ode(t, state, dum, ast) np.testing.assert_equal(state_dot.shape, (18, ))
def planet_coe(JD_curr, planet_flag): r"""Orbital elements for any of the major solar system bodies Given the Julian date this function will output the approximate orbital elements for any of the solar system bodies. It uses an analytic approximation for their positions and propogates the orbital elements to the desired date. Parameters ---------- jd : float Julian date for the epoch planet_flag : int Number 0 to 8 for each of the planets (Pluto forever) Returns ------- coe : tuple p : float Semiparameter of orbit in AU ecc: float Eccentricity of orbit inc : float Inclination in radiands wrt to ecliptic raan : float Longitude/Right ascension of teh ascending node in radians argp : float Argument of perigee in radians nu : float True anomaly in radians r_ecliptic : (3,) ndarray Position wrt J2000 mean ecliptic (Earth's orbit around the sun) in kilometers defined relative to the solar system barycenter v_ecliptic : (3,) ndarray Velocity wrt J2000 mean ecliptic in kilometers per second defined relative to the solar system barycenter r_icrf : (3,) ndarray Position wrt J2000 equatorial/ICRF in kilometers and defined relative to teh solar system barycenter v_icrf : (3,) Velocity wrt J2000 equatorial/ICRF in kilometers per second defined relative to the solar system barycenter See Also -------- planet_approx : Actually has the data for the approximations Notes ----- The orbital elements are defined with respect to the mean ecliptic and equinox of J2000. The fundamental plane is the ecliptic, or the orbit of the Earth about the sun. The fundamental direction is aligned with the first point of Ares or the vernal equinox. The vectors are defined in both the ecliptic and equatorial system with respect to the solar system barycenter Author ------ Shankar Kulumani GWU [email protected] 23 June 2017 References ---------- .. [1] https://ssd.jpl.nasa.gov/?planet_pos .. [2] MEEUS, Jean H.. Astronomical Algorithms. Willmann-Bell, Incorporated, 1991. Examples -------- An example of how to use the function >>> planet_flag = 4 >>> coe = planet_coe(jd, planet_flag) """ # Find the JD centuries past J2000 epoch JD_J2000 = 2451545.0 T = (JD_curr - JD_J2000) / 36525 # load the elements and element rates for the planets (a0, adot, e0, edot, inc0, incdot, meanL0, meanLdot, lonperi0, lonperidot, raan0, raandot, b, c, f, s) = planet_approx(JD_curr, planet_flag) # compute the current elements at this JD a = a0 + adot * T ecc = e0 + edot * T inc = inc0 + incdot * T L = meanL0 + meanLdot * T lonperi = lonperi0 + lonperidot * T raan = raan0 + raandot * T # compute argp and M/v to complete the element set argp = lonperi - raan M = L - lonperi + b * T**2 + c * np.cos(f * T) + s * np.sin(f * T) # M = attitude.normalize(M, -180, 180) # solve kepler's equation to compute E and v E, nu, count = kepler.kepler_eq_E(np.deg2rad(M), ecc) argp = attitude.normalize(np.deg2rad(argp), 0, 2 * np.pi)[0] # package into a vector and output p = a * (1 - ecc**2) au2km = constants.au2km coe = COE(p=p*au2km, ecc=ecc, inc=np.deg2rad(inc), raan=np.deg2rad(raan), argp=argp, nu=nu) # convert to position and velocity vectors in J2000 ecliptic and J2000 # Earth equatorial (ECI) reference frame # need to convert distances to working units of kilometers r_ecliptic, v_ecliptic, r_pqw, v_pqw = kepler.coe2rv(coe.p, coe.ecc, coe.inc, coe.raan, coe.argp, coe.nu.item(), constants.sun.mu) # convert to the Earth J2000 frame (ECI) need to rotate by the obliquity of # the ecliptic Eclip2ECI = attitude.rot1(constants.obliquity) r_eci = Eclip2ECI.dot(r_ecliptic) v_eci = Eclip2ECI.dot(v_ecliptic) return coe, r_ecliptic, v_ecliptic, r_eci, v_eci