diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index a4f472c5e..0b94de0d5 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -22,4 +22,4 @@ repos: omnigibson/utils/numpy_utils\.py| # Utilities specifically for numpy operations and dtype tests/test_transform_utils\.py # This test file uses Scipy and Numpy )$ - stages: [commit] \ No newline at end of file + stages: [pre-commit] \ No newline at end of file diff --git a/omnigibson/action_primitives/curobo.py b/omnigibson/action_primitives/curobo.py index 5df5a4eb1..0fe436b2b 100644 --- a/omnigibson/action_primitives/curobo.py +++ b/omnigibson/action_primitives/curobo.py @@ -90,14 +90,11 @@ def __init__( MotionGenConfig.load_from_robot_config(...) batch_size (int): Size of batches for computing trajectories. This must be FIXED use_cuda_graph (bool): Whether to use CUDA graph for motion generation or not - collision_activation_distance (float): Distance threshold at which a collision is detected. - Increasing this value will make the motion planner more conservative in its planning with respect - to the underlying sphere representation of the robot debug (bool): Whether to debug generation or not, setting this True will set use_cuda_graph to False implicitly use_default_embodiment_only (bool): Whether to use only the default embodiment for the robot or not - collision_activation_distance (float): Activation distance for collision checking; this affects - 1) how close the computed trajectories can get to obstacles and - 2) how close the robot can get to obstacles during collision checks + collision_activation_distance (float): Distance threshold at which a collision with the world is detected. + Increasing this value will make the motion planner more conservative in its planning with respect + to the underlying sphere representation of the robot. Note that this does not affect self-collisions detection. """ # Only support one scene for now -- verify that this is the case assert len(og.sim.scenes) == 1 @@ -132,6 +129,10 @@ def __init__( self.ee_link = dict() self.additional_links = dict() self.base_link = dict() + + # Grab mapping from robot joint name to index + reset_qpos = self.robot.reset_joint_pos + joint_idx_mapping = {joint.joint_name: i for i, joint in enumerate(self.robot.joints.values())} for emb_sel, robot_cfg_path in robot_cfg_path_dict.items(): content_path = lazy.curobo.types.file_path.ContentPath( robot_config_absolute_path=robot_cfg_path, robot_usd_absolute_path=robot_usd_path @@ -139,6 +140,17 @@ def __init__( robot_cfg_dict = lazy.curobo.cuda_robot_model.util.load_robot_yaml(content_path)["robot_cfg"] robot_cfg_dict["kinematics"]["use_usd_kinematics"] = True + # Automatically populate the locked joints and retract config from the robot values + for joint_name, lock_val in robot_cfg_dict["kinematics"]["lock_joints"].items(): + if lock_val is None: + joint_idx = joint_idx_mapping[joint_name] + robot_cfg_dict["kinematics"]["lock_joints"][joint_name] = reset_qpos[joint_idx] + if robot_cfg_dict["kinematics"]["cspace"]["retract_config"] is None: + robot_cfg_dict["kinematics"]["cspace"]["retract_config"] = [ + reset_qpos[joint_idx_mapping[joint_name]] + for joint_name in robot_cfg_dict["kinematics"]["cspace"]["joint_names"] + ] + self.ee_link[emb_sel] = robot_cfg_dict["kinematics"]["ee_link"] # RobotConfig.from_dict will append ee_link to link_names, so we make a copy here. self.additional_links[emb_sel] = robot_cfg_dict["kinematics"]["link_names"].copy() diff --git a/omnigibson/envs/data_wrapper.py b/omnigibson/envs/data_wrapper.py index 01c4006dc..9d22eafa3 100644 --- a/omnigibson/envs/data_wrapper.py +++ b/omnigibson/envs/data_wrapper.py @@ -29,11 +29,13 @@ class DataWrapper(EnvironmentWrapper): An OmniGibson environment wrapper for writing data to an HDF5 file. """ - def __init__(self, env, output_path, only_successes=True): + def __init__(self, env, output_path, overwrite=True, only_successes=True): """ Args: env (Environment): The environment to wrap output_path (str): path to store hdf5 data file + overwrite (bool): If set, will overwrite any pre-existing data found at @output_path. + Otherwise, will load the data and append to it only_successes (bool): Whether to only save successful episodes """ # Make sure the wrapped environment inherits correct omnigibson format @@ -53,13 +55,17 @@ def __init__(self, env, output_path, only_successes=True): Path(os.path.dirname(output_path)).mkdir(parents=True, exist_ok=True) log.info(f"\nWriting OmniGibson dataset hdf5 to: {output_path}\n") - self.hdf5_file = h5py.File(output_path, "w") - data_grp = self.hdf5_file.create_group("data") - env.task.write_task_metadata() - scene_file = og.sim.save()[0] - config = deepcopy(env.config) - self.add_metadata(group=data_grp, name="config", data=config) - self.add_metadata(group=data_grp, name="scene_file", data=scene_file) + self.hdf5_file = h5py.File(output_path, "w" if overwrite else "a") + if "data" not in set(self.hdf5_file.keys()): + data_grp = self.hdf5_file.create_group("data") + else: + data_grp = self.hdf5_file["data"] + if overwrite or "config" not in set(data_grp.attrs.keys()): + env.task.write_task_metadata() + scene_file = og.sim.save()[0] + config = deepcopy(env.config) + self.add_metadata(group=data_grp, name="config", data=config) + self.add_metadata(group=data_grp, name="scene_file", data=scene_file) # Run super super().__init__(env=env) @@ -73,7 +79,6 @@ def step(self, action, n_render_iterations=1): n_render_iterations (int): Number of rendering iterations to use before returning observations Returns: - 5-tuple: 5-tuple: - dict: state, i.e. next observation - float: reward, i.e. reward at this current timestep @@ -277,7 +282,14 @@ class DataCollectionWrapper(DataWrapper): """ def __init__( - self, env, output_path, viewport_camera_path="/World/viewer_camera", only_successes=True, use_vr=False + self, + env, + output_path, + viewport_camera_path="/World/viewer_camera", + overwrite=True, + only_successes=True, + use_vr=False, + obj_attr_keys=None, ): """ Args: @@ -285,14 +297,25 @@ def __init__( output_path (str): path to store hdf5 data file viewport_camera_path (str): prim path to the camera to use when rendering the main viewport during data collection + overwrite (bool): If set, will overwrite any pre-existing data found at @output_path. + Otherwise, will load the data and append to it only_successes (bool): Whether to only save successful episodes use_vr (bool): Whether to use VR headset for data collection + obj_attr_keys (None or list of str): If set, a list of object attributes that should be + cached at the beginning of every episode, e.g.: "scale", "visible", etc. This is useful + for domain randomization settings where specific object attributes not directly tied to + the object's runtime kinematic state are being modified once at the beginning of every episode, + while the simulation is stopped. """ # Store additional variables needed for optimized data collection # Denotes the maximum serialized state size for the current episode self.max_state_size = 0 + # Dict capturing serialized per-episode initial information (e.g.: scales / visibilities) about every object + self.obj_attr_keys = [] if obj_attr_keys is None else obj_attr_keys + self.init_metadata = dict() + # Maps episode step ID to dictionary of systems and objects that should be added / removed to the simulator at # the given simulator step. See add_transition_info() for more info self.current_transitions = dict() @@ -315,7 +338,12 @@ def __init__( ) # Run super - super().__init__(env=env, output_path=output_path, only_successes=only_successes) + super().__init__( + env=env, + output_path=output_path, + overwrite=overwrite, + only_successes=only_successes, + ) # Configure the simulator to optimize for data collection self._optimize_sim_for_data_collection(viewport_camera_path=viewport_camera_path) @@ -390,6 +418,17 @@ def reset(self): # Update max state size self.max_state_size = max(self.max_state_size, len(state)) + # Also store initial metadata not recorded in serialized state + # This is simply serialized + metadata = {key: [] for key in self.obj_attr_keys} + for obj in self.scene.objects: + for key in self.obj_attr_keys: + metadata[key].append(getattr(obj, key)) + self.init_metadata = { + key: th.stack(vals, dim=0) if isinstance(vals[0], th.Tensor) else th.tensor(vals, dtype=type(vals[0])) + for key, vals in metadata.items() + } + return init_obs, init_info def _parse_step_data(self, action, obs, reward, terminated, truncated, info): @@ -422,6 +461,11 @@ def process_traj_to_hdf5(self, traj_data, traj_grp_name, nested_keys=("obs",)): # Add in transition info self.add_metadata(group=traj_grp, name="transitions", data=self.current_transitions) + # Add initial metadata information + metadata_grp = traj_grp.create_group("init_metadata") + for name, data in self.init_metadata.items(): + metadata_grp.create_dataset(name, data=data) + return traj_grp def flush_current_traj(self): @@ -476,6 +520,7 @@ def create_from_hdf5( include_sensor_names=None, exclude_sensor_names=None, n_render_iterations=5, + overwrite=True, only_successes=False, include_env_wrapper=False, additional_wrapper_configs=None, @@ -509,6 +554,8 @@ def create_from_hdf5( underlying physical state by a few frames, and additionally produces transient visual artifacts when the physical state changes. Increasing this number will improve the rendered quality at the expense of speed. + overwrite (bool): If set, will overwrite any pre-existing data found at @output_path. + Otherwise, will load the data and append to it only_successes (bool): Whether to only save successful episodes include_env_wrapper (bool): Whether to include environment wrapper stored in the underlying env config additional_wrapper_configs (None or list of dict): If specified, list of wrapper config(s) specifying @@ -568,10 +615,19 @@ def create_from_hdf5( input_path=input_path, output_path=output_path, n_render_iterations=n_render_iterations, + overwrite=overwrite, only_successes=only_successes, ) - def __init__(self, env, input_path, output_path, n_render_iterations=5, only_successes=False): + def __init__( + self, + env, + input_path, + output_path, + n_render_iterations=5, + overwrite=True, + only_successes=False, + ): """ Args: env (Environment): The environment to wrap @@ -579,6 +635,8 @@ def __init__(self, env, input_path, output_path, n_render_iterations=5, only_suc output_path (str): path to store output hdf5 data file n_render_iterations (int): Number of rendering iterations to use when loading each stored frame from the recorded data + overwrite (bool): If set, will overwrite any pre-existing data found at @output_path. + Otherwise, will load the data and append to it only_successes (bool): Whether to only save successful episodes """ # Make sure transition rules are DISABLED for playback since we manually propagate transitions @@ -592,7 +650,12 @@ def __init__(self, env, input_path, output_path, n_render_iterations=5, only_suc self.n_render_iterations = n_render_iterations # Run super - super().__init__(env=env, output_path=output_path, only_successes=only_successes) + super().__init__( + env=env, + output_path=output_path, + overwrite=overwrite, + only_successes=only_successes, + ) def _process_obs(self, obs, info): """ @@ -634,6 +697,7 @@ def playback_episode(self, episode_id, record_data=True, video_writer=None, vide # Grab episode data transitions = json.loads(traj_grp.attrs["transitions"]) traj_grp = h5py_group_to_torch(traj_grp) + init_metadata = traj_grp["init_metadata"] action = traj_grp["action"] state = traj_grp["state"] state_size = traj_grp["state_size"] @@ -643,6 +707,15 @@ def playback_episode(self, episode_id, record_data=True, video_writer=None, vide # Reset environment og.sim.restore(scene_files=[self.scene_file]) + + # Reset object attributes from the stored metadata + with og.sim.stopped(): + for attr, vals in init_metadata.items(): + assert len(vals) == self.scene.n_objects + for i, obj in enumerate(self.scene.objects): + for attr, vals in init_metadata.items(): + val = vals[i] + setattr(obj, attr, val.item() if val.ndim == 0 else val) self.reset() # Restore to initial state diff --git a/omnigibson/examples/robots/import_custom_robot.py b/omnigibson/examples/robots/import_custom_robot.py index 4837bae65..78ac40581 100644 --- a/omnigibson/examples/robots/import_custom_robot.py +++ b/omnigibson/examples/robots/import_custom_robot.py @@ -128,7 +128,8 @@ lock_joints: {} # (dict) Maps joint name to "locked" joint configuration. Any joints # specified here will not be considered active when motion planning # NOTE: All gripper joints and non-controllable holonomic joints - # will automatically be added here + # will automatically be added here. Null means that the value will be + # dynamically computed at runtime based on the robot's reset qpos self_collision_ignore: # (dict) Maps link name to list of other ignore links to ignore collisions # with. Note that bi-directional specification is not necessary, # e.g.: "torso_link1" does not need to be specified in @@ -317,27 +318,6 @@ "radius": 0.005 - "center": [0.02, 0.0, -0.007] "radius": 0.003 - default_qpos: # (list of float): Default joint configuration - - 0.0 - - 0.0 - - 0.0 - - 0.0 - - 0.0 - - 0.0 - - 1.906 - - 1.906 - - -0.991 - - -0.991 - - 1.571 - - 1.571 - - 0.915 - - 0.915 - - -1.571 - - -1.571 - - 0.03 - - 0.03 - - 0.03 - - 0.03 """ @@ -755,17 +735,13 @@ def get_joint_upper_limit(root_prim, joint_name): joint_prims = find_all_joints(robot_prim, only_articulated=True) all_joint_names = [joint_prim.GetName() for joint_prim in joint_prims] - retract_cfg = curobo_cfg.default_qpos lock_joints = curobo_cfg.lock_joints.to_dict() if curobo_cfg.lock_joints else {} if is_holonomic: # Move the final six joints to the beginning, since the holonomic joints are added at the end all_joint_names = list(reversed(all_joint_names[-6:])) + all_joint_names[:-6] - retract_cfg = [0] * 6 + retract_cfg - lock_joints["base_footprint_z_joint"] = 0.0 - lock_joints["base_footprint_rx_joint"] = 0.0 - lock_joints["base_footprint_ry_joint"] = 0.0 - - joint_to_default_q = {jnt_name: q for jnt_name, q in zip(all_joint_names, retract_cfg)} + lock_joints["base_footprint_z_joint"] = None + lock_joints["base_footprint_rx_joint"] = None + lock_joints["base_footprint_ry_joint"] = None default_generated_cfg = { "usd_robot_root": f"/{robot_prim.GetName()}", @@ -781,13 +757,13 @@ def get_joint_upper_limit(root_prim, joint_name): "collision_sphere_buffer": 0.002, "extra_collision_spheres": {}, "self_collision_ignore": curobo_cfg.self_collision_ignore.to_dict(), - "self_collision_buffer": {root_link: 0.02}, + "self_collision_buffer": curobo_cfg.self_collision_buffer.to_dict(), "use_global_cumul": True, "mesh_link_names": deepcopy(all_collision_link_names), "external_asset_path": None, "cspace": { "joint_names": all_joint_names, - "retract_config": retract_cfg, + "retract_config": None, "null_space_weight": [1] * len(all_joint_names), "cspace_distance_weight": [1] * len(all_joint_names), "max_jerk": 500.0, @@ -798,7 +774,7 @@ def get_joint_upper_limit(root_prim, joint_name): for eef_link_name, gripper_info in curobo_cfg.eef_to_gripper_info.items(): attached_obj_link_name = f"attached_object_{eef_link_name}" for jnt_name in gripper_info["joints"]: - default_generated_cfg["lock_joints"][jnt_name] = get_joint_upper_limit(robot_prim, jnt_name) + default_generated_cfg["lock_joints"][jnt_name] = None default_generated_cfg["extra_links"][attached_obj_link_name] = { "parent_link_name": eef_link_name, "link_name": attached_obj_link_name, @@ -827,10 +803,10 @@ def get_joint_upper_limit(root_prim, joint_name): base_only_cfg = deepcopy(default_generated_cfg) base_only_cfg["ee_link"] = root_link base_only_cfg["link_names"] = [] - for jnt_name, default_q in joint_to_default_q.items(): + for jnt_name in all_joint_names: if jnt_name not in base_only_cfg["lock_joints"] and "base_footprint" not in jnt_name: # Lock this joint - base_only_cfg["lock_joints"][jnt_name] = default_q + base_only_cfg["lock_joints"][jnt_name] = None save_base_fpath = f"{save_dir}/{robot_name}_description_curobo_base.yaml" with open(save_base_fpath, "w+") as f: yaml.dump({"robot_cfg": {"kinematics": base_only_cfg}}, f) @@ -838,11 +814,19 @@ def get_joint_upper_limit(root_prim, joint_name): # Create arm only config arm_only_cfg = deepcopy(default_generated_cfg) for jnt_name in {"base_footprint_x_joint", "base_footprint_y_joint", "base_footprint_rz_joint"}: - arm_only_cfg["lock_joints"][jnt_name] = 0.0 + arm_only_cfg["lock_joints"][jnt_name] = None save_arm_fpath = f"{save_dir}/{robot_name}_description_curobo_arm.yaml" with open(save_arm_fpath, "w+") as f: yaml.dump({"robot_cfg": {"kinematics": arm_only_cfg}}, f) + # Create arm only no torso config + arm_only_no_torso_cfg = deepcopy(arm_only_cfg) + for jnt_name in curobo_cfg.torso_joints: + arm_only_no_torso_cfg["lock_joints"][jnt_name] = None + save_arm_no_torso_fpath = f"{save_dir}/{robot_name}_description_curobo_arm_no_torso.yaml" + with open(save_arm_no_torso_fpath, "w+") as f: + yaml.dump({"robot_cfg": {"kinematics": arm_only_no_torso_cfg}}, f) + @click.command(help=_DOCSTRING) @click.option( diff --git a/omnigibson/objects/controllable_object.py b/omnigibson/objects/controllable_object.py index 599cba909..572c21cd0 100644 --- a/omnigibson/objects/controllable_object.py +++ b/omnigibson/objects/controllable_object.py @@ -42,6 +42,7 @@ def __init__( visual_only=False, self_collisions=False, prim_type=PrimType.RIGID, + link_physics_materials=None, load_config=None, control_freq=None, controller_config=None, @@ -63,6 +64,10 @@ def __init__( 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} + link_physics_materials (None or dict): If specified, dictionary mapping link name to kwargs used to generate + a specific physical material for that link's collision meshes, where the kwargs are arguments directly + passed into the omni.isaac.core.materials.PhysicsMaterial constructor, e.g.: "static_friction", + "dynamic_friction", and "restitution" 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, @@ -125,6 +130,7 @@ def __init__( visual_only=visual_only, self_collisions=self_collisions, prim_type=prim_type, + link_physics_materials=link_physics_materials, load_config=load_config, **kwargs, ) diff --git a/omnigibson/objects/dataset_object.py b/omnigibson/objects/dataset_object.py index 7e6af6161..dace7cd31 100644 --- a/omnigibson/objects/dataset_object.py +++ b/omnigibson/objects/dataset_object.py @@ -50,6 +50,7 @@ def __init__( kinematic_only=None, self_collisions=False, prim_type=PrimType.RIGID, + link_physics_materials=None, load_config=None, abilities=None, include_default_states=True, @@ -82,6 +83,10 @@ def __init__( 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} + link_physics_materials (None or dict): If specified, dictionary mapping link name to kwargs used to generate + a specific physical material for that link's collision meshes, where the kwargs are arguments directly + passed into the omni.isaac.core.materials.PhysicsMaterial constructor, e.g.: "static_friction", + "dynamic_friction", and "restitution" 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 @@ -136,6 +141,7 @@ def __init__( self_collisions=self_collisions, prim_type=prim_type, include_default_states=include_default_states, + link_physics_materials=link_physics_materials, load_config=load_config, abilities=abilities, **kwargs, diff --git a/omnigibson/objects/light_object.py b/omnigibson/objects/light_object.py index cc97af77f..12912057a 100644 --- a/omnigibson/objects/light_object.py +++ b/omnigibson/objects/light_object.py @@ -35,6 +35,7 @@ def __init__( category="light", scale=None, fixed_base=False, + link_physics_materials=None, load_config=None, abilities=None, include_default_states=True, @@ -52,6 +53,10 @@ def __init__( 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 + link_physics_materials (None or dict): If specified, dictionary mapping link name to kwargs used to generate + a specific physical material for that link's collision meshes, where the kwargs are arguments directly + passed into the omni.isaac.core.materials.PhysicsMaterial constructor, e.g.: "static_friction", + "dynamic_friction", and "restitution" 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 @@ -88,6 +93,7 @@ def __init__( self_collisions=False, prim_type=PrimType.RIGID, include_default_states=include_default_states, + link_physics_materials=link_physics_materials, load_config=load_config, abilities=abilities, **kwargs, diff --git a/omnigibson/objects/object_base.py b/omnigibson/objects/object_base.py index c85a9a917..c738c2b45 100644 --- a/omnigibson/objects/object_base.py +++ b/omnigibson/objects/object_base.py @@ -47,6 +47,7 @@ def __init__( kinematic_only=None, self_collisions=False, prim_type=PrimType.RIGID, + link_physics_materials=None, load_config=None, **kwargs, ): @@ -66,6 +67,10 @@ def __init__( 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} + link_physics_materials (None or dict): If specified, dictionary mapping link name to kwargs used to generate + a specific physical material for that link's collision meshes, where the kwargs are arguments directly + passed into the omni.isaac.core.materials.PhysicsMaterial constructor, e.g.: "static_friction", + "dynamic_friction", and "restitution" load_config (None or dict): If specified, should contain keyword-mapped values that are relevant for loading this prim at runtime. kwargs (dict): Additional keyword arguments that are used for other super() calls from subclasses, allowing @@ -80,6 +85,7 @@ def __init__( self._uuid = get_uuid(name, deterministic=True) self.category = category self.fixed_base = fixed_base + self._link_physics_materials = dict() if link_physics_materials is None else link_physics_materials # Values to be created at runtime self._highlight_cached_values = None @@ -212,6 +218,20 @@ def _post_load(self): self.solver_position_iteration_count = m.DEFAULT_SOLVER_POSITION_ITERATIONS self.solver_velocity_iteration_count = m.DEFAULT_SOLVER_VELOCITY_ITERATIONS + # Add link materials if specified + if self._link_physics_materials is not None: + for link_name, material_info in self._link_physics_materials.items(): + # We will permute the link materials dict in place to now point to the created material + mat_name = f"{link_name}_physics_mat" + physics_mat = lazy.omni.isaac.core.materials.PhysicsMaterial( + prim_path=f"{self.prim_path}/Looks/{mat_name}", + name=mat_name, + **material_info, + ) + for msh in self.links[link_name].collision_meshes.values(): + msh.apply_physics_material(physics_mat) + self._link_physics_materials[link_name] = physics_mat + # Add semantics lazy.omni.isaac.core.utils.semantics.add_update_semantics( prim=self._prim, diff --git a/omnigibson/objects/primitive_object.py b/omnigibson/objects/primitive_object.py index 3f280d2cf..c167bdf41 100644 --- a/omnigibson/objects/primitive_object.py +++ b/omnigibson/objects/primitive_object.py @@ -38,6 +38,7 @@ def __init__( kinematic_only=None, self_collisions=False, prim_type=PrimType.RIGID, + link_physics_materials=None, load_config=None, abilities=None, include_default_states=True, @@ -65,6 +66,10 @@ def __init__( 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} + link_physics_materials (None or dict): If specified, dictionary mapping link name to kwargs used to generate + a specific physical material for that link's collision meshes, where the kwargs are arguments directly + passed into the omni.isaac.core.materials.PhysicsMaterial constructor, e.g.: "static_friction", + "dynamic_friction", and "restitution" 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 @@ -109,6 +114,7 @@ def __init__( self_collisions=self_collisions, prim_type=prim_type, include_default_states=include_default_states, + link_physics_materials=link_physics_materials, load_config=load_config, abilities=abilities, **kwargs, diff --git a/omnigibson/objects/stateful_object.py b/omnigibson/objects/stateful_object.py index 3c0eef163..8e54a72b8 100644 --- a/omnigibson/objects/stateful_object.py +++ b/omnigibson/objects/stateful_object.py @@ -75,6 +75,7 @@ def __init__( kinematic_only=None, self_collisions=False, prim_type=PrimType.RIGID, + link_physics_materials=None, load_config=None, abilities=None, include_default_states=True, @@ -96,6 +97,10 @@ def __init__( 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} + link_physics_materials (None or dict): If specified, dictionary mapping link name to kwargs used to generate + a specific physical material for that link's collision meshes, where the kwargs are arguments directly + passed into the omni.isaac.core.materials.PhysicsMaterial constructor, e.g.: "static_friction", + "dynamic_friction", and "restitution" 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 @@ -133,6 +138,7 @@ def __init__( kinematic_only=kinematic_only, self_collisions=self_collisions, prim_type=prim_type, + link_physics_materials=link_physics_materials, load_config=load_config, **kwargs, ) diff --git a/omnigibson/objects/usd_object.py b/omnigibson/objects/usd_object.py index f4d87a3d6..a15de280a 100644 --- a/omnigibson/objects/usd_object.py +++ b/omnigibson/objects/usd_object.py @@ -28,6 +28,7 @@ def __init__( kinematic_only=None, self_collisions=False, prim_type=PrimType.RIGID, + link_physics_materials=None, load_config=None, abilities=None, include_default_states=True, @@ -51,6 +52,10 @@ def __init__( 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} + link_physics_materials (None or dict): If specified, dictionary mapping link name to kwargs used to generate + a specific physical material for that link's collision meshes, where the kwargs are arguments directly + passed into the omni.isaac.core.materials.PhysicsMaterial constructor, e.g.: "static_friction", + "dynamic_friction", and "restitution" 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 @@ -76,6 +81,7 @@ def __init__( self_collisions=self_collisions, prim_type=prim_type, include_default_states=include_default_states, + link_physics_materials=link_physics_materials, load_config=load_config, abilities=abilities, **kwargs, diff --git a/omnigibson/robots/a1.py b/omnigibson/robots/a1.py index 478d1d211..ec7830ecf 100644 --- a/omnigibson/robots/a1.py +++ b/omnigibson/robots/a1.py @@ -21,6 +21,7 @@ def __init__( visible=True, visual_only=False, self_collisions=True, + link_physics_materials=None, load_config=None, fixed_base=True, # Unique to USDObject hierarchy @@ -39,6 +40,8 @@ def __init__( sensor_config=None, # Unique to ManipulationRobot grasping_mode="physical", + finger_static_friction=None, + finger_dynamic_friction=None, end_effector="inspire", **kwargs, ): @@ -52,6 +55,10 @@ def __init__( visible (bool): whether to render this object or not in the stage 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 + link_physics_materials (None or dict): If specified, dictionary mapping link name to kwargs used to generate + a specific physical material for that link's collision meshes, where the kwargs are arguments directly + passed into the omni.isaac.core.materials.PhysicsMaterial constructor, e.g.: "static_friction", + "dynamic_friction", and "restitution" 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 @@ -88,6 +95,10 @@ def __init__( If "physical", no assistive grasping will be applied (relies on contact friction + finger force). If "assisted", will magnetize any object touching and within the gripper's fingers. If "sticky", will magnetize any object touching the gripper's fingers. + finger_static_friction (None or float): If specified, specific static friction to use for robot's fingers + finger_dynamic_friction (None or float): If specified, specific dynamic friction to use for robot's fingers. + Note: If specified, this will override any ways that are found within @link_physics_materials for any + robot finger gripper links 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). """ @@ -128,6 +139,7 @@ def __init__( fixed_base=fixed_base, visual_only=visual_only, self_collisions=self_collisions, + link_physics_materials=link_physics_materials, load_config=load_config, abilities=abilities, control_freq=control_freq, @@ -141,6 +153,8 @@ def __init__( proprio_obs=proprio_obs, sensor_config=sensor_config, grasping_mode=grasping_mode, + finger_static_friction=finger_static_friction, + finger_dynamic_friction=finger_dynamic_friction, grasping_direction=( "lower" if end_effector == "gripper" else "upper" ), # gripper grasps in the opposite direction diff --git a/omnigibson/robots/behavior_robot.py b/omnigibson/robots/behavior_robot.py index 6dfac40cc..34c8b8ae3 100644 --- a/omnigibson/robots/behavior_robot.py +++ b/omnigibson/robots/behavior_robot.py @@ -68,6 +68,7 @@ def __init__( visible=True, visual_only=False, self_collisions=True, + link_physics_materials=None, load_config=None, # Unique to USDObject hierarchy abilities=None, @@ -84,6 +85,8 @@ def __init__( proprio_obs="default", # Unique to ManipulationRobot grasping_mode="assisted", + finger_static_friction=None, + finger_dynamic_friction=None, # unique to BehaviorRobot use_ghost_hands=True, **kwargs, @@ -102,6 +105,7 @@ def __init__( fixed_base=True, visual_only=visual_only, self_collisions=self_collisions, + link_physics_materials=link_physics_materials, load_config=load_config, abilities=abilities, control_freq=control_freq, @@ -114,6 +118,8 @@ def __init__( exclude_sensor_names=exclude_sensor_names, proprio_obs=proprio_obs, grasping_mode=grasping_mode, + finger_static_friction=finger_static_friction, + finger_dynamic_friction=finger_dynamic_friction, grasping_direction="upper", **kwargs, ) diff --git a/omnigibson/robots/fetch.py b/omnigibson/robots/fetch.py index 8677bb65f..647d83a2f 100644 --- a/omnigibson/robots/fetch.py +++ b/omnigibson/robots/fetch.py @@ -28,6 +28,7 @@ def __init__( visible=True, visual_only=False, self_collisions=True, + link_physics_materials=None, load_config=None, fixed_base=False, # Unique to USDObject hierarchy @@ -47,6 +48,8 @@ def __init__( # Unique to ManipulationRobot grasping_mode="physical", disable_grasp_handling=False, + finger_static_friction=None, + finger_dynamic_friction=None, # Unique to MobileManipulationRobot default_reset_mode="untuck", # Unique to UntuckedArmPoseRobot @@ -63,6 +66,10 @@ def __init__( visible (bool): whether to render this object or not in the stage 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 + link_physics_materials (None or dict): If specified, dictionary mapping link name to kwargs used to generate + a specific physical material for that link's collision meshes, where the kwargs are arguments directly + passed into the omni.isaac.core.materials.PhysicsMaterial constructor, e.g.: "static_friction", + "dynamic_friction", and "restitution" load_config (None or dict): If specified, should contain keyword-mapped values that are relevant for loading this prim at runtime. fixed_base (bool): whether to fix the base of this object or not @@ -102,6 +109,10 @@ def __init__( If "sticky", will magnetize any object touching the gripper's fingers. disable_grasp_handling (bool): If True, will disable all grasp handling for this object. This means that sticky and assisted grasp modes will not work unless the connection/release methodsare manually called. + finger_static_friction (None or float): If specified, specific static friction to use for robot's fingers + finger_dynamic_friction (None or float): If specified, specific dynamic friction to use for robot's fingers. + Note: If specified, this will override any ways that are found within @link_physics_materials for any + robot finger gripper links default_reset_mode (str): Default reset mode for the robot. Should be one of: {"tuck", "untuck"} If reset_joint_pos is not None, this will be ignored (since _default_joint_pos won't be used during initialization). default_arm_pose (str): Default pose for the robot arm. Should be one of: @@ -120,6 +131,7 @@ def __init__( fixed_base=fixed_base, visual_only=visual_only, self_collisions=self_collisions, + link_physics_materials=link_physics_materials, load_config=load_config, abilities=abilities, control_freq=control_freq, @@ -134,6 +146,8 @@ def __init__( sensor_config=sensor_config, grasping_mode=grasping_mode, disable_grasp_handling=disable_grasp_handling, + finger_static_friction=finger_static_friction, + finger_dynamic_friction=finger_dynamic_friction, default_reset_mode=default_reset_mode, default_arm_pose=default_arm_pose, **kwargs, diff --git a/omnigibson/robots/franka.py b/omnigibson/robots/franka.py index b7c8a00ca..72029cbdb 100644 --- a/omnigibson/robots/franka.py +++ b/omnigibson/robots/franka.py @@ -21,6 +21,7 @@ def __init__( visible=True, visual_only=False, self_collisions=True, + link_physics_materials=None, load_config=None, fixed_base=True, # Unique to USDObject hierarchy @@ -39,6 +40,8 @@ def __init__( sensor_config=None, # Unique to ManipulationRobot grasping_mode="physical", + finger_static_friction=None, + finger_dynamic_friction=None, # Unique to Franka end_effector="gripper", **kwargs, @@ -53,6 +56,10 @@ def __init__( visible (bool): whether to render this object or not in the stage 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 + link_physics_materials (None or dict): If specified, dictionary mapping link name to kwargs used to generate + a specific physical material for that link's collision meshes, where the kwargs are arguments directly + passed into the omni.isaac.core.materials.PhysicsMaterial constructor, e.g.: "static_friction", + "dynamic_friction", and "restitution" 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 @@ -89,6 +96,10 @@ def __init__( If "physical", no assistive grasping will be applied (relies on contact friction + finger force). If "assisted", will magnetize any object touching and within the gripper's fingers. If "sticky", will magnetize any object touching the gripper's fingers. + finger_static_friction (None or float): If specified, specific static friction to use for robot's fingers + finger_dynamic_friction (None or float): If specified, specific dynamic friction to use for robot's fingers. + Note: If specified, this will override any ways that are found within @link_physics_materials for any + robot finger gripper links end_effector (str): type of end effector to use. One of {"gripper", "allegro", "leap_right", "leap_left", "inspire"} 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). @@ -191,6 +202,7 @@ def __init__( fixed_base=fixed_base, visual_only=visual_only, self_collisions=self_collisions, + link_physics_materials=link_physics_materials, load_config=load_config, abilities=abilities, control_freq=control_freq, @@ -204,6 +216,8 @@ def __init__( proprio_obs=proprio_obs, sensor_config=sensor_config, grasping_mode=grasping_mode, + finger_static_friction=finger_static_friction, + finger_dynamic_friction=finger_dynamic_friction, grasping_direction=( "lower" if end_effector == "gripper" else "upper" ), # gripper grasps in the opposite direction diff --git a/omnigibson/robots/holonomic_base_robot.py b/omnigibson/robots/holonomic_base_robot.py index 14d857cb4..8b6a3d627 100644 --- a/omnigibson/robots/holonomic_base_robot.py +++ b/omnigibson/robots/holonomic_base_robot.py @@ -46,6 +46,7 @@ def __init__( visible=True, visual_only=False, self_collisions=True, + link_physics_materials=None, load_config=None, # Unique to USDObject hierarchy abilities=None, @@ -73,6 +74,10 @@ def __init__( visible (bool): whether to render this object or not in the stage 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 + link_physics_materials (None or dict): If specified, dictionary mapping link name to kwargs used to generate + a specific physical material for that link's collision meshes, where the kwargs are arguments directly + passed into the omni.isaac.core.materials.PhysicsMaterial constructor, e.g.: "static_friction", + "dynamic_friction", and "restitution" 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 @@ -125,6 +130,7 @@ def __init__( fixed_base=True, visual_only=visual_only, self_collisions=self_collisions, + link_physics_materials=link_physics_materials, load_config=load_config, abilities=abilities, control_freq=control_freq, diff --git a/omnigibson/robots/manipulation_robot.py b/omnigibson/robots/manipulation_robot.py index f4588fc06..24048ca20 100644 --- a/omnigibson/robots/manipulation_robot.py +++ b/omnigibson/robots/manipulation_robot.py @@ -51,7 +51,8 @@ m.MIN_ASSIST_FORCE = 0 m.MAX_ASSIST_FORCE = 100 m.MIN_AG_DEFAULT_GRASP_POINT_PROP = 0.2 -m.MAX_AG_DEFAULT_GRASP_POINT_PROP = 0.8 +m.MAX_AG_DEFAULT_GRASP_POINT_PROP = 1.0 +m.AG_DEFAULT_GRASP_POINT_Z_PROP = 0.4 m.CONSTRAINT_VIOLATION_THRESHOLD = 0.1 m.RELEASE_WINDOW = 1 / 30.0 # release window in seconds @@ -88,6 +89,7 @@ def __init__( fixed_base=False, visual_only=False, self_collisions=True, + link_physics_materials=None, load_config=None, # Unique to USDObject hierarchy abilities=None, @@ -107,6 +109,8 @@ def __init__( grasping_mode="physical", grasping_direction="lower", disable_grasp_handling=False, + finger_static_friction=None, + finger_dynamic_friction=None, **kwargs, ): """ @@ -120,6 +124,10 @@ def __init__( 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 + link_physics_materials (None or dict): If specified, dictionary mapping link name to kwargs used to generate + a specific physical material for that link's collision meshes, where the kwargs are arguments directly + passed into the omni.isaac.core.materials.PhysicsMaterial constructor, e.g.: "static_friction", + "dynamic_friction", and "restitution" 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 @@ -161,6 +169,10 @@ def __init__( otherwise upper limit represents a closed grasp. disable_grasp_handling (bool): If True, the robot will not automatically handle assisted or sticky grasps. Instead, you will need to call the grasp handling methods yourself. + finger_static_friction (None or float): If specified, specific static friction to use for robot's fingers + finger_dynamic_friction (None or float): If specified, specific dynamic friction to use for robot's fingers. + Note: If specified, this will override any ways that are found within @link_physics_materials for any + robot finger gripper links 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). """ @@ -192,6 +204,7 @@ def __init__( fixed_base=fixed_base, visual_only=visual_only, self_collisions=self_collisions, + link_physics_materials=link_physics_materials, load_config=load_config, abilities=abilities, control_freq=control_freq, @@ -207,6 +220,17 @@ def __init__( **kwargs, ) + # Update finger link material dictionary based on desired values + if finger_static_friction is not None or finger_dynamic_friction is not None: + for arm, finger_link_names in self.finger_link_names.items(): + for finger_link_name in finger_link_names: + if finger_link_name not in self._link_physics_materials: + self._link_physics_materials[finger_link_name] = dict() + if finger_static_friction is not None: + self._link_physics_materials[finger_link_name]["static_friction"] = finger_static_friction + if finger_dynamic_friction is not None: + self._link_physics_materials[finger_link_name]["dynamic_friction"] = finger_dynamic_friction + def _validate_configuration(self): # Iterate over all arms for arm in self.arm_names: @@ -268,6 +292,7 @@ def _infer_finger_properties(self): assert ( is_parallel_jaw ), "Inferring finger link information can only be done for parallel jaw gripper robots!" + finger_pts_in_eef_frame = [] for i, finger_link in enumerate(finger_links): # Find parent, and make sure one exists parent_prim_path, parent_link = None, None @@ -307,7 +332,17 @@ def _infer_finger_properties(self): # Convert from world frame -> eef frame finger_pts = th.concatenate([finger_pts, th.ones(len(finger_pts), 1)], dim=-1) finger_pts = (finger_pts @ eef_to_world_tf.T)[:, :3] + finger_pts_in_eef_frame.append(finger_pts) + + # Determine how each finger is located relative to the other in the EEF frame along the y-axis + # This is used to infer which side of each finger's set of points correspond to the "inner" surface + finger_pts_mean = [finger_pts[:, 1].mean().item() for finger_pts in finger_pts_in_eef_frame] + first_finger_is_lower_y_finger = finger_pts_mean[0] < finger_pts_mean[1] + is_lower_y_fingers = [first_finger_is_lower_y_finger, not first_finger_is_lower_y_finger] + for i, (finger_link, finger_pts, is_lower_y_finger) in enumerate( + zip(finger_links, finger_pts_in_eef_frame, is_lower_y_fingers) + ): # Since we know the EEF frame always points with z outwards towards the fingers, the outer-most point / # fingertip is the maximum z value finger_max_z = finger_pts[:, 2].max().item() @@ -322,37 +357,57 @@ def _infer_finger_properties(self): finger_pts[:, 2] > (finger_parent_max_z + finger_range * m.MIN_AG_DEFAULT_GRASP_POINT_PROP) )[0] finger_pts = finger_pts[valid_idxs] - # Make sure all points lie on a single side of the EEF's y-axis - y_signs = th.sign(finger_pts[:, 1]) - assert th.all( - y_signs == y_signs[0] - ).item(), "Expected all finger points to lie on single side of EEF y-axis!" - y_sign = y_signs[0].item() - y_abs_min = th.abs(finger_pts[:, 1]).min().item() + # Infer which side of the gripper corresponds to the inner side (i.e.: the side that touches between the + # two fingers + # We use the heuristic that given a set of points defining a gripper finger, we assume that it's one + # of (y_min, y_max) over all points, with the selection being chosen by inferring which of the limits + # corresponds to the inner side of the finger. + # This is the upper side of the y values if this finger is the lower finger, else the lower side + # of the y values + y_min, y_max = finger_pts[:, 1].min(), finger_pts[:, 1].max() + y_offset = y_max if is_lower_y_finger else y_min + y_sign = 1.0 if is_lower_y_finger else -1.0 # Compute the default grasping points for this finger # For now, we only have a strong heuristic defined for parallel jaw grippers, which assumes that # there are exactly 2 fingers # In this case, this is defined as the x2 (x,y,z) tuples where: - # z - the 20% and 80% length between the range from [finger_parent_max_z, finger_max_z] + # z - the +/-40% from the EEF frame, bounded by the 20% and 100% length between the range from + # [finger_parent_max_z, finger_max_z] # This is synonymous to inferring the length of the finger (lower bounded by the gripper base, - # assumed to be the parent link), and then taking the values MIN% and MAX% along its length + # assumed to be the parent link), and then taking the values +/-%, bounded by the MIN% and MAX% + # along its length # y - the value closest to the edge of the finger surface in the direction of +/- EEF y-axis. # This assumes that each individual finger lies completely on one side of the EEF y-axis # x - 0. This assumes that the EEF axis evenly splits each finger symmetrically on each side # (x,y,z,1) -- homogenous form for efficient transforming into finger link frame + z_lower = max( + finger_parent_max_z + finger_range * m.MIN_AG_DEFAULT_GRASP_POINT_PROP, + -finger_range * m.AG_DEFAULT_GRASP_POINT_Z_PROP, + ) + z_upper = min( + finger_parent_max_z + finger_range * m.MAX_AG_DEFAULT_GRASP_POINT_PROP, + finger_range * m.AG_DEFAULT_GRASP_POINT_Z_PROP, + ) + # We want to ensure the z value is symmetric about the EEF z frame, so make sure z_lower is negative + # and z_upper is positive, and use +/- the absolute minimum value between the two + assert ( + z_lower < 0 and z_upper > 0 + ), f"Expected computed z_lower / z_upper bounds for finger grasping points to be negative / positive, but instead got: {z_lower}, {z_upper}" + z_offset = min(abs(z_lower), abs(z_upper)) + grasp_pts = th.tensor( [ [ 0, - (y_abs_min - 0.002) * y_sign, - finger_parent_max_z + finger_range * m.MIN_AG_DEFAULT_GRASP_POINT_PROP, + y_offset + 0.002 * y_sign, + -z_offset, 1, ], [ 0, - (y_abs_min - 0.002) * y_sign, - finger_parent_max_z + finger_range * m.MAX_AG_DEFAULT_GRASP_POINT_PROP, + y_offset + 0.002 * y_sign, + z_offset, 1, ], ] diff --git a/omnigibson/robots/mobile_manipulation_robot.py b/omnigibson/robots/mobile_manipulation_robot.py index f481d12c3..58e613cc8 100644 --- a/omnigibson/robots/mobile_manipulation_robot.py +++ b/omnigibson/robots/mobile_manipulation_robot.py @@ -30,6 +30,7 @@ def __init__( visible=True, visual_only=False, self_collisions=True, + link_physics_materials=None, load_config=None, fixed_base=False, # Unique to USDObject hierarchy @@ -49,6 +50,8 @@ def __init__( # Unique to ManipulationRobot grasping_mode="physical", disable_grasp_handling=False, + finger_static_friction=None, + finger_dynamic_friction=None, # Unique to MobileManipulationRobot default_reset_mode="untuck", **kwargs, @@ -63,6 +66,10 @@ def __init__( visible (bool): whether to render this object or not in the stage 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 + link_physics_materials (None or dict): If specified, dictionary mapping link name to kwargs used to generate + a specific physical material for that link's collision meshes, where the kwargs are arguments directly + passed into the omni.isaac.core.materials.PhysicsMaterial constructor, e.g.: "static_friction", + "dynamic_friction", and "restitution" 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 @@ -101,6 +108,10 @@ def __init__( If "sticky", will magnetize any object touching the gripper's fingers. disable_grasp_handling (bool): If True, will disable all grasp handling for this object. This means that sticky and assisted grasp modes will not work unless the connection/release methodsare manually called. + finger_static_friction (None or float): If specified, specific static friction to use for robot's fingers + finger_dynamic_friction (None or float): If specified, specific dynamic friction to use for robot's fingers. + Note: If specified, this will override any ways that are found within @link_physics_materials for any + robot finger gripper links default_reset_mode (str): Default reset mode for the robot. Should be one of: {"tuck", "untuck"} If reset_joint_pos is not None, this will be ignored (since _default_joint_pos won't be used during initialization). kwargs (dict): Additional keyword arguments that are used for other super() calls from subclasses, allowing @@ -118,6 +129,7 @@ def __init__( fixed_base=fixed_base, visual_only=visual_only, self_collisions=self_collisions, + link_physics_materials=link_physics_materials, load_config=load_config, abilities=abilities, control_freq=control_freq, @@ -132,6 +144,8 @@ def __init__( sensor_config=sensor_config, grasping_mode=grasping_mode, disable_grasp_handling=disable_grasp_handling, + finger_static_friction=finger_static_friction, + finger_dynamic_friction=finger_dynamic_friction, **kwargs, ) diff --git a/omnigibson/robots/r1.py b/omnigibson/robots/r1.py index 2410325f1..c14858682 100644 --- a/omnigibson/robots/r1.py +++ b/omnigibson/robots/r1.py @@ -22,6 +22,7 @@ def __init__( visible=True, visual_only=False, self_collisions=True, + link_physics_materials=None, load_config=None, # Unique to USDObject hierarchy abilities=None, @@ -40,6 +41,8 @@ def __init__( # Unique to ManipulationRobot grasping_mode="physical", disable_grasp_handling=False, + finger_static_friction=None, + finger_dynamic_friction=None, # Unique to MobileManipulationRobot default_reset_mode="untuck", **kwargs, @@ -54,6 +57,10 @@ def __init__( visible (bool): whether to render this object or not in the stage 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 + link_physics_materials (None or dict): If specified, dictionary mapping link name to kwargs used to generate + a specific physical material for that link's collision meshes, where the kwargs are arguments directly + passed into the omni.isaac.core.materials.PhysicsMaterial constructor, e.g.: "static_friction", + "dynamic_friction", and "restitution" 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 @@ -92,6 +99,10 @@ def __init__( If "sticky", will magnetize any object touching the gripper's fingers. disable_grasp_handling (bool): If True, will disable all grasp handling for this object. This means that sticky and assisted grasp modes will not work unless the connection/release methodsare manually called. + finger_static_friction (None or float): If specified, specific static friction to use for robot's fingers + finger_dynamic_friction (None or float): If specified, specific dynamic friction to use for robot's fingers. + Note: If specified, this will override any ways that are found within @link_physics_materials for any + robot finger gripper links default_reset_mode (str): Default reset mode for the robot. Should be one of: {"tuck", "untuck"} If reset_joint_pos is not None, this will be ignored (since _default_joint_pos won't be used during initialization). kwargs (dict): Additional keyword arguments that are used for other super() calls from subclasses, allowing @@ -105,6 +116,7 @@ def __init__( visible=visible, visual_only=visual_only, self_collisions=self_collisions, + link_physics_materials=link_physics_materials, load_config=load_config, abilities=abilities, control_freq=control_freq, @@ -119,6 +131,8 @@ def __init__( sensor_config=sensor_config, grasping_mode=grasping_mode, disable_grasp_handling=disable_grasp_handling, + finger_static_friction=finger_static_friction, + finger_dynamic_friction=finger_dynamic_friction, default_reset_mode=default_reset_mode, **kwargs, ) @@ -227,4 +241,5 @@ def disabled_collision_pairs(self): ["base_link", "wheel_link1"], ["base_link", "wheel_link2"], ["base_link", "wheel_link3"], + ["torso_link2", "torso_link4"], ] diff --git a/omnigibson/robots/robot_base.py b/omnigibson/robots/robot_base.py index 2b55a3854..23449fef9 100644 --- a/omnigibson/robots/robot_base.py +++ b/omnigibson/robots/robot_base.py @@ -53,6 +53,7 @@ def __init__( fixed_base=False, visual_only=False, self_collisions=True, + link_physics_materials=None, load_config=None, # Unique to USDObject hierarchy abilities=None, @@ -81,6 +82,10 @@ def __init__( 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 + link_physics_materials (None or dict): If specified, dictionary mapping link name to kwargs used to generate + a specific physical material for that link's collision meshes, where the kwargs are arguments directly + passed into the omni.isaac.core.materials.PhysicsMaterial constructor, e.g.: "static_friction", + "dynamic_friction", and "restitution" 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 @@ -153,6 +158,7 @@ def __init__( self_collisions=self_collisions, prim_type=PrimType.RIGID, include_default_states=True, + link_physics_materials=link_physics_materials, load_config=load_config, abilities=abilities, control_freq=control_freq, diff --git a/omnigibson/robots/tiago.py b/omnigibson/robots/tiago.py index 4186ef359..df027df12 100644 --- a/omnigibson/robots/tiago.py +++ b/omnigibson/robots/tiago.py @@ -24,6 +24,7 @@ def __init__( visible=True, visual_only=False, self_collisions=True, + link_physics_materials=None, load_config=None, # Unique to USDObject hierarchy abilities=None, @@ -42,6 +43,8 @@ def __init__( # Unique to ManipulationRobot grasping_mode="physical", disable_grasp_handling=False, + finger_static_friction=None, + finger_dynamic_friction=None, # Unique to MobileManipulationRobot default_reset_mode="untuck", # Unique to UntuckedArmPoseRobot @@ -60,6 +63,10 @@ def __init__( visible (bool): whether to render this object or not in the stage 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 + link_physics_materials (None or dict): If specified, dictionary mapping link name to kwargs used to generate + a specific physical material for that link's collision meshes, where the kwargs are arguments directly + passed into the omni.isaac.core.materials.PhysicsMaterial constructor, e.g.: "static_friction", + "dynamic_friction", and "restitution" 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 @@ -98,6 +105,10 @@ def __init__( If "sticky", will magnetize any object touching the gripper's fingers. disable_grasp_handling (bool): If True, will disable all grasp handling for this object. This means that sticky and assisted grasp modes will not work unless the connection/release methodsare manually called. + finger_static_friction (None or float): If specified, specific static friction to use for robot's fingers + finger_dynamic_friction (None or float): If specified, specific dynamic friction to use for robot's fingers. + Note: If specified, this will override any ways that are found within @link_physics_materials for any + robot finger gripper links default_reset_mode (str): Default reset mode for the robot. Should be one of: {"tuck", "untuck"} If reset_joint_pos is not None, this will be ignored (since _default_joint_pos won't be used during initialization). default_arm_pose (str): Default pose for the robot arm. Should be one of: @@ -120,6 +131,7 @@ def __init__( visible=visible, visual_only=visual_only, self_collisions=self_collisions, + link_physics_materials=link_physics_materials, load_config=load_config, abilities=abilities, control_freq=control_freq, @@ -134,6 +146,8 @@ def __init__( sensor_config=sensor_config, grasping_mode=grasping_mode, disable_grasp_handling=disable_grasp_handling, + finger_static_friction=finger_static_friction, + finger_dynamic_friction=finger_dynamic_friction, default_reset_mode=default_reset_mode, default_arm_pose=default_arm_pose, **kwargs, diff --git a/omnigibson/robots/untucked_arm_pose_robot.py b/omnigibson/robots/untucked_arm_pose_robot.py index cf7664d9e..caa456312 100644 --- a/omnigibson/robots/untucked_arm_pose_robot.py +++ b/omnigibson/robots/untucked_arm_pose_robot.py @@ -27,6 +27,7 @@ def __init__( visible=True, visual_only=False, self_collisions=True, + link_physics_materials=None, load_config=None, fixed_base=False, # Unique to USDObject hierarchy @@ -46,6 +47,8 @@ def __init__( # Unique to ManipulationRobot grasping_mode="physical", disable_grasp_handling=False, + finger_static_friction=None, + finger_dynamic_friction=None, # Unique to MobileManipulationRobot default_reset_mode="untuck", # Unique to UntuckedArmPoseRobot @@ -62,6 +65,10 @@ def __init__( visible (bool): whether to render this object or not in the stage 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 + link_physics_materials (None or dict): If specified, dictionary mapping link name to kwargs used to generate + a specific physical material for that link's collision meshes, where the kwargs are arguments directly + passed into the omni.isaac.core.materials.PhysicsMaterial constructor, e.g.: "static_friction", + "dynamic_friction", and "restitution" 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 @@ -100,6 +107,10 @@ def __init__( If "sticky", will magnetize any object touching the gripper's fingers. disable_grasp_handling (bool): If True, will disable all grasp handling for this object. This means that sticky and assisted grasp modes will not work unless the connection/release methodsare manually called. + finger_static_friction (None or float): If specified, specific static friction to use for robot's fingers + finger_dynamic_friction (None or float): If specified, specific dynamic friction to use for robot's fingers. + Note: If specified, this will override any ways that are found within @link_physics_materials for any + robot finger gripper links default_reset_mode (str): Default reset mode for the robot. Should be one of: {"tuck", "untuck"} If reset_joint_pos is not None, this will be ignored (since _default_joint_pos won't be used during initialization). kwargs (dict): Additional keyword arguments that are used for other super() calls from subclasses, allowing @@ -117,6 +128,7 @@ def __init__( fixed_base=fixed_base, visual_only=visual_only, self_collisions=self_collisions, + link_physics_materials=link_physics_materials, load_config=load_config, abilities=abilities, control_freq=control_freq, @@ -131,6 +143,8 @@ def __init__( sensor_config=sensor_config, grasping_mode=grasping_mode, disable_grasp_handling=disable_grasp_handling, + finger_static_friction=finger_static_friction, + finger_dynamic_friction=finger_dynamic_friction, default_reset_mode=default_reset_mode, **kwargs, ) diff --git a/omnigibson/robots/vx300s.py b/omnigibson/robots/vx300s.py index 59905d287..380af37bd 100644 --- a/omnigibson/robots/vx300s.py +++ b/omnigibson/robots/vx300s.py @@ -22,6 +22,7 @@ def __init__( visible=True, visual_only=False, self_collisions=True, + link_physics_materials=None, load_config=None, fixed_base=True, # Unique to USDObject hierarchy @@ -40,6 +41,8 @@ def __init__( sensor_config=None, # Unique to ManipulationRobot grasping_mode="physical", + finger_static_friction=None, + finger_dynamic_friction=None, **kwargs, ): """ @@ -52,6 +55,10 @@ def __init__( visible (bool): whether to render this object or not in the stage 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 + link_physics_materials (None or dict): If specified, dictionary mapping link name to kwargs used to generate + a specific physical material for that link's collision meshes, where the kwargs are arguments directly + passed into the omni.isaac.core.materials.PhysicsMaterial constructor, e.g.: "static_friction", + "dynamic_friction", and "restitution" 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 @@ -88,6 +95,10 @@ def __init__( If "physical", no assistive grasping will be applied (relies on contact friction + finger force). If "assisted", will magnetize any object touching and within the gripper's fingers. If "sticky", will magnetize any object touching the gripper's fingers. + finger_static_friction (None or float): If specified, specific static friction to use for robot's fingers + finger_dynamic_friction (None or float): If specified, specific dynamic friction to use for robot's fingers. + Note: If specified, this will override any ways that are found within @link_physics_materials for any + robot finger gripper links 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). """ @@ -100,6 +111,7 @@ def __init__( fixed_base=fixed_base, visual_only=visual_only, self_collisions=self_collisions, + link_physics_materials=link_physics_materials, load_config=load_config, abilities=abilities, control_freq=control_freq, @@ -113,6 +125,8 @@ def __init__( proprio_obs=proprio_obs, sensor_config=sensor_config, grasping_mode=grasping_mode, + finger_static_friction=finger_static_friction, + finger_dynamic_friction=finger_dynamic_friction, **kwargs, ) diff --git a/omnigibson/sensors/vision_sensor.py b/omnigibson/sensors/vision_sensor.py index f0b0ec11e..a55146df5 100644 --- a/omnigibson/sensors/vision_sensor.py +++ b/omnigibson/sensors/vision_sensor.py @@ -62,6 +62,9 @@ class VisionSensor(BaseSensor): image_height (int): Height of generated images, in pixels image_width (int): Width of generated images, in pixels focal_length (float): Focal length to set + focus_distance (float): Focus distance to set + fstop (float): fStop value to set + horizontal_aperture (float): Horizontal aperture to set clipping_range (2-tuple): (min, max) viewing range of this vision sensor viewport_name (None or str): If specified, will link this camera to the specified viewport, overriding its current camera. Otherwise, creates a new viewport @@ -124,6 +127,9 @@ def __init__( image_height=128, image_width=128, focal_length=17.0, # Default 17.0 since this is roughly the human eye focal length + focus_distance=0.0, + fstop=0.0, + horizontal_aperture=20.995, clipping_range=(0.001, 10000000.0), viewport_name=None, ): @@ -132,6 +138,9 @@ def __init__( load_config["image_height"] = image_height load_config["image_width"] = image_width load_config["focal_length"] = focal_length + load_config["focus_distance"] = focus_distance + load_config["fstop"] = fstop + load_config["horizontal_aperture"] = horizontal_aperture load_config["clipping_range"] = clipping_range load_config["viewport_name"] = viewport_name @@ -245,8 +254,11 @@ def _post_load(self): # Set the viewer size (requires taking one render step afterwards) self._viewport.viewport_api.set_texture_resolution(resolution) - # Also update focal length and clipping range + # Also update relevant camera params from load config self.focal_length = self._load_config["focal_length"] + self.focus_distance = self._load_config["focus_distance"] + self.fstop = self._load_config["fstop"] + self.horizontal_aperture = self._load_config["horizontal_aperture"] self.clipping_range = self._load_config["clipping_range"] # Requires 3 render updates to propagate changes @@ -795,6 +807,42 @@ def focal_length(self, length): """ self.set_attribute("focalLength", length) + @property + def focus_distance(self): + """ + Returns: + float: focus distance of this sensor, in mm + """ + return self.get_attribute("focusDistance") + + @focus_distance.setter + def focus_distance(self, distance): + """ + Sets the focus distance @distance for this sensor + + Args: + distance (float): focus distance of this sensor, in mm + """ + self.set_attribute("focusDistance", distance) + + @property + def fstop(self): + """ + Returns: + float: fstop of this sensor + """ + return self.get_attribute("fStop") + + @fstop.setter + def fstop(self, val): + """ + Sets the fstop for this sensor + + Args: + val (float): fstop of this sensor + """ + self.set_attribute("fStop", val) + @property def active_camera_path(self): """ diff --git a/omnigibson/utils/asset_conversion_utils.py b/omnigibson/utils/asset_conversion_utils.py index dd44aa51b..099b8f45c 100644 --- a/omnigibson/utils/asset_conversion_utils.py +++ b/omnigibson/utils/asset_conversion_utils.py @@ -1768,7 +1768,7 @@ def find_all_prim_children_with_type(prim_type, root_prim): return found_prims -def simplify_convex_hull(tm, max_vertices=60): +def simplify_convex_hull(tm, max_vertices=60, max_faces=128): """ Simplifies a convex hull mesh by using quadric edge collapse to reduce the number of faces @@ -1781,7 +1781,6 @@ def simplify_convex_hull(tm, max_vertices=60): return tm # Use pymeshlab to reduce - max_faces = 64 ms = pymeshlab.MeshSet() ms.add_mesh(pymeshlab.Mesh(vertex_matrix=tm.vertices, face_matrix=tm.faces, v_normals_matrix=tm.vertex_normals)) while len(ms.current_mesh().vertex_matrix()) > max_vertices: diff --git a/omnigibson/utils/python_utils.py b/omnigibson/utils/python_utils.py index 5245cf9ac..91bf64d96 100644 --- a/omnigibson/utils/python_utils.py +++ b/omnigibson/utils/python_utils.py @@ -825,7 +825,7 @@ def h5py_group_to_torch(group): if isinstance(value, h5py.Group): state[key] = h5py_group_to_torch(value) else: - state[key] = th.tensor(value[()], dtype=th.float32) + state[key] = th.from_numpy(value[()]) return state diff --git a/tests/test_curobo.py b/tests/test_curobo.py index 477d518cb..a9498174a 100644 --- a/tests/test_curobo.py +++ b/tests/test_curobo.py @@ -232,7 +232,7 @@ def test_curobo(): batch_size=batch_size, debug=False, use_cuda_graph=True, - collision_activation_distance=0.03, # Use larger activation distance for better reproducibility + collision_activation_distance=0.05, # Use larger activation distance for better reproducibility use_default_embodiment_only=True, ) @@ -353,9 +353,9 @@ def test_curobo(): target_pos=target_pos, target_quat=target_quat, is_local=False, - max_attempts=1, + max_attempts=100, timeout=60.0, - ik_fail_return=5, + ik_fail_return=50, enable_finetune_trajopt=True, finetune_attempts=1, return_full_result=False, diff --git a/tests/test_data_collection.py b/tests/test_data_collection.py index fac7532c2..14d46bf17 100644 --- a/tests/test_data_collection.py +++ b/tests/test_data_collection.py @@ -52,6 +52,7 @@ def test_data_collect_and_playback(): env=env, output_path=collect_hdf5_path, only_successes=False, + obj_attr_keys=["scale", "visible"], ) # Record 2 episodes