Merge pull request #939 from StanfordVL/fix/robot-self-collision

Fix/robot self collision

omnigibson/examples/robots/robot_control_example.py

@@ -44,15 +44,19 @@ def choose_controllers(robot, random_selection=False):
     default_config = robot._default_controller_config
 
     # Iterate over all components in robot
-    for component, controller_options in default_config.items():
+    controller_names = robot.controller_order
+    for controller_name in controller_names:
+        controller_options = default_config[controller_name]
         # Select controller
         options = list(sorted(controller_options.keys()))
         choice = choose_from_options(
-            options=options, name="{} controller".format(component), random_selection=random_selection
+            options=options,
+            name=f"{controller_name} controller",
+            random_selection=random_selection,
         )
 
         # Add to user responses
-        controller_choices[component] = choice
+        controller_choices[controller_name] = choice
 
     return controller_choices
 

omnigibson/robots/a1.py

@@ -218,7 +218,7 @@ def assisted_grasp_end_points(self):
     @property
     def disabled_collision_pairs(self):
         # some dexhand has self collisions that needs to be filtered out
+        pairs = [["base_link", "connector"]]
         if self.end_effector == "inspire":
-            return [["base_link", "link12"]]
-        else:
-            return []
+            pairs.append(["base_link", "link12"])
+        return pairs

omnigibson/robots/articulated_trunk_robot.py

@@ -34,7 +34,7 @@ def __init__(
         scale=None,
         visible=True,
         visual_only=False,
-        self_collisions=False,
+        self_collisions=True,
         load_config=None,
         fixed_base=False,
         # Unique to USDObject hierarchy

omnigibson/robots/fetch.py

@@ -32,7 +32,7 @@ def __init__(
         scale=None,
         visible=True,
         visual_only=False,
-        self_collisions=False,
+        self_collisions=True,
         load_config=None,
         fixed_base=False,
         # Unique to USDObject hierarchy

omnigibson/robots/holonomic_base_robot.py

@@ -40,7 +40,7 @@ def __init__(
         visible=True,
         fixed_base=False,
         visual_only=False,
-        self_collisions=False,
+        self_collisions=True,
         load_config=None,
         # Unique to USDObject hierarchy
         abilities=None,

omnigibson/robots/locobot.py

@@ -35,3 +35,15 @@ def usd_path(self):
     @property
     def urdf_path(self):
         return os.path.join(gm.ASSET_PATH, "models/locobot/locobot.urdf")
+
+    @property
+    def disabled_collision_pairs(self):
+        # badly modeled gripper collision meshes
+        return [
+            ["base_link", "arm_base_link"],
+            ["base_link", "plate_2"],
+            ["cam_mount", "forearm_link"],
+            ["cam_mount", "elbow_link"],
+            ["cam_mount", "plate_1"],
+            ["cam_mount", "plate_2"],
+        ]

omnigibson/robots/manipulation_robot.py

@@ -70,7 +70,7 @@ def __init__(
         visible=True,
         fixed_base=False,
         visual_only=False,
-        self_collisions=False,
+        self_collisions=True,
         load_config=None,
         # Unique to USDObject hierarchy
         abilities=None,

omnigibson/robots/mobile_manipulation_robot.py

@@ -33,7 +33,7 @@ def __init__(
         scale=None,
         visible=True,
         visual_only=False,
-        self_collisions=False,
+        self_collisions=True,
         load_config=None,
         fixed_base=False,
         # Unique to USDObject hierarchy

omnigibson/robots/r1.py

@@ -27,7 +27,7 @@ def __init__(
         scale=None,
         visible=True,
         visual_only=False,
-        self_collisions=False,
+        self_collisions=True,
         load_config=None,
         # Unique to USDObject hierarchy
         abilities=None,
@@ -254,4 +254,7 @@ def disabled_collision_pairs(self):
         return [
             ["left_gripper_link1", "left_gripper_link2"],
             ["right_gripper_link1", "right_gripper_link2"],
+            ["base_link", "wheel_link1"],
+            ["base_link", "wheel_link2"],
+            ["base_link", "wheel_link3"],
         ]

omnigibson/robots/robot_base.py

@@ -56,7 +56,7 @@ def __init__(
         visible=True,
         fixed_base=False,
         visual_only=False,
-        self_collisions=False,
+        self_collisions=True,
         load_config=None,
         # Unique to USDObject hierarchy
         abilities=None,

omnigibson/robots/stretch.py

@@ -117,9 +117,14 @@ def disabled_collision_pairs(self):
             ["link_arm_l4", "link_arm_l3"],
             ["link_arm_l4", "link_arm_l2"],
             ["link_arm_l4", "link_arm_l1"],
+            ["link_arm_l4", "link_aruco_inner_wrist"],
             ["link_arm_l3", "link_arm_l2"],
             ["link_arm_l3", "link_arm_l1"],
+            ["link_arm_l3", "link_aruco_top_wrist"],
+            ["link_arm_l3", "link_aruco_inner_wrist"],
             ["link_arm_l2", "link_arm_l1"],
+            ["link_arm_l2", "link_aruco_top_wrist"],
+            ["link_arm_l2", "link_aruco_inner_wrist"],
             ["link_arm_l0", "link_arm_l1"],
             ["link_arm_l0", "link_arm_l2"],
             ["link_arm_l0", "link_arm_l3"],
@@ -131,6 +136,8 @@ def disabled_collision_pairs(self):
             ["link_arm_l0", "link_wrist_yaw_bottom"],
             ["link_arm_l0", "link_wrist_pitch"],
             ["link_wrist_yaw_bottom", "link_wrist_pitch"],
+            ["link_wrist_yaw_bottom", "link_arm_l4"],
+            ["link_wrist_yaw_bottom", "link_arm_l3"],
             ["gripper_camera_link", "link_gripper_s3_body"],
             ["link_gripper_s3_body", "link_aruco_d405"],
             ["link_gripper_s3_body", "link_gripper_finger_left"],

omnigibson/robots/tiago.py

@@ -31,7 +31,7 @@ def __init__(
         scale=None,
         visible=True,
         visual_only=False,
-        self_collisions=False,
+        self_collisions=True,
         load_config=None,
         # Unique to USDObject hierarchy
         abilities=None,

omnigibson/robots/turtlebot.py

@@ -36,3 +36,29 @@ def usd_path(self):
     @property
     def urdf_path(self):
         return os.path.join(gm.ASSET_PATH, "models/turtlebot/turtlebot.urdf")
+
+    @property
+    def disabled_collision_pairs(self):
+        # badly modeled gripper collision meshes
+        return [
+            ["plate_bottom_link", "pole_middle_0_link"],
+            ["plate_bottom_link", "pole_middle_2_link"],
+            ["plate_middle_link", "pole_top_3_link"],
+            ["plate_middle_link", "pole_kinect_0_link"],
+            ["plate_middle_link", "pole_kinect_1_link"],
+            ["plate_middle_link", "pole_top_1_link"],
+            ["plate_middle_link", "pole_middle_0_link"],
+            ["plate_middle_link", "pole_middle_1_link"],
+            ["plate_middle_link", "pole_middle_2_link"],
+            ["plate_middle_link", "pole_middle_3_link"],
+            ["plate_top_link", "pole_top_0_link"],
+            ["plate_top_link", "pole_top_1_link"],
+            ["plate_top_link", "pole_top_2_link"],
+            ["plate_top_link", "pole_top_3_link"],
+            ["pole_top_0_link", "pole_middle_0_link"],
+            ["pole_top_0_link", "plate_middle_link"],
+            ["pole_top_1_link", "pole_middle_1_link"],
+            ["pole_top_2_link", "pole_middle_2_link"],
+            ["pole_top_2_link", "plate_middle_link"],
+            ["pole_top_3_link", "pole_middle_3_link"],
+        ]

omnigibson/robots/untucked_arm_pose_robot.py

@@ -28,7 +28,7 @@ def __init__(
         scale=None,
         visible=True,
         visual_only=False,
-        self_collisions=False,
+        self_collisions=True,
         load_config=None,
         fixed_base=False,
         # Unique to USDObject hierarchy