diff --git a/.gitignore b/.gitignore index 67383ad6a..2e0bdb364 100644 --- a/.gitignore +++ b/.gitignore @@ -91,4 +91,5 @@ docs/reference site/ # Tests of examples -tests/tests_of_examples \ No newline at end of file +tests/tests_of_examples +.aider* diff --git a/omnigibson/configs/base_config.py b/omnigibson/configs/base_config.py new file mode 100644 index 000000000..90fc7da3a --- /dev/null +++ b/omnigibson/configs/base_config.py @@ -0,0 +1,99 @@ +from dataclasses import dataclass, field +from typing import Optional, List, Dict, Any +from omegaconf import MISSING +from omnigibson.configs.robot_config import ControllerConfig + + +@dataclass +class BaseConfig: + """Base configuration class that all other configs inherit from""" + + name: str = MISSING + + +@dataclass +class BasePrimConfig(BaseConfig): + """Base configuration for all prims""" + + relative_prim_path: Optional[str] = None + load_config: Dict[str, Any] = field(default_factory=dict) + + +@dataclass +class EntityConfig(BasePrimConfig): + """Configuration for entities""" + + scale: Optional[List[float]] = None + visible: bool = True + visual_only: bool = False + kinematic_only: Optional[bool] = None + self_collisions: bool = False + prim_type: str = MISSING + + +@dataclass +class ObjectConfig(EntityConfig): + """Configuration for all objects""" + + category: str = "object" + fixed_base: bool = False + abilities: Dict[str, Dict[str, Any]] = field(default_factory=dict) + include_default_states: bool = True + + +@dataclass +class StatefulObjectConfig(ObjectConfig): + """Configuration for stateful objects""" + + abilities: Optional[Dict[str, Dict[str, Any]]] = None + include_default_states: bool = True + + +@dataclass +class USDObjectConfig(StatefulObjectConfig): + """Configuration for USD-based objects""" + + usd_path: str = MISSING + encrypted: bool = False + + +@dataclass +class DatasetObjectConfig(StatefulObjectConfig): + """Configuration for dataset objects""" + + model: Optional[str] = None + dataset_type: str = "BEHAVIOR" + bounding_box: Optional[List[float]] = None + in_rooms: Optional[List[str]] = None + xform_props_pre_loaded: bool = True + + +@dataclass +class PrimitiveObjectConfig(StatefulObjectConfig): + """Configuration for primitive objects""" + + primitive_type: str = MISSING + rgba: List[float] = field(default_factory=lambda: [0.5, 0.5, 0.5, 1.0]) + radius: Optional[float] = None + height: Optional[float] = None + size: Optional[float] = None + + +@dataclass +class LightObjectConfig(StatefulObjectConfig): + """Configuration for light objects""" + + light_type: str = MISSING + radius: float = 1.0 + intensity: float = 50000.0 + + +@dataclass +class ControllableObjectConfig(StatefulObjectConfig): + """Configuration for controllable objects""" + + control_freq: Optional[float] = None + action_type: str = "continuous" + action_normalize: bool = True + reset_joint_pos: Optional[List[float]] = None + controllers: Dict[str, ControllerConfig] = field(default_factory=dict) diff --git a/omnigibson/configs/controller_config.py b/omnigibson/configs/controller_config.py new file mode 100644 index 000000000..88ff4f1c0 --- /dev/null +++ b/omnigibson/configs/controller_config.py @@ -0,0 +1,122 @@ +from dataclasses import dataclass, field +from typing import Optional, Dict, Any, List, Tuple, Union +from omegaconf import MISSING +import torch as th + + +@dataclass +class BaseControllerConfig: + """Base configuration for all controllers""" + + control_freq: float = MISSING + control_limits: Dict[str, Any] = MISSING # Dict with position, velocity, effort limits and has_limit + dof_idx: List[int] = MISSING + command_input_limits: Optional[Union[str, Tuple[float, float], Tuple[List[float], List[float]]]] = "default" + command_output_limits: Optional[Union[str, Tuple[float, float], Tuple[List[float], List[float]]]] = "default" + + +@dataclass +class JointControllerConfig(BaseControllerConfig): + """Configuration for joint controllers""" + + motor_type: str = MISSING # One of {position, velocity, effort} + pos_kp: Optional[float] = None + pos_damping_ratio: Optional[float] = None + vel_kp: Optional[float] = None + use_impedances: bool = False + use_gravity_compensation: bool = False + use_cc_compensation: bool = True + use_delta_commands: bool = False + compute_delta_in_quat_space: Optional[List[Tuple[int, int, int]]] = None + + +@dataclass +class VisionSensorConfig: + """Configuration for vision sensors""" + + enabled: bool = True + noise_type: Optional[str] = None + noise_kwargs: Optional[Dict[str, Any]] = None + modalities: Optional[List[str]] = None + sensor_kwargs: Dict[str, Any] = field( + default_factory=lambda: { + "image_height": 128, + "image_width": 128, + } + ) + + +@dataclass +class ScanSensorConfig: + """Configuration for scan sensors""" + + enabled: bool = True + noise_type: Optional[str] = None + noise_kwargs: Optional[Dict[str, Any]] = None + modalities: Optional[List[str]] = None + sensor_kwargs: Dict[str, Any] = field( + default_factory=lambda: { + "min_range": 0.05, + "max_range": 10.0, + "horizontal_fov": 360.0, + "vertical_fov": 1.0, + "yaw_offset": 0.0, + "horizontal_resolution": 1.0, + "vertical_resolution": 1.0, + "rotation_rate": 0.0, + "draw_points": False, + "draw_lines": False, + "occupancy_grid_resolution": 128, + "occupancy_grid_range": 5.0, + "occupancy_grid_inner_radius": 0.5, + "occupancy_grid_local_link": None, + } + ) + + +@dataclass +class DifferentialDriveControllerConfig(BaseControllerConfig): + """Configuration for differential drive controllers""" + + wheel_radius: float = MISSING + wheel_axle_length: float = MISSING + wheel_control_idx: List[int] = MISSING # [left_wheel_idx, right_wheel_idx] + + +@dataclass +class InverseKinematicsControllerConfig(BaseControllerConfig): + """Configuration for inverse kinematics controllers""" + + control_freq: float = MISSING + control_limits: Dict[str, Any] = MISSING + dof_idx: List[int] = MISSING + command_input_limits: Optional[Union[str, Tuple[float, float], Tuple[List[float], List[float]]]] = "default" + command_output_limits: Optional[Union[str, Tuple[float, float], Tuple[List[float], List[float]]]] = "default" + rest_poses: Optional[th.Tensor] = None + position_limits: Optional[Tuple[th.Tensor, th.Tensor]] = None + rotation_limits: Optional[Tuple[th.Tensor, th.Tensor]] = None + position_gain: float = 1.0 + rotation_gain: float = 0.5 + position_threshold: float = 0.005 + rotation_threshold: float = 0.05 + num_ik_seeds: int = 10 + num_ik_solutions: int = 1 + regularization_weight: float = 0.01 + collision_checking: bool = True + + +@dataclass +class MultiFingerGripperControllerConfig(BaseControllerConfig): + """Configuration for multi-finger gripper controllers""" + + mode: str = "binary" # One of {binary, continuous} + grasp_thresh: float = 0.5 + release_thresh: float = -0.5 + + +@dataclass +class SensorConfig: + """Configuration for all sensors""" + + VisionSensor: VisionSensorConfig = field(default_factory=VisionSensorConfig) + ScanSensor: ScanSensorConfig = field(default_factory=ScanSensorConfig) diff --git a/omnigibson/configs/env_config.py b/omnigibson/configs/env_config.py new file mode 100644 index 000000000..2676954be --- /dev/null +++ b/omnigibson/configs/env_config.py @@ -0,0 +1,60 @@ +from dataclasses import dataclass, field +from typing import List, Optional, Dict, Any +from omegaconf import MISSING + +from omnigibson.configs.object_config import ObjectConfig, RobotConfig + + +@dataclass +class RenderConfig: + viewer_width: int = 1280 + viewer_height: int = 720 + + +@dataclass +class EnvConfig: + action_frequency: int = 60 + rendering_frequency: int = 60 + physics_frequency: int = 60 + device: Optional[str] = None + automatic_reset: bool = False + flatten_action_space: bool = False + flatten_obs_space: bool = False + initial_pos_z_offset: float = 0.1 + external_sensors: Optional[List[Dict[str, Any]]] = None + + +@dataclass +class SceneConfig: + type: str = MISSING + model: str = MISSING + waypoint_resolution: float = 0.2 + num_waypoints: int = 10 + trav_map_resolution: float = 0.1 + default_erosion_radius: float = 0.0 + trav_map_with_objects: bool = True + scene_instance: Optional[str] = None + scene_file: Optional[str] = None + + +@dataclass +class TaskConfig: + type: str = "DummyTask" + params: Dict[str, Any] = field(default_factory=dict) + + +@dataclass +class WrapperConfig: + type: Optional[str] = None + params: Dict[str, Any] = field(default_factory=dict) + + +@dataclass +class OmniGibsonConfig: + env: EnvConfig = field(default_factory=EnvConfig) + render: RenderConfig = field(default_factory=RenderConfig) + scene: SceneConfig = field(default_factory=SceneConfig) + robots: List[RobotConfig] = field(default_factory=list) + objects: List[ObjectConfig] = field(default_factory=list) + task: TaskConfig = field(default_factory=TaskConfig) + wrapper: WrapperConfig = field(default_factory=WrapperConfig) diff --git a/omnigibson/configs/object_config.py b/omnigibson/configs/object_config.py new file mode 100644 index 000000000..efe34b070 --- /dev/null +++ b/omnigibson/configs/object_config.py @@ -0,0 +1,57 @@ +from dataclasses import dataclass, field +from typing import Dict, List, Optional, Union +from omegaconf import MISSING + +from omnigibson.configs.base_config import ( + ObjectConfig, + USDObjectConfig, + DatasetObjectConfig, + PrimitiveObjectConfig, + LightObjectConfig, + ControllableObjectConfig, +) +from omnigibson.configs.robot_config import ( + ControllerConfig, + JointControllerConfig, + IKControllerConfig, + OSCControllerConfig, + DifferentialDriveConfig, + GripperControllerConfig, +) +from omnigibson.configs.sensor_config import SensorConfig + +__all__ = [ + "ObjectConfig", + "USDObjectConfig", + "DatasetObjectConfig", + "PrimitiveObjectConfig", + "LightObjectConfig", + "ControllableObjectConfig", + "RobotConfig", +] + + +@dataclass +class RobotConfig(ControllableObjectConfig): + """Configuration for robots""" + + type: str = MISSING + obs_modalities: List[str] = field(default_factory=lambda: ["rgb", "proprio"]) + proprio_obs: str = "default" + sensor_config: Optional[SensorConfig] = field(default_factory=SensorConfig) + grasping_mode: str = "physical" + grasping_direction: str = "lower" + disable_grasp_handling: bool = False + default_reset_mode: str = "untuck" + default_arm_pose: str = "vertical" + controllers: Dict[ + str, + Union[ + ControllerConfig, + JointControllerConfig, + IKControllerConfig, + OSCControllerConfig, + DifferentialDriveConfig, + GripperControllerConfig, + ], + ] = field(default_factory=dict) diff --git a/omnigibson/configs/prim_config.py b/omnigibson/configs/prim_config.py new file mode 100644 index 000000000..84fcaaf18 --- /dev/null +++ b/omnigibson/configs/prim_config.py @@ -0,0 +1,68 @@ +from dataclasses import dataclass +from typing import Optional, Any +from omegaconf import MISSING +from omnigibson.configs.base_config import BasePrimConfig + + +@dataclass +class XFormPrimConfig(BasePrimConfig): + """Configuration for XForm prims""" + + pass + + +@dataclass +class GeomPrimConfig(XFormPrimConfig): + """Configuration for geometry prims""" + + pass + + +@dataclass +class CollisionGeomPrimConfig(GeomPrimConfig): + """Configuration for collision geometry prims""" + + pass + + +@dataclass +class VisualGeomPrimConfig(GeomPrimConfig): + """Configuration for visual geometry prims""" + + pass + + +@dataclass +class CollisionVisualGeomPrimConfig(CollisionGeomPrimConfig, VisualGeomPrimConfig): + """Configuration for combined collision and visual geometry prims""" + + pass + + +@dataclass +class RigidPrimConfig(XFormPrimConfig): + """Configuration for rigid body prims""" + + kinematic_only: Optional[bool] = None + belongs_to_articulation: Optional[bool] = None + visual_only: Optional[bool] = None + mass: Optional[float] = None + density: Optional[float] = None + + +@dataclass +class JointPrimConfig(BasePrimConfig): + """Configuration for joint prims""" + + joint_type: Optional[str] = None + body0: Optional[str] = None + body1: Optional[str] = None + articulation_view: Optional[Any] = None + + +@dataclass +class EntityPrimConfig(XFormPrimConfig): + """Configuration for entity prims""" + + visual_only: Optional[bool] = None + prim_type: str = MISSING diff --git a/omnigibson/configs/robot_config.py b/omnigibson/configs/robot_config.py new file mode 100644 index 000000000..ea5732a4e --- /dev/null +++ b/omnigibson/configs/robot_config.py @@ -0,0 +1,225 @@ +from dataclasses import dataclass, field +from typing import Dict, Any, Optional, List +from omegaconf import MISSING + +from omnigibson.configs.object_config import ControllableObjectConfig + + +@dataclass +class ControllerConfig: + """Base configuration for controllers""" + + name: str = MISSING + control_freq: Optional[float] = None + control_limits: Optional[Dict[str, Any]] = None + command_input_limits: Optional[List[float]] = None + command_output_limits: Optional[List[float]] = None + use_delta_commands: bool = False + use_impedances: bool = False + + +@dataclass +class JointControllerConfig(ControllerConfig): + """Configuration for joint controllers""" + + motor_type: str = "position" + dof_idx: Optional[List[int]] = None + pos_kp: Optional[float] = None + pos_kd: Optional[float] = None + + +@dataclass +class IKControllerConfig(ControllerConfig): + """Configuration for IK controllers""" + + task_name: str = MISSING + mode: str = "pose_delta_ori" + smoothing_filter_size: int = 2 + workspace_pose_limiter: Optional[Dict[str, Any]] = None + reset_joint_pos: Optional[List[float]] = None + + +@dataclass +class OSCControllerConfig(ControllerConfig): + """Configuration for operational space controllers""" + + task_name: str = MISSING + mode: str = "pose_delta_ori" + workspace_pose_limiter: Optional[Dict[str, Any]] = None + reset_joint_pos: Optional[List[float]] = None + + +@dataclass +class DifferentialDriveConfig(ControllerConfig): + """Configuration for differential drive controllers""" + + wheel_radius: float = MISSING + wheel_axle_length: float = MISSING + + +@dataclass +class GripperControllerConfig(ControllerConfig): + """Configuration for gripper controllers""" + + mode: str = "binary" + limit_tolerance: float = 0.001 + inverted: bool = False + + +@dataclass +class RobotConfig(ControllableObjectConfig): + """Base configuration for robots""" + + type: str = MISSING + obs_modalities: List[str] = field(default_factory=lambda: ["rgb", "proprio"]) + proprio_obs: str = "default" + sensor_config: Optional[Dict[str, Any]] = None + controllers: Dict[str, ControllerConfig] = field(default_factory=dict) + + +@dataclass +class ManipulationRobotConfig(RobotConfig): + """Configuration for manipulation robots""" + + grasping_mode: str = "physical" + grasping_direction: str = "lower" + disable_grasp_handling: bool = False + + +@dataclass +class MobileManipulationRobotConfig(ManipulationRobotConfig): + """Configuration for mobile manipulation robots""" + + default_reset_mode: str = "untuck" + + +@dataclass +class UntuckedArmPoseRobotConfig(MobileManipulationRobotConfig): + """Configuration for robots with untucked arm poses""" + + default_arm_pose: str = "vertical" + + +@dataclass +class BehaviorRobotConfig(ManipulationRobotConfig): + """Configuration for behavior robots""" + + use_ghost_hands: bool = True + + +@dataclass +class FrankaConfig(ManipulationRobotConfig): + """Configuration for Franka robots""" + + end_effector: str = "gripper" + + +@dataclass +class A1Config(ManipulationRobotConfig): + """Configuration for A1 robot""" + + end_effector: str = "inspire" + + +@dataclass +class TiagoConfig(UntuckedArmPoseRobotConfig): + """Configuration for Tiago robot""" + + variant: str = "default" + + +@dataclass +class FetchConfig(UntuckedArmPoseRobotConfig): + """Configuration for Fetch robot""" + + pass + + +@dataclass +class R1Config(MobileManipulationRobotConfig): + """Configuration for R1 robot""" + + pass + + +@dataclass +class StretchConfig(ManipulationRobotConfig): + """Configuration for Stretch robot""" + + pass + + +@dataclass +class VX300SConfig(ManipulationRobotConfig): + """Configuration for VX300S robot""" + + pass + + +@dataclass +class TwoWheelRobotConfig(RobotConfig): + """Configuration for two wheel robots""" + + wheel_radius: float = MISSING + wheel_axle_length: float = MISSING + + +@dataclass +class HuskyConfig(TwoWheelRobotConfig): + """Configuration for Husky robot""" + + pass + + +@dataclass +class TurtlebotConfig(TwoWheelRobotConfig): + """Configuration for Turtlebot robot""" + + pass + + +@dataclass +class FreightConfig(TwoWheelRobotConfig): + """Configuration for Freight robot""" + + pass + + +@dataclass +class LocobotConfig(TwoWheelRobotConfig): + """Configuration for Locobot robot""" + + pass + + +@dataclass +class ManipulationRobotConfig(RobotConfig): + """Configuration for manipulation robots""" + + n_arms: int = 1 + arm_names: List[str] = field(default_factory=lambda: ["default"]) + finger_lengths: Dict[str, float] = field(default_factory=dict) + assisted_grasp_points: Dict[str, Dict[str, List[float]]] = field(default_factory=dict) + + +@dataclass +class LocomotionRobotConfig(RobotConfig): + """Configuration for locomotion robots""" + + base_joint_names: List[str] = field(default_factory=list) + base_control_idx: List[int] = field(default_factory=list) + + +@dataclass +class TwoWheelRobotConfig(LocomotionRobotConfig): + """Configuration for two wheel robots""" + + wheel_radius: float = MISSING + wheel_axle_length: float = MISSING + + +@dataclass +class HolonomicBaseRobotConfig(LocomotionRobotConfig): + """Configuration for holonomic base robots""" + + base_footprint_link_name: str = MISSING diff --git a/omnigibson/envs/env_base.py b/omnigibson/envs/env_base.py index 6b1796160..eccb5d5d9 100644 --- a/omnigibson/envs/env_base.py +++ b/omnigibson/envs/env_base.py @@ -8,7 +8,6 @@ import torch as th import omnigibson as og -from omnigibson.macros import gm from omnigibson.objects import REGISTERED_OBJECTS from omnigibson.robots import REGISTERED_ROBOTS from omnigibson.scene_graphs.graph_builder import SceneGraphBuilder @@ -27,7 +26,6 @@ Recreatable, assert_valid_key, create_class_from_registry_and_config, - merge_nested_dicts, ) from omnigibson.utils.ui_utils import create_module_logger @@ -58,30 +56,36 @@ def __init__(self, configs, in_vec_env=False): # Store if we are part of a vec env self.in_vec_env = in_vec_env - # Convert config file(s) into a single parsed dict - configs = configs if isinstance(configs, list) or isinstance(configs, tuple) else [configs] + # Convert config into OmegaConf structured config + from omnigibson.configs.env_config import OmniGibsonConfig + from omegaconf import OmegaConf - # Initial default config - self.config = self.default_config + # Handle single config case + configs = configs if isinstance(configs, (list, tuple)) else [configs] - # Merge in specified configs - for config in configs: - merge_nested_dicts(base_dict=self.config, extra_dict=parse_config(config), inplace=True) + # Start with default structured config + self.config = OmegaConf.structured(OmniGibsonConfig()) + + # Merge in all configs + for cfg in configs: + # Parse the config if it's a string/dict + parsed_cfg = parse_config(cfg) + # Convert to OmegaConf and merge + cfg_omega = OmegaConf.create(parsed_cfg) + self.config = OmegaConf.merge(self.config, cfg_omega) # Store settings and other initialized values - self._automatic_reset = self.env_config["automatic_reset"] - self._flatten_action_space = self.env_config["flatten_action_space"] - self._flatten_obs_space = self.env_config["flatten_obs_space"] - self.device = self.env_config["device"] if self.env_config["device"] else "cpu" - self._initial_pos_z_offset = self.env_config[ - "initial_pos_z_offset" - ] # how high to offset object placement to account for one action step of dropping - - physics_dt = 1.0 / self.env_config["physics_frequency"] - rendering_dt = 1.0 / self.env_config["rendering_frequency"] - sim_step_dt = 1.0 / self.env_config["action_frequency"] - viewer_width = self.render_config["viewer_width"] - viewer_height = self.render_config["viewer_height"] + self._automatic_reset = self.config.env.automatic_reset + self._flatten_action_space = self.config.env.flatten_action_space + self._flatten_obs_space = self.config.env.flatten_obs_space + self.device = self.config.env.device if self.config.env.device else "cpu" + self._initial_pos_z_offset = self.config.env.initial_pos_z_offset + + physics_dt = 1.0 / self.config.env.physics_frequency + rendering_dt = 1.0 / self.config.env.rendering_frequency + sim_step_dt = 1.0 / self.config.env.action_frequency + viewer_width = self.config.render.viewer_width + viewer_height = self.config.render.viewer_height # If the sim is launched, check that the parameters match if og.sim is not None: @@ -145,21 +149,28 @@ def reload(self, configs, overwrite_old=True): merge in the new config(s) into the pre-existing one. Setting this to False allows for minor modifications to be made without having to specify entire configs during each reload. """ - # Convert config file(s) into a single parsed dict - configs = [configs] if isinstance(configs, dict) or isinstance(configs, str) else configs + from omegaconf import OmegaConf + from omnigibson.configs.env_config import OmniGibsonConfig + + # Convert config file(s) into OmegaConf + configs = [configs] if isinstance(configs, (dict, str)) else configs - # Initial default config - new_config = self.default_config + # Start with default structured config + new_config = OmegaConf.structured(OmniGibsonConfig()) - # Merge in specified configs - for config in configs: - merge_nested_dicts(base_dict=new_config, extra_dict=parse_config(config), inplace=True) + # Merge in all configs + for cfg in configs: + # Parse the config if it's a string/dict + parsed_cfg = parse_config(cfg) + # Convert to OmegaConf and merge + cfg_omega = OmegaConf.create(parsed_cfg) + new_config = OmegaConf.merge(new_config, cfg_omega) - # Either merge in or overwrite the old config + # Either overwrite or merge with existing config if overwrite_old: self.config = new_config else: - merge_nested_dicts(base_dict=self.config, extra_dict=new_config, inplace=True) + self.config = OmegaConf.merge(self.config, new_config) # Load this environment again self.load() @@ -267,29 +278,29 @@ def _load_robots(self): # Iterate over all robots to generate in the robot config for robot_config in self.robots_config: - # Add a name for the robot if necessary - if "name" not in robot_config: - robot_config["name"] = "robot_" + "".join(random.choices(string.ascii_lowercase, k=6)) - robot_config = deepcopy(robot_config) - position, orientation = robot_config.pop("position", None), robot_config.pop("orientation", None) - pose_frame = robot_config.pop("pose_frame", "scene") - if position is not None: - position = position if isinstance(position, th.Tensor) else th.tensor(position, dtype=th.float32) - if orientation is not None: - orientation = ( - orientation if isinstance(orientation, th.Tensor) else th.tensor(orientation, dtype=th.float32) - ) - - # Make sure robot exists, grab its corresponding kwargs, and create / import the robot + # Generate name if not specified + if not robot_config.name: + robot_config.name = "robot_" + "".join(random.choices(string.ascii_lowercase, k=6)) + + # Convert position/orientation to tensors if specified + if robot_config.position is not None: + robot_config.position = th.tensor(robot_config.position, dtype=th.float32) + if robot_config.orientation is not None: + robot_config.orientation = th.tensor(robot_config.orientation, dtype=th.float32) + + # Create and import the robot robot = create_class_from_registry_and_config( - cls_name=robot_config["type"], + cls_name=robot_config.type, cls_registry=REGISTERED_ROBOTS, cfg=robot_config, cls_type_descriptor="robot", ) - # Import the robot into the simulator self.scene.add_object(robot) - robot.set_position_orientation(position=position, orientation=orientation, frame=pose_frame) + robot.set_position_orientation( + position=robot_config.position, + orientation=robot_config.orientation, + frame=robot_config.pose_frame, + ) assert og.sim.is_stopped(), "Simulator must be stopped after loading robots!" @@ -299,22 +310,29 @@ def _load_objects(self): """ assert og.sim.is_stopped(), "Simulator must be stopped before loading objects!" for i, obj_config in enumerate(self.objects_config): - # Add a name for the object if necessary - if "name" not in obj_config: - obj_config["name"] = f"obj{i}" - # Pop the desired position and orientation - obj_config = deepcopy(obj_config) - position, orientation = obj_config.pop("position", None), obj_config.pop("orientation", None) - # Make sure robot exists, grab its corresponding kwargs, and create / import the robot + # Generate name if not specified + if not obj_config.name: + obj_config.name = f"obj{i}" + + # Convert position/orientation to tensors if specified + if obj_config.position is not None: + obj_config.position = th.tensor(obj_config.position, dtype=th.float32) + if obj_config.orientation is not None: + obj_config.orientation = th.tensor(obj_config.orientation, dtype=th.float32) + + # Create and import the object obj = create_class_from_registry_and_config( - cls_name=obj_config["type"], + cls_name=obj_config.type, cls_registry=REGISTERED_OBJECTS, cfg=obj_config, cls_type_descriptor="object", ) - # Import the robot into the simulator and set the pose self.scene.add_object(obj) - obj.set_position_orientation(position=position, orientation=orientation, frame="scene") + obj.set_position_orientation( + position=obj_config.position, + orientation=obj_config.orientation, + frame="scene", + ) assert og.sim.is_stopped(), "Simulator must be stopped after loading objects!" @@ -774,104 +792,54 @@ def external_sensors(self): def env_config(self): """ Returns: - dict: Environment-specific configuration kwargs + EnvConfig: Environment-specific configuration """ - return self.config["env"] + return self.config.env @property def render_config(self): """ Returns: - dict: Render-specific configuration kwargs + RenderConfig: Render-specific configuration """ - return self.config["render"] + return self.config.render @property def scene_config(self): """ Returns: - dict: Scene-specific configuration kwargs + SceneConfig: Scene-specific configuration """ - return self.config["scene"] + return self.config.scene @property def robots_config(self): """ Returns: - dict: Robot-specific configuration kwargs + List[RobotConfig]: Robot-specific configurations """ - return self.config["robots"] + return self.config.robots @property def objects_config(self): """ Returns: - dict: Object-specific configuration kwargs + List[Dict]: Object-specific configurations """ - return self.config["objects"] + return self.config.objects @property def task_config(self): """ Returns: - dict: Task-specific configuration kwargs + TaskConfig: Task-specific configuration """ - return self.config["task"] + return self.config.task @property def wrapper_config(self): """ Returns: - dict: Wrapper-specific configuration kwargs - """ - return self.config["wrapper"] - - @property - def default_config(self): + WrapperConfig: Wrapper-specific configuration """ - Returns: - dict: Default configuration for this environment. May not be fully specified (i.e.: still requires @config - to be specified during environment creation) - """ - return { - # Environment kwargs - "env": { - "action_frequency": gm.DEFAULT_SIM_STEP_FREQ, - "rendering_frequency": gm.DEFAULT_RENDERING_FREQ, - "physics_frequency": gm.DEFAULT_PHYSICS_FREQ, - "device": None, - "automatic_reset": False, - "flatten_action_space": False, - "flatten_obs_space": False, - "initial_pos_z_offset": 0.1, - "external_sensors": None, - }, - # Rendering kwargs - "render": { - "viewer_width": 1280, - "viewer_height": 720, - }, - # Scene kwargs - "scene": { - # Traversibility map kwargs - "waypoint_resolution": 0.2, - "num_waypoints": 10, - "trav_map_resolution": 0.1, - "default_erosion_radius": 0.0, - "trav_map_with_objects": True, - "scene_instance": None, - "scene_file": None, - }, - # Robot kwargs - "robots": [], # no robots by default - # Object kwargs - "objects": [], # no objects by default - # Task kwargs - "task": { - "type": "DummyTask", - }, - # Wrapper kwargs - "wrapper": { - "type": None, - }, - } + return self.config.wrapper diff --git a/omnigibson/objects/controllable_object.py b/omnigibson/objects/controllable_object.py index ad96d7f10..2d89da8d4 100644 --- a/omnigibson/objects/controllable_object.py +++ b/omnigibson/objects/controllable_object.py @@ -17,6 +17,7 @@ from omnigibson.utils.python_utils import CachedFunctions, assert_valid_key, merge_nested_dicts from omnigibson.utils.ui_utils import create_module_logger from omnigibson.utils.usd_utils import ControllableObjectViewAPI +from omnigibson.configs.base_config import ControllableObjectConfig # Create module logger log = create_module_logger(module_name=__name__) @@ -29,103 +30,43 @@ class ControllableObject(BaseObject): e.g.: a conveyor belt or a robot agent """ - def __init__( - self, - name, - relative_prim_path=None, - category="object", - scale=None, - visible=True, - fixed_base=False, - visual_only=False, - self_collisions=False, - prim_type=PrimType.RIGID, - load_config=None, - control_freq=None, - controller_config=None, - action_type="continuous", - action_normalize=True, - reset_joint_pos=None, - **kwargs, - ): + def __init__(self, config: ControllableObjectConfig): """ Args: - name (str): Name for the object. Names need to be unique per scene - relative_prim_path (None or str): The path relative to its scene prim for this object. If not specified, it defaults to /. - category (str): Category for the object. Defaults to "object". - scale (None or float or 3-array): if specified, sets either the uniform (float) or x,y,z (3-array) scale - for this object. A single number corresponds to uniform scaling along the x,y,z axes, whereas a - 3-array specifies per-axis scaling. - visible (bool): whether to render this object or not in the stage - fixed_base (bool): whether to fix the base of this object or not - visual_only (bool): Whether this object should be visual only (and not collide with any other objects) - self_collisions (bool): Whether to enable self collisions for this object - prim_type (PrimType): Which type of prim the object is, Valid options are: {PrimType.RIGID, PrimType.CLOTH} - load_config (None or dict): If specified, should contain keyword-mapped values that are relevant for - loading this prim at runtime. - control_freq (float): control frequency (in Hz) at which to control the object. If set to be None, - we will automatically set the control frequency to be at the render frequency by default. - controller_config (None or dict): nested dictionary mapping controller name(s) to specific controller - configurations for this object. This will override any default values specified by this class. - action_type (str): one of {discrete, continuous} - what type of action space to use - action_normalize (bool): whether to normalize inputted actions. This will override any default values - specified by this class. - reset_joint_pos (None or n-array): if specified, should be the joint positions that the object should - be set to during a reset. If None (default), self._default_joint_pos will be used instead. - Note that _default_joint_pos are hardcoded & precomputed, and thus should not be modified by the user. - Set this value instead if you want to initialize the object with a different rese joint position. - kwargs (dict): Additional keyword arguments that are used for other super() calls from subclasses, allowing - for flexible compositions of various object subclasses (e.g.: Robot is USDObject + ControllableObject). - """ - # Store inputs - self._control_freq = control_freq - self._controller_config = controller_config - self._reset_joint_pos = None if reset_joint_pos is None else th.tensor(reset_joint_pos, dtype=th.float) - - # Make sure action type is valid, and also save - assert_valid_key(key=action_type, valid_keys={"discrete", "continuous"}, name="action type") - self._action_type = action_type - self._action_normalize = action_normalize - - # Store internal placeholders that will be filled in later - self._dof_to_joints = None # dict that will map DOF indices to JointPrims - self._last_action = None - self._controllers = None - self.dof_names_ordered = None - self._control_enabled = True + config (ControllableObjectConfig): Configuration object for this controllable object + """ + # Make sure action type is valid + assert_valid_key(key=config.action_type, valid_keys={"discrete", "continuous"}, name="action type") + # Handle prim path class_name = self.__class__.__name__.lower() - if relative_prim_path: + if config.relative_prim_path: # If prim path is specified, assert that the last element starts with the right prefix to ensure that # the object will be included in the ControllableObjectViewAPI. - assert relative_prim_path.split("/")[-1].startswith(f"controllable__{class_name}__"), ( + assert config.relative_prim_path.split("/")[-1].startswith(f"controllable__{class_name}__"), ( "If relative_prim_path is specified, the last element of the path must look like " f"'controllable__{class_name}__robotname' where robotname can be an arbitrary " "string containing no double underscores." ) - assert relative_prim_path.split("/")[-1].count("__") == 2, ( + assert config.relative_prim_path.split("/")[-1].count("__") == 2, ( "If relative_prim_path is specified, the last element of the path must look like " f"'controllable__{class_name}__robotname' where robotname can be an arbitrary " "string containing no double underscores." ) else: # If prim path is not specified, set it to the default path, but prepend controllable. - relative_prim_path = f"/controllable__{class_name}__{name}" - - # Run super init - super().__init__( - relative_prim_path=relative_prim_path, - name=name, - category=category, - scale=scale, - visible=visible, - fixed_base=fixed_base, - visual_only=visual_only, - self_collisions=self_collisions, - prim_type=prim_type, - load_config=load_config, - **kwargs, - ) + config.relative_prim_path = f"/controllable__{class_name}__{config.name}" + + # Store config and initialize other attributes + self._config = config + self._dof_to_joints = None # dict that will map DOF indices to JointPrims + self._last_action = None + self._controllers = None + self.dof_names_ordered = None + self._control_enabled = True + + # Run super init with config + super().__init__(config=config) def _initialize(self): # Assert that the prim path matches ControllableObjectViewAPI's expected format @@ -224,67 +165,33 @@ def _load_controllers(self): Stores created controllers as dictionary mapping controller names to specific controller instances used by this object. """ - # Generate the controller config - self._controller_config = self._generate_controller_config(custom_config=self._controller_config) - - # We copy the controller config here because we add/remove some keys in-place that shouldn't persist - _controller_config = deepcopy(self._controller_config) - # Store dof idx mapping to dof name self.dof_names_ordered = list(self._joints.keys()) # Initialize controllers to create self._controllers = dict() - # Keep track of any controllers that are subsumed by other controllers - # We will not instantiate subsumed controllers - controller_subsumes = dict() # Maps independent controller name to list of subsumed controllers - subsume_names = set() + + # Process controller configs from structured config for name in self._raw_controller_order: # Make sure we have the valid controller name specified - assert_valid_key(key=name, valid_keys=_controller_config, name="controller name") - cfg = _controller_config[name] - subsume_controllers = cfg.pop("subsume_controllers", []) - # If this controller subsumes other controllers, it cannot be subsumed by another controller - # (i.e.: we don't allow nested / cyclical subsuming) - if len(subsume_controllers) > 0: - assert ( - name not in subsume_names - ), f"Controller {name} subsumes other controllers, and therefore cannot be subsumed by another controller!" - controller_subsumes[name] = subsume_controllers - for subsume_name in subsume_controllers: - # Make sure it doesn't already exist -- a controller should only be subsumed by up to one other - assert ( - subsume_name not in subsume_names - ), f"Controller {subsume_name} cannot be subsumed by more than one other controller!" - assert ( - subsume_name not in controller_subsumes - ), f"Controller {name} subsumes other controllers, and therefore cannot be subsumed by another controller!" - subsume_names.add(subsume_name) - - # Loop over all controllers, in the order corresponding to @action dim - for name in self._raw_controller_order: - # If this controller is subsumed by another controller, simply skip it - if name in subsume_names: - continue - cfg = _controller_config[name] - # If we subsume other controllers, prepend the subsumed' dof idxs to this controller's idxs - if name in controller_subsumes: - for subsumed_name in controller_subsumes[name]: - subsumed_cfg = _controller_config[subsumed_name] - cfg["dof_idx"] = th.concatenate([subsumed_cfg["dof_idx"], cfg["dof_idx"]]) - - # If we're using normalized action space, override the inputs for all controllers - if self._action_normalize: - cfg["command_input_limits"] = "default" # default is normalized (-1, 1) - - # Create the controller - controller = create_controller(**cb.from_torch_recursive(cfg)) + assert_valid_key(key=name, valid_keys=self.config.controllers, name="controller name") + + # Get controller config and override input limits if using normalized actions + controller_cfg = self.config.controllers[name] + if self.config.action_normalize: + controller_cfg.command_input_limits = "default" # default is normalized (-1, 1) + + # Create the controller from structured config + controller = create_controller(name=controller_cfg.name, **cb.from_torch_recursive(controller_cfg)) + # Verify the controller's DOFs can all be driven for idx in controller.dof_idx: assert self._joints[ self.dof_names_ordered[idx] ].driven, "Controllers should only control driveable joints!" + self._controllers[name] = controller + self.update_controller_mode() def update_controller_mode(self): @@ -330,38 +237,50 @@ def _generate_controller_config(self, custom_config=None): specified in @custom_config Args: - custom_config (None or Dict[str, ...]): nested dictionary mapping controller name(s) to specific custom - controller configurations for this object. This will override any default values specified by this class + custom_config (None or Dict[str, ControllerConfig]): Mapping of controller names to their configs. + If specified, will override the default controller configs. Returns: - dict: Fully-populated nested dictionary mapping controller name(s) to specific controller configurations for - this object + Dict[str, ControllerConfig]: Mapping of controller names to their configs """ - controller_config = {} if custom_config is None else deepcopy(custom_config) + # Start with default controller configs + controller_config = {} - # Update the configs + # For each controller group for group in self._raw_controller_order: - group_controller_name = ( - controller_config[group]["name"] - if group in controller_config and "name" in controller_config[group] - else self._default_controllers[group] - ) - controller_config[group] = merge_nested_dicts( - base_dict=self._default_controller_config[group][group_controller_name], - extra_dict=controller_config.get(group, {}), - ) + # Get default controller type for this group + default_controller = self._default_controllers[group] + + # Get default config for this controller type + default_config = self._default_controller_config[group][default_controller] + + # Override with custom config if specified + if custom_config is not None and group in custom_config: + controller_config[group] = custom_config[group] + else: + controller_config[group] = default_config return controller_config - def reload_controllers(self, controller_config=None): + def reload_controllers(self, controller_configs=None): """ - Reloads controllers based on the specified new @controller_config + Reloads controllers based on the specified new controller configs Args: - controller_config (None or Dict[str, ...]): nested dictionary mapping controller name(s) to specific - controller configurations for this object. This will override any default values specified by this class. + controller_configs (None or Dict[str, ControllerConfig]): Mapping of controller names to their configs. + If None, will use the existing controller configs. """ - self._controller_config = {} if controller_config is None else controller_config + if controller_configs is not None: + # Create new structured configs from the input + from omnigibson.configs.robot_config import ControllerConfig + + new_controllers = {} + for name, cfg in controller_configs.items(): + if isinstance(cfg, dict): + new_controllers[name] = ControllerConfig(**cfg) + else: + new_controllers[name] = cfg + self.config.controllers = new_controllers # (Re-)load controllers self._load_controllers() @@ -369,7 +288,7 @@ def reload_controllers(self, controller_config=None): # (Re-)create the action space self._action_space = ( self._create_discrete_action_space() - if self._action_type == "discrete" + if self.config.action_type == "discrete" else self._create_continuous_action_space() ) diff --git a/omnigibson/objects/dataset_object.py b/omnigibson/objects/dataset_object.py index 98fce936b..e1c5681da 100644 --- a/omnigibson/objects/dataset_object.py +++ b/omnigibson/objects/dataset_object.py @@ -6,6 +6,7 @@ import torch as th import omnigibson.lazy as lazy +from omnigibson.configs.base_config import DatasetObjectConfig, USDObjectConfig import omnigibson.utils.transform_utils as T from omnigibson.macros import create_module_macros, gm from omnigibson.objects.usd_object import USDObject @@ -36,27 +37,7 @@ class DatasetObject(USDObject): object's category, e.g., avg dims, bounding boxes, masses, etc. """ - def __init__( - self, - name, - relative_prim_path=None, - category="object", - model=None, - dataset_type=DatasetType.BEHAVIOR, - scale=None, - visible=True, - fixed_base=False, - visual_only=False, - kinematic_only=None, - self_collisions=False, - prim_type=PrimType.RIGID, - load_config=None, - abilities=None, - include_default_states=True, - bounding_box=None, - in_rooms=None, - **kwargs, - ): + def __init__(self, config: DatasetObjectConfig): """ Args: name (str): Name for the object. Names need to be unique per scene @@ -96,61 +77,59 @@ def __init__( kwargs (dict): Additional keyword arguments that are used for other super() calls from subclasses, allowing for flexible compositions of various object subclasses (e.g.: Robot is USDObject + ControllableObject). """ - # Store variables - if isinstance(in_rooms, str): - assert "," not in in_rooms - self._in_rooms = [in_rooms] if isinstance(in_rooms, str) else in_rooms + # Validate in_rooms + if isinstance(config.in_rooms, str): + assert "," not in config.in_rooms # Make sure only one of bounding_box and scale are specified - if bounding_box is not None and scale is not None: - raise Exception("You cannot define both scale and bounding box size for an DatasetObject") - - # Add info to load config - load_config = dict() if load_config is None else load_config - load_config["bounding_box"] = bounding_box - load_config["dataset_type"] = dataset_type - # All DatasetObjects should have xform properties pre-loaded - load_config["xform_props_pre_loaded"] = True + if config.bounding_box is not None and config.scale is not None: + raise Exception("You cannot define both scale and bounding box size for a DatasetObject") # Infer the correct usd path to use - if model is None: - available_models = get_all_object_category_models(category=category) - assert len(available_models) > 0, f"No available models found for category {category}!" - model = random.choice(available_models) + if config.model is None: + available_models = get_all_object_category_models(category=config.category) + assert len(available_models) > 0, f"No available models found for category {config.category}!" + config.model = random.choice(available_models) # If the model is in BAD_CLOTH_MODELS, raise an error for now -- this is a model that's unstable and needs to be fixed # TODO: Remove this once the asset is fixed! from omnigibson.utils.bddl_utils import BAD_CLOTH_MODELS - if prim_type == PrimType.CLOTH and model in BAD_CLOTH_MODELS.get(category, dict()): + if config.prim_type == PrimType.CLOTH and config.model in BAD_CLOTH_MODELS.get(config.category, dict()): raise ValueError( - f"Cannot create cloth object category: {category}, model: {model} because it is " + f"Cannot create cloth object category: {config.category}, model: {config.model} because it is " f"currently broken ): This will be fixed in the next release!" ) - self._model = model - usd_path = self.get_usd_path(category=category, model=model, dataset_type=dataset_type) + # Store config + self._config = config + usd_path = self.get_usd_path( + category=config.category, + model=config.model, + dataset_type=DatasetType[config.dataset_type], + ) - # Run super init - super().__init__( - relative_prim_path=relative_prim_path, + # Create USDObjectConfig from our config + usd_config = USDObjectConfig( + name=config.name, + relative_prim_path=config.relative_prim_path, + category=config.category, usd_path=usd_path, - encrypted=dataset_type == DatasetType.BEHAVIOR, - name=name, - category=category, - scale=scale, - visible=visible, - fixed_base=fixed_base, - visual_only=visual_only, - kinematic_only=kinematic_only, - self_collisions=self_collisions, - prim_type=prim_type, - include_default_states=include_default_states, - load_config=load_config, - abilities=abilities, - **kwargs, + encrypted=config.dataset_type == "BEHAVIOR", + scale=config.scale, + visible=config.visible, + fixed_base=config.fixed_base, + visual_only=config.visual_only, + kinematic_only=config.kinematic_only, + self_collisions=config.self_collisions, + prim_type=config.prim_type, + abilities=config.abilities, + include_default_states=config.include_default_states, ) + # Run super init + super().__init__(config=usd_config) + @classmethod def get_usd_path(cls, category, model, dataset_type=DatasetType.BEHAVIOR): """ @@ -233,20 +212,18 @@ def recursive_light_update(child_prim): def _post_load(self): # If manual bounding box is specified, scale based on ratio between that and the native bbox - if self._load_config["bounding_box"] is not None: + if self._config.bounding_box is not None: scale = th.ones(3) valid_idxes = self.native_bbox > 1e-4 - scale[valid_idxes] = ( - th.tensor(self._load_config["bounding_box"])[valid_idxes] / self.native_bbox[valid_idxes] - ) + scale[valid_idxes] = th.tensor(self._config.bounding_box)[valid_idxes] / self.native_bbox[valid_idxes] else: - scale = th.ones(3) if self._load_config["scale"] is None else self._load_config["scale"] + scale = th.ones(3) if self._config.scale is None else self._config.scale # Assert that the scale does not have too small dimensions assert th.all(th.tensor(scale) > 1e-4), f"Scale of {self.name} is too small: {scale}" - # Set this scale in the load config -- it will automatically scale the object during self.initialize() - self._load_config["scale"] = scale + # Update config scale + self._config.scale = scale # Run super last super()._post_load() @@ -478,19 +455,3 @@ def avg_obj_dims(self): """ avg_specs = get_og_avg_category_specs() return avg_specs.get(self.category, None) - - def _create_prim_with_same_kwargs(self, relative_prim_path, name, load_config): - # Add additional kwargs (bounding_box is already captured in load_config) - return self.__class__( - relative_prim_path=relative_prim_path, - name=name, - category=self.category, - scale=self.scale, - visible=self.visible, - fixed_base=self.fixed_base, - visual_only=self._visual_only, - prim_type=self._prim_type, - load_config=load_config, - abilities=self._abilities, - in_rooms=self.in_rooms, - ) diff --git a/omnigibson/objects/light_object.py b/omnigibson/objects/light_object.py index cc97af77f..ce1ceddc0 100644 --- a/omnigibson/objects/light_object.py +++ b/omnigibson/objects/light_object.py @@ -3,8 +3,8 @@ import omnigibson as og import omnigibson.lazy as lazy from omnigibson.objects.stateful_object import StatefulObject +from omnigibson.configs.base_config import LightObjectConfig from omnigibson.prims.xform_prim import XFormPrim -from omnigibson.utils.constants import PrimType from omnigibson.utils.python_utils import assert_valid_key from omnigibson.utils.ui_utils import create_module_logger @@ -27,71 +27,22 @@ class LightObject(StatefulObject): "Sphere", } - def __init__( - self, - name, - light_type, - relative_prim_path=None, - category="light", - scale=None, - fixed_base=False, - load_config=None, - abilities=None, - include_default_states=True, - radius=1.0, - intensity=50000.0, - **kwargs, - ): + def __init__(self, config: LightObjectConfig): """ Args: - name (str): Name for the object. Names need to be unique per scene - light_type (str): Type of light to create. Valid options are LIGHT_TYPES - relative_prim_path (None or str): The path relative to its scene prim for this object. If not specified, it defaults to /. - category (str): Category for the object. Defaults to "object". - scale (None or float or 3-array): if specified, sets either the uniform (float) or x,y,z (3-array) scale - for this object. A single number corresponds to uniform scaling along the x,y,z axes, whereas a - 3-array specifies per-axis scaling. - fixed_base (bool): whether to fix the base of this object or not - load_config (None or dict): If specified, should contain keyword-mapped values that are relevant for - loading this prim at runtime. - abilities (None or dict): If specified, manually adds specific object states to this object. It should be - a dict in the form of {ability: {param: value}} containing object abilities and parameters to pass to - the object state instance constructor. - include_default_states (bool): whether to include the default object states from @get_default_states - radius (float): Radius for this light. - intensity (float): Intensity for this light. - kwargs (dict): Additional keyword arguments that are used for other super() calls from subclasses, allowing - for flexible compositions of various object subclasses (e.g.: Robot is USDObject + ControllableObject). - """ - # Compose load config and add rgba values - load_config = dict() if load_config is None else load_config - load_config["scale"] = scale - load_config["intensity"] = intensity - load_config["radius"] = radius if light_type in {"Cylinder", "Disk", "Sphere"} else None - - # Make sure primitive type is valid - assert_valid_key(key=light_type, valid_keys=self.LIGHT_TYPES, name="light_type") - self.light_type = light_type + config (LightObjectConfig): Configuration object containing all parameters for this light object + """ + # Make sure light type is valid + assert_valid_key(key=config.light_type, valid_keys=self.LIGHT_TYPES, name="light_type") + + # Store config + self._config = config # Other attributes to be filled in at runtime self._light_link = None # Run super method - super().__init__( - relative_prim_path=relative_prim_path, - name=name, - category=category, - scale=scale, - visible=True, - fixed_base=fixed_base, - visual_only=True, - self_collisions=False, - prim_type=PrimType.RIGID, - include_default_states=include_default_states, - load_config=load_config, - abilities=abilities, - **kwargs, - ) + super().__init__(config=config) def _load(self): # Define XForm and base link for this light @@ -120,13 +71,12 @@ def _post_load(self): # Apply Shaping API and set default cone angle attribute lazy.pxr.UsdLux.ShapingAPI.Apply(self._light_link.prim).GetShapingConeAngleAttr().Set(180.0) - # Optionally set the intensity - if self._load_config.get("intensity", None) is not None: - self.intensity = self._load_config["intensity"] + # Set the intensity and radius from config + self.intensity = self._config.intensity - # Optionally set the radius - if self._load_config.get("radius", None) is not None: - self.radius = self._load_config["radius"] + # Only set radius for applicable light types + if self._config.light_type in {"Cylinder", "Disk", "Sphere"}: + self.radius = self._config.radius def _initialize(self): # Run super @@ -229,12 +179,10 @@ def texture_file_path(self, texture_file_path): """ self._light_link.set_attribute("inputs:texture:file", lazy.pxr.Sdf.AssetPath(texture_file_path)) - def _create_prim_with_same_kwargs(self, relative_prim_path, name, load_config): - # Add additional kwargs (bounding_box is already captured in load_config) - return self.__class__( - relative_prim_path=relative_prim_path, - light_type=self.light_type, - name=name, - intensity=self.intensity, - load_config=load_config, - ) + @property + def light_type(self): + """ + Returns: + str: Type of light + """ + return self._config.light_type diff --git a/omnigibson/objects/object_base.py b/omnigibson/objects/object_base.py index c85a9a917..0a8adf367 100644 --- a/omnigibson/objects/object_base.py +++ b/omnigibson/objects/object_base.py @@ -37,17 +37,7 @@ class BaseObject(EntityPrim, Registerable, metaclass=ABCMeta): def __init__( self, - name, - relative_prim_path=None, - category="object", - scale=None, - visible=True, - fixed_base=False, - visual_only=False, - kinematic_only=None, - self_collisions=False, - prim_type=PrimType.RIGID, - load_config=None, + config, **kwargs, ): """ @@ -74,36 +64,36 @@ def __init__( that kwargs are only shared between all SUBclasses (children), not SUPERclasses (parents). """ # Generate default prim path if none is specified - relative_prim_path = f"/{name}" if relative_prim_path is None else relative_prim_path + relative_prim_path = f"/{config.name}" if config.relative_prim_path is None else config.relative_prim_path # Store values - self._uuid = get_uuid(name, deterministic=True) - self.category = category - self.fixed_base = fixed_base + self._uuid = get_uuid(config.name, deterministic=True) + self.category = config.category + self.fixed_base = config.fixed_base # Values to be created at runtime self._highlight_cached_values = None self._highlighted = None # Create load config from inputs - load_config = dict() if load_config is None else load_config + load_config = dict() if config.load_config is None else config.load_config load_config["scale"] = ( - scale - if isinstance(scale, th.Tensor) - else th.tensor(scale, dtype=th.float32) - if isinstance(scale, Iterable) - else scale + config.scale + if isinstance(config.scale, th.Tensor) + else th.tensor(config.scale, dtype=th.float32) + if isinstance(config.scale, Iterable) + else config.scale ) - load_config["visible"] = visible - load_config["visual_only"] = visual_only - load_config["kinematic_only"] = kinematic_only - load_config["self_collisions"] = self_collisions - load_config["prim_type"] = prim_type + load_config["visible"] = config.visible + load_config["visual_only"] = config.visual_only + load_config["kinematic_only"] = config.kinematic_only + load_config["self_collisions"] = config.self_collisions + load_config["prim_type"] = config.prim_type # Run super init super().__init__( relative_prim_path=relative_prim_path, - name=name, + name=config.name, load_config=load_config, ) diff --git a/omnigibson/objects/primitive_object.py b/omnigibson/objects/primitive_object.py index 3f280d2cf..857e103b4 100644 --- a/omnigibson/objects/primitive_object.py +++ b/omnigibson/objects/primitive_object.py @@ -4,6 +4,7 @@ import omnigibson.lazy as lazy from omnigibson.objects.stateful_object import StatefulObject from omnigibson.utils.constants import PRIMITIVE_MESH_TYPES, PrimType +from omnigibson.configs.base_config import PrimitiveObjectConfig from omnigibson.utils.physx_utils import bind_material from omnigibson.utils.python_utils import assert_valid_key from omnigibson.utils.render_utils import create_pbr_material @@ -25,94 +26,24 @@ class PrimitiveObject(StatefulObject): PrimitiveObjects are objects defined by a single geom, e.g: sphere, mesh, cube, etc. """ - def __init__( - self, - name, - primitive_type, - relative_prim_path=None, - category="object", - scale=None, - visible=True, - fixed_base=False, - visual_only=False, - kinematic_only=None, - self_collisions=False, - prim_type=PrimType.RIGID, - load_config=None, - abilities=None, - include_default_states=True, - rgba=(0.5, 0.5, 0.5, 1.0), - radius=None, - height=None, - size=None, - **kwargs, - ): + def __init__(self, config: PrimitiveObjectConfig): """ Args: - name (str): Name for the object. Names need to be unique per scene - primitive_type (str): type of primitive object to create. Should be one of: - {"Cone", "Cube", "Cylinder", "Disk", "Plane", "Sphere", "Torus"} - relative_prim_path (None or str): The path relative to its scene prim for this object. If not specified, it defaults to /. - category (str): Category for the object. Defaults to "object". - scale (None or float or 3-array): if specified, sets either the uniform (float) or x,y,z (3-array) scale - for this object. A single number corresponds to uniform scaling along the x,y,z axes, whereas a - 3-array specifies per-axis scaling. - visible (bool): whether to render this object or not in the stage - fixed_base (bool): whether to fix the base of this object or not - visual_only (bool): Whether this object should be visual only (and not collide with any other objects) - kinematic_only (None or bool): Whether this object should be kinematic only (and not get affected by any - collisions). If None, then this value will be set to True if @fixed_base is True and some other criteria - are satisfied (see object_base.py post_load function), else False. - self_collisions (bool): Whether to enable self collisions for this object - prim_type (PrimType): Which type of prim the object is, Valid options are: {PrimType.RIGID, PrimType.CLOTH} - load_config (None or dict): If specified, should contain keyword-mapped values that are relevant for - loading this prim at runtime. - abilities (None or dict): If specified, manually adds specific object states to this object. It should be - a dict in the form of {ability: {param: value}} containing object abilities and parameters to pass to - the object state instance constructor.rgba (4-array): (R, G, B, A) values to set for this object - include_default_states (bool): whether to include the default object states from @get_default_states - radius (None or float): If specified, sets the radius for this object. This value is scaled by @scale - Note: Should only be specified if the @primitive_type is one of {"Cone", "Cylinder", "Disk", "Sphere"} - height (None or float): If specified, sets the height for this object. This value is scaled by @scale - Note: Should only be specified if the @primitive_type is one of {"Cone", "Cylinder"} - size (None or float): If specified, sets the size for this object. This value is scaled by @scale - Note: Should only be specified if the @primitive_type is one of {"Cube", "Torus"} - kwargs (dict): Additional keyword arguments that are used for other super() calls from subclasses, allowing - for flexible compositions of various object subclasses (e.g.: Robot is USDObject + ControllableObject). + config (PrimitiveObjectConfig): Configuration object for this primitive object """ - # Compose load config and add rgba values - load_config = dict() if load_config is None else load_config - load_config["color"] = rgba[:3] - load_config["opacity"] = rgba[3] - load_config["radius"] = radius - load_config["height"] = height - load_config["size"] = size + # Make sure primitive type is valid + assert_valid_key(key=config.primitive_type, valid_keys=PRIMITIVE_MESH_TYPES, name="primitive mesh type") + + # Store config + self._config = config # Initialize other internal variables self._vis_geom = None self._col_geom = None self._extents = th.ones(3) # (x,y,z extents) - # Make sure primitive type is valid - assert_valid_key(key=primitive_type, valid_keys=PRIMITIVE_MESH_TYPES, name="primitive mesh type") - self._primitive_type = primitive_type - - super().__init__( - relative_prim_path=relative_prim_path, - name=name, - category=category, - scale=scale, - visible=visible, - fixed_base=fixed_base, - visual_only=visual_only, - kinematic_only=kinematic_only, - self_collisions=self_collisions, - prim_type=prim_type, - include_default_states=include_default_states, - load_config=load_config, - abilities=abilities, - **kwargs, - ) + # Run super init with config + super().__init__(config=config) def _load(self): # Define an Xform at the specified path @@ -121,10 +52,10 @@ def _load(self): # Define a nested mesh corresponding to the root link for this prim og.sim.stage.DefinePrim(f"{self.prim_path}/base_link", "Xform") self._vis_geom = create_primitive_mesh( - prim_path=f"{self.prim_path}/base_link/visuals", primitive_type=self._primitive_type + prim_path=f"{self.prim_path}/base_link/visuals", primitive_type=self._config.primitive_type ) self._col_geom = create_primitive_mesh( - prim_path=f"{self.prim_path}/base_link/collisions", primitive_type=self._primitive_type + prim_path=f"{self.prim_path}/base_link/collisions", primitive_type=self._config.primitive_type ) # Add collision API to collision geom @@ -142,15 +73,15 @@ def _load(self): def _post_load(self): # Possibly set scalings (only if the scale value is not set) - if self._load_config["scale"] is not None: + if self._config.scale is not None: log.warning("Custom scale specified for primitive object, so ignoring radius, height, and size arguments!") else: - if self._load_config["radius"] is not None: - self.radius = self._load_config["radius"] - if self._load_config["height"] is not None: - self.height = self._load_config["height"] - if self._load_config["size"] is not None: - self.size = self._load_config["size"] + if self._config.radius is not None: + self.radius = self._config.radius + if self._config.height is not None: + self.height = self._config.height + if self._config.size is not None: + self.size = self._config.size # This step might will perform cloth remeshing if self._prim_type == PrimType.CLOTH. # Therefore, we need to apply size, radius, and height before this to scale the points properly. @@ -159,9 +90,9 @@ def _post_load(self): # Cloth primitive does not have collision meshes if self._prim_type != PrimType.CLOTH: # Set the collision approximation appropriately - if self._primitive_type == "Sphere": + if self._config.primitive_type == "Sphere": col_approximation = "boundingSphere" - elif self._primitive_type == "Cube": + elif self._config.primitive_type == "Cube": col_approximation = "boundingCube" else: col_approximation = "convexHull" @@ -179,11 +110,9 @@ def _initialize(self): else: raise ValueError("Prim type must either be PrimType.RIGID or PrimType.CLOTH for loading a primitive object") - visual_geom_prim.color = self._load_config["color"] + visual_geom_prim.color = self._config.rgba[:3] visual_geom_prim.opacity = ( - self._load_config["opacity"].item() - if isinstance(self._load_config["opacity"], th.Tensor) - else self._load_config["opacity"] + self._config.rgba[3].item() if isinstance(self._config.rgba[3], th.Tensor) else self._config.rgba[3] ) @property @@ -196,7 +125,9 @@ def radius(self): Returns: float: radius for this object """ - assert_valid_key(key=self._primitive_type, valid_keys=VALID_RADIUS_OBJECTS, name="primitive object with radius") + assert_valid_key( + key=self._config.primitive_type, valid_keys=VALID_RADIUS_OBJECTS, name="primitive object with radius" + ) return self._extents[0] / 2.0 @radius.setter @@ -209,12 +140,14 @@ def radius(self, radius): Args: radius (float): radius to set """ - assert_valid_key(key=self._primitive_type, valid_keys=VALID_RADIUS_OBJECTS, name="primitive object with radius") + assert_valid_key( + key=self._config.primitive_type, valid_keys=VALID_RADIUS_OBJECTS, name="primitive object with radius" + ) # Update the extents variable original_extent = self._extents.clone() self._extents = ( th.ones(3) * radius * 2.0 - if self._primitive_type == "Sphere" + if self._config.primitive_type == "Sphere" else th.tensor([radius * 2.0, radius * 2.0, self._extents[2]]) ) attr_pairs = [] @@ -236,7 +169,7 @@ def radius(self, radius): scaling_factor = 2.0 * radius / original_extent[0] for attr, vals in attr_pairs: # If this is a sphere, modify all 3 axes - if self._primitive_type == "Sphere": + if self._config.primitive_type == "Sphere": vals = vals * scaling_factor # Otherwise, just modify the first two dimensions else: @@ -254,7 +187,9 @@ def height(self): Returns: float: height for this object """ - assert_valid_key(key=self._primitive_type, valid_keys=VALID_HEIGHT_OBJECTS, name="primitive object with height") + assert_valid_key( + key=self._config.primitive_type, valid_keys=VALID_HEIGHT_OBJECTS, name="primitive object with height" + ) return self._extents[2] @height.setter @@ -267,7 +202,9 @@ def height(self, height): Args: height (float): height to set """ - assert_valid_key(key=self._primitive_type, valid_keys=VALID_HEIGHT_OBJECTS, name="primitive object with height") + assert_valid_key( + key=self._config.primitive_type, valid_keys=VALID_HEIGHT_OBJECTS, name="primitive object with height" + ) # Update the extents variable original_extent = self._extents.clone() self._extents[2] = height @@ -300,7 +237,9 @@ def size(self): Returns: float: size for this object """ - assert_valid_key(key=self._primitive_type, valid_keys=VALID_SIZE_OBJECTS, name="primitive object with size") + assert_valid_key( + key=self._config.primitive_type, valid_keys=VALID_SIZE_OBJECTS, name="primitive object with size" + ) return self._extents[0] @size.setter @@ -313,7 +252,9 @@ def size(self, size): Args: size (float): size to set """ - assert_valid_key(key=self._primitive_type, valid_keys=VALID_SIZE_OBJECTS, name="primitive object with size") + assert_valid_key( + key=self._config.primitive_type, valid_keys=VALID_SIZE_OBJECTS, name="primitive object with size" + ) # Update the extents variable original_extent = self._extents.clone() @@ -336,38 +277,22 @@ def size(self, size): ) ) - def _create_prim_with_same_kwargs(self, relative_prim_path, name, load_config): - # Add additional kwargs (bounding_box is already captured in load_config) - return self.__class__( - relative_prim_path=relative_prim_path, - primitive_type=self._primitive_type, - name=name, - category=self.category, - scale=self.scale, - visible=self.visible, - fixed_base=self.fixed_base, - prim_type=self._prim_type, - load_config=load_config, - abilities=self._abilities, - visual_only=self._visual_only, - ) - def _dump_state(self): state = super()._dump_state() # state["extents"] = self._extents - state["radius"] = self.radius if self._primitive_type in VALID_RADIUS_OBJECTS else -1 - state["height"] = self.height if self._primitive_type in VALID_HEIGHT_OBJECTS else -1 - state["size"] = self.size if self._primitive_type in VALID_SIZE_OBJECTS else -1 + state["radius"] = self.radius if self._config.primitive_type in VALID_RADIUS_OBJECTS else -1 + state["height"] = self.height if self._config.primitive_type in VALID_HEIGHT_OBJECTS else -1 + state["size"] = self.size if self._config.primitive_type in VALID_SIZE_OBJECTS else -1 return state def _load_state(self, state): super()._load_state(state=state) # self._extents = th.tensor(state["extents"]) - if self._primitive_type in VALID_RADIUS_OBJECTS: + if self._config.primitive_type in VALID_RADIUS_OBJECTS: self.radius = state["radius"] - if self._primitive_type in VALID_HEIGHT_OBJECTS: + if self._config.primitive_type in VALID_HEIGHT_OBJECTS: self.height = state["height"] - if self._primitive_type in VALID_SIZE_OBJECTS: + if self._config.primitive_type in VALID_SIZE_OBJECTS: self.size = state["size"] def deserialize(self, state): diff --git a/omnigibson/objects/stateful_object.py b/omnigibson/objects/stateful_object.py index 54e3ba96f..9f1ffce5e 100644 --- a/omnigibson/objects/stateful_object.py +++ b/omnigibson/objects/stateful_object.py @@ -4,6 +4,8 @@ import torch as th from bddl.object_taxonomy import ObjectTaxonomy +from omnigibson.configs.base_config import StatefulObjectConfig + import omnigibson as og import omnigibson.lazy as lazy from omnigibson.macros import create_module_macros, gm @@ -25,7 +27,7 @@ from omnigibson.object_states.on_fire import OnFire from omnigibson.objects.object_base import BaseObject from omnigibson.renderer_settings.renderer_settings import RendererSettings -from omnigibson.utils.constants import EmitterType, PrimType +from omnigibson.utils.constants import EmitterType from omnigibson.utils.python_utils import classproperty, extract_class_init_kwargs_from_dict from omnigibson.utils.ui_utils import create_module_logger @@ -63,79 +65,29 @@ def __call__(self): class StatefulObject(BaseObject): """Objects that support object states.""" - def __init__( - self, - name, - relative_prim_path=None, - category="object", - scale=None, - visible=True, - fixed_base=False, - visual_only=False, - kinematic_only=None, - self_collisions=False, - prim_type=PrimType.RIGID, - load_config=None, - abilities=None, - include_default_states=True, - **kwargs, - ): + def __init__(self, config: StatefulObjectConfig): """ Args: - name (str): Name for the object. Names need to be unique per scene - relative_prim_path (None or str): The path relative to its scene prim for this object. If not specified, it defaults to /. - category (str): Category for the object. Defaults to "object". - scale (None or float or 3-array): if specified, sets either the uniform (float) or x,y,z (3-array) scale - for this object. A single number corresponds to uniform scaling along the x,y,z axes, whereas a - 3-array specifies per-axis scaling. - visible (bool): whether to render this object or not in the stage - fixed_base (bool): whether to fix the base of this object or not - visual_only (bool): Whether this object should be visual only (and not collide with any other objects) - kinematic_only (None or bool): Whether this object should be kinematic only (and not get affected by any - collisions). If None, then this value will be set to True if @fixed_base is True and some other criteria - are satisfied (see object_base.py post_load function), else False. - self_collisions (bool): Whether to enable self collisions for this object - prim_type (PrimType): Which type of prim the object is, Valid options are: {PrimType.RIGID, PrimType.CLOTH} - load_config (None or dict): If specified, should contain keyword-mapped values that are relevant for - loading this prim at runtime. - abilities (None or dict): If specified, manually adds specific object states to this object. It should be - a dict in the form of {ability: {param: value}} containing object abilities and parameters to pass to - the object state instance constructor. - include_default_states (bool): whether to include the default object states from @get_default_states - kwargs (dict): Additional keyword arguments that are used for other super() calls from subclasses, allowing - for flexible compositions of various object subclasses (e.g.: Robot is USDObject + ControllableObject). + config (StatefulObjectConfig): Configuration object for this stateful object """ # Values that will be filled later self._states = None self._emitters = dict() self._visual_states = None self._current_texture_state = None - self._include_default_states = include_default_states + self._config = config # Load abilities from taxonomy if needed & possible - if abilities is None: - abilities = {} - taxonomy_class = OBJECT_TAXONOMY.get_synset_from_category(category) + self._abilities = {} + if config.abilities is None: + taxonomy_class = OBJECT_TAXONOMY.get_synset_from_category(config.category) if taxonomy_class is not None: - abilities = OBJECT_TAXONOMY.get_abilities(taxonomy_class) - assert isinstance(abilities, dict), "Object abilities must be in dictionary form." - self._abilities = abilities + self._abilities = OBJECT_TAXONOMY.get_abilities(taxonomy_class) + else: + self._abilities = config.abilities # Run super init - super().__init__( - relative_prim_path=relative_prim_path, - name=name, - category=category, - scale=scale, - visible=visible, - fixed_base=fixed_base, - visual_only=visual_only, - kinematic_only=kinematic_only, - self_collisions=self_collisions, - prim_type=prim_type, - load_config=load_config, - **kwargs, - ) + super().__init__(config=config) def _post_load(self): # Run super first @@ -188,6 +140,14 @@ def states(self): """ return self._states + @property + def config(self) -> StatefulObjectConfig: + """ + Returns: + StatefulObjectConfig: Configuration object for this stateful object + """ + return self._config + @property def abilities(self): """ @@ -222,7 +182,7 @@ def prepare_object_states(self): """ states_info = ( {state_type: {"ability": None, "params": dict()} for state_type in get_default_states()} - if self._include_default_states + if self.config.include_default_states else dict() ) diff --git a/omnigibson/objects/usd_object.py b/omnigibson/objects/usd_object.py index f4d87a3d6..68c3d6665 100644 --- a/omnigibson/objects/usd_object.py +++ b/omnigibson/objects/usd_object.py @@ -2,9 +2,9 @@ import tempfile import omnigibson as og +from omnigibson.configs.base_config import USDObjectConfig from omnigibson.objects.stateful_object import StatefulObject from omnigibson.utils.asset_utils import decrypt_file -from omnigibson.utils.constants import PrimType from omnigibson.utils.usd_utils import add_asset_to_stage @@ -14,72 +14,17 @@ class USDObject(StatefulObject): or more links and joints. They may or may not be passive. """ - def __init__( - self, - name, - usd_path, - encrypted=False, - relative_prim_path=None, - category="object", - scale=None, - visible=True, - fixed_base=False, - visual_only=False, - kinematic_only=None, - self_collisions=False, - prim_type=PrimType.RIGID, - load_config=None, - abilities=None, - include_default_states=True, - **kwargs, - ): + def __init__(self, config: USDObjectConfig): """ Args: - name (str): Name for the object. Names need to be unique per scene - usd_path (str): global path to the USD file to load - encrypted (bool): whether this file is encrypted (and should therefore be decrypted) or not - relative_prim_path (None or str): The path relative to its scene prim for this object. If not specified, it defaults to /. - category (str): Category for the object. Defaults to "object". - scale (None or float or 3-array): if specified, sets either the uniform (float) or x,y,z (3-array) scale - for this object. A single number corresponds to uniform scaling along the x,y,z axes, whereas a - 3-array specifies per-axis scaling. - visible (bool): whether to render this object or not in the stage - fixed_base (bool): whether to fix the base of this object or not - visual_only (bool): Whether this object should be visual only (and not collide with any other objects) - kinematic_only (None or bool): Whether this object should be kinematic only (and not get affected by any - collisions). If None, then this value will be set to True if @fixed_base is True and some other criteria - are satisfied (see object_base.py post_load function), else False. - self_collisions (bool): Whether to enable self collisions for this object - prim_type (PrimType): Which type of prim the object is, Valid options are: {PrimType.RIGID, PrimType.CLOTH} - load_config (None or dict): If specified, should contain keyword-mapped values that are relevant for - loading this prim at runtime. - abilities (None or dict): If specified, manually adds specific object states to this object. It should be - a dict in the form of {ability: {param: value}} containing object abilities and parameters to pass to - the object state instance constructor. - include_default_states (bool): whether to include the default object states from @get_default_states - kwargs (dict): Additional keyword arguments that are used for other super() calls from subclasses, allowing - for flexible compositions of various object subclasses (e.g.: Robot is USDObject + ControllableObject). - Note that this base object does NOT pass kwargs down into the Prim-type super() classes, and we assume - that kwargs are only shared between all SUBclasses (children), not SUPERclasses (parents). + config (USDObjectConfig): Configuration object containing all relevant parameters for + initializing this USD object. See the USDObjectConfig dataclass for specific parameters. """ - self._usd_path = usd_path - self._encrypted = encrypted - super().__init__( - relative_prim_path=relative_prim_path, - name=name, - category=category, - scale=scale, - visible=visible, - fixed_base=fixed_base, - visual_only=visual_only, - kinematic_only=kinematic_only, - self_collisions=self_collisions, - prim_type=prim_type, - include_default_states=include_default_states, - load_config=load_config, - abilities=abilities, - **kwargs, - ) + # Store config + self._config = config + + # Initialize super + super().__init__(config=config) def prebuild(self, stage): # Load the object into the given USD stage @@ -112,16 +57,16 @@ def _load(self): """ Load the object into pybullet and set it to the correct pose """ - usd_path = self._usd_path - if self._encrypted: + usd_path = self._config.usd_path + if self._config.encrypted: # Create a temporary file to store the decrytped asset, load it, and then delete it - encrypted_filename = self._usd_path.replace(".usd", ".encrypted.usd") - decrypted_fd, usd_path = tempfile.mkstemp(os.path.basename(self._usd_path), dir=og.tempdir) + encrypted_filename = self._config.usd_path.replace(".usd", ".encrypted.usd") + decrypted_fd, usd_path = tempfile.mkstemp(os.path.basename(self._config.usd_path), dir=og.tempdir) decrypt_file(encrypted_filename, usd_path) prim = add_asset_to_stage(asset_path=usd_path, prim_path=self.prim_path) - if self._encrypted: + if self._config.encrypted: os.close(decrypted_fd) # On Windows, Isaac Sim won't let go of the file until the prim is removed, so we can't delete it. if os.name == "posix": @@ -129,27 +74,11 @@ def _load(self): return prim - def _create_prim_with_same_kwargs(self, relative_prim_path, name, load_config): - # Add additional kwargs - return self.__class__( - relative_prim_path=relative_prim_path, - usd_path=self._usd_path, - name=name, - category=self.category, - scale=self.scale, - visible=self.visible, - fixed_base=self.fixed_base, - visual_only=self._visual_only, - prim_type=self._prim_type, - load_config=load_config, - abilities=self._abilities, - ) - @property def usd_path(self): """ Returns: str: absolute path to this model's USD file. By default, this is the loaded usd path - passed in as an argument + from the config """ - return self._usd_path + return self._config.usd_path diff --git a/omnigibson/prims/cloth_prim.py b/omnigibson/prims/cloth_prim.py index ee0a875ef..29e549f89 100644 --- a/omnigibson/prims/cloth_prim.py +++ b/omnigibson/prims/cloth_prim.py @@ -11,6 +11,7 @@ from functools import cached_property import torch as th +from omnigibson.configs.prim_config import ClothPrimConfig import omnigibson as og import omnigibson.lazy as lazy @@ -54,9 +55,7 @@ class ClothPrim(GeomPrim): def __init__( self, - relative_prim_path, - name, - load_config=None, + config: ClothPrimConfig, ): # Internal vars stored self._centroid_idx = None @@ -65,9 +64,7 @@ def __init__( # Run super init super().__init__( - relative_prim_path=relative_prim_path, - name=name, - load_config=load_config, + config=config, ) def _post_load(self): diff --git a/omnigibson/prims/entity_prim.py b/omnigibson/prims/entity_prim.py index b1dc20cc1..1065b8c8e 100644 --- a/omnigibson/prims/entity_prim.py +++ b/omnigibson/prims/entity_prim.py @@ -4,6 +4,7 @@ import networkx as nx import torch as th +from omnigibson.configs.prim_config import EntityPrimConfig import omnigibson as og import omnigibson.lazy as lazy @@ -45,9 +46,7 @@ class EntityPrim(XFormPrim): def __init__( self, - relative_prim_path, - name, - load_config=None, + config: EntityPrimConfig, ): # Other values that will be filled in at runtime self._root_link_name = None # Name of the root link @@ -62,15 +61,15 @@ def __init__( # This needs to be initialized to be used for _load() of PrimitiveObject self._prim_type = ( - load_config["prim_type"] if load_config is not None and "prim_type" in load_config else PrimType.RIGID + config.load_config["prim_type"] + if config.load_config is not None and "prim_type" in config.load_config + else PrimType.RIGID ) assert self._prim_type in iter(PrimType), f"Unknown prim type {self._prim_type}!" # Run super init super().__init__( - relative_prim_path=relative_prim_path, - name=name, - load_config=load_config, + config=config, ) def _initialize(self): @@ -1659,10 +1658,3 @@ def deserialize(self, state): idx += self.n_joints return state_dict, idx - - def _create_prim_with_same_kwargs(self, relative_prim_path, name, load_config): - # Subclass must implement this method for duplication functionality - raise NotImplementedError( - "Subclass must implement _create_prim_with_same_kwargs() to enable duplication " - "functionality for EntityPrim!" - ) diff --git a/omnigibson/prims/geom_prim.py b/omnigibson/prims/geom_prim.py index a27e8ef1f..cb978599d 100644 --- a/omnigibson/prims/geom_prim.py +++ b/omnigibson/prims/geom_prim.py @@ -1,6 +1,7 @@ from functools import cached_property import torch as th +from omnigibson.configs.prim_config import GeomPrimConfig, CollisionGeomPrimConfig import omnigibson as og import omnigibson.lazy as lazy @@ -27,15 +28,11 @@ class GeomPrim(XFormPrim): def __init__( self, - relative_prim_path, - name, - load_config=None, + config: GeomPrimConfig, ): # Run super method super().__init__( - relative_prim_path=relative_prim_path, - name=name, - load_config=load_config, + config=config, ) def _load(self): @@ -194,9 +191,7 @@ def extent(self): class CollisionGeomPrim(GeomPrim): def __init__( self, - relative_prim_path, - name, - load_config=None, + config: CollisionGeomPrimConfig, ): # Store values created at runtime self._collision_api = None @@ -206,9 +201,7 @@ def __init__( # Run super method super().__init__( - relative_prim_path=relative_prim_path, - name=name, - load_config=load_config, + config=config, ) def _post_load(self): diff --git a/omnigibson/prims/joint_prim.py b/omnigibson/prims/joint_prim.py index c3769231b..ce1484863 100644 --- a/omnigibson/prims/joint_prim.py +++ b/omnigibson/prims/joint_prim.py @@ -1,6 +1,7 @@ from collections.abc import Iterable import torch as th +from omnigibson.configs.prim_config import JointPrimConfig import omnigibson as og import omnigibson.lazy as lazy @@ -63,9 +64,7 @@ class JointPrim(BasePrim): def __init__( self, - relative_prim_path, - name, - load_config=None, + config: JointPrimConfig, articulation_view=None, ): # Grab dynamic control reference and set properties @@ -88,9 +87,7 @@ def __init__( # Run super method super().__init__( - relative_prim_path=relative_prim_path, - name=name, - load_config=load_config, + config=config, ) def _load(self): diff --git a/omnigibson/prims/material_prim.py b/omnigibson/prims/material_prim.py index 3098658a3..82ae5f2e7 100644 --- a/omnigibson/prims/material_prim.py +++ b/omnigibson/prims/material_prim.py @@ -4,6 +4,7 @@ import torch as th import omnigibson as og +from omnigibson.configs.prim_config import PrimConfig import omnigibson.lazy as lazy from omnigibson.prims.prim_base import BasePrim from omnigibson.utils.physx_utils import bind_material @@ -54,7 +55,12 @@ def get_material(cls, scene, name, prim_path, load_config=None): # Otherwise, create a new one and return it relative_prim_path = absolute_prim_path_to_scene_relative(scene, prim_path) - new_material = cls(relative_prim_path=relative_prim_path, name=name, load_config=load_config) + config = PrimConfig( + name=name, + relative_prim_path=relative_prim_path, + load_config=load_config, + ) + new_material = cls(config=config) new_material.load(scene) assert ( new_material.prim_path == prim_path @@ -64,9 +70,7 @@ def get_material(cls, scene, name, prim_path, load_config=None): def __init__( self, - relative_prim_path, - name, - load_config=None, + config, ): # Other values that will be filled in at runtime self._shader = None @@ -76,9 +80,7 @@ def __init__( # Run super init super().__init__( - relative_prim_path=relative_prim_path, - name=name, - load_config=load_config, + config=config, ) def _load(self): diff --git a/omnigibson/prims/prim_base.py b/omnigibson/prims/prim_base.py index 0331a736a..c8acad7fe 100644 --- a/omnigibson/prims/prim_base.py +++ b/omnigibson/prims/prim_base.py @@ -30,18 +30,18 @@ class BasePrim(Serializable, Recreatable, ABC): def __init__( self, - relative_prim_path, - name, - load_config=None, + config, ): - self._relative_prim_path = relative_prim_path - assert relative_prim_path.startswith("/"), f"Relative prim path {relative_prim_path} must start with a '/'!" + self._relative_prim_path = config.relative_prim_path + assert self._relative_prim_path.startswith( + "/" + ), f"Relative prim path {self._relative_prim_path} must start with a '/'!" assert all( - component[0] in string.ascii_letters for component in relative_prim_path[1:].split("/") - ), f"Each component of relative prim path {relative_prim_path} must start with a letter!" + component[0] in string.ascii_letters for component in self._relative_prim_path[1:].split("/") + ), f"Each component of relative prim path {self._relative_prim_path} must start with a letter!" - self._name = name - self._load_config = dict() if load_config is None else load_config + self._name = config.name + self._load_config = config.load_config # Other values that will be filled in at runtime self._scene = None @@ -277,22 +277,3 @@ def get_custom_data(self): dict: Dictionary of any custom information """ return self._prim.GetCustomData() - - def _create_prim_with_same_kwargs(self, relative_prim_path, name, load_config): - """ - Generates a new instance of this prim's class with specified @relative_prim_path, @name, and @load_config, but otherwise - all other kwargs should be identical to this instance's values. - - Args: - relative_prim_path (str): Scene-local prim path of the Prim to encapsulate or create. - name (str): Name for the newly created prim - load_config (dict): Keyword-mapped kwargs to use to set specific attributes for the created prim's instance - - Returns: - BasePrim: Generated prim object (not loaded, and not initialized!) - """ - return self.__class__( - relative_prim_path=relative_prim_path, - name=name, - load_config=load_config, - ) diff --git a/omnigibson/prims/rigid_prim.py b/omnigibson/prims/rigid_prim.py index 1d547fdd7..e14ab73d2 100644 --- a/omnigibson/prims/rigid_prim.py +++ b/omnigibson/prims/rigid_prim.py @@ -3,6 +3,7 @@ from typing import Literal import torch as th +from omnigibson.configs.prim_config import RigidPrimConfig from scipy.spatial import ConvexHull import omnigibson as og @@ -60,9 +61,7 @@ class RigidPrim(XFormPrim): def __init__( self, - relative_prim_path, - name, - load_config=None, + config: RigidPrimConfig, ): # Other values that will be filled in at runtime self._rigid_prim_view_direct = None @@ -79,9 +78,7 @@ def __init__( # Run super init super().__init__( - relative_prim_path=relative_prim_path, - name=name, - load_config=load_config, + config=config, ) def _post_load(self): diff --git a/omnigibson/prims/xform_prim.py b/omnigibson/prims/xform_prim.py index 27f71f8f0..e4e9d6616 100644 --- a/omnigibson/prims/xform_prim.py +++ b/omnigibson/prims/xform_prim.py @@ -3,6 +3,7 @@ from typing import Literal import torch as th +from omnigibson.configs.prim_config import XFormPrimConfig import omnigibson as og import omnigibson.lazy as lazy @@ -39,9 +40,7 @@ class XFormPrim(BasePrim): def __init__( self, - relative_prim_path, - name, - load_config=None, + config: XFormPrimConfig, ): # Other values that will be filled in at runtime self._material = None @@ -49,9 +48,7 @@ def __init__( # Run super method super().__init__( - relative_prim_path=relative_prim_path, - name=name, - load_config=load_config, + config=config, ) def _load(self): diff --git a/omnigibson/robots/husky.py b/omnigibson/robots/husky.py index 9f0dfc0a5..31b620f49 100644 --- a/omnigibson/robots/husky.py +++ b/omnigibson/robots/husky.py @@ -1,8 +1,8 @@ from functools import cached_property - import torch as th from omnigibson.robots.locomotion_robot import LocomotionRobot +from omnigibson.configs.robot_config import HuskyConfig class Husky(LocomotionRobot): @@ -11,6 +11,22 @@ class Husky(LocomotionRobot): Reference: https://clearpathrobotics.com/, http://wiki.ros.org/Robots/Husky """ + def __init__( + self, + config: HuskyConfig, + **kwargs, + ): + """ + Args: + config (HuskyConfig): Configuration object for the robot + kwargs (dict): Additional keyword arguments that are used for other super() calls + """ + # Run super init + super().__init__( + config=config, + **kwargs, + ) + def _create_discrete_action_space(self): raise ValueError("Husky does not support discrete actions!") diff --git a/omnigibson/robots/locomotion_robot.py b/omnigibson/robots/locomotion_robot.py index 136189bac..0ebce0a12 100644 --- a/omnigibson/robots/locomotion_robot.py +++ b/omnigibson/robots/locomotion_robot.py @@ -1,7 +1,7 @@ -from abc import abstractmethod from functools import cached_property import torch as th +from omegaconf import MISSING from omnigibson.controllers import LocomotionController from omnigibson.robots.robot_base import BaseRobot @@ -14,17 +14,21 @@ class LocomotionRobot(BaseRobot): Robot that is is equipped with locomotive (navigational) capabilities. Provides common interface for a wide variety of robots. - NOTE: controller_config should, at the minimum, contain: - base: controller specifications for the controller to control this robot's base (locomotion). - Should include: - - - name: Controller to create - - relevant to the controller being created. Note that all values will have default - values specified, but setting these individual kwargs will override them - + Args: + config (LocomotionRobotConfig): Configuration object containing: + - base_joint_names: List of joint names for base control + - base_control_idx: List of control indices for base joints + - controllers: Dict of controller configurations including: + base: Controller specifications for locomotion control + - name: Controller to create + - relevant to the controller being created """ def _validate_configuration(self): + # Make sure base joint configuration is specified + assert self.config.base_joint_names is not MISSING, "base_joint_names must be specified in config!" + assert self.config.base_control_idx is not MISSING, "base_control_idx must be specified in config!" + # We make sure that our base controller exists and is a locomotion controller assert ( "base" in self._controllers @@ -206,25 +210,13 @@ def base_action_idx(self): action_start_idx = sum([self.controllers[self.controller_order[i]].command_dim for i in range(controller_idx)]) return th.arange(action_start_idx, action_start_idx + self.controllers["base"].command_dim) - @property - @abstractmethod - def base_joint_names(self): - """ - Returns: - list: Array of joint names corresponding to this robot's base joints (e.g.: wheels). - - Note: the ordering within the list is assumed to be intentional, and is - directly used to define the set of corresponding control idxs. - """ - raise NotImplementedError - @cached_property def base_control_idx(self): """ Returns: n-array: Indices in low-level control vector corresponding to base joints. """ - return th.tensor([list(self.joints.keys()).index(name) for name in self.base_joint_names]) + return th.tensor(self.config.base_control_idx) @classproperty def _do_not_register_classes(cls): diff --git a/omnigibson/robots/two_wheel_robot.py b/omnigibson/robots/two_wheel_robot.py index 3ba55d2ae..c0dcba42f 100644 --- a/omnigibson/robots/two_wheel_robot.py +++ b/omnigibson/robots/two_wheel_robot.py @@ -1,7 +1,6 @@ -from abc import abstractmethod - import gymnasium as gym import torch as th +from omegaconf import MISSING from omnigibson.controllers import DifferentialDriveController from omnigibson.robots.locomotion_robot import LocomotionRobot @@ -28,6 +27,10 @@ def _validate_configuration(self): # Make sure base only has two indices (i.e.: two wheels for differential drive) assert len(self.base_control_idx) == 2, "Differential drive can only be used with robot with two base joints!" + # Make sure wheel parameters are specified + assert self.config.wheel_radius is not MISSING, "wheel_radius must be specified in config!" + assert self.config.wheel_axle_length is not MISSING, "wheel_axle_length must be specified in config!" + # run super super()._validate_configuration() @@ -107,9 +110,9 @@ def _default_base_differential_drive_controller_config(self): """ return { "name": "DifferentialDriveController", - "control_freq": self._control_freq, - "wheel_radius": self.wheel_radius, - "wheel_axle_length": self.wheel_axle_length, + "control_freq": self.config.control_freq, + "wheel_radius": self.config.wheel_radius, + "wheel_axle_length": self.config.wheel_axle_length, "control_limits": self.control_limits, "dof_idx": self.base_control_idx, } @@ -127,22 +130,20 @@ def _default_controller_config(self): return cfg @property - @abstractmethod def wheel_radius(self): """ Returns: float: radius of each wheel at the base, in metric units """ - raise NotImplementedError + return self.config.wheel_radius @property - @abstractmethod def wheel_axle_length(self): """ Returns: float: perpendicular distance between the robot's two wheels, in metric units """ - raise NotImplementedError + return self.config.wheel_axle_length @classproperty def _do_not_register_classes(cls):