Skip to content

Commit

Permalink
Merge pull request #902 from StanfordVL/asset-conversion
Browse files Browse the repository at this point in the history
External asset + robot import script
  • Loading branch information
cremebrule authored Dec 12, 2024
2 parents c83956f + 002a536 commit 4cbc359
Show file tree
Hide file tree
Showing 36 changed files with 8,784 additions and 614 deletions.
4 changes: 2 additions & 2 deletions docs/modules/object_states.md
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down
1,146 changes: 989 additions & 157 deletions docs/tutorials/custom_robot_import.md

Large diffs are not rendered by default.

Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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,
Expand Down Expand Up @@ -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:
Expand Down
57 changes: 51 additions & 6 deletions omnigibson/controllers/multi_finger_gripper_controller.py
Original file line number Diff line number Diff line change
@@ -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 = {
Expand All @@ -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):
Expand Down Expand Up @@ -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)
Expand All @@ -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":
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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]

Expand All @@ -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)
Expand Down Expand Up @@ -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)
Expand Down
151 changes: 151 additions & 0 deletions omnigibson/examples/objects/import_custom_object.py
Original file line number Diff line number Diff line change
@@ -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()
Loading

0 comments on commit 4cbc359

Please sign in to comment.