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
