class TestDistanceToFacesCubeOutsideRandom():
"""Ranom location that is always outside the cube
"""
pt_out=np.array([1, np.random.uniform(-0.4, 0.4), 0])
v_cube, f_cube=wavefront.read_obj('./integration/cube.obj')
ast = asteroid.Asteroid('castalia', 256, 'mat')
ast = ast.loadmesh(v_cube, f_cube, 'cube')
edge_vertex_map=ast.asteroid_grav['edge_vertex_map']
edge_face_map=ast.asteroid_grav['edge_face_map']
normal_face=ast.asteroid_grav['normal_face']
vf_map=ast.asteroid_grav['vertex_face_map']
D, P, V, E, F=wavefront.distance_to_faces(pt_out, v_cube, f_cube,
normal_face,
edge_vertex_map,
edge_face_map,
vf_map)
def test_distance(self):
np.testing.assert_allclose(np.isfinite(self.D), True)
def test_point(self):
np.testing.assert_allclose(len(self.P) >= 3, True)
def test_vertex(self):
np.testing.assert_allclose(np.isfinite(self.V), True)
def test_edge(self):
np.testing.assert_allclose(np.isfinite(self.E), True)
def test_face(self):
np.testing.assert_allclose(self.F.size >= 1, True)
class TestDistanceToEdgesCubeOutsideEdge():
pt_out = np.array([0.6, 0.6, 0.6])
v_cube, f_cube = wavefront.read_obj('./integration/cube.obj')
ast = asteroid.Asteroid('castalia', 256, 'mat')
ast = ast.loadmesh(v_cube, f_cube, 'cube')
edge_vertex_map = ast.asteroid_grav['edge_vertex_map']
edge_face_map = ast.asteroid_grav['edge_face_map']
normal_face = ast.asteroid_grav['normal_face']
vf_map = ast.asteroid_grav['vertex_face_map']
D, P, V, E, F = wavefront.distance_to_edges(pt_out, v_cube, f_cube,
normal_face, edge_vertex_map,
edge_face_map, vf_map)
D_exp = P_exp = E_exp = F_exp = []
def test_distance(self):
np.testing.assert_allclose(self.D, self.D_exp)
def test_point(self):
np.testing.assert_allclose(self.P, np.empty(shape=(0, 3)))
def test_vertex(self):
np.testing.assert_allclose(self.V, np.empty(shape=(0, 2)))
def test_edge(self):
np.testing.assert_allclose(self.E, np.empty(shape=(0, 2)))
def test_face(self):
np.testing.assert_allclose(self.F, self.F_exp)
class TestDistanceToEdgesCubeSurfaceSingle():
pt_out = np.array([0.5, 0.4, 0.3])
v_cube, f_cube = wavefront.read_obj('./integration/cube.obj')
ast = asteroid.Asteroid('castalia', 256, 'mat')
ast = ast.loadmesh(v_cube, f_cube, 'cube')
edge_vertex_map = ast.asteroid_grav['edge_vertex_map']
edge_face_map = ast.asteroid_grav['edge_face_map']
normal_face = ast.asteroid_grav['normal_face']
vf_map = ast.asteroid_grav['vertex_face_map']
D, P, V, E, F = wavefront.distance_to_edges(pt_out, v_cube, f_cube,
normal_face, edge_vertex_map,
edge_face_map, vf_map)
D_exp = 0.1 / 2 * np.sqrt(2)
P_exp = np.array([0.5, 0.35, 0.35])
V_exp = np.array([4, 7])
E_exp = np.array([4, 7])
F_exp = np.array([6, 7])
def test_distance(self):
np.testing.assert_allclose(self.D, self.D_exp)
def test_point(self):
np.testing.assert_allclose(self.P, self.P_exp)
def test_vertex(self):
np.testing.assert_allclose(self.V, self.V_exp)
def test_edge(self):
np.testing.assert_allclose(self.E, self.E_exp)
def test_face(self):
np.testing.assert_allclose(self.F, self.F_exp)
class TestDistanceToEdgesCubeInside():
pt_out = np.array([0.4, 0.4, 0.2])
v_cube, f_cube = wavefront.read_obj('./integration/cube.obj')
ast = asteroid.Asteroid('castalia', 256, 'mat')
ast = ast.loadmesh(v_cube, f_cube, 'cube')
edge_vertex_map = ast.asteroid_grav['edge_vertex_map']
edge_face_map = ast.asteroid_grav['edge_face_map']
normal_face = ast.asteroid_grav['normal_face']
vf_map = ast.asteroid_grav['vertex_face_map']
D, P, V, E, F = wavefront.distance_to_edges(pt_out, v_cube, f_cube,
normal_face, edge_vertex_map,
edge_face_map, vf_map)
D_exp = -0.1 * np.sqrt(2) * np.ones_like(D)
P_exp = np.array([[0.5, 0.5, 0.2], [0.5, 0.5, 0.2]])
V_exp = np.array([[6, 7], [7, 6]])
E_exp = np.array([[6, 7], [7, 6]])
F_exp = np.array([[4, 6], [4, 6]])
def test_distance(self):
np.testing.assert_allclose(self.D, self.D_exp)
def test_point(self):
np.testing.assert_allclose(self.P, self.P_exp)
def test_vertex(self):
np.testing.assert_allclose(self.V, self.V_exp)
def test_edge(self):
for E, E_exp in zip(self.E, self.E_exp):
np.testing.assert_allclose(E, E_exp)
def test_face(self):
np.testing.assert_allclose(self.F, self.F_exp)
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 load_data(inertial_filename, relative_filename, mode):
"""Load saved data and extract out the states
"""
with np.load(inertial_filename, allow_pickle=True) as data:
inertial_state = data['state']
inertial_time = data['time']
inertial_KE = data['KE']
inertial_PE = data['PE']
ast_name = data['ast_name'][()]
num_faces = data['num_faces'][()]
tf = data['tf'][()]
num_steps = data['num_steps'][()]
with np.load(relative_filename, allow_pickle=True) as data:
relative_state = data['state']
relative_time = data['time']
relative_KE = data['KE']
relative_PE = data['PE']
relative_ast_name = data['ast_name'][()]
relative_num_faces = data['num_faces'][()]
relative_tf = data['tf'][()]
relative_num_steps = data['num_steps'][()]
# make sure we're dealing with the same simulation results or else the comparison is meaningless
np.testing.assert_string_equal(relative_ast_name, ast_name)
np.testing.assert_allclose(relative_num_faces, num_faces)
np.testing.assert_allclose(relative_tf, tf)
np.testing.assert_allclose(relative_num_steps, num_steps)
np.testing.assert_allclose(relative_state.shape, inertial_state.shape)
ast = asteroid.Asteroid(ast_name, num_faces)
dum = dumbbell.Dumbbell()
return relative_time, inertial_time, relative_state, inertial_state, ast, dum
def inertial_frame_comparison(ast_name='castalia',
num_faces=64,
tf=1e5,
num_steps=1e6):
"""Compare EOMs in the inertial frame
"""
initial_w = np.array([0.01, 0.0, 0.0])
print("Running inertial EOMS")
i_time, i_state = idriver.inertial_eoms_driver(ast_name, num_faces, tf,
num_steps, initial_w)
print("Running asteroid EOMs")
r_time, r_state = rdriver.relative_eoms_driver(ast_name, num_faces, tf,
num_steps, initial_w)
ast = asteroid.Asteroid(ast_name, num_faces)
dum = dumbbell.Dumbbell()
# also compute and compare the energy behavior
print("Computing inertial energy")
i_KE, i_PE = dum.inertial_energy(i_time, i_state, ast)
print("Computing asteroid energy")
r_KE, r_PE = dum.relative_energy(r_time, r_state, ast)
plotting.plot_energy(i_time, i_KE, i_PE)
# also look at the animation of both and the converted form as well
print("Plot comparison in the inertial frame")
plotting.plot_inertial_comparison(r_time, i_time, r_state, i_state, ast,
dum)
return 0
class TestDistanceToEdgesCubeOutsideFixedSingle():
pt_out = np.array([1, 0.5, 0.5])
ast = asteroid.Asteroid('castalia', 256, 'mat')
v, f = wavefront.read_obj('./integration/cube.obj')
ast = ast.loadmesh(v, f, 'cube')
edge_vertex_map = ast.asteroid_grav['edge_vertex_map']
edge_face_map = ast.asteroid_grav['edge_face_map']
normal_face = ast.asteroid_grav['normal_face']
vf_map = ast.asteroid_grav['vertex_face_map']
D, P, V, E, F = wavefront.distance_to_edges(pt_out, v, f, normal_face,
edge_vertex_map, edge_face_map,
vf_map)
D_exp = 0.5
P_exp = np.array([0.5, 0.5, 0.5])
V_exp = [7, 4]
E_exp = [7, 4]
F_exp = [6, 7]
def test_distance(self):
np.testing.assert_allclose(self.D, self.D_exp)
def test_point(self):
np.testing.assert_array_almost_equal(self.P, self.P_exp)
def test_vertex(self):
np.testing.assert_allclose(self.V, self.V_exp)
def test_edge(self):
np.testing.assert_allclose(self.E, self.E_exp)
def test_face(self):
np.testing.assert_allclose(self.F, self.F_exp)
class TestDistanceToFacesCubeSurfaceSingle():
pt_out=np.array([0.51, -0.1, 0.0])
v_cube, f_cube=wavefront.read_obj('./integration/cube.obj')
ast = asteroid.Asteroid('castalia', 256, 'mat')
ast = ast.loadmesh(v_cube, f_cube, 'cube')
edge_vertex_map=ast.asteroid_grav['edge_vertex_map']
edge_face_map=ast.asteroid_grav['edge_face_map']
normal_face=ast.asteroid_grav['normal_face']
vf_map=ast.asteroid_grav['vertex_face_map']
D, P, V, E, F=wavefront.distance_to_faces(pt_out, v_cube, f_cube,
normal_face,
edge_vertex_map,
edge_face_map,
vf_map)
D_exp = np.array(0.01)
P_exp = np.array([0.5, -0.1, 0])
V_exp = np.array([4, 7, 5])
E_exp = np.array([[7, 4],
[5, 7],
[4, 5]])
F_exp = np.array(7)
def test_distance(self):
np.testing.assert_allclose(self.D, self.D_exp)
def test_point(self):
np.testing.assert_allclose(self.P, self.P_exp)
def test_vertex(self):
np.testing.assert_allclose(self.V, self.V_exp)
def test_edge(self):
for E, E_exp in zip(self.E, self.E_exp):
np.testing.assert_allclose(E, E_exp)
def test_face(self):
np.testing.assert_allclose(self.F, self.F_exp)
def test_eros_obj():
# pick a known position
pos = np.random.uniform(1, 2, (3, ))
# apply a small perturbation
deltapos = delta * np.random.uniform(0, 1, pos.shape)
ast = asteroid.Asteroid('eros', 0, 'obj')
diff_U, dUdx = finite_difference(pos, deltapos, ast)
np.testing.assert_almost_equal(dUdx.dot(deltapos), diff_U)
class TestDistanceToVerticesCubeOutsideFixedMultiple():
"""This point is equally close to an entire face of the cube
So there are 4 vertices equidistant from pt
"""
pt_out = np.array([1, 0, 0])
v_cube, f_cube = wavefront.read_obj('./integration/cube.obj')
ast = asteroid.Asteroid('castalia', 256, 'mat')
ast = ast.loadmesh(v_cube, f_cube, 'cube')
edge_vertex_map = ast.asteroid_grav['edge_vertex_map']
edge_face_map = ast.asteroid_grav['edge_face_map']
normal_face = ast.asteroid_grav['normal_face']
vf_map = ast.asteroid_grav['vertex_face_map']
D, P, V, E, F = wavefront.distance_to_vertices(pt_out, v_cube, f_cube,
normal_face,
edge_vertex_map,
edge_face_map, vf_map)
D_exp = np.ones_like(D) * 0.5 * np.sqrt(3)
P_exp = np.array([[0.5, -0.5, -0.5], [0.5, -0.5, 0.5], [0.5, 0.5, -0.5],
[0.5, 0.5, 0.5]])
V_exp = np.array([4, 5, 6, 7])
E_exp = np.array([
np.array([[6, 4], [7, 4], [4, 0], [4, 6], [5, 4], [0, 4], [4, 7],
[4, 5]]),
np.array([[5, 0], [5, 1], [5, 7], [5, 4], [1, 5], [7, 5], [4, 5],
[0, 5]]),
np.array([[6, 0], [6, 4], [4, 6], [6, 2], [6, 7], [7, 6], [0, 6],
[2, 6]]),
np.array([[7, 2], [7, 4], [7, 1], [6, 7], [7, 3], [7, 6], [5, 7],
[7, 5], [3, 7], [2, 7], [4, 7], [1, 7]])
])
F_exp = np.array([
list([0, 6, 7, 8]),
list([7, 8, 9, 10]),
list([0, 1, 4, 6]),
list([4, 5, 6, 7, 10, 11])
])
def test_distance(self):
np.testing.assert_allclose(np.absolute(self.D), self.D_exp)
def test_point(self):
np.testing.assert_allclose(self.P, self.P_exp)
def test_vertex(self):
np.testing.assert_allclose(self.V, self.V_exp)
def test_edge(self):
for E, E_exp in zip(self.E, self.E_exp):
np.testing.assert_allclose(E, E_exp)
def test_face(self):
for F, F_exp in zip(self.F, self.F_exp):
np.testing.assert_allclose(F, F_exp)
def test_asteroid():
# plot the asteroid as a triangular mesh
ast = asteroid.Asteroid('castalia', 64)
V = ast.asteroid_grav.get('V') * 1e5
F = ast.asteroid_grav.get('F')
F = F.astype(int)
surface = mlab.pipeline.triangular_mesh_source(V[:, 0], V[:, 1], V[:, 2],
F - 1)
# numpy array of points which define the vertices of the surface
points = np.array([[0, 0, 0], [1, 0, 0], [1, 1, 0]])
# numpy array defining the triangle which connects those points
element = np.array([[0, 1, 2], [0, 1, 2]])
def initialize():
"""Initialize all the things for the simulation
"""
logger = logging.getLogger(__name__)
logger.info('Initialize asteroid and dumbbell objects')
ast = asteroid.Asteroid('castalia', 4092, 'obj')
dum = dumbbell.Dumbbell(m1=500, m2=500, l=0.003)
des_att_func = controller.random_sweep_attitude
des_tran_func = controller.inertial_fixed_state
AbsTol = 1e-9
RelTol = 1e-9
return ast, dum, des_att_func, des_tran_func, AbsTol, RelTol
def create_plots(plot_flags):
# load the h5py file with all the imagery and simulation data
with h5py.File('./data/itokawa_landing/cycles_high_7200.hdf5', 'r') as sim_data:
sim_data.visit(printname)
K = sim_data['K']
i_state = sim_data['i_state']
time = sim_data['time']
images = sim_data['landing']
RT_vector = sim_data['RT']
R_bcam2i_vector = sim_data['R_i2bcam'] # the name is incorrect - actually it's bcamera to inertial frame
R_ast2int = sim_data['Rast2inertial']
# define the asteroid and dumbbell objects like the simulation driver
ast_name = 'itokawa'
num_faces = 64
ast = asteroid.Asteroid(ast_name,num_faces)
dum = dumbbell.Dumbbell(m1=500, m2=500, l=0.003)
# draw some of the features from an example image
if plot_flags.feature_matching:
sift_flann_matching_image(images[:, :, :, 3000],
images[:, :, :, 3200], ratio=0.3,
plot=True,
filename='/tmp/itokawa_feature_matching.png',
save_fig=plot_flags.save_plots)
# draw the true and estimated trajectory
if plot_flags.simulation_plots:
plotting.plot_controlled_blender_inertial(time,
i_state,
ast,
dum,
plot_flags.save_plots,
1,
controller.traverse_then_land_vertically,
controller.body_fixed_pointing_attitude)
# create animation
if plot_flags.animation:
plotting.animate_inertial_trajectory(time, i_state, ast, dum, 3600, plot_flags.save_plots)
if plot_flags.keyframe:
# plot_keyframe_trajectory(time, i_state, R_ast2int, R_bcam2i_vector,
# plot_flags.save_plots, fwidth=1,
# filename='/tmp/keyframe_estimate.eps')
plot_keyframe_original(time, i_state, R_ast2int, R_bcam2i_vector,
plot_flags.save_plots, fwidth=1,
filename='/tmp/keyframe_estimate.eps')
def test_closest_edge_plot_asteroid(pt=np.random.uniform(0.8, 1.5) *
sphere.rand(2)):
ast = asteroid.Asteroid('itokawa', 0, 'obj')
v, f = ast.asteroid_grav['V'], ast.asteroid_grav['F']
edge_vertex_map = ast.asteroid_grav['edge_vertex_map']
edge_face_map = ast.asteroid_grav['edge_face_map']
normal_face = ast.asteroid_grav['normal_face']
vf_map = ast.asteroid_grav['vertex_face_map']
D, P, V, E, F = wavefront.distance_to_edges(pt, v, f, normal_face,
edge_vertex_map, edge_face_map,
vf_map)
plot_data(pt, v, f, D, P, V, E, F, 'Closest Edge')
return D, P, V, E, F
def test_closest_edge_plot_cube(pt=np.random.uniform(0.9, 1.5) *
sphere.rand(2)):
ast = asteroid.Asteroid('castalia', 256, 'mat')
v, f = wavefront.read_obj('./integration/cube.obj')
ast = ast.loadmesh(v, f, 'cube')
edge_vertex_map = ast.asteroid_grav['edge_vertex_map']
edge_face_map = ast.asteroid_grav['edge_face_map']
normal_face = ast.asteroid_grav['normal_face']
vf_map = ast.asteroid_grav['vertex_face_map']
D, P, V, E, F = wavefront.distance_to_edges(pt, v, f, normal_face,
edge_vertex_map, edge_face_map,
vf_map)
plot_data(pt, v, f, D, P, V, E, F, '')
return D, P, V, E, F
def test_update_raycasting_mesh():
# load a polyhedron
ast = asteroid.Asteroid('castalia', 0, 'obj')
v, f = ast.V, ast.F
nv = ast.rotate_vertices(0)
nf = f
# define the raycaster object
caster = raycaster.RayCaster.loadmesh(v, f, flag='obb', scale=1.0)
ncaster = raycaster.RayCaster.updatemesh(nv, nf)
# test methods of caster
V, F = wavefront.polydatatomesh(caster.polydata)
nV, nF = wavefront.polydatatomesh(ncaster.polydata)
np.testing.assert_allclose(V, v)
def test_ensure_asteroid_potential_matching(self):
# some random numbers for the state
from dynamics import asteroid as asteroid_python
ast_python = asteroid_python.Asteroid('castalia', 0, 'obj')
mesh = mesh_data.MeshData(self.v, self.f)
ast_cpp = asteroid.Asteroid('castalia', mesh)
for ii in range(10):
x = np.random.rand()
state = np.array([x, 1, 1])
Up, _, _, _ = ast_python.polyhedron_potential(state)
ast_cpp.polyhedron_potential(state)
np.testing.assert_allclose(ast_cpp.get_potential(), Up, 1e-6)
def castalia_reconstruction(img_path):
"""Incrementally modify an ellipse into a low resolution verision of castalia
by adding vertices and modifying the mesh
"""
surf_area = 0.01
a = 0.22
delta = 0.01
# load a low resolution ellipse to start
ast = asteroid.Asteroid('castalia', 0, 'obj')
ellipsoid = surface_mesh.SurfMesh(ast.axes[0], ast.axes[1], ast.axes[2],
10, 0.025, 0.5)
ve, fe = ellipsoid.verts(), ellipsoid.faces()
vc, fc = ast.V, ast.F
# sort the vertices in in order (x component)
vc = vc[vc[:, 0].argsort()]
# uncertainty for each vertex in meters (1/variance)
vert_weight = np.full(ve.shape[0], (np.pi * np.max(ast.axes))**2)
# calculate maximum angle as function of surface area
max_angle = wavefront.spherical_surface_area(np.max(ast.axes), surf_area)
# loop and create many figures
mfig = graphics.mayavi_figure(offscreen=False)
mesh = graphics.mayavi_addMesh(mfig, ve, fe)
ms = mesh.mlab_source
index = 0
for ii, pt in enumerate(vc):
index += 1
filename = os.path.join(
img_path, 'castalia_reconstruct_' + str(index).zfill(7) + '.jpg')
# graphics.mlab.savefig(filename, magnification=4)
ve, vert_weight = wavefront.spherical_incremental_mesh_update(
pt, ve, fe, vertex_weight=vert_weight, max_angle=max_angle)
ms.reset(x=ve[:, 0], y=ve[:, 1], z=ve[:, 2], triangles=fe)
graphics.mayavi_addPoint(mfig, pt, radius=0.01)
graphics.mayavi_points3d(mfig, ve, scale_factor=0.01, color=(1, 0, 0))
return 0
def test_closest_face_plot_asteroid(pt=np.random.uniform(0.8, 1.5) *
sphere.rand(2)):
"""Needs to be aligned with a face
i.e. not in the corners
"""
ast = asteroid.Asteroid('itokawa', 0, 'obj')
v, f = ast.asteroid_grav['V'], ast.asteroid_grav['F']
edge_vertex_map = ast.asteroid_grav['edge_vertex_map']
edge_face_map = ast.asteroid_grav['edge_face_map']
normal_face = ast.asteroid_grav['normal_face']
vf_map = ast.asteroid_grav['vertex_face_map']
D, P, V, E, F = wavefront.distance_to_faces(pt, v, f, normal_face,
edge_vertex_map, edge_face_map,
vf_map)
plot_data(pt, v, f, D, P, V, E, F, 'Closest Face')
return D, P, V, E, F
def asteroid_frame_comparison():
"""Compare EOMs in the asteroid frame
"""
ast_name = 'castalia'
num_faces = 64
tf = 1e4
num_steps = 1e5
initial_w = np.array([0.0, 0.01, 0.0])
i_time, i_state = id.inertial_eoms_driver(ast_name, num_faces, tf,
num_steps, initial_w)
r_time, r_state = rd.relative_eoms_driver(ast_name, num_faces, tf,
num_steps, initial_w)
ast = asteroid.Asteroid(ast_name, num_faces)
dum = dumbbell.Dumbbell()
# also compute and compare the energy behavior
i_KE, i_PE = dum.inertial_energy(i_time, i_state, ast)
r_KE, r_PE = dum.relative_energy(r_time, r_state, ast)
class TestDistanceToVerticesCubeSurfaceSingle():
pt_out = np.array([0.5, 0.4, 0.4])
v_cube, f_cube = wavefront.read_obj('./integration/cube.obj')
ast = asteroid.Asteroid('castalia', 256, 'mat')
ast = ast.loadmesh(v_cube, f_cube, 'cube')
edge_vertex_map = ast.asteroid_grav['edge_vertex_map']
edge_face_map = ast.asteroid_grav['edge_face_map']
normal_face = ast.asteroid_grav['normal_face']
vf_map = ast.asteroid_grav['vertex_face_map']
D, P, V, E, F = wavefront.distance_to_vertices(pt_out, v_cube, f_cube,
normal_face,
edge_vertex_map,
edge_face_map, vf_map)
D_exp = -0.1 * np.sqrt(2)
P_exp = np.array([0.5, 0.5, 0.5])
V_exp = 7
E_exp = np.array([[7, 2], [7, 4], [7, 1], [6, 7], [7, 3], [7, 6], [5, 7],
[7, 5], [3, 7], [2, 7], [4, 7], [1, 7]])
F_exp = [4, 5, 6, 7, 10, 11]
def test_distance(self):
np.testing.assert_allclose(self.D, self.D_exp)
def test_point(self):
np.testing.assert_allclose(self.P, self.P_exp)
def test_vertex(self):
np.testing.assert_allclose(self.V, self.V_exp)
def test_edge(self):
for E, E_exp in zip(self.E, self.E_exp):
np.testing.assert_allclose(E, E_exp)
def test_face(self):
np.testing.assert_allclose(self.F, self.F_exp)
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])
class TestDistanceToEdgesCubeOutsideFixedMultiple():
"""This point is equally close to vertex so hopefully multiple edges
have equal distance
"""
pt_out = np.array([1, 0, 0])
ast = asteroid.Asteroid('castalia', 256, 'mat')
v, f = wavefront.read_obj('./integration/cube.obj')
ast = ast.loadmesh(v, f, 'cube')
edge_vertex_map = ast.asteroid_grav['edge_vertex_map']
edge_face_map = ast.asteroid_grav['edge_face_map']
normal_face = ast.asteroid_grav['normal_face']
vf_map = ast.asteroid_grav['vertex_face_map']
D, P, V, E, F = wavefront.distance_to_edges(pt_out, v, f, normal_face,
edge_vertex_map, edge_face_map,
vf_map)
D_exp = np.array([0.5, 0.5])
P_exp = np.array([[0.5, -0, -0], [0.5, 0, 0]])
V_exp = np.array([[7, 4], [4, 7]])
E_exp = np.array([[7, 4], [4, 7]])
F_exp = np.array([[6, 7], [6, 7]])
def test_distance(self):
np.testing.assert_allclose(self.D, self.D_exp)
def test_point(self):
np.testing.assert_array_almost_equal(self.P, self.P_exp)
def test_vertex(self):
np.testing.assert_allclose(self.V, self.V_exp)
def test_edge(self):
np.testing.assert_allclose(self.E, self.E_exp)
def test_face(self):
np.testing.assert_allclose(self.F, self.F_exp)
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])
import relative_driver as rdriver
RelTol = 1e-6
AbsTol = 1e-6
ast_name = 'itokawa'
num_faces = 64
tf = 1e2
num_steps = 1e2
time = np.linspace(0, int(tf), int(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 + attitude.hat_map(
ast.omega * np.array([0, 0, 1])).dot(initial_pos)
initial_R = attitude.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_control,
initial_state,
time,
args=(ast, ),
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 incremental_reconstruction(input_filename,
output_filename,
asteroid_name='castalia'):
"""Incrementally update the mesh
Now we'll use the radial mesh reconstruction.
"""
logger = logging.getLogger(__name__)
# output_filename = './data/raycasting/20180226_castalia_reconstruct_highres_45deg_cone.hdf5'
logger.info('Loading {}'.format(input_filename))
data = np.load(input_filename)
point_cloud = data['point_cloud'][()]
# define the asteroid and dumbbell objects
asteroid_faces = 0
asteroid_type = 'obj'
m1, m2, l = 500, 500, 0.003
ellipsoid_min_angle = 10
ellipsoid_max_radius = 0.03
ellipsoid_max_distance = 0.5
surf_area = 0.01
ast = asteroid.Asteroid(asteroid_name, asteroid_faces, asteroid_type)
dum = dumbbell.Dumbbell(m1=m1, m2=m2, l=l)
logger.info('Creating ellipsoid mesh')
# define a simple mesh to start
ellipsoid = surface_mesh.SurfMesh(ast.axes[0], ast.axes[1], ast.axes[2],
ellipsoid_min_angle,
ellipsoid_max_radius,
ellipsoid_max_distance)
v_est, f_est = ellipsoid.verts(), ellipsoid.faces()
vert_weight = np.full(v_est.shape[0], (np.pi * np.max(ast.axes))**2)
max_angle = wavefront.spherical_surface_area(np.max(ast.axes), surf_area)
# extract out all the points in the asteroid frame
time = point_cloud['time'][::1]
ast_ints = point_cloud['ast_ints'][::1]
logger.info('Create HDF5 file {}'.format(output_filename))
with h5py.File(output_filename, 'w') as fout:
# store some extra data about teh simulation
v_group = fout.create_group('reconstructed_vertex')
f_group = fout.create_group('reconstructed_face')
w_group = fout.create_group('reconstructed_weight')
sim_data = fout.create_group('simulation_data')
sim_data.attrs['asteroid_name'] = np.string_(asteroid_name)
sim_data.attrs['asteroid_faces'] = asteroid_faces
sim_data.attrs['asteroid_type'] = np.string_(asteroid_type)
sim_data.attrs['m1'] = dum.m1
sim_data.attrs['m2'] = dum.m2
sim_data.attrs['l'] = dum.l
sim_data.attrs['ellipsoid_axes'] = ast.axes
sim_data.attrs['ellipsoid_min_angle'] = ellipsoid_min_angle
sim_data.attrs['ellipsoid_max_radius'] = ellipsoid_max_radius
sim_data.attrs['ellipsoid_max_distance'] = ellipsoid_max_distance
sim_data.attrs['surf_area'] = surf_area
sim_data.attrs['max_angle'] = max_angle
fout.create_dataset('truth_vertex', data=ast.V)
fout.create_dataset('truth_faces', data=ast.F)
fout.create_dataset('estimate_faces', data=f_est)
fout.create_dataset('initial_vertex', data=v_est)
fout.create_dataset('initial_faces', data=f_est)
fout.create_dataset('initial_weight', data=vert_weight)
logger.info('Starting loop over point cloud')
for ii, (t, points) in enumerate(zip(time, ast_ints)):
# check if points is empty
logger.info('Current : t = {} with {} points'.format(
t, len(points)))
for pt in points:
# incremental update for each point in points
# check to make sure each pt is not nan
if not np.any(np.isnan(pt)):
v_est, vert_weight = wavefront.spherical_incremental_mesh_update(
pt,
v_est,
f_est,
vertex_weight=vert_weight,
max_angle=max_angle)
# use HD5PY instead
# save every so often and delete v_array,f_array to save memory
if (ii % 1) == 0:
logger.info('Saving data to HDF5. ii = {}, t = {}'.format(
ii, t))
v_group.create_dataset(str(ii), data=v_est)
f_group.create_dataset(str(ii), data=f_est)
w_group.create_dataset(str(ii), data=vert_weight)
logger.info('Completed the reconstruction')
return 0
def asteroid_reconstruct_generate_data(output_filename,
asteroid_name='castalia'):
"""Generate all the data for an example for reconstructing asteroid
"""
asteroid_type = 'obj'
asteroid_faces = 0
if asteroid_name == 'castalia':
ellipsoid_min_angle = 10
ellipsoid_max_radius = 0.03
ellipsoid_max_distance = 0.5
step = 1
surf_area = 0.01
elif asteroid_name == 'itokawa':
ellipsoid_min_angle = 10
ellipsoid_max_radius = 0.003
ellipsoid_max_distance = 0.5
step = 1
surf_area = 0.0001
# load asteroid castalia
ast = asteroid.Asteroid(asteroid_name, asteroid_faces, asteroid_type)
ellipsoid = surface_mesh.SurfMesh(ast.axes[0], ast.axes[1], ast.axes[2],
ellipsoid_min_angle,
ellipsoid_max_radius,
ellipsoid_max_distance)
ve, fe = ellipsoid.verts(), ellipsoid.faces()
vc, fc = ast.V, ast.F
# cort the truth vertices in increasing order of x component
vc = vc[vc[:, 0].argsort()]
vc = vc[::step, :]
# np.random.shuffle(vc)
# define initial uncertainty for our estimate
vert_weight = np.full(ve.shape[0], (np.pi * np.max(ast.axes))**2)
# calculate the maximum angle as a function of desired surface area
max_angle = wavefront.spherical_surface_area(np.max(ast.axes), surf_area)
# generate a mesh and reconstruct object from c++
mesh = mesh_data.MeshData(ve, fe)
rmesh = reconstruct.ReconstructMesh(ve, fe, vert_weight)
# loop over all the points and save the data
output_path = os.path.join(output_filename)
with h5py.File(output_path, 'w') as fout:
reconstructed_vertex = fout.create_group('reconstructed_vertex')
reconstructed_weight = fout.create_group('reconstructed_weight')
reconstructed_vertex.attrs['asteroid_name'] = np.string_(asteroid_name)
reconstructed_vertex.attrs['asteroid_faces'] = asteroid_faces
reconstructed_vertex.attrs['asteroid_type'] = np.string_(asteroid_type)
reconstructed_vertex.attrs['ellipsoid_axes'] = ast.axes
reconstructed_vertex.attrs['ellipsoid_min_angle'] = ellipsoid_min_angle
reconstructed_vertex.attrs[
'ellipsoid_max_radius'] = ellipsoid_max_radius
reconstructed_vertex.attrs[
'ellipsoid_max_distance'] = ellipsoid_max_distance
reconstructed_vertex.attrs['surf_area'] = surf_area
fout.create_dataset('truth_vertex', data=vc)
fout.create_dataset('truth_faces', data=fc)
fout.create_dataset('estimate_faces', data=fe)
fout.create_dataset('initial_vertex', data=ve)
fout.create_dataset('initial_faces', data=fe)
fout.create_dataset('initial_weight', data=vert_weight)
for ii, pt in enumerate(vc):
# ve, vert_weight = wavefront.spherical_incremental_mesh_update(pt, ve, fe,
# vertex_weight=vert_weight,
# max_angle=max_angle)
rmesh.update(pt, max_angle)
ve = rmesh.get_verts()
vert_weight = np.squeeze(rmesh.get_weights())
# save the current array and weight to htpy
reconstructed_vertex.create_dataset(str(ii),
data=ve,
compression='lzf')
reconstructed_weight.create_dataset(str(ii),
data=vert_weight,
compression='lzf')
print('Finished generating data. Saved to {}'.format(output_path))
return 0
help='String - Filename for npz archive',
type=str)
parser.add_argument(
"-m",
"--mode",
type=int,
choices=[0, 1],
help=
"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)