def body2ast(time, state, ast, dum):
"""Convert state from the body frame to the asteroid fixed frame
This function will convert the state output of eoms_inertial to the asteroid fixed frame.
This wil allow for comparision with the state from eoms_relative.
Inputs:
time - nx1 array of time stamps for the state vector
state - nx18 array of the state vector defined as follows:
inertial_pos - state[:, 0:3] is the position of the body with respect to the asteroid
center and defined in the inertial frame
inertial_vel - state[:, 4:6] is the velocity of the sc with respect to the asteroid
center of mass and defined in the inertial frame
R_sc2int - state[:, 6:15] the rotation matrix which transforms vectors from the sc frame
to the inertial frame
body_w - state[:, 15:18] the angular velocity of the sc with respect to the inertial
frame and defined in the spacecraft frame
ast - Instance of Asteroid class
dum - Instance of Dumbbell class
Outputs:
ast_state - the state as represented in the rotating asteroid frame
ast_pos - ast_state[:, 0:3] is the position of the sc with respect to the asteroid
fixed frame and represented in the asteroid frame
ast_vel - ast_state[:, 3:6] is the velocity of the sc with respect to the asteroid and
represented in the asteroid fixed frame
ast_R_sc2ast - ast_state[:, 6:15] is the transformation from the sc frame to the asteroid
frame
ast_w - ast_state[:, 15:18] is the angular velocity of the sc with respect to the inertial
frame and defined in the asteroid frame
"""
# transformation between asteroid fixed frame and inertial frame
# figure out transformation from inertial frame to relative frame
Rast2int = np.zeros((3, 3, time.shape[0]))
Rint2ast = np.zeros((3, 3, time.shape[0]))
ast_state = np.zeros(state.shape)
for ii, t in np.ndenumerate(time):
Rast2int[:, :, ii] = attitude.rot3(
ast.omega * t,
'c')[:, :, np.newaxis] # asteroid body frame to inertial frame
Rint2ast[:, :, ii] = attitude.rot3(ast.omega * t, 'c').T[:, :,
np.newaxis]
Ra = np.squeeze(Rast2int[:, :, ii])
# convert the relative state to the inertial frame
body_pos = np.squeeze(state[ii, 0:3])
body_vel = np.squeeze(state[ii, 3:6])
R_sc2int = np.squeeze(state[ii, 6:15].reshape(3, 3))
body_w = np.squeeze(state[ii, 15:18])
ast_pos = Ra.T.dot(body_pos)
ast_vel = Ra.T.dot(body_vel)
ast_R_sc2ast = Ra.T.dot(R_sc2int).reshape(9)
ast_w = ast_R_sc2ast.reshape((3, 3)).dot(body_w)
ast_state[ii, :] = np.hstack((ast_pos, ast_vel, ast_R_sc2ast, ast_w))
return ast_states
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
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 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)
def test_expm_rot3_derivative(self):
axis = np.array([0, 0, 1])
R_dot = self.alpha * attitude.hat_map(axis).dot(
scipy.linalg.expm(self.alpha * self.t * attitude.hat_map(axis)))
R3_dot = attitude.rot3(self.alpha * self.t).dot(
attitude.hat_map(self.alpha * axis))
np.testing.assert_almost_equal(R_dot, R3_dot)
def eoms_inertial(self, state, t, ast):
"""Inertial dumbbell equations of motion about an asteroid
Inputs:
t -
state -
ast - Asteroid class object holding the asteroid gravitational model and
other useful parameters
"""
# unpack the state
pos = state[0:
3] # location of the center of mass in the inertial frame
vel = state[3:6] # vel of com in inertial frame
R = np.reshape(state[6:15], (3, 3)) # sc body frame to inertial frame
ang_vel = state[
15:
18] # angular velocity of sc wrt inertial frame defined in body frame
Ra = attitude.rot3(ast.omega * t,
'c') # asteroid body frame to inertial frame
# unpack parameters for the dumbbell
J = self.J
rho1 = self.zeta1
rho2 = self.zeta2
# position of each mass in the asteroid frame
z1 = Ra.T.dot(pos + R.dot(rho1))
z2 = Ra.T.dot(pos + R.dot(rho2))
z = Ra.T.dot(pos) # position of COM in asteroid frame
# compute the potential at this state
(U1, U1_grad, U1_grad_mat, U1laplace) = ast.polyhedron_potential(z1)
(U2, U2_grad, U2_grad_mat, U2laplace) = ast.polyhedron_potential(z2)
F1 = self.m1 * Ra.dot(U1_grad)
F2 = self.m2 * Ra.dot(U2_grad)
M1 = self.m1 * attitude.hat_map(rho1).dot(R.T.dot(Ra).dot(U1_grad))
M2 = self.m2 * attitude.hat_map(rho2).dot(R.T.dot(Ra).dot(U2_grad))
# M1 = np.zeros(3)
# M2 = np.zeros_like(3)
pos_dot = vel
vel_dot = 1 / (self.m1 + self.m2) * (F1 + F2)
R_dot = R.dot(attitude.hat_map(ang_vel)).reshape(9)
ang_vel_dot = np.linalg.inv(J).dot(-np.cross(ang_vel, J.dot(ang_vel)) +
M1 + M2)
statedot = np.hstack((pos_dot, vel_dot, R_dot, ang_vel_dot))
return statedot
def traverse_then_land_vertically(time,
ast,
final_pos=[0.550, 0, 0],
initial_pos=[2.550, 0, 0],
descent_tf=3600):
"""Desired translational states for vertical landing on asteroid
First the spacecraft will traverse horizontally in the equatorial plane
over the asteroid fixed x axis, then descend vertically
Inputs :
--------
"""
if time < descent_tf:
# linear interpolation between whatever the current state is and the landing staging point
omega = np.pi / 2 / descent_tf
inertial_pos = 2.550 * np.array(
[np.sin(omega * time), -np.cos(omega * time), 0])
inertial_vel = 2.550 * omega * np.array(
[np.cos(omega * time),
np.sin(omega * time), 0])
inertial_accel = 2.550 * omega**2 * np.array(
[-np.sin(omega * time),
np.cos(omega * time), 0])
else:
# rotation state of the asteroid - assume all simulations start with asteroid aligned with the inertial frame
omega_ast = ast.omega
omega_ast_dot = 0
omega_ast_vec = np.array([0, 0, omega_ast])
omega_ast_dot_vec = np.zeros(3)
Ra2i = attitude.rot3(omega_ast * (time - descent_tf), 'c')
# determine desired position and velocity in the body fixed frame at this current time input
# we'll use a simple linear interpolation between initial and final states
xslope = (final_pos[0] - initial_pos[0]) / (
descent_tf) # how long for teh descent
xdes = xslope * (time - descent_tf) + initial_pos[0]
body_pos_des = np.array([xdes, 0, 0])
body_vel_des = np.array([xslope, 0, 0])
body_acc_des = np.zeros(3)
# transform this body position/velocity into the inertial frame
inertial_pos = Ra2i.dot(body_pos_des)
inertial_vel = body_vel_des + np.cross(omega_ast_vec, body_pos_des)
inertial_accel = body_acc_des + 2 * np.cross(
omega_ast_vec, body_vel_des) + np.cross(
omega_ast_vec, np.cross(omega_ast_vec, body_pos_des))
# output
return inertial_pos, inertial_vel, inertial_accel
def dcm_eci2ecef(jd):
"""Rotation matrix to convert from ECI to ECEF
Will gradually improve this function to include all of the Earth
motion terms. For now, just use sidereal time to get the rotation matrix
"""
gst, _ = time.gstlst(jd, 0)
dcm = attitude.rot3(gst, 'r')
return dcm
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 castalia_raycasting_plot(img_path):
"""Generate an image of the raycaster in work
"""
if not os.path.exists(img_path):
os.makedirs(img_path)
output_filename = os.path.join(img_path, 'castalia_raycasting_plot.jpg')
input_filename = './data/shape_model/CASTALIA/castalia.obj'
polydata = wavefront.read_obj_to_polydata(input_filename)
# initialize the raycaster
caster_obb = raycaster.RayCaster(polydata, flag='obb')
caster_bsp = raycaster.RayCaster(polydata, flag='bsp')
sensor = raycaster.Lidar(view_axis=np.array([1, 0, 0]), num_step=3)
# need to translate the sensor and give it a pointing direction
pos = np.array([2, 0, 0])
dist = 2 # distance for each raycast
R = attitude.rot3(np.pi)
# find the inersections
# targets = pos + sensor.rotate_fov(R) * dist
targets = sensor.define_targets(pos, R, dist)
intersections_obb = caster_obb.castarray(pos, targets)
intersections_bsp = caster_bsp.castarray(pos, targets)
# plot
fig = graphics.mayavi_figure()
graphics.mayavi_addPoly(fig, polydata)
graphics.mayavi_addPoint(fig, pos, radius=0.05)
# draw lines from pos to all targets
for pt in targets:
graphics.mayavi_addLine(fig, pos, pt, color=(1, 0, 0))
for ints in intersections_bsp:
graphics.mayavi_addPoint(fig, ints, radius=0.05, color=(1, 1, 0))
graphics.mayavi_axes(fig=fig, extent=[-1, 1, -1, 1, -1, 1])
graphics.mayavi_view(fig=fig)
# savefig
graphics.mayavi_savefig(fig=fig, filename=output_filename, magnification=4)
def inertial_energy(self, time, state, ast):
"""Compute the kinetic and potential energy of the dumbbell given the current state
Input:
vel - nx3 velocity in inertial frame (km/sec)
ang_vel - nx3 angular velocity of dumbbell frame wrt to inertial frame expressed in dumbbell frame (rad/sec)
Outputs:
T - nx1 kinetic energy array which should be the same length as state input
"""
# some constants
m = self.m1 + self.m2 # total mass of dumbbell in kg
Jd = self.Jd
KE = np.zeros(time.shape[0])
PE = np.zeros(time.shape[0])
for ii in range(time.shape[0]):
pos = state[
ii,
0:3] # location of the center of mass in the inertial frame
vel = state[ii, 3:6] # vel of com in inertial frame
R = np.reshape(state[ii, 6:15],
(3, 3)) # sc body frame to inertial frame
ang_vel = state[
ii, 15:
18] # angular velocity of sc wrt inertial frame defined in body frame
Ra = attitude.rot3(ast.omega * time[ii],
'c') # asteroid body frame to inertial frame
# position of each mass in the inertial frame
z1 = Ra.T.dot(pos + R.dot(self.zeta1))
z2 = Ra.T.dot(pos + R.dot(self.zeta2))
(U1, _, _, _) = ast.polyhedron_potential(z1)
(U2, _, _, _) = ast.polyhedron_potential(z2)
PE[ii] = -self.m1 * U1 - self.m2 * U2
KE[ii] = 1.0 / 2 * m * vel.dot(vel) + 1.0 / 2 * np.trace(
attitude.hat_map(ang_vel).dot(Jd).dot(
attitude.hat_map(ang_vel).T))
return KE, PE
def fixed_attitude(time, state):
"""Desired attitude to ensure that the x axis is always pointing at the origin (asteroid)
"""
# extract out the states
pos = state[0:3] # location of the center of mass in the inertial frame
vel = state[3:6] # vel of com in inertial frame
R = np.reshape(state[6:15], (3, 3)) # sc body frame to inertial frame
ang_vel = state[
15:
18] # angular velocity of sc wrt inertial frame defined in body frame
Rd = attitude.rot3(np.pi, 'c')
Rd_dot = np.zeros_like(Rd)
ang_vel_d = np.zeros(3)
ang_vel_d_dot = np.zeros(3)
return (Rd, Rd_dot, ang_vel_d, ang_vel_d_dot)
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])
def linear_x_descent_translation(time,
ast,
final_pos=[0.550, 0, 0],
initial_pos=[2.550, 0, 0],
descent_tf=3600):
"""Desired translational states for vertical landing on asteroid
Inputs :
--------
"""
# rotation state of the asteroid - assume all simulations start with asteroid aligned with the inertial frame
omega_ast = ast.omega
omega_ast_dot = 0
omega_ast_vec = np.array([0, 0, omega_ast])
omega_ast_dot_vec = np.zeros(3)
Ra2i = attitude.rot3(omega_ast * time, 'c')
# determine desired position and velocity in the body fixed frame at this current time input
# we'll use a simple linear interpolation between initial and final states
xslope = (final_pos[0] - initial_pos[0]) / (descent_tf)
xdes = xslope * time + initial_pos[0]
body_pos_des = np.array([xdes, 0, 0])
body_vel_des = np.array([xslope, 0, 0])
body_acc_des = np.zeros(3)
# transform this body position/velocity into the inertial frame
inertial_pos = Ra2i.dot(body_pos_des)
inertial_vel = body_vel_des + np.cross(omega_ast_vec, body_pos_des)
inertial_accel = body_acc_des + 2 * np.cross(
omega_ast_vec, body_vel_des) + np.cross(
omega_ast_vec, np.cross(omega_ast_vec, body_pos_des))
# output
return inertial_pos, inertial_vel, inertial_accel
def plot_keyframe_original(time, i_state, R_ast2int, R_bcam2i, save_fig=False,
fwidth=1, filename='/tmp/estimate.eps',
kf_path='./data/itokawa_landing/cycles_high_7200_keyframe_poses_remove_first_kf.txt'):
"""Plot keyframe trajectory without any transformation
"""
# convert inertial position into asteriod fixed frame
inertial_pos = i_state[:, 0:3]
asteroid_pos = np.zeros_like(inertial_pos)
for ii, (ip, Ra2i) in enumerate(zip(inertial_pos, R_ast2int)):
asteroid_pos[ii, :] = Ra2i.reshape((3,3)).T.dot(ip)
# 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.rot3(np.deg2rad(-60)).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 = i_state[653, 6:15].reshape((3, 3))
R0_a2i = R_ast2int[653, :].reshape((3, 3))
R0_s2a = (R0_a2i.T.dot(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)
# rotate each keyframe point by the corresponding angle of asteroid
# for ii,index in enumerate(kf_time):
# kf_traj[ii,:] = R_ast2int[ii,:].reshape((3,3)).T.dot(kf_traj[ii,:])
# kf_R_first2cur[ii, :] = R_ast2int[ii, :].reshape((3, 3)).T.dot(kf_R_first2cur[ii, :].reshape((3,3))).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]
Rb2i = R_bcam2i[kf_time[0], :].reshape((3,3))
Rb2a = R_ast2int[kf_time[0], :].reshape((3, 3)).T.dot(Rb2i)
# 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(-1, 1)
kf_orig_ax.set_xlim3d(-0, 2)
kf_orig_ax.set_ylim3d(-1, 1)
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(0, 3)
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(-2, 1)
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(-0.5, 2.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')
if save_fig:
plt.figure(kf_comp_fig.number)
plt.savefig(filename)
plt.show()
def ast2inertial(time, state, ast, dum):
"""Convert from the asteroid frame to the inertial frame
This will convert a state vector which is defined in the asteroid frame
into the equivalent representation in the inertial frame.
Inputs:
time - nx1 array of time stamps for the state vector
state - nx18 array of the state vector defined as follows:
ast_pos - state[:, 0:3] is the position of the body with respect to the asteroid
center and defined in the asteroid frame
ast_vel - state[:, 4:6] is the velocity of the sc with respect to the asteroid
center of mass and defined in the asteroid frame
R_sc2ast - state[:, 6:15] the rotation matrix which transforms vectors from the sc frame
to the asteroid frame
ast_w - state[:, 15:18] the angular velocity of the sc with respect to the inertial
frame and defined in the asteroid frame
ast - Instance of Asteroid class
dum - Instance of Dumbbell class
Outputs:
inertial_state - the state as represented in the rotating asteroid fixed frame
inertial_pos - inertial_state[:, 0:3] is the position of the sc with respect to the asteroid
fixed frame and represented in the inertial frame
inertial_vel - inertial_state[:, 3:6] is the velocity of the sc with respect to the asteroid and
represented in the inertial fixed frame
inertial_R_sc2int - inertial_state[:, 6:15] is the transformation from the sc frame to the inertial
frame
inertial_w - inertial_state[:, 15:18] is the angular velocity of the sc with respect to the inertial
frame and defined in the inertial frame
"""
# transformation between asteroid fixed frame and inertial frame
# figure out transformation from inertial frame to relative frame
Rast2int = np.zeros((3, 3, time.shape[0]))
Rint2ast = np.zeros((3, 3, time.shape[0]))
inertial_state = np.zeros(state.shape)
for ii, t in np.ndenumerate(time):
Rast2int[:, :, ii] = attitude.rot3(
ast.omega * t,
'c')[:, :, np.newaxis] # asteroid body frame to inertial frame
Rint2ast[:, :, ii] = attitude.rot3(ast.omega * t, 'c').T[:, :,
np.newaxis]
Ra = np.squeeze(Rast2int[:, :, ii])
# convert the relative state to the inertial frame
ast_pos = np.squeeze(state[ii, 0:3])
ast_vel = np.squeeze(state[ii, 3:6])
R_sc2ast = np.squeeze(state[ii, 6:15].reshape(3, 3))
ast_w = np.squeeze(state[ii, 15:18])
inertial_pos = Ra.dot(ast_pos)
inertial_vel = Ra.dot(ast_vel)
inertial_R_sc2int = Ra.dot(R_sc2ast).reshape(9)
inertial_w = Ra.dot(ast_w)
inertial_state[ii, :] = np.hstack(
(inertial_pos, inertial_vel, inertial_R_sc2int, inertial_w))
return inertial_state, Rast2int, Rint2ast
def eoms_hamilton_relative_to_asteroid(time, state, ast, dum):
"""Convert eoms_hamilton_relative to the asteroid rotating frame
This function will convert the output from the eoms_hamilton_relative into the asteroid frame.
Since the equations are defined in the hamiltonian form first the inverse legendre transform
is used followed by a transformation into the asteroid frame.
The output of this function along with eoms_inertial_to_asteroid should be in the same frame
and allows for easy comparison.
Parameters
----------
time : nx1 numpy array
time array of the simulation
state : nx18 numpy array
pos - state[0:3] in km position of the dumbbell with respect to the asteroid and defined in the asteroid fixed frame
lin_mom - state[3:6] in kg km/sec is the linear momentum of dumbbell wrt the asteroid and defined in the asteroid fixed frame
R - state[6:15] rotation matrix which converts vectors from the dumbbell frame to the asteroid frame
ang_mom - state[15:18] J rad/sec angular momentum of the dumbbell wrt inertial frame and defined in the asteroid frame
ast - Instance of Asteroid class
dum - Instance of Dumbbell class
Returns
-------
ast_state : nx18 numpy array
ast_pos - ast_state[:, 0:3] is the position of the sc with respect to the asteroid
fixed frame and represented in the asteroid frame
ast_vel - ast_state[:, 3:6] is the velocity of the sc with respect to the asteroid and
represented in the asteroid fixed frame
ast_R_sc2ast - ast_state[:, 6:15] is the transformation from the sc frame to the asteroid
frame
ast_w - ast_state[:, 15:18] is the angular velocity of the sc with respect to the inertial
frame and defined in the asteroid frame
"""
if state.ndim == 1:
# first do the inverse legendre transform
rel_lin_mom = state[3:6]
rel_ang_mom = state[15:18]
R = state[6:15].reshape((3, 3))
rh_vel = rel_lin_mom / (dum.m1 + dum.m2)
Rast2int = attitude.rot3(ast.omega * time, 'c')
Ra = Rast2int
Jr = R.dot(dum.J).dot(R.T)
rh_ang_vel = np.linalg.inv(Jr).dot(rel_ang_mom)
ast_state = np.hstack((state[0:3], rh_vel, R.reshape(9), rh_ang_vel))
elif state.ndim == 2:
rel_lin_mom = state[:, 3:6]
rel_ang_mom = state[:, 15:18]
rh_vel = rel_lin_mom / (dum.m1 + dum.m2)
rh_ang_vel = np.zeros_like(rel_ang_mom)
Rast2int = np.zeros((3, 3, time.shape[0]))
ast_state = np.zeros(state.shape)
for ii, t in np.ndenumerate(time):
Rast2int[:, :, ii] = attitude.rot3(ast.omega * t, 'c')[:, :,
np.newaxis]
Ra = np.squeeze(Rast2int[:, :, ii])
R = state[ii, 6:15].reshape((3, 3))
Jr = R.dot(dum.J).dot(R.T)
rh_ang_vel[ii, :] = np.linalg.inv(Jr).dot(
np.squeeze(rel_ang_mom[ii, :]))
ast_state[ii, :] = np.hstack(
(state[ii, 0:3], rh_vel[ii, :], R.reshape(
(1, 9)), rh_ang_vel[ii, :]))
return ast_state
def eoms_inertial_to_asteroid(time, state, ast, dum):
"""Convert eoms_inertial output into the asteroid rotating frame
The eoms_inertial are not completely defined in the inertial frame.
The angular velocity needs to be converted. This function eases that issue by simply accpeting
the entire state output from eoms_inertial and transforming it.
Parameters
----------
time : nx1 numpy array
time array of the simulation
state : nx18 numpy array
pos - state[0:3] in km position of the dumbbell with respect to the asteroid and defined in the asteroid fixed frame
lin_mom - state[3:6] in kg km/sec is the linear momentum of dumbbell wrt the asteroid and defined in the asteroid fixed frame
R - state[6:15] rotation matrix which converts vectors from the dumbbell frame to the asteroid frame
ang_mom - state[15:18] J rad/sec angular momentum of the dumbbell wrt inertial frame and defined in the asteroid frame
ast : Instance of Asteroid class
dum : Instance of Dumbbell class
Returns
-------
ast_state : nx18 numpy array
ast_pos - ast_state[:, 0:3] is the position of the sc with respect to the asteroid
fixed frame and represented in the asteroid rotating frame
ast_vel - ast_state[:, 3:6] is the velocity of the sc with respect to the asteroid and
represented in the asteroid rotating frame
ast_R_sc2ast - ast_state[:, 6:15] is the transformation from the sc frame to the inertial
frame
ast_w - ast_state[:, 15:18] is the angular velocity of the sc with respect to the inertial
frame and defined in the asteroid frame
"""
ast_state = np.zeros(state.shape)
if state.ndim == 1:
inertial_pos = state[0:3]
inertial_vel = state[3:6]
R_sc2int = state[6:15]
body_w = state[15:18]
Rast2int = attitude.rot3(ast.omega * time,
'c') # asteroid body frame to inertial frame
Ra = Rast2int
ast_pos = Ra.T.dot(inertial_pos)
ast_vel = Ra.T.dot(inertial_vel)
R_sc2ast = Ra.T.dot(R_sc2int.reshape((3, 3))).reshape(9)
ast_w = R_sc2ast.reshape((3, 3)).dot(body_w)
ast_state = np.hstack((ast_pos, ast_vel, R_sc2ast, ast_w))
elif state.ndim == 2:
# transformation between asteroid fixed frame and inertial frame
# figure out transformation from inertial frame to relative frame
Rast2int = np.zeros((3, 3, time.shape[0]))
for ii, t in np.ndenumerate(time):
Rast2int[:, :, ii] = attitude.rot3(
ast.omega * t,
'c')[:, :, np.newaxis] # asteroid body frame to inertial frame
Ra = np.squeeze(Rast2int[:, :, ii])
# convert the relative state to the inertial frame
inertial_pos = np.squeeze(state[ii, 0:3])
inertial_vel = np.squeeze(state[ii, 3:6])
R_sc2int = np.squeeze(state[ii, 6:15].reshape(3, 3))
body_w = np.squeeze(state[ii, 15:18])
ast_pos = Ra.T.dot(inertial_pos)
ast_vel = Ra.T.dot(inertial_vel)
ast_R_sc2ast = Ra.T.dot(R_sc2int).reshape(9)
ast_w = ast_R_sc2ast.reshape((3, 3)).dot(body_w)
ast_state[ii, :] = np.hstack(
(ast_pos, ast_vel, ast_R_sc2ast, ast_w))
return ast_state
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
v1 = np.linalg.norm(out[8])
fpa1 = out[4]
# compute burn
rf, vf, fpaf = manuever.single_impulse(r1, v1, fpa1, delta_v, alpha)
an, pn, eccn, nun = manuever.rvfpa2orbit_el(rf, vf, fpaf, mu)
delta_arg_p = nuf - nun
print('Orbital Elements after burn')
kepler.orbit_el(pn, eccn, 0, 0, delta_arg_p, nun, mu, True)
# generate a plot of the orbit
_, state_pqw1, _, _, sat_pqw1, _ = kepler.conic_orbit(p, ecc, 0, 0, 0, nuf, nuf, mu)
_, state_pqw2, _, _, sat_pqw2, _ = kepler.conic_orbit(pn, eccn, 0, 0, 0, nun, nun, mu)
# rotate the second orbit by the change in argument of perigee
rot_old2new = attitude.rot3(delta_arg_p)
state_pqw2 = rot_old2new.dot(state_pqw2[:, 0:3].T).T
sat_pqw2 = rot_old2new.dot(sat_pqw2[0:3])
fig, ax = plt.subplots()
ax.plot(state_pqw1[:, 0], state_pqw1[:, 1], label='Initial')
ax.plot(state_pqw2[:, 0], state_pqw2[:, 1], label='Final')
ax.plot([0, sat_pqw2[0]], [0, sat_pqw2[1]], 'b.-', markersize=10)
ax.set_title('Single Impulse Manuever')
ax.set_xlabel(r'$\hat p$')
ax.set_ylabel(r'$\hat q$')
plt.legend()
plt.show()
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 simulate():
logger = logging.getLogger(__name__)
ast, dum, des_att_func, des_tran_func, AbsTol, RelTol = initialize()
num_steps = int(1e3)
time = np.linspace(0, num_steps, num_steps)
t0, tf = time[0], time[-1]
dt = time[1] - time[0]
initial_pos = np.array([1.5, 0, 0])
initial_vel = np.array([0, 0, 0])
initial_R = attitude.rot3(np.pi / 2).reshape(-1)
initial_w = np.array([0, 0, 0])
initial_state = np.hstack((initial_pos, initial_vel, initial_R, initial_w))
# TODO Initialize a coarse asteroid mesh model and combine with piont cloud data
# initialize the raycaster and lidar
polydata = wavefront.meshtopolydata(ast.V, ast.F)
caster = raycaster.RayCaster(polydata)
sensor = raycaster.Lidar(dist=5)
# try both a controlled and uncontrolled simulation
# t, istate, astate, bstate = eoms.inertial_eoms_driver(initial_state, time, ast, dum)
# TODO Dynamics should be based on the course model
# TODO Asteroid class will need a method to update mesh
system = integrate.ode(eoms.eoms_controlled_inertial)
system.set_integrator('lsoda', atol=AbsTol, rtol=RelTol, nsteps=1e4)
system.set_initial_value(initial_state, t0)
system.set_f_params(ast, dum, des_att_func, des_tran_func)
point_cloud = defaultdict(list)
state = np.zeros((num_steps + 1, 18))
t = np.zeros(num_steps + 1)
int_array = []
state[0, :] = initial_state
ii = 1
while system.successful() and system.t < tf:
# integrate the system and save state to an array
t[ii] = (system.t + dt)
state[ii, :] = system.integrate(system.t + dt)
logger.info('Step : {} Time: {}'.format(ii, t[ii]))
# now do the raycasting
if not (np.floor(t[ii]) % 1):
logger.info('Raycasting at t = {}'.format(t[ii]))
targets = sensor.define_targets(state[ii, 0:3],
state[ii, 6:15].reshape((3, 3)),
np.linalg.norm(state[ii, 0:3]))
# new asteroid rotation with vertices
nv = ast.rotate_vertices(t[ii])
Ra = ast.rot_ast2int(t[ii])
# update the mesh inside the caster
caster = raycaster.RayCaster.updatemesh(nv, ast.F)
# these intersections are points in the inertial frame
intersections = caster.castarray(state[ii, 0:3], targets)
point_cloud['time'].append(t[ii])
point_cloud['ast_state'].append(Ra.reshape(-1))
point_cloud['sc_state'].append(state[ii, :])
point_cloud['targets'].append(targets)
point_cloud['inertial_ints'].append(intersections)
logger.info('Found {} intersections'.format(len(intersections)))
ast_ints = []
for pt in intersections:
if pt.size > 0:
pt_ast = Ra.T.dot(pt)
else:
logger.info('No intersection for this point')
pt_ast = np.array([np.nan, np.nan, np.nan])
ast_ints.append(pt_ast)
point_cloud['ast_ints'].append(np.asarray(ast_ints))
# TODO Eventually call the surface reconstruction function and update asteroid model
# create an asteroid and dumbbell
ii += 1
# plot the simulation
# plotting.animate_inertial_trajectory(t, istate, ast, dum)
# plotting.plot_controlled_inertial(t, istate, ast, dum, fwidth=1)
return time, state, point_cloud
def dcm_pqw2lvlh(nu):
dcm = attitude.rot3(-nu)
return dcm
import pdb from lib import mesh_data, cgal filename = './data/shape_model/CASTALIA/castalia.obj' polydata = wavefront.read_obj_to_polydata(filename) caster_obb = raycaster.RayCaster(polydata, flag='obb') caster_bsp = raycaster.RayCaster(polydata, flag='bsp') sensor = raycaster.Lidar(view_axis=np.array([1, 0, 0]), num_step=3) # need to translate the sensor and give it a pointing direction pos = np.array([2, 0, 0]) dist = 2 # distance for each raycast R = attitude.rot3(np.pi) # find the inersections # targets = pos + sensor.rotate_fov(R) * dist targets = sensor.define_targets(pos, R, dist) intersections_obb = caster_obb.castarray(pos, targets) intersections_bsp = caster_bsp.castarray(pos, targets) # plot fig = graphics.mayavi_figure(bg=(0, 0, 0)) graphics.mayavi_addPoly(fig, polydata) graphics.mayavi_addPoint(fig, pos, radius=0.05) # draw lines from pos to all targets for pt in targets:
def eoms_controlled_blender(t, state, dum, ast):
"""Inertial dumbbell equations of motion about an asteroid
This method must be used with the scipy.integrate.ode class instead of the
more convienent scipy.integrate.odeint. In addition, we can control the
dumbbell given full state feedback. Blender is used to generate imagery
during the simulation.
Inputs:
t - Current simulation time step
state - (18,) array which defines the state of the vehicle
pos - (3,) position of the dumbbell with respect to the
asteroid center of mass and expressed in the inertial frame
vel - (3,) velocity of the dumbbell with respect to the
asteroid center of mass and expressed in the inertial frame
R - (9,) attitude of the dumbbell with defines the
transformation of a vector in the dumbbell frame to the
inertial frame ang_vel - (3,) angular velocity of the dumbbell
with respect to the inertial frame and represented in the
dumbbell frame
ast - Asteroid class object holding the asteroid gravitational
model and other useful parameters
"""
# unpack the state
pos = state[0:3] # location of the center of mass in the inertial frame
vel = state[3:6] # vel of com in inertial frame
R = np.reshape(state[6:15], (3, 3)) # sc body frame to inertial frame
ang_vel = state[
15:
18] # angular velocity of sc wrt inertial frame defined in body frame
Ra = attitude.rot3(ast.omega * t,
'c') # asteroid body frame to inertial frame
# unpack parameters for the dumbbell
J = dum.J
rho1 = dum.zeta1
rho2 = dum.zeta2
# position of each mass in the asteroid frame
z1 = Ra.T.dot(pos + R.dot(rho1))
z2 = Ra.T.dot(pos + R.dot(rho2))
z = Ra.T.dot(pos) # position of COM in asteroid frame
# compute the potential at this state
(U1, U1_grad, U1_grad_mat, U1laplace) = ast.polyhedron_potential(z1)
(U2, U2_grad, U2_grad_mat, U2laplace) = ast.polyhedron_potential(z2)
F1 = dum.m1 * Ra.dot(U1_grad)
F2 = dum.m2 * Ra.dot(U2_grad)
M1 = dum.m1 * attitude.hat_map(rho1).dot(R.T.dot(Ra).dot(U1_grad))
M2 = dum.m2 * attitude.hat_map(rho2).dot(R.T.dot(Ra).dot(U2_grad))
# generate image at this current state only at a specifc time
# blender.driver(pos, R, ast.omega * t, [5, 0, 1], 'test' + str.zfill(str(t), 4))
# use the imagery to figure out motion and pass to the controller instead
# of the true state
# calculate the desired attitude and translational trajectory
des_att_tuple = controller.body_fixed_pointing_attitude(t, state)
des_tran_tuple = controller.traverse_then_land_vertically(
t,
ast,
final_pos=[0.550, 0, 0],
initial_pos=[2.550, 0, 0],
descent_tf=3600)
# input trajectory and compute the control inputs
# compute the control input
u_m = controller.attitude_controller(t, state, M1 + M2, dum, ast,
des_att_tuple)
u_f = controller.translation_controller(t, state, F1 + F2, dum, ast,
des_tran_tuple)
pos_dot = vel
vel_dot = 1 / (dum.m1 + dum.m2) * (F1 + F2 + u_f)
R_dot = R.dot(attitude.hat_map(ang_vel)).reshape(9)
ang_vel_dot = np.linalg.inv(J).dot(-np.cross(ang_vel, J.dot(ang_vel)) +
M1 + M2 + u_m)
statedot = np.hstack((pos_dot, vel_dot, R_dot, ang_vel_dot))
return statedot
def test_expm_rot3_equivalent(self):
R = scipy.linalg.expm(self.angle *
attitude.hat_map(np.array([0, 0, 1])))
R3 = attitude.rot3(self.angle)
np.testing.assert_almost_equal(R.T.dot(R3), np.eye(3, 3))
def inertial_asteroid_trajectory(time, state, ast, dum, point_cloud,
mayavi_objects):
"""Animate the rotation of an asteroid and the motion of SC
"""
mesh, ast_axes, com, dum_axes, pc_lines = mayavi_objects
# animate the rotation fo the asteroid
ms = mesh.mlab_source
ts = com.mlab_source
ast_xs = ast_axes[0].mlab_source
ast_ys = ast_axes[1].mlab_source
ast_zs = ast_axes[2].mlab_source
dum_xs = dum_axes[0].mlab_source
dum_ys = dum_axes[1].mlab_source
dum_zs = dum_axes[2].mlab_source
pc_sources = [line.mlab_source for line in pc_lines]
for (t, pos, Rb2i, intersections) in zip(time, state[:, 0:3], state[:,
6:15],
point_cloud['inertial_ints']):
# rotate teh asteroid
Ra = attitude.rot3(ast.omega * t, 'c')
Rb2i = Rb2i.reshape((3, 3))
# parse out the vertices x, y, z
new_vertices = ast.rotate_vertices(t)
# update asteroid
ms.set(x=new_vertices[:, 0],
y=new_vertices[:, 1],
z=new_vertices[:, 2])
# update the satellite
ts.set(x=pos[0], y=pos[1], z=pos[2])
# update the asteroid axes
ast_xs.reset(x=[0, Ra[0, 0]], y=[0, Ra[1, 0]], z=[0, Ra[2, 0]])
ast_ys.reset(x=[0, Ra[0, 1]], y=[0, Ra[1, 1]], z=[0, Ra[2, 1]])
ast_zs.reset(x=[0, Ra[0, 2]], y=[0, Ra[1, 2]], z=[0, Ra[2, 2]])
dum_xs.reset(x=[pos[0], pos[0] + Rb2i[0, 0]],
y=[pos[1], pos[1] + Rb2i[1, 0]],
z=[pos[2], pos[2] + Rb2i[2, 0]])
dum_ys.reset(x=[pos[0], pos[0] + Rb2i[0, 1]],
y=[pos[1], pos[1] + Rb2i[1, 1]],
z=[pos[2], pos[2] + Rb2i[2, 1]])
dum_zs.reset(x=[pos[0], pos[0] + Rb2i[0, 2]],
y=[pos[1], pos[1] + Rb2i[1, 2]],
z=[pos[2], pos[2] + Rb2i[2, 2]])
for pcs, inter in zip(pc_sources, intersections):
# check if intersection is empty
if inter.size > 2:
pcs.reset(x=[pos[0], inter[0]],
y=[pos[1], inter[1]],
z=[pos[2], inter[2]])
else:
pcs.reset(x=[pos[0], pos[0] + 0.01],
y=[pos[1], pos[1] + 0.01],
z=[pos[2], pos[2] + 0.01])
yield
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 test_rotation():
R = attitude.rot3(np.pi / 2)
sensor_x = raycaster.Lidar()
rot_arr = raycaster.Lidar().rotate_fov(R)
np.testing.assert_array_almost_equal(rot_arr,
R.dot(sensor_x.lidar_arr.T).T)
def test_rot3_orthogonal_row(self):
mat = attitude.rot3(self.angle, 'r')
np.testing.assert_array_almost_equal(mat.T.dot(mat), np.eye(3, 3))
def test_rot3_determinant_1_row(self):
mat = attitude.rot3(self.angle, 'r')
np.testing.assert_allclose(np.linalg.det(mat), 1)
def blender_asteroid_frame_sim(gen_images=False):
# simulation parameters
output_path = './visualization/blender'
asteroid_name = 'itokawa_low'
# create a HDF5 dataset
hdf5_path = './data/asteroid_circumnavigate/{}_asteroid_circumnavigate.hdf5'.format(
datetime.datetime.now().strftime("%Y-%m-%dT%H:%M:%S"))
dataset_name = 'landing'
render = 'BLENDER'
image_modulus = 1
RelTol = 1e-6
AbsTol = 1e-6
ast_name = 'itokawa'
num_faces = 64
t0 = 0
dt = 1
tf = 3600 * 1
num_steps = 3600 * 1
ast = asteroid.Asteroid(ast_name,num_faces)
dum = dumbbell.Dumbbell(m1=500, m2=500, l=0.003)
# instantiate the blender scene once
camera_obj, camera, lamp_obj, lamp, itokawa_obj, scene = blender.blender_init(render_engine=render, asteroid_name=asteroid_name)
# get some of the camera parameters
K = blender_camera.get_calibration_matrix_K_from_blender(camera)
periodic_pos = np.array([1.495746722510590,0.000001002669660,0.006129720493607])
periodic_vel = np.array([0.000000302161724,-0.000899607989820,-0.000000013286327])
# set initial state for inertial EOMs
# initial_pos = np.array([2.550, 0, 0]) # km for center of mass in body frame
initial_pos = np.array([3, 0, 0])
initial_vel = periodic_vel + attitude.hat_map(ast.omega*np.array([0,0,1])).dot(initial_pos)
initial_R = attitude.rot3(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))
# instantiate ode object
# system = integrate.ode(eoms_controlled_blender)
system = integrate.ode(eoms.eoms_controlled_relative_blender_ode)
system.set_integrator('lsoda', atol=AbsTol, rtol=RelTol, nsteps=1000)
system.set_initial_value(initial_state, t0)
system.set_f_params(dum, ast)
i_state = np.zeros((num_steps+1, 18))
time = np.zeros(num_steps+1)
i_state[0, :] = initial_state
with h5py.File(hdf5_path) as image_data:
# create a dataset
if gen_images:
images = image_data.create_dataset(dataset_name, (244, 537, 3,
num_steps/image_modulus),
dtype='uint8')
RT_blender = image_data.create_dataset('RT',
(num_steps/image_modulus,
12))
R_i2bcam = image_data.create_dataset('R_i2bcam',
(num_steps/image_modulus, 9))
ii = 1
while system.successful() and system.t < tf:
# integrate the system and save state to an array
time[ii] = (system.t + dt)
i_state[ii, :] = (system.integrate(system.t + dt))
# generate the view of the asteroid at this state
if int(time[ii]) % image_modulus== 0 and gen_images:
img, RT, R = blender.gen_image_fixed_ast(i_state[ii,0:3],
i_state[ii,6:15].reshape((3,3)),
ast.omega * (time[ii] - 3600),
camera_obj, camera,
lamp_obj, lamp,
itokawa_obj, scene,
[5, 0, 1], 'test')
images[:, :, :, ii//image_modulus - 1] = img
RT_blender[ii//image_modulus -1, :] = RT.reshape(12)
R_i2bcam[ii//image_modulus -1, :] = R.reshape(9)
# do some image processing and visual odometry
print(system.t)
ii += 1
image_data.create_dataset('K', data=K)
image_data.create_dataset('i_state', data=i_state)
image_data.create_dataset('time', data=time)
def test_view_axis():
sensor_y = raycaster.Lidar(view_axis=np.array([0, 1, 0]))
sensor_x = raycaster.Lidar()
R = attitude.rot3(np.pi / 2)
np.testing.assert_array_almost_equal(sensor_y.lidar_arr,
R.dot(sensor_x.lidar_arr.T).T)