Skip to content

Commit

Permalink
Address comments
Browse files Browse the repository at this point in the history
  • Loading branch information
hang-yin committed Jun 25, 2024
1 parent 8bcb38d commit ad825c6
Show file tree
Hide file tree
Showing 9 changed files with 29 additions and 35 deletions.
9 changes: 6 additions & 3 deletions omnigibson/controllers/controller_base.py
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,6 @@

import numpy as np

from omnigibson.utils.constants import MAGIC_DEFAULT
from omnigibson.utils.python_utils import Recreatable, Registerable, Serializable, assert_valid_key, classproperty

# Global dicts that will contain mappings
Expand Down Expand Up @@ -126,13 +125,17 @@ def __init__(
self._command_input_transform = None

# Standardize command input / output limits to be (min_array, max_array)
command_input_limits = (-1.0, 1.0) if MAGIC_DEFAULT == command_input_limits else command_input_limits
command_input_limits = (
(-1.0, 1.0)
if type(command_input_limits) == str and command_input_limits == "default"
else command_input_limits
)
command_output_limits = (
(
np.array(self._control_limits[self.control_type][0])[self.dof_idx],
np.array(self._control_limits[self.control_type][1])[self.dof_idx],
)
if MAGIC_DEFAULT == command_output_limits
if type(command_input_limits) == str and command_input_limits == "default"
else command_output_limits
)
self._command_input_limits = (
Expand Down
3 changes: 1 addition & 2 deletions omnigibson/controllers/dd_controller.py
Original file line number Diff line number Diff line change
@@ -1,7 +1,6 @@
import numpy as np

from omnigibson.controllers import ControlType, LocomotionController
from omnigibson.utils.constants import MAGIC_DEFAULT


class DifferentialDriveController(LocomotionController):
Expand Down Expand Up @@ -55,7 +54,7 @@ def __init__(
self._wheel_axle_halflength = wheel_axle_length / 2.0

# If we're using default command output limits, map this to maximum linear / angular velocities
if MAGIC_DEFAULT == command_output_limits:
if type(command_output_limits) == str and command_output_limits == "default":
min_vels = control_limits["velocity"][0][dof_idx]
assert (
min_vels[0] == min_vels[1]
Expand Down
5 changes: 2 additions & 3 deletions omnigibson/controllers/ik_controller.py
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,6 @@
from omnigibson.controllers import ControlType, ManipulationController
from omnigibson.controllers.joint_controller import JointController
from omnigibson.macros import create_module_macros, gm
from omnigibson.utils.constants import MAGIC_DEFAULT
from omnigibson.utils.control_utils import IKSolver
from omnigibson.utils.processing_utils import MovingAverageFilter
from omnigibson.utils.python_utils import assert_valid_key
Expand Down Expand Up @@ -154,7 +153,7 @@ def limiter(target_pos: Array[float], target_quat: Array[float], control_dict: D
# The output orientation limits are also set to be values assuming delta commands, so those are updated too
if self.mode == "pose_absolute_ori":
if command_input_limits is not None:
if MAGIC_DEFAULT == command_input_limits:
if type(command_input_limits) == str and command_input_limits == "default":
command_input_limits = [
[-1.0, -1.0, -1.0, -np.pi, -np.pi, -np.pi],
[1.0, 1.0, 1.0, np.pi, np.pi, np.pi],
Expand All @@ -163,7 +162,7 @@ def limiter(target_pos: Array[float], target_quat: Array[float], control_dict: D
command_input_limits[0][3:] = -np.pi
command_input_limits[1][3:] = np.pi
if command_output_limits is not None:
if MAGIC_DEFAULT == command_output_limits:
if type(command_output_limits) == str and command_output_limits == "default":
command_output_limits = [
[-1.0, -1.0, -1.0, -np.pi, -np.pi, -np.pi],
[1.0, 1.0, 1.0, np.pi, np.pi, np.pi],
Expand Down
3 changes: 1 addition & 2 deletions omnigibson/controllers/joint_controller.py
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,6 @@
ManipulationController,
)
from omnigibson.macros import create_module_macros
from omnigibson.utils.constants import MAGIC_DEFAULT
from omnigibson.utils.python_utils import assert_valid_key

# Create settings for this module
Expand Down Expand Up @@ -102,7 +101,7 @@ def __init__(
# When in delta mode, it doesn't make sense to infer output range using the joint limits (since that's an
# absolute range and our values are relative). So reject the default mode option in that case.
assert not (
self._use_delta_commands and MAGIC_DEFAULT == command_output_limits
self._use_delta_commands and type(command_output_limits) == str and command_output_limits == "default"
), "Cannot use 'default' command output limits in delta commands mode of JointController. Try None instead."

# Run super init
Expand Down
1 change: 0 additions & 1 deletion omnigibson/controllers/multi_finger_gripper_controller.py
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,6 @@
import omnigibson.utils.transform_utils as T
from omnigibson.controllers import ControlType, GripperController, IsGraspingState
from omnigibson.macros import create_module_macros
from omnigibson.utils.constants import MAGIC_DEFAULT
from omnigibson.utils.python_utils import assert_valid_key

VALID_MODES = {
Expand Down
5 changes: 2 additions & 3 deletions omnigibson/controllers/osc_controller.py
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,6 @@

import omnigibson.utils.transform_utils as T
from omnigibson.controllers import ControlType, ManipulationController
from omnigibson.utils.constants import MAGIC_DEFAULT
from omnigibson.utils.control_utils import orientation_error
from omnigibson.utils.processing_utils import MovingAverageFilter
from omnigibson.utils.python_utils import assert_valid_key, nums2array
Expand Down Expand Up @@ -158,7 +157,7 @@ def limiter(target_pos: Array[float], target_quat: Array[float], control_dict: D
self.mode = mode
if self.mode == "pose_absolute_ori":
if command_input_limits is not None:
if MAGIC_DEFAULT == command_input_limits:
if type(command_input_limits) == str and command_input_limits == "default":
command_input_limits = [
[-1.0, -1.0, -1.0, -np.pi, -np.pi, -np.pi],
[1.0, 1.0, 1.0, np.pi, np.pi, np.pi],
Expand All @@ -167,7 +166,7 @@ def limiter(target_pos: Array[float], target_quat: Array[float], control_dict: D
command_input_limits[0][3:] = -np.pi
command_input_limits[1][3:] = np.pi
if command_output_limits is not None:
if MAGIC_DEFAULT == command_output_limits:
if type(command_output_limits) == str and command_output_limits == "default":
command_output_limits = [
[-1.0, -1.0, -1.0, -np.pi, -np.pi, -np.pi],
[1.0, 1.0, 1.0, np.pi, np.pi, np.pi],
Expand Down
28 changes: 16 additions & 12 deletions omnigibson/sensors/vision_sensor.py
Original file line number Diff line number Diff line change
Expand Up @@ -596,14 +596,16 @@ def image_height(self, height):
self._viewport.viewport_api.set_texture_resolution((width, height))

# Also update render product and update all annotators
old_render_product = self._render_product
new_render_product = lazy.omni.replicator.core.create.render_product(self.prim_path, (width, height))
for annotator in self._annotators.values():
annotator.detach([old_render_product.path])
annotator.attach([new_render_product])
annotator.detach([self._render_product.path])

old_render_product.destroy()
self._render_product = new_render_product
self._render_product.destroy()
self._render_product = lazy.omni.replicator.core.create.render_product(
self.prim_path, (width, height), force_new=True
)

for annotator in self._annotators.values():
annotator.attach([self._render_product])

# Requires 3 updates to propagate changes
for i in range(3):
Expand All @@ -629,14 +631,16 @@ def image_width(self, width):
self._viewport.viewport_api.set_texture_resolution((width, height))

# Also update render product and update all annotators
old_render_product = self._render_product
new_render_product = lazy.omni.replicator.core.create.render_product(self.prim_path, (width, height))
for annotator in self._annotators.values():
annotator.detach([old_render_product.path])
annotator.attach([new_render_product])
annotator.detach([self._render_product.path])

old_render_product.destroy()
self._render_product = new_render_product
self._render_product.destroy()
self._render_product = lazy.omni.replicator.core.create.render_product(
self.prim_path, (width, height), force_new=True
)

for annotator in self._annotators.values():
annotator.attach([self._render_product])

# Requires 3 updates to propagate changes
for i in range(3):
Expand Down
8 changes: 0 additions & 8 deletions omnigibson/utils/constants.py
Original file line number Diff line number Diff line change
Expand Up @@ -18,14 +18,6 @@
MAX_VIEWER_SIZE = 2048


class strWithCompare(str):
def __eq__(self, other):
return isinstance(other, str) and str.__eq__(self, other)


MAGIC_DEFAULT = strWithCompare("default")


class ViewerMode(IntEnum):
NAVIGATION = 0
MANIPULATION = 1
Expand Down
2 changes: 1 addition & 1 deletion omnigibson/utils/registry_utils.py
Original file line number Diff line number Diff line change
Expand Up @@ -352,7 +352,7 @@ def _load_state(self, state):
# loading will be skipped.
for obj in self.objects:
if obj.name not in state:
# log.warning(f"Object '{obj.name}' is not in the state dict to load from. Skip loading its state.")
log.warning(f"Object '{obj.name}' is not in the state dict to load from. Skip loading its state.")
continue
obj.load_state(state[obj.name], serialized=False)

Expand Down

0 comments on commit ad825c6

Please sign in to comment.