Integrate murp mock robot and other changes for Simulator Process

examples/hitl/robot_teleop/README.md

@@ -1,9 +1,11 @@
 # Robot Tele-op HITL application
 
-TL;DR A HITL app that loads robots from URDF and simulates them in a Habitat Simulator instance with basic UI teleoperation and hot-reloading for quick morphology iteration.
+TL;DR A HITL app that loads robots from URDF and simulates them in a Habitat Simulator instance with basic UI teleoperation and hot-reloading for quick morphology iteration. In addition to data-collection and general debugging use cases, the app can also be run as the [Simulator Process](#simulator-process) for deployment in sim.
 
 # Build Steps
 
+Note: to run the robot teleop app as a [Simulator Process](#simulator-process), you must also [install the `murp` package](#installing-murp).
+
 ## Install habitat-sim
 
 Install the habitat-sim from source using the following steps:
@@ -167,7 +169,7 @@ python examples/hitl/robot_teleop/robot_teleop.py --config-name robot_teleop_vr.
 ```
 
 ## Habitat Quest Viewer
-Please run the latest build of Quest-Habitat Unity build on your Quest headset after launching habitat. The Headset and Laptop need to be connected to the same network without VPN. 
+Please run the latest build of Quest-Habitat Unity build on your Quest headset after launching habitat. The Headset and Laptop need to be connected to the same network without VPN.
 
 ## User Interface
 
@@ -184,3 +186,32 @@ This section describes teleoperating the robot. All commands are associated with
 - `0` on keyboard to change scenes.
 - `Y` *( if `use_cursor` is set to `True` in `robot_teleop_vr.yaml` )* : Object is loaded at the position the cursor is pointing at. The user can select which YCB object to add using the terminal. List of possible options that may be added can be modified in the `robot_teleop_vr.yaml`
 - `Y` *( if `use_cursor` is set to `False` in `robot_teleop_vr.yaml` )* : Objects are loaded in the scene at the defined positions inside yaml.
+
+# Simulator Process
+
+The robot teleop app can be run as our "Simulator Process" for [deployment in sim](https://github.com/fairinternal/murp/blob/smoke_test/DEPLOY_IN_SIM.md). If you haven't already, browse the rest of this readme to learn about the robot teleop app including [build steps](#build-steps). See also our [Workplace demo video](https://fb.workplace.com/groups/1643312812949607/permalink/1711217802825774/). If you're developing the Simulator Process, see also this `murp` mock API [example integration with a simulator](https://github.com/fairinternal/murp/blob/smoke_test/core/murp/murp/mock/README.md#example-integration-with-a-simulator).
+
+## Installing `murp`
+When running the robot teleop app as the Simulator Process, we require an additional dependency, the `murp` package, which isn't mentioned in the earlier [build steps](#build-steps). We've developed special [lightweight install instructions](https://github.com/fairinternal/murp/blob/smoke_test/DEPLOY_IN_SIM.md#how-should-i-install-ros-and-the-murp-package) for `murp` aimed at deployment in sim. We recommend creating a new conda/mamba env from scratch for `murp`, then proceed as follows:
+```
+# create murp_env as described at https://github.com/fairinternal/murp/blob/smoke_test/DEPLOY_IN_SIM.md
+# activate murp env
+mamba activate murp_env
+# install proper version of cmake (v4+ won't build habitat)
+mamba install cmake==3.31.6
+# we don't recommend the cmake Python package
+pip uninstall cmake
+# continue with Build Steps for the robot teleop app at top of this page
+```
+
+## Usage and Tips
+Use the following flags to run robot_teleop.py as the Simulator Process. Choose any convenient window size:
+```
+python examples/hitl/robot_teleop/robot_teleop.py habitat_hitl.enable_sim_driver_renderer=True robot_teleop.do_murp_mock_robot=True habitat_hitl.window.width=960 habitat_hitl.window.height=540
+```
+
+See our recommended [workflow](https://github.com/fairinternal/murp/blob/smoke_test/DEPLOY_IN_SIM.md#workflow) for deployment in sim.
+
+Once the Simulator Process is running, you can verify that it's sending and receiving ROS messages:
+1. Run [test_mobile_tmr_robot.py](https://github.com/fairinternal/murp/blob/smoke_test/core/murp/examples/test_mobile_tmr_robot.py) to randomly drive the robot.
+2. Use [Foxglove](https://github.com/fairinternal/murp/blob/smoke_test/DEPLOY_IN_SIM.md#foxglove-for-ros-visualization) to verify that it's publishing messages, e.g. [camera topics](https://github.com/fairinternal/murp/blob/smoke_test/core/murp/murp/mock/mock_camera_suite_topics.py).

examples/hitl/robot_teleop/mock_robot_helper.py

@@ -0,0 +1,201 @@
+#!/usr/bin/env python3
+
+# Copyright (c) Meta Platforms, Inc. and its affiliates.
+# This source code is licensed under the MIT license found in the
+# LICENSE file in the root directory of this source tree.
+
+from typing import Optional
+
+import magnum as mn
+import numpy as np
+from robot_camera_sensor_suite import RobotCameraSensorSuite
+
+from scripts.robot import Robot
+
+
+class MockRobotHelper:
+    def __init__(self, sim):
+        try:
+            from murp.mock.mock_mobile_tmr_robot import MockMobileTMRRobot
+        except ImportError as e:
+            raise ImportError(
+                f"Failed to import murp.mock.mock_mobile_tmr_robot. Did you install the `murp` package? See examples/hitl/robot_teleop/README.md Simulator Process. Raw import error: {e}"
+            )
+        self._murp_mock_robot = MockMobileTMRRobot(do_synthesize_images=False)
+        self._hitl_robot: Optional[Robot] = None
+        self._sim = sim
+
+        self._robot_camera_sensor_suite: Optional[
+            RobotCameraSensorSuite
+        ] = None
+
+    def set_hitl_robot(self, hitl_robot: Robot, robot_cfg):
+        self._hitl_robot = hitl_robot
+
+        if self._robot_camera_sensor_suite:
+            self._robot_camera_sensor_suite.close()
+        if "camera_sensors" in robot_cfg:
+            self._robot_camera_sensor_suite = RobotCameraSensorSuite(
+                self._sim, hitl_robot.ao, robot_cfg["camera_sensors"]
+            )
+
+    def draw_debug(self, dblr):
+        if self._robot_camera_sensor_suite:
+            self._robot_camera_sensor_suite.draw_debug(dblr)
+
+    def update_pre_sim_step(self, dt):
+        if not self._murp_mock_robot or not self._hitl_robot:
+            return
+
+        self._murp_mock_robot.poll_for_messages()
+
+        if True:  # base linear and angular vel
+            base_vel = self._murp_mock_robot.base.get_commanded_velocity()
+            assert base_vel[1] == 0.0
+
+            start = self._hitl_robot.ao.translation
+            end = mn.Vector3(start)
+
+            end = end + self._hitl_robot.ao.transformation.transform_vector(
+                mn.Vector3(base_vel[0] * dt, 0, 0)
+            )
+
+            r = mn.Quaternion.rotation(
+                mn.Rad(base_vel[2] * dt), mn.Vector3(0, 1, 0)
+            )
+            self._hitl_robot.ao.rotation = r * self._hitl_robot.ao.rotation
+
+            if start != end:
+                self._hitl_robot.ao.translation = (
+                    self._sim.pathfinder.try_step(start, end)
+                )
+
+        motor_ids = []
+        commanded_positions = np.array([], dtype=np.float32)
+
+        # convention for murp: index, middle, pinky, thumb
+        # convention for robot_settings.xml: should now be the same
+
+        if self._hitl_robot.using_joint_motors:
+            for hand_idx in range(2):
+                joint_motor_lists = self._hitl_robot.pos_subsets[
+                    "left_hand" if hand_idx == 0 else "right_hand"
+                ].joint_motors
+                for motor_list in joint_motor_lists:
+                    assert len(motor_list) == 1
+                    motor_ids.append(motor_list[0])
+                commanded_positions = np.append(
+                    commanded_positions,
+                    (
+                        self._murp_mock_robot.left_hand
+                        if hand_idx == 0
+                        else self._murp_mock_robot.right_hand
+                    ).commanded_positions,
+                )
+                assert len(motor_ids) == len(commanded_positions)
+
+            for arm_idx in range(2):
+                joint_motor_lists = self._hitl_robot.pos_subsets[
+                    "left_arm" if arm_idx == 0 else "right_arm"
+                ].joint_motors
+                for motor_list in joint_motor_lists:
+                    assert len(motor_list) == 1
+                    motor_ids.append(motor_list[0])
+                commanded_positions = np.append(
+                    commanded_positions,
+                    (
+                        self._murp_mock_robot.left_arm
+                        if arm_idx == 0
+                        else self._murp_mock_robot.right_arm
+                    ).get_target_joint_positions(),
+                )
+                assert len(motor_ids) == len(commanded_positions)
+
+            for motor_id, commanded_pos in zip(motor_ids, commanded_positions):
+                jms = self._hitl_robot.ao.get_joint_motor_settings(motor_id)
+                jms.position_target = commanded_pos
+                self._hitl_robot.ao.update_joint_motor(motor_id, jms)
+        else:
+            # directly set joint positions
+            curr_robot_joint_positions = self._hitl_robot.ao.joint_positions
+
+            for hand_idx in range(2):
+                commanded_positions = (
+                    self._murp_mock_robot.left_hand
+                    if hand_idx == 0
+                    else self._murp_mock_robot.right_hand
+                ).commanded_positions
+                link_ixs = self._hitl_robot.pos_subsets[
+                    "left_hand" if hand_idx == 0 else "right_hand"
+                ].link_ixs
+                for i, link_ix in enumerate(link_ixs):
+                    dof = self._hitl_robot.ao.get_link_joint_pos_offset(
+                        link_ix
+                    )
+                    curr_robot_joint_positions[dof] = commanded_positions[i]
+
+            for arm_idx in range(2):
+                commanded_positions = (
+                    self._murp_mock_robot.left_arm
+                    if arm_idx == 0
+                    else self._murp_mock_robot.right_arm
+                ).get_target_joint_positions()
+                link_ixs = self._hitl_robot.pos_subsets[
+                    "left_arm" if arm_idx == 0 else "right_arm"
+                ].link_ixs
+                for i, link_ix in enumerate(link_ixs):
+                    dof = self._hitl_robot.ao.get_link_joint_pos_offset(
+                        link_ix
+                    )
+                    curr_robot_joint_positions[dof] = commanded_positions[i]
+
+            self._hitl_robot.ao.joint_positions = curr_robot_joint_positions
+
+    def update_post_sim_step(self, post_sim_update_dict):
+        if not self._murp_mock_robot or not self._hitl_robot:
+            return
+
+        if True:  # base
+            base_xyz = self._hitl_robot.ao.translation
+
+            mat = self._hitl_robot.ao.transformation
+            yaw = np.arctan2(mat[2][0], mat[0][0])
+            self._murp_mock_robot.base.set_pose(base_xyz.x, base_xyz.z, yaw)
+
+        curr_robot_joint_positions = self._hitl_robot.ao.joint_positions
+
+        for hand_idx in range(2):
+            link_ixs = self._hitl_robot.pos_subsets[
+                "left_hand" if hand_idx == 0 else "right_hand"
+            ].link_ixs
+            curr_joint_positions = np.zeros(len(link_ixs), dtype=np.float32)
+            for i, link_ix in enumerate(link_ixs):
+                dof = self._hitl_robot.ao.get_link_joint_pos_offset(link_ix)
+                curr_joint_positions[i] = curr_robot_joint_positions[dof]
+            mock_hand = (
+                self._murp_mock_robot.left_hand
+                if hand_idx == 0
+                else self._murp_mock_robot.right_hand
+            )
+            mock_hand.set_joint_state(curr_joint_positions)
+
+        for arm_idx in range(2):
+            link_ixs = self._hitl_robot.pos_subsets[
+                "left_arm" if arm_idx == 0 else "right_arm"
+            ].link_ixs
+            curr_joint_positions = np.zeros(len(link_ixs), dtype=np.float32)
+            for i, link_ix in enumerate(link_ixs):
+                dof = self._hitl_robot.ao.get_link_joint_pos_offset(link_ix)
+                curr_joint_positions[i] = curr_robot_joint_positions[dof]
+            mock_arm = (
+                self._murp_mock_robot.left_arm
+                if arm_idx == 0
+                else self._murp_mock_robot.right_arm
+            )
+            mock_arm.set_current_joint_positions(curr_joint_positions)
+
+        self._murp_mock_robot.publish_proprioception_state()
+
+        self._robot_camera_sensor_suite.draw_and_publish_observations(
+            self._murp_mock_robot.camera_suite
+        )