PathFinder and CoordinateFrame refactor to support robotics coordinat…

…e system

examples/coordinate_system.py

@@ -55,7 +55,11 @@ def display_sensor_image(camera_sensor):
     rgb_image.show()
 
 
-if __name__ == "__main__":
+def main_coordinate_system_visual_examples():
+    """
+    Initializes a simulator and runs a set of incremental visual examples to demonstrate the default behavior of the system and cameras.
+    """
+
     # setup the settings configuration
     sim_settings: Dict[str, Any] = default_sim_settings
     # make camera coincident with agent scenenode
@@ -228,9 +232,7 @@ def display_sensor_image(camera_sensor):
         axis_template.render_asset_handle = "data/test_assets/objects/axis.glb"
         # don't let the CoM be computed from centroid of the shape
         axis_template.compute_COM_from_shape = False
-        axis_template_id = rotm.register_template(
-            axis_template, specified_handle="axis"
-        )
+        rotm.register_template(axis_template, specified_handle="axis")
         rom = sim.get_rigid_object_manager()
         axis_ro = rom.add_object_by_template_handle("axis")
         print(
@@ -261,7 +263,7 @@ def display_sensor_image(camera_sensor):
         display_sensor_image(camera_sensor)
 
         # add a spot robot
-        spot_urdf = aom.add_articulated_object_from_urdf(
+        aom.add_articulated_object_from_urdf(
             "data/robots/hab_spot_arm/urdf/hab_spot_arm.urdf",
             fixed_base=True,
         )
@@ -287,3 +289,79 @@ def display_sensor_image(camera_sensor):
             mn.Vector3(0, 0, -1), mn.Vector3(0, 1, -1), mn.Color4.magenta()
         )
         display_sensor_image(camera_sensor)
+    # exit initial sim instancing
+
+
+def main_fremont_robotics():
+    """
+    Load a Fremont scene configured for robotics coordinate system testing.
+    """
+    # setup the settings configuration
+    sim_settings: Dict[str, Any] = default_sim_settings
+    # make camera coincident with agent scenenode
+    sim_settings["sensor_height"] = 0
+    sim_settings[
+        "scene_dataset_config_file"
+    ] = "data/Fremont-Knuckles/fremont_knuckles.scene_dataset_config.json"
+    # NOTE: This loads the scan.stage_config.json
+    # NOTE: This file is modified to re-orient into robotics coordinate system.
+    sim_settings["scene"] = "scan"
+
+    cfg = make_cfg(sim_settings)
+
+    with habitat_sim.Simulator(cfg) as sim:
+        # set gravity to -Z direction
+        sim.set_gravity(mn.Vector3(0, 0, -9.8))
+        sim.navmesh_visualization = True
+
+        # agent local transform is identity
+        camera_agent = sim.get_agent(0)
+        # set the default agent transform to identity (it gets randomized when added upon initialization if there is a navmesh)
+        camera_agent.scene_node.transformation = mn.Matrix4()
+        print(f"Agent transform: {camera_agent.scene_node.transformation}")
+        # camera local transform is identity
+        camera_sensor = sim._Simulator__sensors[0]["color_sensor"]
+        print(f"Camera transform: {camera_sensor._sensor_object.node.transformation}")
+        # camera absolute transform is identity
+        print(
+            f"Camera absolute transform: {camera_sensor._sensor_object.node.absolute_transformation()}"
+        )
+
+        # add a spot robot
+        aom = sim.get_articulated_object_manager()
+        spot = aom.add_articulated_object_from_urdf(
+            "data/robots/hab_spot_arm/urdf/hab_spot_arm_no_rot.urdf",
+            fixed_base=True,
+        )
+        spot.translation = sim.pathfinder.snap_point(spot.translation) + mn.Vector3(
+            0, 0, 0.8
+        )
+
+        render_dist = 4
+        for diagonal in [
+            mn.Vector3(2, 2, 2),
+            mn.Vector3(-2, 2, 2),
+            mn.Vector3(-2, -2, 2),
+            mn.Vector3(2, -2, 2),
+        ]:
+            diagonal = diagonal.normalized() * render_dist
+            camera_sensor._sensor_object.node.transformation = mn.Matrix4.look_at(
+                eye=diagonal,  # from along the diagonal
+                target=mn.Vector3(),  # to the origin
+                up=mn.Vector3(0, 0, 1),  # up is robotics in +Z
+            )
+
+            draw_axes(sim)
+            display_sensor_image(camera_sensor)
+
+        stage_attributes = sim.get_stage_initialization_template()
+        print(stage_attributes)
+        # breakpoint()
+        pass
+
+
+if __name__ == "__main__":
+    # test the conventions and produce images
+    # main_coordinate_system_visual_examples()
+
+    main_fremont_robotics()

src/esp/bindings/ShortestPathBindings.cpp

@@ -57,9 +57,8 @@ void initShortestPathBindings(py::module& m) {
       .def(py::init(&MultiGoalShortestPath::create<>))
       .def_readwrite("requested_start", &MultiGoalShortestPath::requestedStart,
                      R"(The starting point for the path.)")
-      .def_property("requested_ends", &MultiGoalShortestPath::getRequestedEnds,
-                    &MultiGoalShortestPath::setRequestedEnds,
-                    R"(The list of desired potential end points.)")
+      .def_readwrite("requested_ends", &MultiGoalShortestPath::requestedEnds,
+                     R"(The list of desired potential end points.)")
       .def_readwrite(
           "points", &MultiGoalShortestPath::points,
           R"(A list of points that specify the shortest path on the navigation mesh between requestedStart and the closest (by geodesic distance) point in requestedEnds. Will be empty if no path exists.)")
@@ -172,26 +171,17 @@ void initShortestPathBindings(py::module& m) {
           py::overload_cast<MultiGoalShortestPath&>(&PathFinder::findPath),
           "path"_a,
           R"(Finds the shortest path between a start point and the closest of a set of end points (in geodesic distance) on the navigation mesh using MultiGoalShortestPath module. Path variable is filled if successful. Returns boolean success.)")
-      .def("try_step", &PathFinder::tryStep<Magnum::Vector3>, "start"_a,
+      .def("try_step", &PathFinder::tryStep, "start"_a, "end"_a)
+      .def("try_step_no_sliding", &PathFinder::tryStepNoSliding, "start"_a,
            "end"_a)
-      .def("try_step", &PathFinder::tryStep<vec3f>, "start"_a, "end"_a)
-      .def("try_step_no_sliding",
-           &PathFinder::tryStepNoSliding<Magnum::Vector3>, "start"_a, "end"_a)
-      .def("try_step_no_sliding", &PathFinder::tryStepNoSliding<vec3f>,
-           "start"_a, "end"_a)
-      .def("snap_point", &PathFinder::snapPoint<Magnum::Vector3>, "point"_a,
+      .def("snap_point", &PathFinder::snapPoint, "point"_a,
            "island_index"_a = ID_UNDEFINED)
-      .def("snap_point", &PathFinder::snapPoint<vec3f>, "point"_a,
-           "island_index"_a = ID_UNDEFINED)
-      .def(
-          "get_island", &PathFinder::getIsland<Magnum::Vector3>, "point"_a,
-          R"(Query the island closest to a point. Snaps the point to the NavMesh first, so check the snap distance also if unsure.)")
       .def(
-          "get_island", &PathFinder::getIsland<vec3f>, "point"_a,
+          "get_island", &PathFinder::getIsland, "point"_a,
           R"(Query the island closest to a point. Snaps the point to the NavMesh first, so check the snap distance also if unsure.)")
       .def(
           "island_radius",
-          [](PathFinder& self, const vec3f& pt) {
+          [](PathFinder& self, const Magnum::Vector3& pt) {
             return self.islandRadius(pt);
           },
           "pt"_a,

src/esp/geo/CoordinateFrame.cpp

@@ -6,6 +6,8 @@
 
 #include "esp/geo/Geo.h"
 
+#include <Magnum/Math/Matrix4.h>
+
 namespace esp {
 namespace geo {
 
@@ -21,9 +23,12 @@ CoordinateFrame::CoordinateFrame(const quatf& rotation,
     : CoordinateFrame(rotation * ESP_UP, rotation * ESP_FRONT, origin) {}
 
 quatf CoordinateFrame::rotationWorldToFrame() const {
-  const quatf R_frameUp_worldUp = quatf::FromTwoVectors(ESP_UP, up_);
-  return quatf::FromTwoVectors(R_frameUp_worldUp * ESP_FRONT, front_) *
-         R_frameUp_worldUp;
+  auto front = (Mn::Vector3(front_)).normalized();
+  auto right = Mn::Math::cross(Mn::Vector3(up_), front).normalized();
+  auto realUp = Mn::Math::cross(front, right);
+  auto myMat = Mn::Matrix4::from({front, right, realUp}, Mn::Vector3());
+  return Magnum::EigenIntegration::cast<quatf>(
+      Mn::Quaternion::fromMatrix(myMat.rotation()));
 }
 
 quatf CoordinateFrame::rotationFrameToWorld() const {

src/esp/geo/Geo.h

@@ -46,11 +46,11 @@ enum class ColorSpace {
 };
 
 //! global/world up direction
-static const vec3f ESP_UP = vec3f::UnitY();
+static const vec3f ESP_UP = vec3f::UnitZ();
 //! global/world gravity (down) direction
 static const vec3f ESP_GRAVITY = -ESP_UP;
 //! global/world front direction
-static const vec3f ESP_FRONT = -vec3f::UnitZ();
+static const vec3f ESP_FRONT = vec3f::UnitX();
 //! global/world back direction
 static const vec3f ESP_BACK = -ESP_FRONT;
 

src/esp/nav/GreedyFollower.cpp

@@ -39,8 +39,8 @@ GreedyGeodesicFollowerImpl::GreedyGeodesicFollowerImpl(
 
 float GreedyGeodesicFollowerImpl::geoDist(const Mn::Vector3& start,
                                           const Mn::Vector3& end) {
-  geoDistPath_.requestedStart = cast<vec3f>(start);
-  geoDistPath_.requestedEnd = cast<vec3f>(end);
+  geoDistPath_.requestedStart = start;
+  geoDistPath_.requestedEnd = end;
   pathfinder_->findPath(geoDistPath_);
   return geoDistPath_.geodesicDistance;
 }
@@ -56,7 +56,7 @@ GreedyGeodesicFollowerImpl::TryStepResult GreedyGeodesicFollowerImpl::tryStep(
 
   const float geoDistAfter = geoDist(newPose, end);
   const float distToObsAfter = pathfinder_->distanceToClosestObstacle(
-      cast<vec3f>(newPose), 1.1 * closeToObsThreshold_);
+      newPose, 1.1 * closeToObsThreshold_);
 
   return {geoDistAfter, distToObsAfter, didCollide};
 }
@@ -163,8 +163,8 @@ GreedyGeodesicFollowerImpl::CODES GreedyGeodesicFollowerImpl::nextActionAlong(
     const core::RigidState& start,
     const Mn::Vector3& end) {
   ShortestPath path;
-  path.requestedStart = cast<vec3f>(start.translation);
-  path.requestedEnd = cast<vec3f>(end);
+  path.requestedStart = start.translation;
+  path.requestedEnd = end;
   pathfinder_->findPath(path);
 
   CODES nextAction;
@@ -200,8 +200,8 @@ GreedyGeodesicFollowerImpl::findPath(const core::RigidState& start,
     core::RigidState state{findPathDummyNode_.rotation(),
                            findPathDummyNode_.MagnumObject::translation()};
     ShortestPath path;
-    path.requestedStart = cast<vec3f>(state.translation);
-    path.requestedEnd = cast<vec3f>(end);
+    path.requestedStart = state.translation;
+    path.requestedEnd = end;
     pathfinder_->findPath(path);
     const auto nextPrim = nextBestPrimAlong(state, path);
     if (nextPrim.empty()) {