class TestInertialDesiredAttitudeBodyFixedHovering():
Rd, Rd_dot, ang_vel_d, ang_vel_d_dot = controller.body_fixed_pointing_attitude(
1, state)
def test_desired_rotation_matrix_determinant(self):
np.testing.assert_almost_equal(np.linalg.det(self.Rd), 1)
def test_desired_rotation_matrix_orthogonal(self):
np.testing.assert_array_almost_equal(self.Rd.T.dot(self.Rd),
np.eye(3, 3))
def test_desired_attitude_satifies_kinematics(self):
np.testing.assert_array_almost_equal(
self.Rd_dot, self.Rd.dot(attitude.hat_map(self.ang_vel_d)))
def test_x_axis_anti_aligned_with_position_vector(self):
dot_product = np.dot(self.Rd[:, 0], pos / np.linalg.norm(pos))
np.testing.assert_almost_equal(dot_product, -1)
def test_z_axis_aligned_with_positive_pole(self):
bodyz_inertial = self.Rd[:, 2]
z_inertial = np.array([0, 0, 1])
angle = np.arccos(np.dot(bodyz_inertial, z_inertial))
np.testing.assert_array_less(angle, np.pi / 2)
class TestInertialAttitudeController():
"""Test the attitude controller for the inertial eoms
"""
des_att_tuple = controller.body_fixed_pointing_attitude(1, state)
u_m = controller.attitude_controller(t, state, np.zeros(3), dum, ast,
des_att_tuple)
def test_control_moment_size(self):
np.testing.assert_equal(self.u_m.shape, (3, ))
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 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