Fairmotion Character Positioning via MOTION MouseMode

examples/fairmotion_interface.py

@@ -12,9 +12,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,7 +44,8 @@ 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.rgd_obj_mgr = self.sim.get_rigid_object_manager()
+        self.model: phy.ManagedArticulatedObject = None
         self.motion: motion.Motion = None
         self.metadata = {}
         self.metadata_dir = metadata_dir or METADATA_DIR
@@ -204,7 +204,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 +224,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 +284,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 +293,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 +360,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,11 +432,7 @@ 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:
@@ -451,7 +444,7 @@ def define_preview_points(joint_names: List[str]) -> List[mn.Vector3]:
                             points=p,
                             num_segments=3,
                             radius=traj_radius,
-                            smooth=False,
+                            smooth=True,
                             num_interpolations=10,
                         )
                     )
@@ -470,6 +463,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,14 @@ def __init__(
             metadata_dir=fm_settings["metadata_dir"],
         )
 
+        # motion mode attributes
+        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]
+        self.selected_mocap_char: Optional[FairmotionInterface] = None
+        self.select_icon_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
@@ -74,7 +84,9 @@ 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
 
@@ -83,8 +95,94 @@ def key_press_event(self, event: Application.KeyEvent) -> None:
             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)
+        """
+        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()
+
+                        # if hit_object >= 0:
+                # end if didn't hit the scene
+            # end has raycast hit
+
+        super().mouse_press_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, recreate: bool = False
+    ):
+        """
+        Creates the selection icon above the given fairmotion character.
+        """
+        self.remove_selector_obj()
+        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.20, 0)
+
+        self.select_icon_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.
+        """
+        if self.select_icon_obj_id == -1:
+            self.selected_mocap_char = None
+            return
+        manager = self.sim.get_rigid_object_manager()
+        manager.remove_object_by_id(self.select_icon_obj_id)
+        self.selected_mocap_char = None
+        self.select_icon_obj_id = -1
+
     def print_help_text(self) -> None:
         """
         Print the Key Command help text.

examples/viewer.py

@@ -483,7 +483,7 @@ def update_grab_position(self, point: mn.Vector2i) -> None:
         )
         self.mouse_grabber.update_transform(mn.Matrix4.from_(rotation, translation))
 
-    def get_mouse_position(self, mouse_event_position: mn.Vector2i) -> None:
+    def get_mouse_position(self, mouse_event_position: mn.Vector2i) -> mn.Vector2i:
         """
         This function will get a screen-space mouse position appropriately
         scaled based on framebuffer size and window size.  Generally these would be