Skip to content

Commit

Permalink
--update tests to use magnum quats
Browse files Browse the repository at this point in the history
  • Loading branch information
@jturner65
jturner65 committed Jan 13, 2025
1 parent 72f0808 commit e3509c6
Show file tree
Hide file tree
Showing 4 changed files with 53 additions and 36 deletions.
2 changes: 1 addition & 1 deletion src_python/habitat_sim/agent/agent.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -294,7 +294,7 @@ def state(self):
return self.get_state()

@state.setter
def state(self, new_state):
def state(self, new_state: AgentState):
self.set_state(
new_state, reset_sensors=True, infer_sensor_states=True, is_initial=False
)
Expand Down
13 changes: 12 additions & 1 deletion src_python/habitat_sim/utils/common.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -122,7 +122,7 @@ def quat_from_two_vectors(v0: np.ndarray, v1: np.ndarray) -> qt.quaternion:
return qt.quaternion(s * 0.5, *(axis / s))


def angle_between_quats(q1: qt.quaternion, q2: qt.quaternion) -> float:
def angle_between_quats_old(q1: qt.quaternion, q2: qt.quaternion) -> float:
r"""Computes the angular distance between two quaternions
:return: The angular distance between q1 and q2 in radians
Expand All @@ -134,6 +134,17 @@ def angle_between_quats(q1: qt.quaternion, q2: qt.quaternion) -> float:
return 2 * np.arctan2(np.linalg.norm(dq.imag), np.abs(dq.real))


def angle_between_quats(q1: mn.Quaternion, q2: mn.Quaternion) -> float:
r"""Computes the angular distance between two magnum quaternions
:return: The angular distance between q1 and q2 in radians
"""

dq = q1.inverted() * q2

return 2 * np.arctan2(dq.vector.length(), np.abs(dq.scalar))


def quat_rotate_vector(q: qt.quaternion, v: np.ndarray) -> np.ndarray:
r"""Helper function to rotate a vector by a quaternion
Expand Down
12 changes: 10 additions & 2 deletions tests/test_agent.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,12 +10,20 @@

import habitat_sim
import habitat_sim.errors
from habitat_sim.utils.common import angle_between_quats, quat_from_angle_axis
from habitat_sim.utils.common import (
angle_between_quats,
quat_from_angle_axis,
quat_to_magnum,
)


def _check_state_same(s1, s2):
assert np.allclose(s1.position, s2.position, atol=1e-5)
assert angle_between_quats(s1.rotation, s2.rotation) < 1e-5
# remove quat_to_magnum once AgentState is refactored to use magnum quaternions
assert (
angle_between_quats(quat_to_magnum(s1.rotation), quat_to_magnum(s2.rotation))
< 1e-5
)


def test_reconfigure():
Expand Down
62 changes: 30 additions & 32 deletions tests/test_controls.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,12 +9,11 @@
import magnum as mn
import numpy as np
import pytest
import quaternion as qt
from hypothesis import strategies as st

import habitat_sim
import habitat_sim.errors
from habitat_sim.utils.common import angle_between_quats, quat_from_angle_axis
from habitat_sim.utils.common import angle_between_quats, quat_to_magnum


def test_no_action():
Expand Down Expand Up @@ -48,7 +47,7 @@ def test_no_move_fun():
@attr.s(auto_attribs=True, cmp=False)
class ExpectedDelta:
delta_pos: np.ndarray = attr.Factory(lambda: np.array([0, 0, 0]))
delta_rot: qt.quaternion = attr.Factory(lambda: qt.quaternion(1, 0, 0, 0))
delta_rot: mn.Quaternion = attr.Factory(lambda: mn.Quaternion.identity_init())


def _check_state_same(s1, s2):
Expand All @@ -59,29 +58,29 @@ def _check_state_same(s1, s2):
def _check_state_expected(s1, s2, expected: ExpectedDelta):
assert np.linalg.norm(s2.position - s1.position - expected.delta_pos) < 1e-5
assert (
angle_between_quats(s2.rotation * expected.delta_rot.inverse(), s1.rotation)
angle_between_quats(s2.rotation * expected.delta_rot.inverted(), s1.rotation)
< 1e-5
)


default_body_control_testdata = [
("move_backward", ExpectedDelta(delta_pos=0.25 * np.array(habitat_sim.geo.BACK))),
("move_forward", ExpectedDelta(delta_pos=0.25 * np.array(habitat_sim.geo.FRONT))),
("move_right", ExpectedDelta(delta_pos=0.25 * np.array(habitat_sim.geo.RIGHT))),
("move_left", ExpectedDelta(delta_pos=0.25 * np.array(habitat_sim.geo.LEFT))),
("move_backward", ExpectedDelta(delta_pos=0.25 * habitat_sim.geo.BACK)),
("move_forward", ExpectedDelta(delta_pos=0.25 * habitat_sim.geo.FRONT)),
("move_right", ExpectedDelta(delta_pos=0.25 * habitat_sim.geo.RIGHT)),
("move_left", ExpectedDelta(delta_pos=0.25 * habitat_sim.geo.LEFT)),
(
"turn_right",
ExpectedDelta(
delta_rot=quat_from_angle_axis(
np.deg2rad(10.0), np.array(habitat_sim.geo.GRAVITY)
delta_rot=mn.Quaternion.rotation(
mn.Rad(mn.math.pi / 18.0), habitat_sim.geo.GRAVITY
)
),
),
(
"turn_left",
ExpectedDelta(
delta_rot=quat_from_angle_axis(
np.deg2rad(10.0), np.array(habitat_sim.geo.UP)
delta_rot=mn.Quaternion.rotation(
mn.Rad(mn.math.pi / 18.0), habitat_sim.geo.UP
)
),
),
Expand Down Expand Up @@ -118,61 +117,58 @@ def test_default_body_controls(action, expected):
agent.act(action)
new_state = agent.state

print(
f"pre state rot : [{state.rotation.real}, {state.rotation.imag}] | new state rot : [{new_state.rotation.real}, {new_state.rotation.imag}] "
)

state.rotation = quat_to_magnum(state.rotation)
new_state.rotation = quat_to_magnum(new_state.rotation)

print(f"post state : {state} | new state : {new_state} ")

_check_state_expected(state, new_state, expected)
for k, v in state.sensor_states.items():
assert k in new_state.sensor_states
_check_state_expected(v, new_state.sensor_states[k], expected)


default_sensor_control_testdata = [
("move_up", ExpectedDelta(delta_pos=0.25 * np.array(habitat_sim.geo.UP))),
("move_down", ExpectedDelta(delta_pos=0.25 * np.array(habitat_sim.geo.GRAVITY))),
("move_up", ExpectedDelta(delta_pos=0.25 * habitat_sim.geo.UP)),
("move_down", ExpectedDelta(delta_pos=0.25 * habitat_sim.geo.GRAVITY)),
(
"look_right",
ExpectedDelta(
delta_rot=quat_from_angle_axis(
np.deg2rad(-10.0), np.array(habitat_sim.geo.UP)
)
delta_rot=mn.Quaternion.rotation(mn.Rad(-10.0), habitat_sim.geo.UP)
),
),
(
"look_left",
ExpectedDelta(
delta_rot=quat_from_angle_axis(
np.deg2rad(10.0), np.array(habitat_sim.geo.UP)
)
delta_rot=mn.Quaternion.rotation(mn.Rad(10.0), habitat_sim.geo.UP)
),
),
(
"look_up",
ExpectedDelta(
delta_rot=quat_from_angle_axis(
np.deg2rad(10.0), np.array(habitat_sim.geo.RIGHT)
)
delta_rot=mn.Quaternion.rotation(mn.Rad(10.0), habitat_sim.geo.RIGHT)
),
),
(
"look_down",
ExpectedDelta(
delta_rot=quat_from_angle_axis(
np.deg2rad(-10.0), np.array(habitat_sim.geo.RIGHT)
)
delta_rot=mn.Quaternion.rotation(mn.Rad(-10.0), habitat_sim.geo.RIGHT)
),
),
(
"rotate_sensor_clockwise",
ExpectedDelta(
delta_rot=quat_from_angle_axis(
np.deg2rad(-10.0), np.array(habitat_sim.geo.FRONT)
)
delta_rot=mn.Quaternion.rotation(mn.Rad(-10.0), habitat_sim.geo.FRONT)
),
),
(
"rotate_sensor_anti_clockwise",
ExpectedDelta(
delta_rot=quat_from_angle_axis(
np.deg2rad(10.0), np.array(habitat_sim.geo.FRONT)
)
delta_rot=mn.Quaternion.rotation(mn.Rad(10.0), habitat_sim.geo.FRONT)
),
),
]
Expand Down Expand Up @@ -211,8 +207,10 @@ def test_default_sensor_contorls(action, expected):
agent = habitat_sim.Agent(scene_graph.get_root_node().create_child(), agent_config)

state = agent.state
state.rotation = quat_to_magnum(state.rotation)
agent.act(action)
new_state = agent.state
new_state.rotation = quat_to_magnum(new_state.rotation)

_check_state_same(state, new_state)
for k, v in state.sensor_states.items():
Expand Down

0 comments on commit e3509c6

Please sign in to comment.