refactor: remove habitat_sim dependency in motor_policies.py

[tristanls]

This pull request makes progress on decoupling Monty from HabitatSim (identified in #52). Specifically, the habitat_sim dependency within motor_policies.py is removed.

These temporary tests (excluded from the pull request) guided the changes in addition to the standard test suite. Notably, Rotation expects an x, y, z, w quaternion format.

    def test_hab_utils_quat_rotate_vector(self):
        """Verify hab_utils.quat_rotate_vector replacement results."""
        quat_wxyz = qt.quaternion(1, 2, 3, 4)
        vec_xyz = np.array([5, 6, 7])
        hu_vec_rotated_xyz = hab_utils.quat_rotate_vector(quat_wxyz, vec_xyz)
        print(hu_vec_rotated_xyz)  # [2.6 6.  8.2]

        qt_vec_rotated_xyz = qt.rotate_vectors(quat_wxyz, vec_xyz)
        print(qt_vec_rotated_xyz)  # [2.6 6.  8.2]

        rot_vec_rotated_xyz = Rotation.from_quat(qt.as_float_array(quat_wxyz)).apply(
            vec_xyz
        )
        print(rot_vec_rotated_xyz)  # [1.8 7.6 7. ]

        [w, x, y, z] = qt.as_float_array(quat_wxyz)
        rot_vec_rotated_xyz = Rotation.from_quat([x, y, z, w]).apply(vec_xyz)
        print(rot_vec_rotated_xyz)  # [2.6 6.  8.2]

        assert np.allclose(
            hu_vec_rotated_xyz, rot_vec_rotated_xyz, rtol=1.0e-10, atol=1.0e-10
        ), "hu_vec_rotated_xyz and rot_vec_rotated_xyz do not match"

    def test_hab_utils_inverse(self):
        """Verify get_inverse_agent_rot replacement results."""
        vec_xyz = np.array([5, 6, 7])
        quat_wxyz = qt.quaternion(1, 2, 3, 4)
        inverse_magnum_rotation = hab_utils.common.quat_to_magnum(quat_wxyz).inverted()
        inverse_quaternion_rotation = hab_utils.common.quat_from_magnum(
            inverse_magnum_rotation
        )
        [w, x, y, z] = qt.as_float_array(inverse_quaternion_rotation)
        magnum_vec_rotated_xyz = Rotation.from_quat([x, y, z, w]).apply(vec_xyz)
        print(magnum_vec_rotated_xyz)  # [3.00000014 5.1999997 8.60000013]

        [w, x, y, z] = qt.as_float_array(quat_wxyz)
        rot_vec_rotated_xyz = Rotation.from_quat([x, y, z, w]).inv().apply(vec_xyz)
        print(rot_vec_rotated_xyz)  # [3.  5.2 8.6]

        assert np.allclose(
            magnum_vec_rotated_xyz, rot_vec_rotated_xyz, rtol=1.0e-5, atol=1.0e-8
        ), "magnum_vec_rotated_xyz and rot_vec_rotated_xyz do not match"

[tristanls]

note: I think this could become qt.rotate_vectors(inverse_quaternion_rotation, adjusted_prev_locs) based on qt.rotate_vectors documentation: https://quaternion.readthedocs.io/en/latest/quaternion/#quaternion.rotate_vectors

[tristanls]

I removed the extraneous list comprehension, which is now qt.rotate_vectors. This function is intended to be applied to multiple vectors and will return the correct shape.

[vkakerbeck]

Really nice, thanks for the update! Just one optional comment that could be useful for future reference (since we've been tripped up by this several times in the past. Could also add the note here https://thousandbrainsproject.readme.io/docs/common-issues-and-how-to-fix-them#quaternions)

[vkakerbeck]

Maybe add a comment here clarifying that magnum and scipy rotation use different quaternion conventions.

[nielsleadholm]

Yeah as in quaternion and scipy right (not magnum)?

[vkakerbeck]

yes, sorry thats what I meant

[nielsleadholm]

Awesome, thanks for sorting this Tristan.

[nielsleadholm]

Yeah as in quaternion and scipy right (not magnum)?