External asset + robot import script

docs/modules/object_states.md

@@ -210,9 +210,9 @@ These are object states that are agnostic to other objects in a given scene.
         <tr>
         <td valign="top" width="60%">
             [**`ObjectsInFOVOfRobot`**](../reference/object_states/robot_related_states.md)<br><br>  
-            A robot-specific state. Comptues the list of objects that are currently in the robot's field of view.<br><br>
+            A robot-specific state. Comptues the set of objects that are currently in the robot's field of view.<br><br>
             <ul>
-                <li>`get_value()`: returns `obj_list`, the list of `BaseObject`s</li>
+                <li>`get_value()`: returns `obj_set`, the set of `BaseObject`s</li>
                 <li>`set_value(new_value)`: Not supported</li>
             </ul>
         </td>

omnigibson/action_primitives/starter_semantic_action_primitives.py

@@ -59,7 +59,7 @@
     Stretch: 0.5,
     Turtlebot: 0.3,
     Husky: 0.05,
-    Freight: 0.05,
+    Freight: 0.2,
     Locobot: 1.5,
     BehaviorRobot: 0.3,
     R1: 0.3,
@@ -70,7 +70,7 @@
     Stretch: 0.7,
     Turtlebot: 0.2,
     Husky: 0.05,
-    Freight: 0.05,
+    Freight: 0.1,
     Locobot: 1.5,
     BehaviorRobot: 0.2,
     R1: 0.2,
@@ -1763,10 +1763,6 @@ def _rotate_in_place(self, end_pose, angle_threshold=m.DEFAULT_ANGLE_THRESHOLD):
             direction = -1.0 if diff_yaw < 0.0 else 1.0
             ang_vel = m.KP_ANGLE_VEL[type(self.robot)] * direction
 
-            if isinstance(self.robot, Locobot) or isinstance(self.robot, Freight):
-                # Locobot and Freight wheel joints are reversed
-                ang_vel = -ang_vel
-
             base_action = action[self.robot.controller_action_idx["base"]]
 
             if not self._base_controller_is_joint:

omnigibson/controllers/multi_finger_gripper_controller.py

@@ -1,8 +1,8 @@
 import torch as th
 
-import omnigibson.utils.transform_utils as T
 from omnigibson.controllers import ControlType, GripperController, IsGraspingState
 from omnigibson.macros import create_module_macros
+from omnigibson.utils.processing_utils import MovingAverageFilter
 from omnigibson.utils.python_utils import assert_valid_key
 
 VALID_MODES = {
@@ -17,7 +17,7 @@
 
 # is_grasping heuristics parameters
 m.POS_TOLERANCE = 0.002  # arbitrary heuristic
-m.VEL_TOLERANCE = 0.01  # arbitrary heuristic
+m.VEL_TOLERANCE = 0.02  # arbitrary heuristic
 
 
 class MultiFingerGripperController(GripperController):
@@ -101,6 +101,9 @@ def __init__(
         # Create other args to be filled in at runtime
         self._is_grasping = IsGraspingState.FALSE
 
+        # Create ring buffer for velocity history to avoid high frequency nosie during grasp state inference
+        self._vel_filter = MovingAverageFilter(obs_dim=len(dof_idx), filter_width=5)
+
         # If we're using binary signal, we override the command output limits
         if mode == "binary":
             command_output_limits = (-1.0, 1.0)
@@ -118,9 +121,17 @@ def reset(self):
         # Call super first
         super().reset()
 
+        # Reset the filter
+        self._vel_filter.reset()
+
         # reset grasping state
         self._is_grasping = IsGraspingState.FALSE
 
+    @property
+    def state_size(self):
+        # Add state size from the control filter
+        return super().state_size + self._vel_filter.state_size
+
     def _preprocess_command(self, command):
         # We extend this method to make sure command is always n-dimensional
         if self._mode != "independent":
@@ -208,6 +219,9 @@ def _update_grasping_state(self, control_dict):
                     joint_position: Array of current joint positions
                     joint_velocity: Array of current joint velocities
         """
+        # Update velocity history
+        finger_vel = self._vel_filter.estimate(control_dict["joint_velocity"][self.dof_idx])
+
         # Calculate grasping state based on mode of this controller
 
         # Independent mode of MultiFingerGripperController does not have any good heuristics to determine is_grasping
@@ -240,7 +254,6 @@ def _update_grasping_state(self, control_dict):
 
             # Otherwise, the last control signal intends to "move" the gripper
             else:
-                finger_vel = control_dict["joint_velocity"][self.dof_idx]
                 min_pos = self._control_limits[ControlType.POSITION][0][self.dof_idx]
                 max_pos = self._control_limits[ControlType.POSITION][1][self.dof_idx]
 
@@ -251,10 +264,9 @@ def _update_grasping_state(self, control_dict):
                 dist_from_lower_limit = finger_pos - min_pos
                 dist_from_upper_limit = max_pos - finger_pos
 
-                # If the joint positions are not near the joint limits with some tolerance (m.POS_TOLERANCE)
+                # If either of the joint positions are not near the joint limits with some tolerance (m.POS_TOLERANCE)
                 valid_grasp_pos = (
-                    th.mean(dist_from_lower_limit) > m.POS_TOLERANCE
-                    and th.mean(dist_from_upper_limit) > m.POS_TOLERANCE
+                    th.mean(dist_from_lower_limit) > m.POS_TOLERANCE or th.mean(dist_from_upper_limit) > m.POS_TOLERANCE
                 )
 
                 # And the joint velocities are close to zero with some tolerance (m.VEL_TOLERANCE)
@@ -298,6 +310,39 @@ def is_grasping(self):
         # Return cached value
         return self._is_grasping
 
+    def _dump_state(self):
+        # Run super first
+        state = super()._dump_state()
+
+        # Add filter state
+        state["vel_filter"] = self._vel_filter.dump_state(serialized=False)
+
+        return state
+
+    def _load_state(self, state):
+        # Run super first
+        super()._load_state(state=state)
+
+        # Also load velocity filter state
+        self._vel_filter.load_state(state["vel_filter"], serialized=False)
+
+    def serialize(self, state):
+        # Run super first
+        state_flat = super().serialize(state=state)
+
+        # Serialize state for this controller
+        return th.cat([state_flat, self._vel_filter.serialize(state=state["vel_filter"])])
+
+    def deserialize(self, state):
+        # Run super first
+        state_dict, idx = super().deserialize(state=state)
+
+        # Deserialize state for the velocity filter
+        state_dict["vel_filter"], deserialized_items = self._vel_filter.deserialize(state=state[idx:])
+        idx += deserialized_items
+
+        return state_dict, idx
+
     @property
     def control_type(self):
         return ControlType.get_type(type_str=self._motor_type)

omnigibson/examples/objects/import_custom_object.py

@@ -0,0 +1,151 @@
+"""
+Helper script to download OmniGibson dataset and assets.
+"""
+
+import math
+from typing import Literal
+
+import click
+import trimesh
+
+import omnigibson as og
+from omnigibson.utils.asset_conversion_utils import (
+    generate_collision_meshes,
+    generate_urdf_for_obj,
+    import_og_asset_from_urdf,
+)
+from omnigibson.utils.python_utils import assert_valid_key
+
+
+@click.command()
+@click.option(
+    "--asset-path",
+    required=True,
+    type=click.Path(exists=True, dir_okay=False),
+    help="Absolute path to asset file to import. This can be a raw visual mesh (for single-bodied, static objects), e.g. .obj, .glb, etc., or a more complex (such as articulated) objects defined in .urdf format.",
+)
+@click.option("--category", required=True, type=click.STRING, help="Category name to assign to the imported asset")
+@click.option(
+    "--model",
+    required=True,
+    type=click.STRING,
+    help="Model name to assign to the imported asset. This MUST be a 6-character long string that exclusively contains letters, and must be unique within the given @category",
+)
+@click.option(
+    "--collision-method",
+    type=click.Choice(["coacd", "convex", "none"]),
+    default="coacd",
+    help="Method to generate the collision mesh. 'coacd' generates a set of convex decompositions, while 'convex' generates a single convex hull. 'none' will not generate any explicit mesh",
+)
+@click.option(
+    "--hull-count",
+    type=int,
+    default=32,
+    help="Maximum number of convex hulls to decompose individual visual meshes into. Only relevant if --collision-method=coacd",
+)
+@click.option("--scale", type=float, default=1.0, help="Scale factor to apply to the mesh.")
+@click.option("--up-axis", type=click.Choice(["z", "y"]), default="z", help="Up axis for the mesh.")
+@click.option("--headless", is_flag=True, help="Run the script in headless mode.")
+@click.option("--confirm-bbox", default=True, help="Whether to confirm the scale factor.")
+@click.option("--overwrite", is_flag=True, help="Overwrite any pre-existing files")
+def import_custom_object(
+    asset_path: str,
+    category: str,
+    model: str,
+    collision_method: Literal["coacd", "convex", "none"],
+    hull_count: int,
+    scale: float,
+    up_axis: Literal["z", "y"],
+    headless: bool,
+    confirm_bbox: bool,
+    overwrite: bool,
+):
+    """
+    Imports an externally-defined object asset into an OmniGibson-compatible USD format and saves the imported asset
+    files to the external dataset directory (gm.CUSTOM_DATASET_PATH)
+    """
+    assert len(model) == 6 and model.isalpha(), "Model name must be 6 characters long and contain only letters."
+    collision_method = None if collision_method == "none" else collision_method
+
+    # Sanity check mesh type
+    valid_formats = trimesh.available_formats()
+    mesh_format = asset_path.split(".")[-1]
+
+    # If we're not a URDF, import the mesh directly first
+    if mesh_format != "urdf":
+        assert_valid_key(key=mesh_format, valid_keys=valid_formats, name="mesh format")
+
+        # Load the mesh
+        visual_mesh: trimesh.Trimesh = trimesh.load(asset_path, force="mesh", process=False)
+
+        # Generate collision meshes if requested
+        collision_meshes = (
+            generate_collision_meshes(visual_mesh, method=collision_method, hull_count=hull_count)
+            if collision_method is not None
+            else []
+        )
+
+        # If the up axis is y, we need to rotate the meshes
+        if up_axis == "y":
+            rotation_matrix = trimesh.transformations.rotation_matrix(math.pi / 2, [1, 0, 0])
+            visual_mesh.apply_transform(rotation_matrix)
+            for collision_mesh in collision_meshes:
+                collision_mesh.apply_transform(rotation_matrix)
+
+        # If the scale is nonzero, we apply it to the meshes
+        if scale != 1.0:
+            scale_transform = trimesh.transformations.scale_matrix(scale)
+            visual_mesh.apply_transform(scale_transform)
+            for collision_mesh in collision_meshes:
+                collision_mesh.apply_transform(scale_transform)
+
+        # Check the bounding box size and complain if it's larger than 3 meters
+        bbox_size = visual_mesh.bounding_box.extents
+        click.echo(f"Visual mesh bounding box size: {bbox_size}")
+
+        if confirm_bbox:
+            if any(size > 3.0 for size in bbox_size):
+                click.echo(
+                    f"Warning: The bounding box sounds a bit large. Are you sure you don't need to scale? "
+                    f"We just wanted to confirm this is intentional. You can skip this check by passing --no-confirm-bbox."
+                )
+                click.confirm("Do you want to continue?", abort=True)
+
+            elif any(size < 0.01 for size in bbox_size):
+                click.echo(
+                    f"Warning: The bounding box sounds a bit small. Are you sure you don't need to scale? "
+                    f"We just wanted to confirm this is intentional. You can skip this check by passing --no-confirm-bbox."
+                )
+                click.confirm("Do you want to continue?", abort=True)
+
+        # Generate the URDF
+        click.echo(f"Generating URDF for {category}/{model}...")
+        generate_urdf_for_obj(visual_mesh, collision_meshes, category, model, overwrite=overwrite)
+        click.echo("URDF generation complete!")
+
+        urdf_path = None
+        collision_method = None
+    else:
+        urdf_path = asset_path
+        collision_method = collision_method
+
+    # Convert to USD
+    import_og_asset_from_urdf(
+        category=category,
+        model=model,
+        urdf_path=urdf_path,
+        collision_method=collision_method,
+        hull_count=hull_count,
+        overwrite=overwrite,
+        use_usda=False,
+    )
+
+    # Visualize if not headless
+    if not headless:
+        click.echo("The asset has been successfully imported. You can view it and make changes and save if you'd like.")
+        while True:
+            og.sim.render()
+
+
+if __name__ == "__main__":
+    import_custom_object()