def testTransInState(self):
for i in range(100):
state_a = trans_in.TransInState(
omega_b=_GetRandomVec(3),
dcm_ti2b=geometry.AxisToDcm(_GetRandomVec(3)),
wing_vel_ti=_GetRandomVec(3),
wing_pos_ti=_GetRandomVec(3))
state_b = trans_in.TransInState(
omega_b=_GetRandomVec(3),
dcm_ti2b=geometry.AxisToDcm(_GetRandomVec(3)),
wing_vel_ti=_GetRandomVec(3),
wing_pos_ti=_GetRandomVec(3))
tangent = state_a.Difference(state_b)
state_c = state_a.Increment(tangent)
for i in range(3):
self.assertAlmostEqual(state_b.omega_b[i], state_c.omega_b[i])
for j in range(3):
self.assertAlmostEqual(state_b.dcm_ti2b[i, j],
state_c.dcm_ti2b[i, j])
self.assertAlmostEqual(state_b.wing_vel_ti[i], state_c.wing_vel_ti[i])
self.assertAlmostEqual(state_b.wing_pos_ti[i], state_c.wing_pos_ti[i])
wing_state_a = self._trans_in_frame.StateToWingState(state_a)
wing_state_b = self._trans_in_frame.StateToWingState(state_b)
wing_state_tangent = wing_state_a.Difference(wing_state_b)
tangent_cmp = self._trans_in_frame.StateTangentFromWingStateTangent(
wing_state_a, wing_state_tangent)
for i in range(3):
self.assertAlmostEqual(tangent.domega_b[i],
tangent_cmp.domega_b[i])
# The increment in Eulers will not be equal for large steps.
self.assertAlmostEqual(tangent.dwing_vel_ti[i],
tangent_cmp.dwing_vel_ti[i])
self.assertAlmostEqual(tangent.dwing_pos_ti[i],
tangent_cmp.dwing_pos_ti[i])
h = 1e-6
state_b = state_a.Increment(tangent, step=h)
wing_state_b = self._trans_in_frame.StateToWingState(state_b)
tangent_cmp = self._trans_in_frame.StateTangentFromWingStateTangent(
wing_state_a, wing_state_a.Difference(wing_state_b))
for i in range(3):
self.assertAlmostEqual(tangent.domega_b[i],
tangent_cmp.domega_b[i]/h, places=5)
self.assertAlmostEqual(tangent.ddcm_ti2b[i],
tangent_cmp.ddcm_ti2b[i]/h, places=5)
self.assertAlmostEqual(tangent.dwing_vel_ti[i],
tangent_cmp.dwing_vel_ti[i]/h, places=5)
self.assertAlmostEqual(tangent.dwing_pos_ti[i],
tangent_cmp.dwing_pos_ti[i]/h, places=5)
def testIncrementDecrement(self):
axis = _GetRandomVec(3)
dcm = geometry.AxisToDcm(axis)
state_a = dynamics.WingState(omega_b=_GetRandomVec(3),
dcm_g2b=np.eye(3),
wing_vel_g=_GetRandomVec(3),
wing_pos_g=_GetRandomVec(3))
state_b = dynamics.WingState(omega_b=_GetRandomVec(3),
dcm_g2b=dcm,
wing_vel_g=_GetRandomVec(3),
wing_pos_g=_GetRandomVec(3))
tangent = state_a.Difference(state_b)
state_c = state_a.Increment(tangent)
for i in range(3):
self.assertAlmostEqual(state_b.omega_b[i, 0], state_c.omega_b[i, 0])
for j in range(3):
self.assertAlmostEqual(state_b.dcm_g2b[i, j],
state_c.dcm_g2b[i, j])
self.assertAlmostEqual(state_b.wing_vel_g[i, 0],
state_c.wing_vel_g[i, 0])
self.assertAlmostEqual(state_b.wing_pos_g[i, 0],
state_c.wing_pos_g[i, 0])
self.assertAlmostEqual(state_b.omega_b[i, 0] - state_a.omega_b[i, 0],
tangent.domega_b[i, 0])
self.assertAlmostEqual(axis[i, 0], tangent.ddcm_g2b[i, 0])
self.assertAlmostEqual(
state_b.wing_vel_g[i, 0] - state_a.wing_vel_g[i, 0],
tangent.dwing_vel_g[i, 0])
self.assertAlmostEqual(
state_b.wing_pos_g[i, 0] - state_a.wing_pos_g[i, 0],
tangent.dwing_pos_g[i, 0])
def testAxisToDcm(self):
v = np.matrix(np.random.rand(3, 1))
dcm = np.matrix(scipy.linalg.expm(geometry.CrossMatrix(-v)))
nearly_eye = dcm.T * geometry.AxisToDcm(v)
for i in range(3):
for j in range(3):
self.assertAlmostEqual(1.0 if i == j else 0.0, nearly_eye[i,
j])
def Increment(self, tangent, step=1.0):
"""Return a state evolved from this state along a tangent direction.
Args:
tangent: A WingState.Tangent along which to move.
step: A scaling of how far to move.
Returns:
A new WingState.
"""
return TransInState(
omega_b=self.omega_b + step * tangent.domega_b,
dcm_ti2b=geometry.AxisToDcm(step * tangent.ddcm_ti2b) *
self.dcm_ti2b,
wing_vel_ti=self.wing_vel_ti + step * tangent.dwing_vel_ti,
wing_pos_ti=self.wing_pos_ti + step * tangent.dwing_pos_ti)