Fairmotion Character Positioning via MOTION MouseMode

examples/fairmotion_interface.py

@@ -4,6 +4,7 @@
 
 import json
 import os
+import time
 from enum import Enum
 from typing import Any, Dict, List, Optional, Tuple
 
@@ -12,9 +13,8 @@
 from fairmotion.data import amass
 from fairmotion.ops import conversions
 
-import habitat_sim
+import habitat_sim.physics as phy
 from habitat_sim.logging import LoggingContext, logger
-from habitat_sim.physics import JointType
 
 #### Constants
 ROOT = 0
@@ -45,8 +45,9 @@ def __init__(
         LoggingContext.reinitialize_from_env()
         self.sim = sim
         self.art_obj_mgr = self.sim.get_articulated_object_manager()
-        self.model: habitat_sim.physics.ManagedArticulatedObject = None
-        self.motion: motion.Motion = None
+        self.rgd_obj_mgr = self.sim.get_rigid_object_manager()
+        self.model: Optional[phy.ManagedArticulatedObject] = None
+        self.motion: Optional[motion.Motion] = None
         self.metadata = {}
         self.metadata_dir = metadata_dir or METADATA_DIR
         self.motion_stepper = 0
@@ -181,6 +182,25 @@ def fetch_metadata(self, name):
 
         self.metadata[name] = self.metadata_parser(data[name], to_file=False)
 
+    def set_transform_offsets(
+        self, rotate_offset: mn.Quaternion = None, translate_offset: mn.Vector3 = None
+    ) -> None:
+        """
+        This method updates the offset of the model with the positional data passed to it.
+        Use this for changing the location and orientation of the model.
+        """
+        self.rotation_offset = rotate_offset or self.rotation_offset
+        self.translation_offset = translate_offset or self.translation_offset
+        if self.traj_ids:
+            # removes trajectory
+            for t_id in self.traj_ids:
+                self.sim.get_rigid_object_manager().remove_object_by_id(t_id)
+                self.traj_ids = []
+        self.next_pose(repeat=True)
+        self.setup_key_frames()
+        for _ in range(len(Preview)):
+            self.cycle_model_previews()
+
     def load_motion(self) -> None:
         """
         Loads the motion currently set by metadata.
@@ -204,7 +224,7 @@ def load_model(self) -> None:
         assert self.model.is_alive
 
         # change motion_type to KINEMATIC
-        self.model.motion_type = habitat_sim.physics.MotionType.KINEMATIC
+        self.model.motion_type = phy.MotionType.KINEMATIC
 
         self.model.translation = self.translation_offset
         self.next_pose()
@@ -224,7 +244,6 @@ def next_pose(self, repeat=False) -> None:
         Set the model state from the next frame in the motion trajectory. `repeat` is
         set to `True` when the user would like to repeat the last frame.
         """
-
         # This function tracks is_reversed and changes the direction of
         # the motion accordingly.
         def sign(i):
@@ -285,7 +304,7 @@ def convert_CMUamass_single_pose(
             pose_joint_index = pose.skel.index_joint[joint_name]
 
             # When the target joint do not have dof, we simply ignore it
-            if joint_type == JointType.Fixed:
+            if joint_type == phy.JointType.Fixed:
                 continue
 
             # When there is no matching between the given pose and the simulated character,
@@ -294,13 +313,13 @@ def convert_CMUamass_single_pose(
                 raise KeyError(
                     "Error: pose data does not have a transform for that joint name"
                 )
-            elif joint_type not in [JointType.Spherical]:
+            elif joint_type not in [phy.JointType.Spherical]:
                 raise NotImplementedError(
                     f"Error: {joint_type} is not a supported joint type"
                 )
             else:
                 T = pose.get_transform(pose_joint_index, local=True)
-                if joint_type == JointType.Spherical:
+                if joint_type == phy.JointType.Spherical:
                     Q, _ = conversions.T2Qp(T)
 
             new_pose += list(Q)
@@ -361,9 +380,7 @@ def toggle_key_frames(self) -> None:
                     new_root_rotation,
                 ) = self.convert_CMUamass_single_pose(k, self.key_frame_models[-1])
 
-                self.key_frame_models[
-                    -1
-                ].motion_type = habitat_sim.physics.MotionType.KINEMATIC
+                self.key_frame_models[-1].motion_type = phy.MotionType.KINEMATIC
                 self.key_frame_models[-1].joint_positions = new_pose
                 self.key_frame_models[-1].rotation = new_root_rotation
                 self.key_frame_models[-1].translation = new_root_translate
@@ -435,23 +452,19 @@ def define_preview_points(joint_names: List[str]) -> List[mn.Vector3]:
 
         # TODO: This function is not working. It is supposed to produce a gradient
         #       from RED to YELLOW to GREEN but it is producing a black solely
-        colors = [
-            mn.Color3(255, 0, 0),
-            mn.Color3(255, 255, 0),
-            mn.Color3(0, 255, 0),
-        ]
+        colors = [mn.Color3(255, 0, 0), mn.Color3(255, 255, 0), mn.Color3(0, 255, 0)]
 
         if self.preview_mode in [Preview.TRAJECTORY, Preview.ALL]:
             if not self.traj_ids:
                 for i, p in enumerate(points_to_preview):
                     self.traj_ids.append(
                         self.sim.add_gradient_trajectory_object(
-                            traj_vis_name=f"{joint_names[i]}",
+                            traj_vis_name=f"{joint_names[i]}{int(time.time() * 1000)}",
                             colors=colors,
                             points=p,
                             num_segments=3,
                             radius=traj_radius,
-                            smooth=False,
+                            smooth=True,
                             num_interpolations=10,
                         )
                     )
@@ -470,6 +483,32 @@ def cycle_model_previews(self) -> None:
         self.toggle_key_frames()
         self.build_trajectory_vis()
 
+    def belongs_to(self, obj_id: int) -> bool:
+        """
+        Accepts an object id and returns True if the obj_id belongs to an object
+        owned by this Fairmotion character.
+        """
+        # checking our model links
+        if self.model and obj_id in self.model.link_object_ids:
+            return True
+
+        # checking our model
+        if self.model and obj_id is self.model.object_id:
+            return True
+
+        # checking all key frame models
+        if any(
+            obj_id in ko_ids
+            for ko_ids in [i.link_object_ids.keys() for i in self.key_frame_models]
+        ):
+            return True
+
+        # checking all key frame models
+        if any(obj_id == to for to in self.traj_ids):
+            return True
+
+        return False
+
 
 class Preview(Enum):
     OFF = 0

examples/motion_viewer.py

@@ -9,9 +9,11 @@
 flags = sys.getdlopenflags()
 sys.setdlopenflags(flags | ctypes.RTLD_GLOBAL)
 
+import magnum as mn
 from magnum.platform.glfw import Application
-from viewer import HabitatSimInteractiveViewer
+from viewer import HabitatSimInteractiveViewer, MouseMode
 
+import habitat_sim.physics as phy
 from examples.fairmotion_interface import FairmotionInterface
 from examples.settings import default_sim_settings
 from habitat_sim.logging import logger
@@ -31,6 +33,29 @@ def __init__(
             metadata_dir=fm_settings["metadata_dir"],
         )
 
+        # configuring MOTION display objects
+        # selection sphere icon
+        obj_tmp_mgr = self.sim.get_object_template_manager()
+        self.sphere_template_id = obj_tmp_mgr.load_configs(
+            "../habitat-sim/data/test_assets/objects/sphere"
+        )[0]
+        sphere_template = obj_tmp_mgr.get_template_by_id(self.sphere_template_id)
+        sphere_template.scale = [0.30, 0.30, 0.30]
+        obj_tmp_mgr.register_template(sphere_template)
+
+        # selection origin box
+        self.box_template_id = obj_tmp_mgr.load_configs(
+            "../habitat-sim/data/test_assets/objects/nested_box"
+        )[0]
+        box_template = obj_tmp_mgr.get_template_by_id(self.box_template_id)
+        box_template.scale = [0.15, 0.025, 2.5]
+        obj_tmp_mgr.register_template(box_template)
+
+        # motion mode attributes
+        self.selected_mocap_char: Optional[FairmotionInterface] = None
+        self.select_sphere_obj_id: int = -1
+        self.select_box_obj_id: int = -1
+
     def draw_event(self, simulation_call: Optional[Callable] = None) -> None:
         """
         Calls continuously to re-render frames and swap the two frame buffers
@@ -58,6 +83,7 @@ def key_press_event(self, event: Application.KeyEvent) -> None:
 
         if key == pressed.F:
             if event.modifiers == mod.SHIFT:
+                self.remove_selector_obj()
                 self.fm_demo.hide_model()
                 logger.info("Command: hide model")
             else:
@@ -74,17 +100,163 @@ def key_press_event(self, event: Application.KeyEvent) -> None:
 
         elif key == pressed.M:
             # cycle through mouse modes
-            super().cycle_mouse_mode()
+            if self.mouse_interaction == MouseMode.MOTION:
+                self.remove_selector_obj()
+            self.cycle_mouse_mode()
             logger.info(f"Command: mouse mode set to {self.mouse_interaction}")
             return
 
         elif key == pressed.R:
+            self.remove_selector_obj()
             super().reconfigure_sim()
             self.fm_demo = FairmotionInterface(self, metadata_name="fm_demo")
             logger.info("Command: simulator re-loaded")
 
+        elif key == pressed.SPACE:
+            if not self.sim.config.sim_cfg.enable_physics:
+                logger.warn("Warning: physics was not enabled during setup")
+            else:
+                self.simulating = not self.simulating
+                logger.info(f"Command: physics simulating set to {self.simulating}")
+            if self.simulating:
+                self.remove_selector_obj()
+            return
+
+        elif key == pressed.PERIOD:
+            if self.simulating:
+                logger.warn("Warning: physic simulation already running")
+            else:
+                self.simulate_single_step = True
+                logger.info("Command: physics step taken")
+                self.remove_selector_obj()
+            return
+
         super().key_press_event(event)
 
+    def mouse_press_event(self, event: Application.MouseEvent) -> None:
+        """
+        Handles `Application.MouseEvent`. When in GRAB mode, click on
+        objects to drag their position. (right-click for fixed constraints).
+        When in MOTION mode select Fairmotion characters with left-click,
+        place them in a new location with right-click.
+        """
+        button = Application.MouseEvent.Button
+        physics_enabled = self.sim.get_physics_simulation_library()
+
+        # if interactive mode is True -> MOTION MODE
+        if self.mouse_interaction == MouseMode.MOTION and physics_enabled:
+            render_camera = self.render_camera.render_camera
+            ray = render_camera.unproject(self.get_mouse_position(event.position))
+            raycast_results = self.sim.cast_ray(ray=ray)
+
+            if raycast_results.has_hits():
+                hit_info = raycast_results.hits[0]
+
+                if event.button == button.LEFT:
+                    if self.fm_demo.belongs_to(hit_info.object_id):
+                        if not self.fm_demo.model:
+                            self.fm_demo.load_model()
+                        self.simulating = False
+                        self.create_selector_obj(self.fm_demo)
+                    else:
+                        self.remove_selector_obj()
+
+                elif event.button == button.RIGHT and self.selected_mocap_char:
+                    point = hit_info.point
+                    self.fm_demo.set_transform_offsets(translate_offset=point)
+                    self.create_selector_obj(self.fm_demo)
+            # end has raycast hit
+
+        super().mouse_press_event(event)
+
+    def mouse_scroll_event(self, event: Application.MouseScrollEvent) -> None:
+        """
+        Handles `Application.MouseScrollEvent`. When in LOOK mode, enables camera
+        zooming (fine-grained zoom using shift). When in GRAB mode, adjusts the depth
+        of the grabber's object. (larger depth change rate using shift). When in MOTION
+        mode, rotate them about the floor-normal axis with the scroll wheel. (fine-grained
+        rotate using shift).
+        """
+        if self.mouse_interaction == MouseMode.MOTION and self.selected_mocap_char:
+            physics_enabled = self.sim.get_physics_simulation_library()
+
+            scroll_mod_val = (
+                event.offset.y
+                if abs(event.offset.y) > abs(event.offset.x)
+                else event.offset.x
+            )
+
+            if not scroll_mod_val:
+                return
+
+            # use shift to scale action response
+            shift_pressed = event.modifiers == Application.InputEvent.Modifier.SHIFT
+
+            if (
+                self.mouse_interaction == MouseMode.MOTION
+                and physics_enabled
+                and self.selected_mocap_char
+            ):
+                delta = mn.Quaternion.rotation(
+                    mn.Deg(scroll_mod_val * (1 if shift_pressed else 20)),
+                    mn.Vector3.z_axis(),
+                )
+                self.fm_demo.set_transform_offsets(
+                    rotate_offset=self.fm_demo.rotation_offset * delta
+                )
+            self.create_selector_obj(self.fm_demo)
+
+        super().mouse_scroll_event(event)
+
+    def cycle_mouse_mode(self):
+        """
+        Cycles through mouse modes that belong to the MouseMode emun.
+        """
+        self.mouse_interaction = MouseMode(
+            (self.mouse_interaction.value + 1) % len(MouseMode)
+        )
+
+    def create_selector_obj(self, mocap_char: FairmotionInterface):
+        """
+        Creates the selection icon above the given fairmotion character.
+        """
+        self.remove_selector_obj()
+
+        # selection sphere icon
+        obj = mocap_char.rgd_obj_mgr.add_object_by_template_id(self.sphere_template_id)
+        obj.collidable = False
+        obj.motion_type = phy.MotionType.KINEMATIC
+        obj.translation = mocap_char.model.translation + mn.Vector3(0, 1.10, 0)
+        self.select_sphere_obj_id = obj.object_id
+
+        # selection origin box
+        obj = mocap_char.rgd_obj_mgr.add_object_by_template_id(self.box_template_id)
+        obj.collidable = False
+        obj.motion_type = phy.MotionType.KINEMATIC
+        obj.translation = mocap_char.translation_offset + mn.Vector3(0, 0.8, 0)
+        obj.rotation = mocap_char.rotation_offset
+        self.select_box_obj_id = obj.object_id
+
+        self.selected_mocap_char = mocap_char
+
+    def remove_selector_obj(self):
+        """
+        Removes the selection icon from the sim to indicate de-selection.
+        """
+        manager = self.sim.get_rigid_object_manager()
+
+        # selection sphere icon
+        if self.select_sphere_obj_id != -1:
+            manager.remove_object_by_id(self.select_sphere_obj_id)
+            self.select_sphere_obj_id = -1
+
+        # selection origin box
+        if self.select_box_obj_id != -1:
+            manager.remove_object_by_id(self.select_box_obj_id)
+            self.select_box_obj_id = -1
+
+        self.selected_mocap_char = None
+
     def print_help_text(self) -> None:
         """
         Print the Key Command help text.