diff --git a/examples/hitl/robot_teleop/README.md b/examples/hitl/robot_teleop/README.md index 7661b4411b..9db8491cba 100644 --- a/examples/hitl/robot_teleop/README.md +++ b/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). diff --git a/examples/hitl/robot_teleop/mock_robot_helper.py b/examples/hitl/robot_teleop/mock_robot_helper.py new file mode 100644 index 0000000000..243097b133 --- /dev/null +++ b/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 + ) diff --git a/examples/hitl/robot_teleop/robot_camera_sensor_suite.py b/examples/hitl/robot_teleop/robot_camera_sensor_suite.py new file mode 100644 index 0000000000..6950c78f5a --- /dev/null +++ b/examples/hitl/robot_teleop/robot_camera_sensor_suite.py @@ -0,0 +1,191 @@ +#!/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 dataclasses import dataclass +from typing import Any, Optional + +import magnum as mn + +import habitat_sim +from habitat_hitl.core.hydra_utils import omegaconf_to_object + + +@dataclass +class SensorCacheEntry: + publish_topic: Optional[str] = None + sim_sensor: Optional[Any] = None + recent_obs = None + + +class RobotCameraSensorSuite: + def __init__(self, sim, robot_ao, camera_sensors_config): + self.sim = sim + if self.sim.renderer is None: + raise RuntimeError( + "RobotCameraSensorSuite requires a sim with a renderer. See hitl_defaults.yaml enable_sim_driver_renderer." + ) + self._robot_ao = robot_ao + self._configs = omegaconf_to_object(camera_sensors_config) + # quick sanity check that we have the right kind of camera_sensors config + assert ( + isinstance(self._configs, list) + and len(self._configs) + and hasattr(self._configs[0], "sensor_uuid") + ) + self._equirect = False + self.clear_color = mn.Color4.from_linear_rgb_int(0) + self.agent: habitat_sim.simulator.Agent = None + self.agent_id = 0 + + self.sensor_cache = {} + for config in self._configs: + self.sensor_cache[config.sensor_uuid] = SensorCacheEntry() + + self._create_agent_and_sensors() + + from murp.mock.mock_camera_suite_topics import MockCameraSuiteTopics + + self.topics_by_sensor = {} + for config in self._configs: + topic = MockCameraSuiteTopics.find_full_topic(config.sensor_uuid) + self.sensor_cache[config.sensor_uuid].publish_topic = topic + + def close(self): + # NOTE: this guards against cases where the Simulator is deconstructed before the DBV + if self.agent_id < len(self.sim.agents): + # remove the agent and sensor from the Simulator instance + self.agent.close() + del self.sim._Simulator__sensors[self.agent_id] + del self.sim.agents[self.agent_id] + + self.agent = None + self.agent_id = 0 + self.sensors = None + + def _create_sensor_spec_from_config(self, config): + debug_sensor_spec = ( + habitat_sim.CameraSensorSpec() + if not self._equirect + else habitat_sim.EquirectangularSensorSpec() + ) + debug_sensor_spec.sensor_type = ( + habitat_sim.SensorType.COLOR + if config.sensor_type == "color" + else habitat_sim.SensorType.DEPTH + ) + debug_sensor_spec.position = [0.0, 0.0, 0.0] + debug_sensor_spec.resolution = [ + config.resolution[0], + config.resolution[1], + ] + debug_sensor_spec.uuid = config.sensor_uuid + debug_sensor_spec.clear_color = self.clear_color + debug_sensor_spec.hfov = config.hfov + + return debug_sensor_spec + + def _create_agent_and_sensors(self): + sensor_specifications = [] + for config in self._configs: + sensor_specifications.append( + self._create_sensor_spec_from_config(config) + ) + + debug_agent_config = habitat_sim.agent.AgentConfiguration() + debug_agent_config.sensor_specifications = sensor_specifications + self.sim.agents.append( + habitat_sim.Agent( + self.sim.get_active_scene_graph() + .get_root_node() + .create_child(), + debug_agent_config, + ) + ) + self.agent = self.sim.agents[-1] + self.agent_id = len(self.sim.agents) - 1 + self.sim._Simulator__sensors.append({}) + self.sensors = {} + for config in self._configs: + self.sim._update_simulator_sensors( + config.sensor_uuid, self.agent_id + ) + self.sensor_cache[ + config.sensor_uuid + ].sim_sensor = self.sim._Simulator__sensors[self.agent_id][ + config.sensor_uuid + ] + + def _update_sensor_transforms(self): + # this should be identity + inv_T = self.agent.scene_node.transformation.inverted() + + for config in self._configs: + link_trans = self._robot_ao.get_link_scene_node( + config.attached_link_id + ).transformation + + pos = mn.Vector3(config.cam_offset_pos) + ori = mn.Vector3(config.cam_orientation) + Mt = mn.Matrix4.translation(pos) + Mz = mn.Matrix4.rotation_z(mn.Rad(ori[2])) + My = mn.Matrix4.rotation_y(mn.Rad(ori[1])) + Mx = mn.Matrix4.rotation_x(mn.Rad(ori[0])) + cam_transform = Mt @ Mz @ My @ Mx + + cam_info_relative_transform = mn.Matrix4.rotation_z(mn.Deg(-90)) + + cam_transform = ( + link_trans @ cam_transform @ cam_info_relative_transform + ) + # todo: assert inv_T is identity and then remove this line + cam_transform = inv_T @ cam_transform + + sim_sensor = self.sensor_cache[config.sensor_uuid].sim_sensor + # sim_sensor._sensor_object.node + + from habitat_sim.utils.common import orthonormalize_rotation_shear + + sim_sensor._sensor_object.node.transformation = ( + orthonormalize_rotation_shear(cam_transform) + ) + + def draw_debug(self, gui_drawer): + for config in self._configs: + sim_sensor = self.sensor_cache[config.sensor_uuid].sim_sensor + gui_drawer.draw_axes( + sim_sensor._sensor_object.node.transformation, scale=0.5 + ) + + # todo: rename to convey that this does drawing/rendering + def _draw_observations( + self, + ): + assert self.sensors is not None + + self._update_sensor_transforms() + + for config in self._configs: + entry = self.sensor_cache[config.sensor_uuid] + sensor = entry.sim_sensor + sensor.draw_observation() + entry.recent_obs = sensor.get_observation() + + def get_recent_observations(self): + observations: list[Any] = [] + for config in self._configs: + entry = self.sensor_cache[config.sensor_uuid] + observations.append(entry.recent_obs) + return observations + + def draw_and_publish_observations(self, mock_camera_suite): + self._draw_observations() + + suite = mock_camera_suite + for config in self._configs: + entry = self.sensor_cache[config.sensor_uuid] + suite.publish_image_rgb_or_depth( + entry.publish_topic, entry.recent_obs + ) diff --git a/examples/hitl/robot_teleop/robot_settings.yaml b/examples/hitl/robot_teleop/robot_settings.yaml index 01a2d46440..47088f8e80 100644 --- a/examples/hitl/robot_teleop/robot_settings.yaml +++ b/examples/hitl/robot_teleop/robot_settings.yaml @@ -17,7 +17,7 @@ navmesh_height: 1.2 viewpoint_offset: [0.25,1.2,0] #joint motor settings -create_joint_motors: True +create_joint_motors: False # temp turn off joint motors and physics because hand joint control isn't working very well joint_motor_pos_gains: 0.05 joint_motor_vel_gains: 0.25 joint_motor_max_impulse: 1.0 @@ -25,9 +25,9 @@ joint_motor_max_impulse: 1.0 configuration_subsets: #define the hand link indices #NOTE: we assume these are in the same order such that setting a symmetric pose is straightforward - #[thumb(4), index(4), middle(4), ring(4)] - left_hand: [26,27,28,29, 44,45,46,47, 32,33,34,35, 38,39,40,41] - right_hand: [67,68,69,70, 61,62,63,64, 73,74,75,76, 79,80,81,82] + #[index(4), middle(4), pinky(4), thumb(4)] # allegro finger convention used by murp library + left_hand: [44,45,46,47, 38,39,40,41, 26,27,28,29, 32,33,34,35] + right_hand: [61,62,63,64, 73,74,75,76, 79,80,81,82, 67,68,69,70] #define arm link indices left_arm: [17,18,19,20,21,22,23] right_arm: [52,53,54,55,56,57,58] @@ -43,6 +43,35 @@ link_subsets: left_arm_base: [15] right_arm_base: [50] +camera_sensors: + # when publishing via ROS, sensor_uuid will be used to find the full ROS topic name; see also https://github.com/fairinternal/murp/blob/smoke_test/core/murp/murp/mock/mock_camera_suite_topics.py . + - sensor_uuid: zed_multi/torso/rgb + # for resolution and hfov, see https://docs.google.com/document/d/1WI0YFor_FOXcGujfm3B7uERAv0VWoyhGRkOFZPs2WRQ/edit?tab=t.k9j45snkmy2h + # hfov must be derived from intrinsic matrix above + resolution: [600, 960] + hfov: 105.5 + sensor_type: "color" + attached_link_id: 14 # 14==torso + cam_offset_pos: [0.4, 0.0, 0.62] # approximate + cam_orientation: [3.14159, 1.571, 0.0] # look forward + + - sensor_uuid: zed_multi/torso/depth + # for resolution and hfov, see https://docs.google.com/document/d/1WI0YFor_FOXcGujfm3B7uERAv0VWoyhGRkOFZPs2WRQ/edit?tab=t.k9j45snkmy2h + # hfov must be derived from intrinsic matrix above + resolution: [600, 960] + hfov: 105.5 + sensor_type: "depth" + attached_link_id: 14 # 14==torso + cam_offset_pos: [0.4, 0.0, 0.62] # approximate + cam_orientation: [3.14159, 1.571, 0.0] # look forward + + - sensor_uuid: zed_multi/head/rgb + resolution: [600, 960] + hfov: 105.5 + sensor_type: "color" + attached_link_id: 14 # 14==torso + cam_offset_pos: [0.4, 0.0, 0.9] # approximate + cam_orientation: [3.14159, 1.571, 0.0] # look forward

 # todo: add other camera sensors! See https://github.com/fairinternal/murp/blob/smoke_test/core/murp/murp/mock/mock_camera_suite_topics.py . #initial pose options. This refers to an entry in the robot_poses.json file. diff --git a/examples/hitl/robot_teleop/robot_teleop.py b/examples/hitl/robot_teleop/robot_teleop.py index 132017afd3..2ddeb043a3 100644 --- a/examples/hitl/robot_teleop/robot_teleop.py +++ b/examples/hitl/robot_teleop/robot_teleop.py @@ -11,6 +11,7 @@ import hydra import magnum as mn from hydra import compose +from mock_robot_helper import MockRobotHelper from omegaconf import DictConfig import habitat.sims.habitat_simulator.sim_utilities as sutils @@ -239,6 +240,11 @@ def __init__(self, app_service: AppService): List[Tuple[mn.Vector3, mn.Vector3]] ] = None + if self._app_cfg.do_murp_mock_robot: + self._mock_robot_helper = MockRobotHelper(self._sim) + else: + self._mock_robot_helper = None + # setup the simulator self.set_scene(self._current_scene_index) @@ -254,7 +260,7 @@ def __init__(self, app_service: AppService): self.xr_traj.load_json("test_xr_pose.json") self.sync_xr_local_state(self.xr_traj.get_pose(0)) - self._sps_tracker = AverageRateTracker(2.0) + self._sps_tracker = AverageRateTracker(0.5) self._do_pause_physics = False self._app_service.users.activate_user(0) @@ -425,6 +431,9 @@ def import_robot(self) -> None: ) for hand_subset_key in ["hand_open", "grasp"] ] + + if self._mock_robot_helper: + self._mock_robot_helper.set_hitl_robot(self.robot, robot_cfg) else: print("No robot configured.") @@ -473,13 +482,20 @@ def _update_help_text(self) -> None: if self._hide_gui: return + base_xyz = self.robot.ao.translation help_text = ( "Controls:\n" + " 'WASD' to translate laterally and 'ZX' to move up|down.\n" + " hold 'R' and move the mouse to rotate camera and mouse wheel to zoom.\n" + " '0' to change scene.\n" + + f"Pos: {base_xyz.x:.1f}, {base_xyz.z:.1f}\n" ) + if self._sps_tracker.get_smoothed_rate(): + help_text += ( + f"{self._sps_tracker.get_smoothed_rate():.1f} steps/sec\n" + ) + # show some details about hits under the cursor cursor_cast_results_text = "\nCursor RayCast: " if ( @@ -802,18 +818,22 @@ def handle_keys( ) if gui_input.get_key_down(KeyCode.Y): - if self._app_cfg.ycb_objects.use_cursor: # insert an object at the mouse raycast position. Ask for the index position of object. - idx = input( - "Enter the index of the object to add 0 - " + str(len(self._app_cfg.ycb_objects.names) - 1 ) + " > ") - - assert idx.isnumeric(), "Index must be a number." - assert idx != "", "Index must be a number." - assert int(idx) >= 0, "Index must be a positive number." - assert int(idx) < len(self._app_cfg.ycb_objects.names), "Invalid index for object to add." - - obj_shortname = self._app_cfg.ycb_objects.names[int(idx)] + idx_str = input( + "Enter the index of the object to add 0 - " + + str(len(self._app_cfg.ycb_objects.names) - 1) + + " > " + ) + + assert idx_str.isnumeric(), "Index must be a number." + assert idx_str != "", "Index must be a number." + assert int(idx_str) >= 0, "Index must be a positive number." + assert int(idx_str) < len( + self._app_cfg.ycb_objects.names + ), "Invalid index for object to add." + + obj_shortname = self._app_cfg.ycb_objects.names[int(idx_str)] obj_template_handle = list( self._sim.get_object_template_manager() .get_templates_by_handle_substring(obj_shortname) @@ -835,12 +855,10 @@ def handle_keys( new_obj.friction_coefficient = 5 else: - assert len(self._app_cfg.ycb_objects.names) == len( self._app_cfg.ycb_objects.positions ), "YCB object names and positions must be the same length." - for idx in range(len(self._app_cfg.ycb_objects.names)): obj_shortname = self._app_cfg.ycb_objects.names[idx] obj_template_handle = list( @@ -851,20 +869,19 @@ def handle_keys( new_obj = self.add_object_at(obj_template_handle) obj_size_down = sutils.get_obj_size_along( - self._sim, new_obj.object_id, mn.Vector3(0, -1, 0) + self._sim, new_obj.object_id, mn.Vector3(0, -1, 0) ) position = self._app_cfg.ycb_objects.positions[idx] - new_obj.translation = mn.Vector3(position[0], position[1], position[2]) + new_obj.translation = mn.Vector3( + position[0], position[1], position[2] + ) # TO DO: ensure habitat gets these from the object itself instead of manual setting. new_obj.mass = 0.01 new_obj.rolling_friction_coefficient = 5 new_obj.spinning_friction_coefficient = 5 new_obj.friction_coefficient = 5 - - - if gui_input.get_key_down(KeyCode.U): self.remove_object(self.mouse_cast_results.hits[0].object_id) @@ -1266,15 +1283,15 @@ def sim_update( self.handle_xr_input(dt) self.move_robot_on_navmesh() self._update_cursor_pos() + if self._mock_robot_helper: + self._mock_robot_helper.update_pre_sim_step(dt) # step the simulator if self.robot is not None and self.robot.using_joint_motors: self._sim.step_physics(dt) - # update robot finger raycast sensors - if self.robot is not None: - for finger_raycast_sensor in self.robot.finger_raycast_sensors: - finger_raycast_sensor.update_sensor_raycasts() + if self._mock_robot_helper: + self._mock_robot_helper.update_post_sim_step(post_sim_update_dict) # update the camera position self._camera_helper.update(self._cursor_pos, dt) @@ -1320,13 +1337,18 @@ def sim_update( # NOTE: do debug drawing here # draw lookat ring - self.draw_lookat() - self.debug_draw_quest() - self.robot.draw_debug(dblr) - if self.dof_editor is not None: - self.dof_editor.debug_draw(dblr, self._cam_transform.translation) - self.draw_navmesh_lines() - self._update_help_text() + if not self._hide_gui: + self.draw_lookat() + self.debug_draw_quest() + self.robot.draw_debug(dblr) + if self.dof_editor is not None: + self.dof_editor.debug_draw( + dblr, self._cam_transform.translation + ) + self.draw_navmesh_lines() + if self._mock_robot_helper: + self._mock_robot_helper.draw_debug(dblr) + self._update_help_text() @hydra.main(version_base=None, config_path="./", config_name="robot_teleop") diff --git a/examples/hitl/robot_teleop/robot_teleop.yaml b/examples/hitl/robot_teleop/robot_teleop.yaml index 1aec147dfb..8829041f9c 100644 --- a/examples/hitl/robot_teleop/robot_teleop.yaml +++ b/examples/hitl/robot_teleop/robot_teleop.yaml @@ -31,6 +31,9 @@ robot_teleop: - "108736677_177263328" camera_move_speed: 0.1 + # See examples/hitl/robot_teleop/README.md Simulator Process + do_murp_mock_robot: False + defaults: # Load default parameters for the HITL framework. See diff --git a/examples/hitl/robot_teleop/scripts/robot.py b/examples/hitl/robot_teleop/scripts/robot.py index 09a397b0f4..24023c8ad5 100644 --- a/examples/hitl/robot_teleop/scripts/robot.py +++ b/examples/hitl/robot_teleop/scripts/robot.py @@ -470,6 +470,7 @@ def __init__(self, sim: habitat_sim.Simulator, robot_cfg: DictConfig): if hasattr(self.robot_cfg, "fixed_base") else False, force_reload=True, + maintain_link_order=False, # todo: consider setting to True for better consistency with other URDF readers and simulators ) self.obj_ids = [self.ao.object_id] + list( self.ao.link_object_ids.keys() diff --git a/habitat-hitl/habitat_hitl/_internal/gui_application.py b/habitat-hitl/habitat_hitl/_internal/gui_application.py index 954a6ef073..7ecce5b163 100644 --- a/habitat-hitl/habitat_hitl/_internal/gui_application.py +++ b/habitat-hitl/habitat_hitl/_internal/gui_application.py @@ -35,6 +35,9 @@ def __init__(self, config): self._gui_input = GuiInput() self._mouse_ray = None + # Sloppy: disable v-sync to improve SPS. Todo: hook this up to a HITL config. + self.swap_interval = 0 + def key_press_event(self, event: Application.KeyEvent) -> None: key = MagnumKeyConverter.convert_key(event.key) if key is not None: diff --git a/habitat-hitl/habitat_hitl/_internal/sim_driver.py b/habitat-hitl/habitat_hitl/_internal/sim_driver.py index c81f190c6b..537d21af50 100644 --- a/habitat-hitl/habitat_hitl/_internal/sim_driver.py +++ b/habitat-hitl/habitat_hitl/_internal/sim_driver.py @@ -35,6 +35,10 @@ def __init__( """ HITL application driver that instantiates a `habitat-sim` simulator, without a `habitat-lab` environment. """ + self._enable_renderer = config["habitat_hitl"][ + "enable_sim_driver_renderer" + ] + # Initialize simulator. cfg_settings = habitat_sim.utils.settings.default_sim_settings.copy() # keyword "NONE" initializes a scene with no scene mesh @@ -43,8 +47,8 @@ def __init__( "scene_dataset_config_file" ] = "data/fpss/hssd-hab-siro.scene_dataset_config.json" cfg_settings["scene"] = "NONE" - cfg_settings["depth_sensor"] = False - cfg_settings["color_sensor"] = False + cfg_settings["depth_sensor"] = self._enable_renderer + cfg_settings["color_sensor"] = self._enable_renderer hab_cfg = habitat_sim.utils.settings.make_cfg(cfg_settings) # required for HITL apps hab_cfg.sim_cfg.enable_gfx_replay_save = True @@ -59,7 +63,7 @@ def __init__( sim=sim, ) - assert self.get_sim().renderer is None + assert (self.get_sim().renderer is not None) == self._enable_renderer data_collection_config = self._hitl_config.data_collection @@ -129,14 +133,14 @@ def _reconfigure_sim(self, dataset: Optional[str], scene: Optional[str]): cfg_settings["scene_dataset_config_file"] = ( dataset if dataset else None ) - cfg_settings["depth_sensor"] = False - cfg_settings["color_sensor"] = False + cfg_settings["depth_sensor"] = self._enable_renderer + cfg_settings["color_sensor"] = self._enable_renderer hab_cfg = habitat_sim.utils.settings.make_cfg(cfg_settings) # required for HITL apps hab_cfg.sim_cfg.enable_gfx_replay_save = True self.get_sim().reconfigure(hab_cfg) - assert self.get_sim().renderer is None + assert (self.get_sim().renderer is not None) == self._enable_renderer def _reset_environment(self): if self.network_server_enabled: diff --git a/habitat-hitl/habitat_hitl/config/hitl_defaults.yaml b/habitat-hitl/habitat_hitl/config/hitl_defaults.yaml index 50bef5db9f..f53493e0be 100644 --- a/habitat-hitl/habitat_hitl/config/hitl_defaults.yaml +++ b/habitat-hitl/habitat_hitl/config/hitl_defaults.yaml @@ -6,6 +6,9 @@ habitat_baselines: habitat_hitl: # LabDriver or SimDriver. LabDriver creates a Habitat-lab env. SimDriver only creates a Habitat-sim simulator. driver: "LabDriver" + + enable_sim_driver_renderer: False + window: # title displayed in application title bar GUI title: "Habitat HITL Application" diff --git a/habitat-hitl/habitat_hitl/core/gui_drawer.py b/habitat-hitl/habitat_hitl/core/gui_drawer.py index d49bbc2f39..9cca407a3f 100644 --- a/habitat-hitl/habitat_hitl/core/gui_drawer.py +++ b/habitat-hitl/habitat_hitl/core/gui_drawer.py @@ -65,6 +65,25 @@ def set_line_width( # Networking not implemented pass + def draw_axes(self, transform: mn.Matrix4, scale): + self.push_transform(transform) + self.draw_transformed_line( + mn.Vector3(0.0, 0.0, 0.0), + mn.Vector3(scale, 0.0, 0.0), + mn.Color4(1.0, 0.0, 0.0, 1.0), + ) + self.draw_transformed_line( + mn.Vector3(0.0, 0.0, 0.0), + mn.Vector3(0.0, scale, 0.0), + mn.Color4(0.0, 1.0, 0.0, 1.0), + ) + self.draw_transformed_line( + mn.Vector3(0.0, 0.0, 0.0), + mn.Vector3(0.0, 0.0, scale), + mn.Color4(0.0, 0.0, 1.0, 1.0), + ) + self.pop_transform() + def push_transform( self, transform: mn.Matrix4,