From f8a8a1bfdb748d45d1665b7caef3a548492bec77 Mon Sep 17 00:00:00 2001 From: Sujay Garlanka Date: Fri, 29 Mar 2024 21:21:42 -0700 Subject: [PATCH 1/9] wip --- .../starter_semantic_action_primitives.py | 12 +-- omnigibson/configs/tiago_primitives.yaml | 92 +++++++++++++++++++ 2 files changed, 98 insertions(+), 6 deletions(-) create mode 100644 omnigibson/configs/tiago_primitives.yaml diff --git a/omnigibson/action_primitives/starter_semantic_action_primitives.py b/omnigibson/action_primitives/starter_semantic_action_primitives.py index a93753f5b..2fd44d35d 100644 --- a/omnigibson/action_primitives/starter_semantic_action_primitives.py +++ b/omnigibson/action_primitives/starter_semantic_action_primitives.py @@ -349,12 +349,12 @@ def _load_robot_copy(self): robot_copy = RobotCopy() robots_to_copy = {"original": {"robot": self.robot, "copy_path": "/World/robot_copy"}} - if hasattr(self.robot, "simplified_mesh_usd_path"): - simplified_robot = { - "robot": USDObject("simplified_copy", self.robot.simplified_mesh_usd_path), - "copy_path": "/World/simplified_robot_copy", - } - robots_to_copy["simplified"] = simplified_robot + # if hasattr(self.robot, "simplified_mesh_usd_path"): + # simplified_robot = { + # "robot": USDObject("simplified_copy", self.robot.simplified_mesh_usd_path), + # "copy_path": "/World/simplified_robot_copy", + # } + # robots_to_copy["simplified"] = simplified_robot for robot_type, rc in robots_to_copy.items(): copy_robot = None diff --git a/omnigibson/configs/tiago_primitives.yaml b/omnigibson/configs/tiago_primitives.yaml new file mode 100644 index 000000000..06079391e --- /dev/null +++ b/omnigibson/configs/tiago_primitives.yaml @@ -0,0 +1,92 @@ +env: + action_frequency: 30 # (int): environment executes action at the action_frequency rate + physics_frequency: 120 # (int): physics frequency (1 / physics_timestep for physx) + device: null # (None or str): specifies the device to be used if running on the gpu with torch backend + automatic_reset: false # (bool): whether to automatic reset after an episode finishes + flatten_action_space: false # (bool): whether to flatten the action space as a sinle 1D-array + flatten_obs_space: false # (bool): whether the observation space should be flattened when generated + use_external_obs: false # (bool): Whether to use external observations or not + initial_pos_z_offset: 0.1 + external_sensors: null # (None or list): If specified, list of sensor configurations for external sensors to add. Should specify sensor "type" and any additional kwargs to instantiate the sensor. Each entry should be the kwargs passed to @create_sensor, in addition to position, orientation + +render: + viewer_width: 1280 + viewer_height: 720 + +scene: + type: InteractiveTraversableScene + scene_model: Rs_int + trav_map_resolution: 0.1 + default_erosion_radius: 0.0 + trav_map_with_objects: true + num_waypoints: 1 + waypoint_resolution: 0.2 + load_object_categories: null + not_load_object_categories: null + load_room_types: null + load_room_instances: null + load_task_relevant_only: false + seg_map_resolution: 0.1 + scene_source: OG + include_robots: false + +robots: + - type: Tiago + name: Tiago + action_type: continuous + action_normalize: true + proprio_obs: + - eef_left_pos + - eef_left_quat + - eef_right_pos + - eef_right_quat + - trunk_qpos + - arm_left_qpos_sin + - arm_left_qpos_cos + - arm_right_qpos_sin + - arm_right_qpos_cos + - gripper_left_qpos + - gripper_right_qpos + - grasp_left + - grasp_right + reset_joint_pos: null + base_name: null + scale: 1.0 + self_collision: true + grasping_mode: physical + rigid_trunk: false + default_trunk_offset: 0.365 + default_arm_pose: vertical + sensor_config: + VisionSensor: + sensor_kwargs: + image_height: 512 + image_width: 512 + ScanSensor: + sensor_kwargs: + min_range: 512 + max_range: 512 + controller_config: + base: + name: JointController + arm_left: + name: InverseKinematicsController + arm_right: + name: InverseKinematicsController + gripper_left: + name: MultiFingerGripperController + gripper_right: + name: MultiFingerGripperController + camera: + name: JointController + +objects: [] + +task: + type: DummyTask + +scene_graph: + egocentric: true + full_obs: true + only_true: true + merge_parallel_edges: false From de29f5243204c959d66984116d7c38b04eec8fc8 Mon Sep 17 00:00:00 2001 From: Sujay Garlanka Date: Mon, 15 Apr 2024 17:40:15 -0700 Subject: [PATCH 2/9] wip --- .../starter_semantic_action_primitives.py | 70 +++++++++++-------- omnigibson/configs/tiago_primitives.yaml | 55 ++++++--------- .../action_primitives/solve_simple_task.py | 2 +- 3 files changed, 62 insertions(+), 65 deletions(-) diff --git a/omnigibson/action_primitives/starter_semantic_action_primitives.py b/omnigibson/action_primitives/starter_semantic_action_primitives.py index 2fd44d35d..a5ab9e1c4 100644 --- a/omnigibson/action_primitives/starter_semantic_action_primitives.py +++ b/omnigibson/action_primitives/starter_semantic_action_primitives.py @@ -681,6 +681,7 @@ def _grasp(self, obj): """ # Update the tracking to track the object. self._tracking_object = obj + self._reset_hand() # Don't do anything if the object is already grasped. obj_in_hand = self._get_obj_in_hand() @@ -1477,37 +1478,46 @@ def _get_reset_joint_pos(self): ] ) + # reset_pose_tiago = np.array( + # [ + # -1.78029833e-04, + # 3.20231302e-05, + # -1.85759447e-07, + # 0.0, + # -0.2, + # 0.0, + # 0.1, + # -6.10000000e-01, + # -1.10000000e00, + # 0.00000000e00, + # -1.10000000e00, + # 1.47000000e00, + # 0.00000000e00, + # 8.70000000e-01, + # 2.71000000e00, + # 1.50000000e00, + # 1.71000000e00, + # -1.50000000e00, + # -1.57000000e00, + # 4.50000000e-01, + # 1.39000000e00, + # 0.00000000e00, + # 0.00000000e00, + # 4.50000000e-02, + # 4.50000000e-02, + # 4.50000000e-02, + # 4.50000000e-02, + # ] + # ) reset_pose_tiago = np.array( - [ - -1.78029833e-04, - 3.20231302e-05, - -1.85759447e-07, - 0.0, - -0.2, - 0.0, - 0.1, - -6.10000000e-01, - -1.10000000e00, - 0.00000000e00, - -1.10000000e00, - 1.47000000e00, - 0.00000000e00, - 8.70000000e-01, - 2.71000000e00, - 1.50000000e00, - 1.71000000e00, - -1.50000000e00, - -1.57000000e00, - 4.50000000e-01, - 1.39000000e00, - 0.00000000e00, - 0.00000000e00, - 4.50000000e-02, - 4.50000000e-02, - 4.50000000e-02, - 4.50000000e-02, - ] - ) + [ 1.90735658e-08, + 3.85301071e-08, -3.14324353e-07, -9.16700102e-08, + -2.73824355e-07, 8.56583853e-08, 3.85000000e-01, 8.58460000e-01, + 0.00000000e+00, 0.00000000e+00, -1.48520000e-01, 0.00000000e+00, + -4.50000000e-01, 1.81008000e+00, 0.00000000e+00, 1.63368000e+00, + 0.00000000e+00, 1.37640000e-01, 0.00000000e+00, -1.32488000e+00, + 0.00000000e+00, -6.84150000e-01, 0.00000000e+00, 4.50000000e-02, + 4.50000000e-02, 0.00000000e+00, 0.00000000e+00]) return reset_pose_tiago if self.robot_model == "Tiago" else reset_pose_fetch def _navigate_to_pose(self, pose_2d): diff --git a/omnigibson/configs/tiago_primitives.yaml b/omnigibson/configs/tiago_primitives.yaml index 06079391e..a43338de1 100644 --- a/omnigibson/configs/tiago_primitives.yaml +++ b/omnigibson/configs/tiago_primitives.yaml @@ -32,53 +32,40 @@ scene: robots: - type: Tiago - name: Tiago - action_type: continuous - action_normalize: true - proprio_obs: - - eef_left_pos - - eef_left_quat - - eef_right_pos - - eef_right_quat - - trunk_qpos - - arm_left_qpos_sin - - arm_left_qpos_cos - - arm_right_qpos_sin - - arm_right_qpos_cos - - gripper_left_qpos - - gripper_right_qpos - - grasp_left - - grasp_right - reset_joint_pos: null - base_name: null + obs_modalities: [scan, rgb, depth] scale: 1.0 - self_collision: true - grasping_mode: physical + self_collisions: true + action_normalize: false + action_type: continuous + grasping_mode: sticky rigid_trunk: false - default_trunk_offset: 0.365 default_arm_pose: vertical - sensor_config: - VisionSensor: - sensor_kwargs: - image_height: 512 - image_width: 512 - ScanSensor: - sensor_kwargs: - min_range: 512 - max_range: 512 controller_config: base: name: JointController arm_left: name: InverseKinematicsController + command_output_limits: + - [-0.2, -0.2, -0.2, -0.5, -0.5, -0.5] + - [0.2, 0.2, 0.2, 0.5, 0.5, 0.5] + mode: pose_absolute_ori + kp: 300.0 arm_right: - name: InverseKinematicsController + name: NullJointController gripper_left: - name: MultiFingerGripperController + name: JointController + motor_type: position + command_input_limits: [-1, 1] + command_output_limits: null + use_delta_commands: true gripper_right: - name: MultiFingerGripperController + name: NullJointController camera: name: JointController + motor_type: position + command_input_limits: null + command_output_limits: null + use_delta_commands: false objects: [] diff --git a/omnigibson/examples/action_primitives/solve_simple_task.py b/omnigibson/examples/action_primitives/solve_simple_task.py index 9ab221e21..777376176 100644 --- a/omnigibson/examples/action_primitives/solve_simple_task.py +++ b/omnigibson/examples/action_primitives/solve_simple_task.py @@ -26,7 +26,7 @@ def main(): It loads Rs_int with a Fetch robot, and the robot picks and places a bottle of cologne. """ # Load the config - config_filename = os.path.join(og.example_config_path, "fetch_primitives.yaml") + config_filename = os.path.join(og.example_config_path, "tiago_primitives.yaml") config = yaml.load(open(config_filename, "r"), Loader=yaml.FullLoader) # Update it to create a custom environment and run some actions From 7385d8029ca8d92c363903ec7f2d109d30d2cf37 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 19 Apr 2024 02:09:58 +0000 Subject: [PATCH 3/9] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- .../starter_semantic_action_primitives.py | 38 +++++++++++++++---- 1 file changed, 30 insertions(+), 8 deletions(-) diff --git a/omnigibson/action_primitives/starter_semantic_action_primitives.py b/omnigibson/action_primitives/starter_semantic_action_primitives.py index a5ab9e1c4..ebc435eef 100644 --- a/omnigibson/action_primitives/starter_semantic_action_primitives.py +++ b/omnigibson/action_primitives/starter_semantic_action_primitives.py @@ -1510,14 +1510,36 @@ def _get_reset_joint_pos(self): # ] # ) reset_pose_tiago = np.array( - [ 1.90735658e-08, - 3.85301071e-08, -3.14324353e-07, -9.16700102e-08, - -2.73824355e-07, 8.56583853e-08, 3.85000000e-01, 8.58460000e-01, - 0.00000000e+00, 0.00000000e+00, -1.48520000e-01, 0.00000000e+00, - -4.50000000e-01, 1.81008000e+00, 0.00000000e+00, 1.63368000e+00, - 0.00000000e+00, 1.37640000e-01, 0.00000000e+00, -1.32488000e+00, - 0.00000000e+00, -6.84150000e-01, 0.00000000e+00, 4.50000000e-02, - 4.50000000e-02, 0.00000000e+00, 0.00000000e+00]) + [ + 1.90735658e-08, + 3.85301071e-08, + -3.14324353e-07, + -9.16700102e-08, + -2.73824355e-07, + 8.56583853e-08, + 3.85000000e-01, + 8.58460000e-01, + 0.00000000e00, + 0.00000000e00, + -1.48520000e-01, + 0.00000000e00, + -4.50000000e-01, + 1.81008000e00, + 0.00000000e00, + 1.63368000e00, + 0.00000000e00, + 1.37640000e-01, + 0.00000000e00, + -1.32488000e00, + 0.00000000e00, + -6.84150000e-01, + 0.00000000e00, + 4.50000000e-02, + 4.50000000e-02, + 0.00000000e00, + 0.00000000e00, + ] + ) return reset_pose_tiago if self.robot_model == "Tiago" else reset_pose_fetch def _navigate_to_pose(self, pose_2d): From 71fabc79ddf0b977ec8dc391c1ff1c01afed8a24 Mon Sep 17 00:00:00 2001 From: Sujay Garlanka Date: Thu, 18 Apr 2024 19:36:41 -0700 Subject: [PATCH 4/9] clean up code --- .../starter_semantic_action_primitives.py | 97 ++++++------------- 1 file changed, 31 insertions(+), 66 deletions(-) diff --git a/omnigibson/action_primitives/starter_semantic_action_primitives.py b/omnigibson/action_primitives/starter_semantic_action_primitives.py index a5ab9e1c4..7c500c599 100644 --- a/omnigibson/action_primitives/starter_semantic_action_primitives.py +++ b/omnigibson/action_primitives/starter_semantic_action_primitives.py @@ -102,7 +102,6 @@ def __init__(self): self.links_relative_poses = {} self.reset_pose = { "original": ([0, 0, -5.0], [0, 0, 0, 1]), - "simplified": ([5, 0, -5.0], [0, 0, 0, 1]), } @@ -189,16 +188,6 @@ def _construct_disabled_collision_pairs(self): m.GetPrimPath().pathString for m in meshes.values() ] - # Filter out all self-collisions - if self.robot_copy_type == "simplified": - all_meshes = [ - mesh.GetPrimPath().pathString - for link in robot_meshes_copy.keys() - for mesh in robot_meshes_copy[link].values() - ] - for link in robot_meshes_copy.keys(): - for mesh in robot_meshes_copy[link].values(): - self.disabled_collision_pairs_dict[mesh.GetPrimPath().pathString] += all_meshes # Filter out collision pairs of meshes part of disabled collision pairs else: for pair in self.robot.disabled_collision_pairs: @@ -349,12 +338,6 @@ def _load_robot_copy(self): robot_copy = RobotCopy() robots_to_copy = {"original": {"robot": self.robot, "copy_path": "/World/robot_copy"}} - # if hasattr(self.robot, "simplified_mesh_usd_path"): - # simplified_robot = { - # "robot": USDObject("simplified_copy", self.robot.simplified_mesh_usd_path), - # "copy_path": "/World/simplified_robot_copy", - # } - # robots_to_copy["simplified"] = simplified_robot for robot_type, rc in robots_to_copy.items(): copy_robot = None @@ -372,11 +355,7 @@ def _load_robot_copy(self): copy_robot.GetAttribute("xformOp:orient").Set(lazy.pxr.Gf.Quatd(*orientation)) robot_to_copy = None - if robot_type == "simplified": - robot_to_copy = rc["robot"] - og.sim.import_object(robot_to_copy) - else: - robot_to_copy = rc["robot"] + robot_to_copy = rc["robot"] # Copy robot meshes for link in robot_to_copy.links.values(): @@ -406,9 +385,6 @@ def _load_robot_copy(self): *link.get_position_orientation(), *self.robot.get_position_orientation() ) - if robot_type == "simplified": - og.sim.remove_object(robot_to_copy) - robot_copy.prims[robot_type] = copy_robot robot_copy.meshes[robot_type] = copy_robot_meshes robot_copy.relative_poses[robot_type] = copy_robot_meshes_relative_poses @@ -1478,46 +1454,35 @@ def _get_reset_joint_pos(self): ] ) - # reset_pose_tiago = np.array( - # [ - # -1.78029833e-04, - # 3.20231302e-05, - # -1.85759447e-07, - # 0.0, - # -0.2, - # 0.0, - # 0.1, - # -6.10000000e-01, - # -1.10000000e00, - # 0.00000000e00, - # -1.10000000e00, - # 1.47000000e00, - # 0.00000000e00, - # 8.70000000e-01, - # 2.71000000e00, - # 1.50000000e00, - # 1.71000000e00, - # -1.50000000e00, - # -1.57000000e00, - # 4.50000000e-01, - # 1.39000000e00, - # 0.00000000e00, - # 0.00000000e00, - # 4.50000000e-02, - # 4.50000000e-02, - # 4.50000000e-02, - # 4.50000000e-02, - # ] - # ) reset_pose_tiago = np.array( - [ 1.90735658e-08, - 3.85301071e-08, -3.14324353e-07, -9.16700102e-08, - -2.73824355e-07, 8.56583853e-08, 3.85000000e-01, 8.58460000e-01, - 0.00000000e+00, 0.00000000e+00, -1.48520000e-01, 0.00000000e+00, - -4.50000000e-01, 1.81008000e+00, 0.00000000e+00, 1.63368000e+00, - 0.00000000e+00, 1.37640000e-01, 0.00000000e+00, -1.32488000e+00, - 0.00000000e+00, -6.84150000e-01, 0.00000000e+00, 4.50000000e-02, - 4.50000000e-02, 0.00000000e+00, 0.00000000e+00]) + [ 1.90735658e-08, + 3.85301071e-08, + -3.14324353e-07, + -9.16700102e-08, + -2.73824355e-07, + 8.56583853e-08, + 3.85000000e-01, + 8.58460000e-01, + 0.00000000e+00, + 0.00000000e+00, + -1.48520000e-01, + 0.00000000e+00, + -4.50000000e-01, + 1.81008000e+00, + 0.00000000e+00, + 1.63368000e+00, + 0.00000000e+00, + 1.37640000e-01, + 0.00000000e+00, + -1.32488000e+00, + 0.00000000e+00, + -6.84150000e-01, + 0.00000000e+00, + 4.50000000e-02, + 4.50000000e-02, + 0.00000000e+00, + 0.00000000e+00 + ]) return reset_pose_tiago if self.robot_model == "Tiago" else reset_pose_fetch def _navigate_to_pose(self, pose_2d): @@ -1530,7 +1495,7 @@ def _navigate_to_pose(self, pose_2d): Returns: np.array or None: Action array for one step for the robot to navigate or None if it is done navigating """ - with PlanningContext(self.robot, self.robot_copy, "simplified") as context: + with PlanningContext(self.robot, self.robot_copy, "original") as context: plan = plan_base_motion( robot=self.robot, end_conf=pose_2d, @@ -1710,7 +1675,7 @@ def _sample_pose_near_object(self, obj, pose_on_obj=None, **kwargs): - 3-array: (x,y,z) Position in the world frame - 4-array: (x,y,z,w) Quaternion orientation in the world frame """ - with PlanningContext(self.robot, self.robot_copy, "simplified") as context: + with PlanningContext(self.robot, self.robot_copy, "original") as context: for _ in range(m.MAX_ATTEMPTS_FOR_SAMPLING_POSE_NEAR_OBJECT): if pose_on_obj is None: pos_on_obj = self._sample_position_on_aabb_side(obj) From f5406da917949841cafeabba661525683ad8102c Mon Sep 17 00:00:00 2001 From: Sujay Garlanka Date: Thu, 18 Apr 2024 19:39:30 -0700 Subject: [PATCH 5/9] clean up code --- .../starter_semantic_action_primitives.py | 27 +++++++++---------- 1 file changed, 13 insertions(+), 14 deletions(-) diff --git a/omnigibson/action_primitives/starter_semantic_action_primitives.py b/omnigibson/action_primitives/starter_semantic_action_primitives.py index 48410e607..2a72aa269 100644 --- a/omnigibson/action_primitives/starter_semantic_action_primitives.py +++ b/omnigibson/action_primitives/starter_semantic_action_primitives.py @@ -189,20 +189,19 @@ def _construct_disabled_collision_pairs(self): ] # Filter out collision pairs of meshes part of disabled collision pairs - else: - for pair in self.robot.disabled_collision_pairs: - link_1 = pair[0] - link_2 = pair[1] - if link_1 in robot_meshes_copy.keys() and link_2 in robot_meshes_copy.keys(): - for mesh in robot_meshes_copy[link_1].values(): - self.disabled_collision_pairs_dict[mesh.GetPrimPath().pathString] += [ - m.GetPrimPath().pathString for m in robot_meshes_copy[link_2].values() - ] - - for mesh in robot_meshes_copy[link_2].values(): - self.disabled_collision_pairs_dict[mesh.GetPrimPath().pathString] += [ - m.GetPrimPath().pathString for m in robot_meshes_copy[link_1].values() - ] + for pair in self.robot.disabled_collision_pairs: + link_1 = pair[0] + link_2 = pair[1] + if link_1 in robot_meshes_copy.keys() and link_2 in robot_meshes_copy.keys(): + for mesh in robot_meshes_copy[link_1].values(): + self.disabled_collision_pairs_dict[mesh.GetPrimPath().pathString] += [ + m.GetPrimPath().pathString for m in robot_meshes_copy[link_2].values() + ] + + for mesh in robot_meshes_copy[link_2].values(): + self.disabled_collision_pairs_dict[mesh.GetPrimPath().pathString] += [ + m.GetPrimPath().pathString for m in robot_meshes_copy[link_1].values() + ] # Filter out colliders all robot copy meshes should ignore disabled_colliders = [] From 19d1a11b61acf60f03e4b170dd63d6ec5ec62b77 Mon Sep 17 00:00:00 2001 From: Sujay Garlanka Date: Sat, 20 Apr 2024 01:40:20 -0700 Subject: [PATCH 6/9] remove code to support multiple meshes --- .../starter_semantic_action_primitives.py | 124 ++++++++---------- .../action_primitives/solve_simple_task.py | 2 +- omnigibson/utils/motion_planning_utils.py | 15 +-- 3 files changed, 65 insertions(+), 76 deletions(-) diff --git a/omnigibson/action_primitives/starter_semantic_action_primitives.py b/omnigibson/action_primitives/starter_semantic_action_primitives.py index 2a72aa269..b6be97582 100644 --- a/omnigibson/action_primitives/starter_semantic_action_primitives.py +++ b/omnigibson/action_primitives/starter_semantic_action_primitives.py @@ -96,13 +96,11 @@ class RobotCopy: """A data structure for storing information about a robot copy, used for collision checking in planning.""" def __init__(self): - self.prims = {} + self.prim = None self.meshes = {} self.relative_poses = {} self.links_relative_poses = {} - self.reset_pose = { - "original": ([0, 0, -5.0], [0, 0, 0, 1]), - } + self.reset_pose = ([0, 0, -5.0], [0, 0, 0, 1]) class PlanningContext(object): @@ -110,10 +108,9 @@ class PlanningContext(object): A context manager that sets up a robot copy for collision checking in planning. """ - def __init__(self, robot, robot_copy, robot_copy_type="original"): + def __init__(self, robot, robot_copy): self.robot = robot self.robot_copy = robot_copy - self.robot_copy_type = robot_copy_type if robot_copy_type in robot_copy.prims.keys() else "original" self.disabled_collision_pairs_dict = {} def __enter__(self): @@ -123,7 +120,7 @@ def __enter__(self): def __exit__(self, *args): self._set_prim_pose( - self.robot_copy.prims[self.robot_copy_type], self.robot_copy.reset_pose[self.robot_copy_type] + self.robot_copy.prim, self.robot_copy.reset_pose ) def _assemble_robot_copy(self): @@ -153,10 +150,10 @@ def _assemble_robot_copy(self): link_poses = self.fk_solver.get_link_poses(joint_pos, arm_links) # Set position of robot copy root prim - self._set_prim_pose(self.robot_copy.prims[self.robot_copy_type], self.robot.get_position_orientation()) + self._set_prim_pose(self.robot_copy.prim, self.robot.get_position_orientation()) # Assemble robot meshes - for link_name, meshes in self.robot_copy.meshes[self.robot_copy_type].items(): + for link_name, meshes in self.robot_copy.meshes.items(): for mesh_name, copy_mesh in meshes.items(): # Skip grasping frame (this is necessary for Tiago, but should be cleaned up in the future) if "grasping_frame" in link_name: @@ -165,10 +162,10 @@ def _assemble_robot_copy(self): link_pose = ( link_poses[link_name] if link_name in arm_links - else self.robot_copy.links_relative_poses[self.robot_copy_type][link_name] + else self.robot_copy.links_relative_poses[link_name] ) mesh_copy_pose = T.pose_transform( - *link_pose, *self.robot_copy.relative_poses[self.robot_copy_type][link_name][mesh_name] + *link_pose, *self.robot_copy.relative_poses[link_name][mesh_name] ) self._set_prim_pose(copy_mesh, mesh_copy_pose) @@ -179,7 +176,7 @@ def _set_prim_pose(self, prim, pose): prim.GetAttribute("xformOp:orient").Set(lazy.pxr.Gf.Quatd(*orientation)) def _construct_disabled_collision_pairs(self): - robot_meshes_copy = self.robot_copy.meshes[self.robot_copy_type] + robot_meshes_copy = self.robot_copy.meshes # Filter out collision pairs of meshes part of the same link for meshes in robot_meshes_copy.values(): @@ -336,58 +333,53 @@ def _load_robot_copy(self): """Loads a copy of the robot that can be manipulated into arbitrary configurations for collision checking in planning.""" robot_copy = RobotCopy() - robots_to_copy = {"original": {"robot": self.robot, "copy_path": "/World/robot_copy"}} - - for robot_type, rc in robots_to_copy.items(): - copy_robot = None - copy_robot_meshes = {} - copy_robot_meshes_relative_poses = {} - copy_robot_links_relative_poses = {} - - # Create prim under which robot meshes are nested and set position - lazy.omni.usd.commands.CreatePrimCommand("Xform", rc["copy_path"]).do() - copy_robot = lazy.omni.isaac.core.utils.prims.get_prim_at_path(rc["copy_path"]) - reset_pose = robot_copy.reset_pose[robot_type] - translation = lazy.pxr.Gf.Vec3d(*np.array(reset_pose[0], dtype=float)) - copy_robot.GetAttribute("xformOp:translate").Set(translation) - orientation = np.array(reset_pose[1], dtype=float)[[3, 0, 1, 2]] - copy_robot.GetAttribute("xformOp:orient").Set(lazy.pxr.Gf.Quatd(*orientation)) - - robot_to_copy = None - robot_to_copy = rc["robot"] - - # Copy robot meshes - for link in robot_to_copy.links.values(): - link_name = link.prim_path.split("/")[-1] - for mesh_name, mesh in link.collision_meshes.items(): - split_path = mesh.prim_path.split("/") - # Do not copy grasping frame (this is necessary for Tiago, but should be cleaned up in the future) - if "grasping_frame" in link_name: - continue - - copy_mesh_path = rc["copy_path"] + "/" + link_name - copy_mesh_path += f"_{split_path[-1]}" if split_path[-1] != "collisions" else "" - lazy.omni.usd.commands.CopyPrimCommand(mesh.prim_path, path_to=copy_mesh_path).do() - copy_mesh = lazy.omni.isaac.core.utils.prims.get_prim_at_path(copy_mesh_path) - relative_pose = T.relative_pose_transform( - *mesh.get_position_orientation(), *link.get_position_orientation() - ) - relative_pose = (relative_pose[0], np.array([0, 0, 0, 1])) - if link_name not in copy_robot_meshes.keys(): - copy_robot_meshes[link_name] = {mesh_name: copy_mesh} - copy_robot_meshes_relative_poses[link_name] = {mesh_name: relative_pose} - else: - copy_robot_meshes[link_name][mesh_name] = copy_mesh - copy_robot_meshes_relative_poses[link_name][mesh_name] = relative_pose - - copy_robot_links_relative_poses[link_name] = T.relative_pose_transform( - *link.get_position_orientation(), *self.robot.get_position_orientation() + copy_path = "/World/robot_copy" + copy_robot = None + copy_robot_meshes = {} + copy_robot_meshes_relative_poses = {} + copy_robot_links_relative_poses = {} + + # Create prim under which robot meshes are nested and set position + lazy.omni.usd.commands.CreatePrimCommand("Xform", copy_path).do() + copy_robot = lazy.omni.isaac.core.utils.prims.get_prim_at_path(copy_path) + reset_pose = robot_copy.reset_pose + translation = lazy.pxr.Gf.Vec3d(*np.array(reset_pose[0], dtype=float)) + copy_robot.GetAttribute("xformOp:translate").Set(translation) + orientation = np.array(reset_pose[1], dtype=float)[[3, 0, 1, 2]] + copy_robot.GetAttribute("xformOp:orient").Set(lazy.pxr.Gf.Quatd(*orientation)) + + # Copy robot meshes + for link in self.robot.links.values(): + link_name = link.prim_path.split("/")[-1] + for mesh_name, mesh in link.collision_meshes.items(): + split_path = mesh.prim_path.split("/") + # Do not copy grasping frame (this is necessary for Tiago, but should be cleaned up in the future) + if "grasping_frame" in link_name: + continue + + copy_mesh_path = copy_path + "/" + link_name + copy_mesh_path += f"_{split_path[-1]}" if split_path[-1] != "collisions" else "" + lazy.omni.usd.commands.CopyPrimCommand(mesh.prim_path, path_to=copy_mesh_path).do() + copy_mesh = lazy.omni.isaac.core.utils.prims.get_prim_at_path(copy_mesh_path) + relative_pose = T.relative_pose_transform( + *mesh.get_position_orientation(), *link.get_position_orientation() ) + relative_pose = (relative_pose[0], np.array([0, 0, 0, 1])) + if link_name not in copy_robot_meshes.keys(): + copy_robot_meshes[link_name] = {mesh_name: copy_mesh} + copy_robot_meshes_relative_poses[link_name] = {mesh_name: relative_pose} + else: + copy_robot_meshes[link_name][mesh_name] = copy_mesh + copy_robot_meshes_relative_poses[link_name][mesh_name] = relative_pose + + copy_robot_links_relative_poses[link_name] = T.relative_pose_transform( + *link.get_position_orientation(), *self.robot.get_position_orientation() + ) - robot_copy.prims[robot_type] = copy_robot - robot_copy.meshes[robot_type] = copy_robot_meshes - robot_copy.relative_poses[robot_type] = copy_robot_meshes_relative_poses - robot_copy.links_relative_poses[robot_type] = copy_robot_links_relative_poses + robot_copy.prim = copy_robot + robot_copy.meshes = copy_robot_meshes + robot_copy.relative_poses = copy_robot_meshes_relative_poses + robot_copy.links_relative_poses = copy_robot_links_relative_poses og.sim.step() return robot_copy @@ -937,7 +929,7 @@ def _move_hand_joint(self, joint_pos): Returns: np.array or None: Action array for one step for the robot to move arm or None if its at the joint positions """ - with PlanningContext(self.robot, self.robot_copy, "original") as context: + with PlanningContext(self.robot, self.robot_copy) as context: plan = plan_arm_motion( robot=self.robot, end_conf=joint_pos, @@ -972,7 +964,7 @@ def _move_hand_ik(self, eef_pose, stop_if_stuck=False): eef_ori = T.quat2axisangle(eef_pose[1]) end_conf = np.append(eef_pos, eef_ori) - with PlanningContext(self.robot, self.robot_copy, "original") as context: + with PlanningContext(self.robot, self.robot_copy) as context: plan = plan_arm_motion_ik( robot=self.robot, end_conf=end_conf, @@ -1496,7 +1488,7 @@ def _navigate_to_pose(self, pose_2d): Returns: np.array or None: Action array for one step for the robot to navigate or None if it is done navigating """ - with PlanningContext(self.robot, self.robot_copy, "original") as context: + with PlanningContext(self.robot, self.robot_copy) as context: plan = plan_base_motion( robot=self.robot, end_conf=pose_2d, @@ -1676,7 +1668,7 @@ def _sample_pose_near_object(self, obj, pose_on_obj=None, **kwargs): - 3-array: (x,y,z) Position in the world frame - 4-array: (x,y,z,w) Quaternion orientation in the world frame """ - with PlanningContext(self.robot, self.robot_copy, "original") as context: + with PlanningContext(self.robot, self.robot_copy) as context: for _ in range(m.MAX_ATTEMPTS_FOR_SAMPLING_POSE_NEAR_OBJECT): if pose_on_obj is None: pos_on_obj = self._sample_position_on_aabb_side(obj) diff --git a/omnigibson/examples/action_primitives/solve_simple_task.py b/omnigibson/examples/action_primitives/solve_simple_task.py index 777376176..9ab221e21 100644 --- a/omnigibson/examples/action_primitives/solve_simple_task.py +++ b/omnigibson/examples/action_primitives/solve_simple_task.py @@ -26,7 +26,7 @@ def main(): It loads Rs_int with a Fetch robot, and the robot picks and places a bottle of cologne. """ # Load the config - config_filename = os.path.join(og.example_config_path, "tiago_primitives.yaml") + config_filename = os.path.join(og.example_config_path, "fetch_primitives.yaml") config = yaml.load(open(config_filename, "r"), Loader=yaml.FullLoader) # Update it to create a custom environment and run some actions diff --git a/omnigibson/utils/motion_planning_utils.py b/omnigibson/utils/motion_planning_utils.py index 8780ff300..bb4ecd5fc 100644 --- a/omnigibson/utils/motion_planning_utils.py +++ b/omnigibson/utils/motion_planning_utils.py @@ -443,13 +443,12 @@ def set_base_and_detect_collision(context, pose): bool: Whether the robot is in collision """ robot_copy = context.robot_copy - robot_copy_type = context.robot_copy_type translation = lazy.pxr.Gf.Vec3d(*np.array(pose[0], dtype=float)) - robot_copy.prims[robot_copy_type].GetAttribute("xformOp:translate").Set(translation) + robot_copy.prim.GetAttribute("xformOp:translate").Set(translation) orientation = np.array(pose[1], dtype=float)[[3, 0, 1, 2]] - robot_copy.prims[robot_copy_type].GetAttribute("xformOp:orient").Set(lazy.pxr.Gf.Quatd(*orientation)) + robot_copy.prim.GetAttribute("xformOp:orient").Set(lazy.pxr.Gf.Quatd(*orientation)) return detect_robot_collision(context) @@ -466,16 +465,15 @@ def set_arm_and_detect_collision(context, joint_pos): bool: Whether the robot is in a valid state i.e. not in collision """ robot_copy = context.robot_copy - robot_copy_type = context.robot_copy_type arm_links = context.robot.manipulation_link_names link_poses = context.fk_solver.get_link_poses(joint_pos, arm_links) for link in arm_links: pose = link_poses[link] - if link in robot_copy.meshes[robot_copy_type].keys(): - for mesh_name, mesh in robot_copy.meshes[robot_copy_type][link].items(): - relative_pose = robot_copy.relative_poses[robot_copy_type][link][mesh_name] + if link in robot_copy.meshes.keys(): + for mesh_name, mesh in robot_copy.meshes[link].items(): + relative_pose = robot_copy.relative_poses[link][mesh_name] mesh_pose = T.pose_transform(*pose, *relative_pose) translation = lazy.pxr.Gf.Vec3d(*np.array(mesh_pose[0], dtype=float)) mesh.GetAttribute("xformOp:translate").Set(translation) @@ -496,7 +494,6 @@ def detect_robot_collision(context): bool: Whether the robot is in collision """ robot_copy = context.robot_copy - robot_copy_type = context.robot_copy_type # Define function for checking overlap valid_hit = False @@ -509,7 +506,7 @@ def overlap_callback(hit): return not valid_hit - for meshes in robot_copy.meshes[robot_copy_type].values(): + for meshes in robot_copy.meshes.values(): for mesh in meshes.values(): if valid_hit: return valid_hit From 40952a7b6d38e06f812ed526c574e06458f3ed80 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Sat, 20 Apr 2024 08:40:45 +0000 Subject: [PATCH 7/9] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- .../starter_semantic_action_primitives.py | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/omnigibson/action_primitives/starter_semantic_action_primitives.py b/omnigibson/action_primitives/starter_semantic_action_primitives.py index b6be97582..444d3628a 100644 --- a/omnigibson/action_primitives/starter_semantic_action_primitives.py +++ b/omnigibson/action_primitives/starter_semantic_action_primitives.py @@ -119,9 +119,7 @@ def __enter__(self): return self def __exit__(self, *args): - self._set_prim_pose( - self.robot_copy.prim, self.robot_copy.reset_pose - ) + self._set_prim_pose(self.robot_copy.prim, self.robot_copy.reset_pose) def _assemble_robot_copy(self): if m.TIAGO_TORSO_FIXED: @@ -160,13 +158,9 @@ def _assemble_robot_copy(self): continue # Set poses of meshes relative to the robot to construct the robot link_pose = ( - link_poses[link_name] - if link_name in arm_links - else self.robot_copy.links_relative_poses[link_name] - ) - mesh_copy_pose = T.pose_transform( - *link_pose, *self.robot_copy.relative_poses[link_name][mesh_name] + link_poses[link_name] if link_name in arm_links else self.robot_copy.links_relative_poses[link_name] ) + mesh_copy_pose = T.pose_transform(*link_pose, *self.robot_copy.relative_poses[link_name][mesh_name]) self._set_prim_pose(copy_mesh, mesh_copy_pose) def _set_prim_pose(self, prim, pose): From 6daebfa3239cb9fa47db7da66fa67206dfc418c5 Mon Sep 17 00:00:00 2001 From: Sujay Garlanka Date: Sat, 20 Apr 2024 13:16:58 -0700 Subject: [PATCH 8/9] remove reference to simplified mesh --- omnigibson/robots/tiago.py | 8 -------- 1 file changed, 8 deletions(-) diff --git a/omnigibson/robots/tiago.py b/omnigibson/robots/tiago.py index 26513e3a2..02bec8d7a 100644 --- a/omnigibson/robots/tiago.py +++ b/omnigibson/robots/tiago.py @@ -671,14 +671,6 @@ def usd_path(self): gm.ASSET_PATH, "models/tiago/tiago_dual_omnidirectional_stanford/tiago_dual_omnidirectional_stanford_33.usd" ) - @property - def simplified_mesh_usd_path(self): - # TODO: How can we make this more general - maybe some automatic way to generate these? - return os.path.join( - gm.ASSET_PATH, - "models/tiago/tiago_dual_omnidirectional_stanford/tiago_dual_omnidirectional_stanford_33_simplified_collision_mesh.usd", - ) - @property def robot_arm_descriptor_yamls(self): # TODO: Remove the need to do this by making the arm descriptor yaml files generated automatically From 3e31f50a2535f5f7ce33755f53f5d1b7ca84c58c Mon Sep 17 00:00:00 2001 From: Sujay Garlanka Date: Tue, 23 Apr 2024 23:26:54 -0700 Subject: [PATCH 9/9] works with faster assets --- .../action_primitives/starter_semantic_action_primitives.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/omnigibson/action_primitives/starter_semantic_action_primitives.py b/omnigibson/action_primitives/starter_semantic_action_primitives.py index 444d3628a..86799b1f0 100644 --- a/omnigibson/action_primitives/starter_semantic_action_primitives.py +++ b/omnigibson/action_primitives/starter_semantic_action_primitives.py @@ -63,7 +63,7 @@ m.MAX_CARTESIAN_HAND_STEP = 0.002 m.MAX_STEPS_FOR_HAND_MOVE_JOINT = 500 m.MAX_STEPS_FOR_HAND_MOVE_IK = 1000 -m.MAX_STEPS_FOR_GRASP_OR_RELEASE = 250 +m.MAX_STEPS_FOR_GRASP_OR_RELEASE = 150 m.MAX_STEPS_FOR_WAYPOINT_NAVIGATION = 500 m.MAX_ATTEMPTS_FOR_OPEN_CLOSE = 20