diff --git a/omnigibson/envs/env_base.py b/omnigibson/envs/env_base.py index 7aa9deb2e..b280f6d80 100644 --- a/omnigibson/envs/env_base.py +++ b/omnigibson/envs/env_base.py @@ -732,8 +732,8 @@ def default_config(self): return { # Environment kwargs "env": { - "action_frequency": 30, - "physics_frequency": 120, + "action_frequency": gm.DEFAULT_RENDERING_FREQ, + "physics_frequency": gm.DEFAULT_PHYSICS_FREQ, "device": None, "automatic_reset": False, "flatten_action_space": False, diff --git a/omnigibson/macros.py b/omnigibson/macros.py index 8f43f074e..73d83568b 100644 --- a/omnigibson/macros.py +++ b/omnigibson/macros.py @@ -105,6 +105,10 @@ gm.DEFAULT_VIEWER_WIDTH = 1280 gm.DEFAULT_VIEWER_HEIGHT = 720 +# Default physics / rendering frequencies (Hz) +gm.DEFAULT_RENDERING_FREQ = 30 +gm.DEFAULT_PHYSICS_FREQ = 120 + # (Demo-purpose) Whether to activate Assistive Grasping mode for Cloth (it's handled differently from RigidBody) gm.AG_CLOTH = False diff --git a/omnigibson/robots/__init__.py b/omnigibson/robots/__init__.py index f2a4145c8..87feadc26 100644 --- a/omnigibson/robots/__init__.py +++ b/omnigibson/robots/__init__.py @@ -12,5 +12,6 @@ from omnigibson.robots.franka import FrankaPanda from omnigibson.robots.franka_allegro import FrankaAllegro from omnigibson.robots.franka_leap import FrankaLeap +from omnigibson.robots.franka_mounted import FrankaMounted from omnigibson.robots.vx300s import VX300S from omnigibson.robots.behavior_robot import BehaviorRobot diff --git a/omnigibson/robots/behavior_robot.py b/omnigibson/robots/behavior_robot.py index 0e6b5428c..2212f76cb 100644 --- a/omnigibson/robots/behavior_robot.py +++ b/omnigibson/robots/behavior_robot.py @@ -9,6 +9,7 @@ import omnigibson as og import omnigibson.lazy as lazy import omnigibson.utils.transform_utils as T +from omnigibson.utils.python_utils import classproperty from omnigibson.macros import gm, create_module_macros from omnigibson.robots.locomotion_robot import LocomotionRobot from omnigibson.robots.manipulation_robot import ManipulationRobot, GraspingPoint @@ -139,12 +140,12 @@ def usd_path(self): def model_name(self): return "BehaviorRobot" - @property - def n_arms(self): + @classproperty + def n_arms(cls): return 2 - @property - def arm_names(self): + @classproperty + def arm_names(cls): return ["left", "right"] @property diff --git a/omnigibson/robots/fetch.py b/omnigibson/robots/fetch.py index e633502af..0378ed7f8 100644 --- a/omnigibson/robots/fetch.py +++ b/omnigibson/robots/fetch.py @@ -258,12 +258,6 @@ def _initialize(self): # Run super method first super()._initialize() - # Set the joint friction for EEF to be higher - for arm in self.arm_names: - for joint in self.finger_joints[arm]: - if joint.joint_type != JointType.JOINT_FIXED: - joint.friction = 500 - def _postprocess_control(self, control, control_type): # Run super method first u_vec, u_type_vec = super()._postprocess_control(control=control, control_type=control_type) diff --git a/omnigibson/robots/franka_mounted.py b/omnigibson/robots/franka_mounted.py new file mode 100644 index 000000000..65eb37c7d --- /dev/null +++ b/omnigibson/robots/franka_mounted.py @@ -0,0 +1,65 @@ +import os +import numpy as np + +from omnigibson.macros import gm +from omnigibson.robots.manipulation_robot import ManipulationRobot, GraspingPoint +from omnigibson.robots.franka import FrankaPanda +from omnigibson.utils.transform_utils import euler2quat + + +class FrankaMounted(FrankaPanda): + """ + The Franka Emika Panda robot mounted on a custom chassis with a custom gripper + """ + + @property + def model_name(self): + return "FrankaMounted" + + @property + def controller_order(self): + return ["arm_{}".format(self.default_arm), "gripper_{}".format(self.default_arm)] + + @property + def _default_controllers(self): + controllers = super()._default_controllers + controllers["arm_{}".format(self.default_arm)] = "InverseKinematicsController" + controllers["gripper_{}".format(self.default_arm)] = "MultiFingerGripperController" + return controllers + + @property + def finger_lengths(self): + return {self.default_arm: 0.15} + + @property + def usd_path(self): + return os.path.join(gm.ASSET_PATH, "models/franka/franka_mounted.usd") + + @property + def robot_arm_descriptor_yamls(self): + return {self.default_arm: os.path.join(gm.ASSET_PATH, "models/franka/franka_mounted_description.yaml")} + + @property + def urdf_path(self): + return os.path.join(gm.ASSET_PATH, "models/franka/franka_mounted.urdf") + + @property + def eef_usd_path(self): + # TODO: Update! + return {self.default_arm: os.path.join(gm.ASSET_PATH, "models/franka/franka_panda_eef.usd")} + + @property + def assisted_grasp_start_points(self): + return { + self.default_arm: [ + GraspingPoint(link_name="panda_rightfinger", position=[0.0, 0.001, 0.045]), + ] + } + + @property + def assisted_grasp_end_points(self): + return { + self.default_arm: [ + GraspingPoint(link_name="panda_leftfinger", position=[0.0, 0.001, 0.045]), + ] + } diff --git a/omnigibson/robots/husky.py b/omnigibson/robots/husky.py index 37f302194..8ee7cd092 100644 --- a/omnigibson/robots/husky.py +++ b/omnigibson/robots/husky.py @@ -14,6 +14,14 @@ class Husky(LocomotionRobot): def _create_discrete_action_space(self): raise ValueError("Husky does not support discrete actions!") + @property + def wheel_radius(self): + return 0.165 + + @property + def wheel_axle_length(self): + return 0.670 + @property def base_control_idx(self): return np.array([0, 1, 2, 3]) diff --git a/omnigibson/robots/manipulation_robot.py b/omnigibson/robots/manipulation_robot.py index 6f08f543f..cb3f7f3b3 100644 --- a/omnigibson/robots/manipulation_robot.py +++ b/omnigibson/robots/manipulation_robot.py @@ -478,23 +478,23 @@ def _default_controllers(self): return controllers - @property - def n_arms(self): + @classproperty + def n_arms(cls): """ Returns: int: Number of arms this robot has. Returns 1 by default """ return 1 - @property - def arm_names(self): + @classproperty + def arm_names(cls): """ Returns: list of str: List of arm names for this robot. Should correspond to the keys used to index into arm- and gripper-related dictionaries, e.g.: eef_link_names, finger_link_names, etc. Default is string enumeration based on @self.n_arms. """ - return [str(i) for i in range(self.n_arms)] + return [str(i) for i in range(cls.n_arms)] @property def default_arm(self): diff --git a/omnigibson/robots/tiago.py b/omnigibson/robots/tiago.py index 26513e3a2..45bac5ef1 100644 --- a/omnigibson/robots/tiago.py +++ b/omnigibson/robots/tiago.py @@ -8,7 +8,7 @@ from omnigibson.robots.active_camera_robot import ActiveCameraRobot from omnigibson.robots.manipulation_robot import GraspingPoint, ManipulationRobot from omnigibson.robots.locomotion_robot import LocomotionRobot -from omnigibson.utils.python_utils import assert_valid_key +from omnigibson.utils.python_utils import assert_valid_key, classproperty from omnigibson.utils.usd_utils import JointType # Create settings for this module @@ -185,12 +185,12 @@ def arm_joint_names(self): def model_name(self): return "Tiago" - @property - def n_arms(self): + @classproperty + def n_arms(cls): return 2 - @property - def arm_names(self): + @classproperty + def arm_names(cls): return ["left", "right"] @property diff --git a/omnigibson/scenes/interactive_traversable_scene.py b/omnigibson/scenes/interactive_traversable_scene.py index 646eee2cf..aa7f60749 100644 --- a/omnigibson/scenes/interactive_traversable_scene.py +++ b/omnigibson/scenes/interactive_traversable_scene.py @@ -138,10 +138,9 @@ def filter_rooms_and_object_categories( load_room_instances = [load_room_instances] load_room_instances_filtered = [] for room_instance in load_room_instances: - if room_instance in self._seg_map.room_ins_name_to_ins_id: - load_room_instances_filtered.append(room_instance) - else: + if room_instance not in self._seg_map.room_ins_name_to_ins_id: log.warning("room_instance [{}] does not exist.".format(room_instance)) + load_room_instances_filtered.append(room_instance) self.load_room_instances = load_room_instances_filtered elif load_room_types is not None: if isinstance(load_room_types, str): diff --git a/omnigibson/sensors/vision_sensor.py b/omnigibson/sensors/vision_sensor.py index 9fd0d79ad..df6ba9a18 100644 --- a/omnigibson/sensors/vision_sensor.py +++ b/omnigibson/sensors/vision_sensor.py @@ -756,6 +756,9 @@ def clear(cls): # Render to update render() + cls.SEMANTIC_REMAPPER = Remapper() + cls.INSTANCE_REMAPPER = Remapper() + cls.INSTANCE_ID_REMAPPER = Remapper() cls.SENSORS = dict() cls.KNOWN_SEMANTIC_IDS = set() cls.KEY_ARRAY = None diff --git a/omnigibson/simulator.py b/omnigibson/simulator.py index a581a3914..83b1578a7 100644 --- a/omnigibson/simulator.py +++ b/omnigibson/simulator.py @@ -214,10 +214,12 @@ class Simulator(lazy.omni.isaac.core.simulation_context.SimulationContext, Seria Args: gravity (float): gravity on z direction. - physics_dt (float): dt between physics steps. Defaults to 1.0 / 120.0. - rendering_dt (float): dt between rendering steps. Note: rendering means rendering a frame of the current - application and not only rendering a frame to the viewports/ cameras. So UI elements of Isaac Sim will - be refreshed with this dt as well if running non-headless. Defaults to 1.0 / 30.0. + physics_dt (None or float): dt between physics steps. If None, will use default value + 1 / gm.DEFAULT_PHYSICS_FREQ + rendering_dt (None or float): dt between rendering steps. Note: rendering means rendering a frame of the + current application and not only rendering a frame to the viewports/ cameras. So UI elements of + Isaac Sim will be refreshed with this dt as well if running non-headless. If None, will use default + value 1 / gm.DEFAULT_RENDERING_FREQ stage_units_in_meters (float): The metric units of assets. This will affect gravity value..etc. Defaults to 0.01. viewer_width (int): width of the camera image, in pixels @@ -230,8 +232,8 @@ class Simulator(lazy.omni.isaac.core.simulation_context.SimulationContext, Seria def __init__( self, gravity=9.81, - physics_dt=1.0 / 120.0, - rendering_dt=1.0 / 30.0, + physics_dt=None, + rendering_dt=None, stage_units_in_meters=1.0, viewer_width=gm.DEFAULT_VIEWER_WIDTH, viewer_height=gm.DEFAULT_VIEWER_HEIGHT, @@ -244,8 +246,8 @@ def __init__( # Run super init super().__init__( - physics_dt=physics_dt, - rendering_dt=rendering_dt, + physics_dt=1.0 / gm.DEFAULT_PHYSICS_FREQ if physics_dt is None else physics_dt, + rendering_dt=1.0 / gm.DEFAULT_RENDERING_FREQ if rendering_dt is None else rendering_dt, stage_units_in_meters=stage_units_in_meters, device=device, ) @@ -1223,13 +1225,16 @@ def restore(self, json_path): return - def save(self, json_path): + def save(self, json_path=None): """ Saves the current simulation environment to @json_path. Args: - json_path (str): Full path of JSON file to save (should end with .json), which contains information - to recreate the current scene. + json_path (None or str): Full path of JSON file to save (should end with .json), which contains information + to recreate the current scene, if specified. If None, will return json string insted + + Returns: + None or str: If @json_path is None, returns dumped json string. Else, None """ # Make sure the sim is not stopped, since we need to grab joint states assert not self.is_stopped(), "Simulator cannot be stopped when saving to USD!" @@ -1258,11 +1263,15 @@ def save(self, json_path): } # Write this to the json file - Path(os.path.dirname(json_path)).mkdir(parents=True, exist_ok=True) - with open(json_path, "w+") as f: - json.dump(scene_info, f, cls=NumpyEncoder, indent=4) + if json_path is None: + return json.dumps(scene_info, cls=NumpyEncoder, indent=4) + + else: + Path(os.path.dirname(json_path)).mkdir(parents=True, exist_ok=True) + with open(json_path, "w+") as f: + json.dump(scene_info, f, cls=NumpyEncoder, indent=4) - log.info("The current simulation environment saved.") + log.info("The current simulation environment saved.") def _open_new_stage(self): """ diff --git a/omnigibson/utils/ui_utils.py b/omnigibson/utils/ui_utils.py index a57152130..34d6cc973 100644 --- a/omnigibson/utils/ui_utils.py +++ b/omnigibson/utils/ui_utils.py @@ -485,7 +485,7 @@ def get_interpolated_positions(step): pan_angle = np.arctan2(-xy_direction[0], xy_direction[1]) tilt_angle = np.arcsin(z) # Infer global quat orientation from these angles - quat = T.euler2quat([np.pi / 2 - tilt_angle, 0.0, pan_angle]) + quat = T.euler2quat([np.pi / 2 + tilt_angle, 0.0, pan_angle]) poses.append([positions[j], quat]) # Record the generated trajectory