Physics tests: kinematics

.circleci/config.yml

@@ -208,6 +208,16 @@ jobs:
                 wget http://dl.fbaipublicfiles.com/habitat/habitat-test-scenes.zip
                 unzip habitat-test-scenes.zip
                 rm habitat-test-scenes.zip
+                cd ..
+              fi
+
+              if [ ! -d ./habitat-sim/data/objects/cheezit.glb ]
+              then
+                cd habitat-sim
+                wget http://dl.fbaipublicfiles.com/habitat/objects_v0.1.zip
+                unzip objects_v0.1.zip -d data/objects/
+                rm objects_v0.1.zip
+                cd ..
               fi
       - run:
           name: Run sim benchmark

habitat_sim/simulator.py

@@ -231,7 +231,7 @@ def get_physics_object_library_size(self):
         return self._sim.get_physics_object_library_size()
 
     def remove_object(self, object_id):
-        self._sim.remove_object(object_id)
+        return self._sim.remove_object(object_id)
 
     def get_existing_object_ids(self, scene_id=0):
         return self._sim.get_existing_object_ids(scene_id)
@@ -260,6 +260,9 @@ def apply_force(self, force, relative_position, object_id, scene_id=0):
     def apply_torque(self, torque, object_id, scene_id=0):
         self._sim.apply_torque(torque, object_id, scene_id)
 
+    def get_world_time(self, scene_id=0):
+        return self._sim.get_world_time()
+
 
 class Sensor:
     r"""Wrapper around habitat_sim.Sensor

src/esp/gfx/Simulator.cpp

@@ -230,10 +230,11 @@ std::vector<int> Simulator::getExistingObjectIDs(const int sceneID) {
 }
 
 // remove object objectID instance in sceneID
-void Simulator::removeObject(const int objectID, const int sceneID) {
+int Simulator::removeObject(const int objectID, const int sceneID) {
   if (physicsManager_ != nullptr && sceneID >= 0 && sceneID < sceneID_.size()) {
-    physicsManager_->removeObject(objectID);
+    return physicsManager_->removeObject(objectID);
   }
+  return ID_UNDEFINED;
 }
 
 // apply forces and torques to objects

src/esp/gfx/Simulator.h

@@ -108,8 +108,10 @@ class Simulator {
    * esp::physics::PhysicsManager::existingObjects_.
    * @param sceneID !! Not used currently !! Specifies which physical scene to
    * remove the object from.
+   * @return The deallocated object ID previously idnetifying the removed object
+   * or @ref esp::ID_UNDEFINED if failed.
    */
-  void removeObject(const int objectID, const int sceneID = 0);
+  int removeObject(const int objectID, const int sceneID = 0);
 
   /**
    * @brief Get the IDs of the physics objects instanced in a physical scene.

src/esp/physics/PhysicsManager.cpp

@@ -179,9 +179,10 @@ void PhysicsManager::stepPhysics(double dt) {
   // handle in-between step times? Ideally dt is a multiple of
   // sceneMetaData_.timestep
   double targetTime = worldTime_ + dt;
-  while (worldTime_ < targetTime)  // per fixed-step operations can be added
-                                   // here
+  while (worldTime_ < targetTime) {
+    // per fixed-step operations can be added here
     worldTime_ += fixedTimeStep_;
+  }
 }
 
 //! Profile function. In BulletPhysics stationery objects are

tests/test_physics.py

@@ -0,0 +1,75 @@
+import os.path as osp
+import random
+
+import numpy as np
+import pytest
+import quaternion
+
+import examples.settings
+import habitat_sim
+
+
+@pytest.mark.skipif(
+    not osp.exists("data/scene_datasets/habitat-test-scenes/skokloster-castle.glb")
+    or not osp.exists("data/objects/"),
+    reason="Requires the habitat-test-scenes and habitat test objects",
+)
+def test_kinematics(sim):
+    cfg_settings = examples.settings.default_sim_settings.copy()
+
+    cfg_settings[
+        "scene"
+    ] = "data/scene_datasets/habitat-test-scenes/skokloster-castle.glb"
+    # enable the physics simulator: also clears available actions to no-op
+    cfg_settings["enable_physics"] = True
+    cfg_settings["depth_sensor"] = True
+
+    # test loading the physical scene
+    hab_cfg = examples.settings.make_cfg(cfg_settings)
+    sim.reconfigure(hab_cfg)
+    assert sim.get_physics_object_library_size() > 0
+
+    # test adding an object to the world
+    object_id = sim.add_object(0)
+    assert len(sim.get_existing_object_ids()) > 0
+
+    # test kinematics
+    I = np.identity(4)
+
+    # test get and set translation
+    sim.set_translation(np.array([0, 1.0, 0]), object_id)
+    assert np.allclose(sim.get_translation(object_id), np.array([0, 1.0, 0]))
+
+    # test get and set transform
+    sim.set_transformation(I, object_id)
+    assert np.allclose(sim.get_transformation(object_id), I)
+
+    # test get and set rotation
+    Q = habitat_sim.utils.quat_from_angle_axis(np.pi, np.array([0, 1.0, 0]))
+    expected = np.eye(4)
+    expected[0:3, 0:3] = quaternion.as_rotation_matrix(Q)
+    sim.set_rotation(habitat_sim.utils.quat_to_magnum(Q), object_id)
+    assert np.allclose(sim.get_transformation(object_id), expected)
+    assert np.allclose(
+        habitat_sim.utils.quat_from_magnum(sim.get_rotation(object_id)), Q
+    )
+
+    # test object removal
+    old_object_id = sim.remove_object(object_id)
+    assert len(sim.get_existing_object_ids()) == 0
+    assert old_object_id == object_id
+
+    object_id = sim.add_object(0)
+
+    prev_time = 0.0
+    for _ in range(2):
+        # do some kinematics here (todo: translating or rotating instead of absolute)
+        sim.set_translation(np.random.rand(3), object_id)
+        T = sim.get_transformation(object_id)
+
+        # test getting observation
+        obs = sim.step(random.choice(list(hab_cfg.agents[0].action_space.keys())))
+
+        # check that time is increasing in the world
+        assert sim.get_world_time() > prev_time
+        prev_time = sim.get_world_time()