[BE] - fix inconsistent base_rot property (#2085)

* add humnaoid data to CI download to activate the test_humanoid.py pytest

* Fix the inconsistency between base_rot getter and setter. Add unit tests. Activate and fix skipped humanoid unit tests.

* remove the precise test for pi since that is the singularity wrap point and will often produce -pi

.circleci/config.yml

@@ -211,7 +211,7 @@ jobs:
               . activate habitat
               git lfs install
               conda install -y gitpython git-lfs
-              python -m habitat_sim.utils.datasets_download --uids ci_test_assets franka_panda hab_spot_arm hab3_bench_assets ycb rearrange_dataset_v2 --data-path habitat-sim/data/ --no-replace --no-prune
+              python -m habitat_sim.utils.datasets_download --uids ci_test_assets franka_panda hab_spot_arm hab3_bench_assets habitat_humanoids ycb rearrange_dataset_v2 --data-path habitat-sim/data/ --no-replace --no-prune
       - run:
           name: Run sim benchmark
           command: |

habitat-lab/habitat/articulated_agents/articulated_agent_base.py

@@ -176,10 +176,28 @@ def base_pos(self, position: mn.Vector3):
 
     @property
     def base_rot(self) -> float:
-        return float(self.sim_obj.rotation.angle())
+        """
+        Returns scalar rotation angle of the agent around the Y axis.
+        Within range (-pi,pi) consistency with setter is tested. Outside that range, an equivalent but distinct rotation angle may be returned (e.g. 2pi == -2pi == 0).
+        """
+        angle = float(self.sim_obj.rotation.angle())
+        # NOTE: if the quaternion axis is inverted (-Y) then the angle will be negated
+        if self.sim_obj.rotation.axis()[1] < 0:
+            angle = -1 * angle
+        # NOTE: This offsetting gives us guarantees of consistency in the (-pi, pi) range.
+        if angle > mn.math.pi:
+            angle -= mn.math.pi * 2
+        elif angle <= -mn.math.pi:
+            angle += mn.math.pi * 2
+        # NOTE: This final fmod ensures that large angles are mapped back into the -2pi, 2pi range.
+        angle = mn.math.fmod(angle, 2 * mn.math.pi)
+        return angle
 
     @base_rot.setter
     def base_rot(self, rotation_y_rad: float):
+        """
+        Set the scalar rotation angle of the agent around the Y axis.
+        """
         if self._base_type == "mobile" or self._base_type == "leg":
             self.sim_obj.rotation = mn.Quaternion.rotation(
                 mn.Rad(rotation_y_rad), mn.Vector3(0, 1, 0)

habitat-lab/habitat/articulated_agents/humanoids/kinematic_humanoid.py

@@ -145,10 +145,31 @@ def base_pos(self, position: mn.Vector3):
 
     @property
     def base_rot(self) -> float:
-        return float(self.sim_obj.rotation.angle() + mn.Rad(self.offset_rot))
+        """
+        Returns scalar rotation angle of the humanoid around the Y axis.
+        Within range (-pi,pi) consistency with setter is tested. Outside that range, an equivalent but distinct rotation angle may be returned (e.g. 2pi == -2pi == 0).
+        NOTE: for humanoid, an additional offset_rot is considered between the model and this wrapper class.
+        """
+        angle = float(self.sim_obj.rotation.angle())
+        # NOTE: if the quaternion axis is inverted (-Y) then the angle will be negated
+        if self.sim_obj.rotation.axis()[1] < 0:
+            angle = -1 * angle
+        angle += float(mn.Rad(self.offset_rot))
+        # NOTE: This offsetting gives us guarantees of consistency in the (-pi, pi) range.
+        if angle > mn.math.pi:
+            angle -= mn.math.pi * 2
+        elif angle < -mn.math.pi:
+            angle += mn.math.pi * 2
+        # NOTE: This final fmod ensures that large angles are mapped back into the -2pi, 2pi range.
+        angle = mn.math.fmod(angle, 2 * mn.math.pi)
+        return angle
 
     @base_rot.setter
     def base_rot(self, rotation_y_rad: float):
+        """
+        Set the scalar rotation angle of the humanoid around the Y axis.
+        NOTE: for humanoid, an additional offset_rot is considered between the model and this wrapper class.
+        """
         if self._base_type == "mobile" or self._base_type == "leg":
             angle_rot = -self.offset_rot
             self.sim_obj.rotation = mn.Quaternion.rotation(

test/test_humanoid.py

@@ -4,6 +4,7 @@
 # This source code is licensed under the MIT license found in the
 # LICENSE file in the root directory of this source tree.
 
+import math
 from os import path as osp
 
 import gym
@@ -202,14 +203,17 @@ def test_humanoid_controller(humanoid_name):
             {
                 "articulated_agent_urdf": humanoid_path,
                 "motion_data_path": walk_pose_path,
+                "auto_update_sensor_transform": True,
             }
         )
         if not osp.exists(humanoid_path):
             pytest.skip(f"No humanoid file {humanoid_path}")
         kin_humanoid = kinematic_humanoid.KinematicHumanoid(agent_config, sim)
         kin_humanoid.reconfigure()
         kin_humanoid.update()
-        assert kin_humanoid.get_robot_sim_id() == 1  # 0 is the ground plane
+        assert (
+            kin_humanoid.get_robot_sim_id() == 2
+        )  # 0 is the stage and 1 is the ground plane
         print(kin_humanoid.get_link_and_joint_names())
         observations += simulate(sim, 1.0, produce_debug_video)
 
@@ -272,6 +276,89 @@ def test_humanoid_controller(humanoid_name):
             )
 
 
+@pytest.mark.skipif(
+    not osp.exists("data/humanoids/humanoid_data/female_2/female_2.urdf"),
+    reason="Test requires female 2 humanoid URDF and assets.",
+)
+@pytest.mark.skipif(
+    not habitat_sim.built_with_bullet,
+    reason="Robot wrapper API requires Bullet physics.",
+)
+def test_base_rot():
+    # set this to output test results as video for easy investigation
+    produce_debug_video = False
+    observations = []
+    cfg_settings = default_sim_settings.copy()
+    cfg_settings["scene"] = "NONE"
+    cfg_settings["enable_physics"] = True
+
+    # loading the physical scene
+    hab_cfg = make_cfg(cfg_settings)
+
+    with habitat_sim.Simulator(hab_cfg) as sim:
+        # setup the camera for debug video (looking at 0,0,0)
+        sim.agents[0].scene_node.translation = [0.0, -1.0, 2.0]
+
+        # add the humanoid to the world via the wrapper
+        humanoid_path = "data/humanoids/humanoid_data/female_2/female_2.urdf"
+        walk_pose_path = "data/humanoids/humanoid_data/female_2/female_2_motion_data_smplx.pkl"
+
+        agent_config = DictConfig(
+            {
+                "articulated_agent_urdf": humanoid_path,
+                "motion_data_path": walk_pose_path,
+                "auto_update_sensor_transform": True,
+            }
+        )
+        if not osp.exists(humanoid_path):
+            pytest.skip(f"No humanoid file {humanoid_path}")
+        kin_humanoid = kinematic_humanoid.KinematicHumanoid(agent_config, sim)
+        kin_humanoid.reconfigure()
+        kin_humanoid.update()
+
+        # check that for range (-pi, pi) getter and setter are consistent
+        num_samples = 100
+        for i in range(1, num_samples):
+            angle = -math.pi + (math.pi * 2 * i) / num_samples
+            kin_humanoid.base_rot = angle
+            assert np.allclose(angle, kin_humanoid.base_rot, atol=1e-5)
+            if produce_debug_video:
+                observations.append(sim.get_sensor_observations())
+
+        # check that for range [-2pi, 2pi] increment is accurate
+        kin_humanoid.base_rot = -math.pi * 2
+        inc = (math.pi * 4) / num_samples
+        rot_check = -math.pi * 2
+        for _ in range(0, num_samples):
+            kin_humanoid.base_rot = kin_humanoid.base_rot + inc
+            rot_check += inc
+            # NOTE: here we check that the angle is accurate (allow an offset of one full rotation to cover redundant options)
+            accurate_angle = False
+            for offset in [0, math.pi * 2, math.pi * -2]:
+                if np.allclose(
+                    rot_check, kin_humanoid.base_rot + offset, atol=1e-5
+                ):
+                    accurate_angle = True
+                    break
+            assert (
+                accurate_angle
+            ), f"should be {rot_check}, but was {kin_humanoid.base_rot}."
+            if produce_debug_video:
+                observations.append(sim.get_sensor_observations())
+
+        # produce some test debug video
+        if produce_debug_video:
+            from habitat_sim.utils import viz_utils as vut
+
+            vut.make_video(
+                observations,
+                "color_sensor",
+                "color",
+                "test_base_rot",
+                open_vid=True,
+            )
+
+
 @pytest.mark.skipif(
     not osp.exists("data/humanoids/humanoid_data/female_2/female_2.urdf"),
     reason="Test requires a human armature.",
@@ -369,14 +456,17 @@ def test_humanoid_seqpose_controller(humanoid_name):
             {
                 "articulated_agent_urdf": humanoid_path,
                 "motion_data_path": walk_pose_path,
+                "auto_update_sensor_transform": True,
             }
         )
         if not osp.exists(humanoid_path):
             pytest.skip(f"No humanoid file {humanoid_path}")
         kin_humanoid = kinematic_humanoid.KinematicHumanoid(agent_config, sim)
         kin_humanoid.reconfigure()
         kin_humanoid.update()
-        assert kin_humanoid.get_robot_sim_id() == 1  # 0 is the ground plane
+        assert (
+            kin_humanoid.get_robot_sim_id() == 2
+        )  # 0 is the stage and 1 is the ground plane
 
         # set base ground position from navmesh
         # NOTE: because the navmesh floats above the collision geometry we should see a pop/settle with dynamics and no fixed base

test/test_robot_wrapper.py

@@ -4,6 +4,7 @@
 # This source code is licensed under the MIT license found in the
 # LICENSE file in the root directory of this source tree.
 
+import math
 from os import path as osp
 
 import numpy as np
@@ -353,7 +354,8 @@ def test_fetch_robot_wrapper(fixed_base):
 
         # set base ground position using object transformation approach
         target_base_pos = sim.pathfinder.snap_point(fetch.sim_obj.translation)
-        target_base_rots = [0.0, np.pi * 0.25, np.pi * 0.50, np.pi]
+        # Note, don't test equivalency of pi, because that is the wrap point and is often negated.
+        target_base_rots = [0.0, np.pi * 0.25, np.pi * 0.50, np.pi * 0.99]
         for target_base_rot in target_base_rots:
             set_agent_base_via_obj_trans(
                 target_base_pos, target_base_rot, fetch
@@ -614,7 +616,8 @@ def test_spot_robot_wrapper(fixed_base):
 
         # set base ground position using object transformation approach
         target_base_pos = sim.pathfinder.snap_point(spot.sim_obj.translation)
-        target_base_rots = [0.0, np.pi * 0.25, np.pi * 0.50, np.pi]
+        # Note, don't test equivalency of pi, because that is the wrap point and is often negated.
+        target_base_rots = [0.0, np.pi * 0.25, np.pi * 0.50, np.pi * 0.99]
         for target_base_rot in target_base_rots:
             set_agent_base_via_obj_trans(
                 target_base_pos, target_base_rot, spot
@@ -705,6 +708,77 @@ def test_spot_robot_wrapper(fixed_base):
             )
 
 
+@pytest.mark.skipif(
+    not osp.exists("data/robots/hab_spot_arm"),
+    reason="Test requires Spot w/ arm robot URDF and assets.",
+)
+@pytest.mark.skipif(
+    not habitat_sim.built_with_bullet,
+    reason="Robot wrapper API requires Bullet physics.",
+)
+def test_base_rot():
+    # set this to output test results as video for easy investigation
+    produce_debug_video = False
+    observations = []
+    cfg_settings = default_sim_settings.copy()
+    cfg_settings["scene"] = "NONE"
+    cfg_settings["enable_physics"] = True
+
+    # loading the physical scene
+    hab_cfg = make_cfg(cfg_settings)
+
+    with habitat_sim.Simulator(hab_cfg) as sim:
+        # setup the camera for debug video (looking at 0,0,0)
+        sim.agents[0].scene_node.translation = [0.0, -1.0, 2.0]
+
+        # add the robot to the world via the wrapper
+        robot_path = "data/robots/hab_spot_arm/urdf/hab_spot_arm.urdf"
+        agent_config = DictConfig({"articulated_agent_urdf": robot_path})
+        spot = spot_robot.SpotRobot(agent_config, sim)
+        spot.reconfigure()
+        spot.update()
+
+        # check that for range (-pi, pi) getter and setter are consistent
+        num_samples = 100
+        for i in range(1, num_samples):
+            angle = -math.pi + (math.pi * 2 * i) / num_samples
+            spot.base_rot = angle
+            assert np.allclose(angle, spot.base_rot, atol=1e-5)
+            if produce_debug_video:
+                observations.append(sim.get_sensor_observations())
+
+        # check that for range [-2pi, 2pi] increment is accurate
+        spot.base_rot = -math.pi * 2
+        inc = (math.pi * 4) / num_samples
+        rot_check = -math.pi * 2
+        for _ in range(0, num_samples):
+            spot.base_rot = spot.base_rot + inc
+            rot_check += inc
+            # NOTE: here we check that the angle is accurate (allow an offset of one full rotation to cover redundant options)
+            accurate_angle = False
+            for offset in [0, math.pi * 2, math.pi * -2]:
+                if np.allclose(rot_check, spot.base_rot + offset, atol=1e-5):
+                    accurate_angle = True
+                    break
+            assert (
+                accurate_angle
+            ), f"should be {rot_check}, but was {spot.base_rot}."
+            if produce_debug_video:
+                observations.append(sim.get_sensor_observations())
+
+        # produce some test debug video
+        if produce_debug_video:
+            from habitat_sim.utils import viz_utils as vut
+
+            vut.make_video(
+                observations,
+                "color_sensor",
+                "color",
+                "test_base_rot",
+                open_vid=True,
+            )
+
+
 @pytest.mark.skipif(
     not osp.exists("data/robots/hab_stretch"),
     reason="Test requires Stretch w/ robot URDF and assets.",
@@ -763,7 +837,8 @@ def test_stretch_robot_wrapper(fixed_base):
         target_base_pos = sim.pathfinder.snap_point(
             stretch.sim_obj.translation
         )
-        target_base_rots = [0.0, np.pi * 0.25, np.pi * 0.50, np.pi]
+        # Note, don't test equivalency of pi, because that is the wrap point and is often negated.
+        target_base_rots = [0.0, np.pi * 0.25, np.pi * 0.50, np.pi * 0.99]
         for target_base_rot in target_base_rots:
             set_agent_base_via_obj_trans(
                 target_base_pos, target_base_rot, stretch