Merge pull request #725 from StanfordVL/franka

Merge franka family

omnigibson/examples/robots/robot_control_example.py

@@ -154,6 +154,7 @@ def main(random_selection=False, headless=False, short_exec=False, quickstart=Fa
     # Loop control until user quits
     max_steps = -1 if not short_exec else 100
     step = 0
+
     while step != max_steps:
         action = (
             action_generator.get_random_action() if control_mode == "random" else action_generator.get_teleop_action()

omnigibson/robots/__init__.py

@@ -2,8 +2,6 @@
 from omnigibson.robots.behavior_robot import BehaviorRobot
 from omnigibson.robots.fetch import Fetch
 from omnigibson.robots.franka import FrankaPanda
-from omnigibson.robots.franka_allegro import FrankaAllegro
-from omnigibson.robots.franka_leap import FrankaLeap
 from omnigibson.robots.franka_mounted import FrankaMounted
 from omnigibson.robots.freight import Freight
 from omnigibson.robots.husky import Husky

omnigibson/robots/franka.py

@@ -38,6 +38,8 @@ def __init__(
         sensor_config=None,
         # Unique to ManipulationRobot
         grasping_mode="physical",
+        # Unique to Franka
+        end_effector="gripper",
         **kwargs,
     ):
         """
@@ -84,9 +86,99 @@ 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.
+            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).
         """
+        # store end effector information
+        self.end_effector = end_effector
+        if end_effector == "gripper":
+            self._model_name = "franka_panda"
+            self._gripper_control_idx = np.arange(7, 9)
+            self._eef_link_names = "panda_hand"
+            self._finger_link_names = ["panda_leftfinger", "panda_rightfinger"]
+            self._finger_joint_names = ["panda_finger_joint1", "panda_finger_joint2"]
+            self._default_robot_model_joint_pos = np.array([0.00, -1.3, 0.00, -2.87, 0.00, 2.00, 0.75, 0.00, 0.00])
+            self._teleop_rotation_offset = np.array([-1, 0, 0, 0])
+            self._ag_start_points = [
+                GraspingPoint(link_name="panda_rightfinger", position=[0.0, 0.001, 0.045]),
+            ]
+            self._ag_end_points = [
+                GraspingPoint(link_name="panda_leftfinger", position=[0.0, 0.001, 0.045]),
+            ]
+        elif end_effector == "allegro":
+            self._model_name = "franka_allegro"
+            self._eef_link_names = "base_link"
+            # thumb.proximal, ..., thumb.tip, ..., ring.tip
+            self._finger_link_names = [f"link_{i}_0" for i in range(16)]
+            self._finger_joint_names = [f"joint_{i}_0" for i in [12, 13, 14, 15, 8, 9, 10, 11, 4, 5, 6, 7, 0, 1, 2, 3]]
+            # position where the hand is parallel to the ground
+            self._default_robot_model_joint_pos = np.concatenate(
+                ([0.86, -0.27, -0.68, -1.52, -0.18, 1.29, 1.72], np.zeros(16))
+            )
+            self._teleop_rotation_offset = np.array([0, 0.7071, 0, 0.7071])
+            self._ag_start_points = [
+                GraspingPoint(link_name=f"base_link", position=[0.015, 0, -0.03]),
+                GraspingPoint(link_name=f"base_link", position=[0.015, 0, -0.08]),
+                GraspingPoint(link_name=f"link_15_0_tip", position=[0, 0.015, 0.007]),
+            ]
+            self._ag_end_points = [
+                GraspingPoint(link_name=f"link_3_0_tip", position=[0.012, 0, 0.007]),
+                GraspingPoint(link_name=f"link_7_0_tip", position=[0.012, 0, 0.007]),
+                GraspingPoint(link_name=f"link_11_0_tip", position=[0.012, 0, 0.007]),
+            ]
+        elif "leap" in end_effector:
+            self._model_name = f"franka_{end_effector}"
+            self._eef_link_names = "palm_center"
+            # thumb.proximal, ..., thumb.tip, ..., ring.tip
+            self._finger_link_names = [
+                f"{link}_{i}" for i in range(1, 5) for link in ["mcp_joint", "pip", "dip", "fingertip", "realtip"]
+            ]
+            self._finger_joint_names = [
+                f"finger_joint_{i}" for i in [12, 13, 14, 15, 1, 0, 2, 3, 5, 4, 6, 7, 9, 8, 10, 11]
+            ]
+            # position where the hand is parallel to the ground
+            self._default_robot_model_joint_pos = np.concatenate(
+                ([0.86, -0.27, -0.68, -1.52, -0.18, 1.29, 1.72], np.zeros(16))
+            )
+            self._teleop_rotation_offset = np.array([-0.7071, 0.7071, 0, 0])
+            self._ag_start_points = [
+                GraspingPoint(link_name=f"palm_center", position=[0, -0.025, 0.035]),
+                GraspingPoint(link_name=f"palm_center", position=[0, 0.03, 0.035]),
+                GraspingPoint(link_name=f"fingertip_4", position=[-0.0115, -0.07, -0.015]),
+            ]
+            self._ag_end_points = [
+                GraspingPoint(link_name=f"fingertip_1", position=[-0.0115, -0.06, 0.015]),
+                GraspingPoint(link_name=f"fingertip_2", position=[-0.0115, -0.06, 0.015]),
+                GraspingPoint(link_name=f"fingertip_3", position=[-0.0115, -0.06, 0.015]),
+            ]
+        elif end_effector == "inspire":
+            self._model_name = f"franka_{end_effector}"
+            self._eef_link_names = "palm_center"
+            # thumb.proximal, ..., thumb.tip, ..., ring.tip
+            hand_part_names = [11, 12, 13, 14, 21, 22, 31, 32, 41, 42, 51, 52]
+            self._finger_link_names = [f"link{i}" for i in hand_part_names]
+            self._finger_joint_names = [f"joint{i}" for i in hand_part_names]
+            # position where the hand is parallel to the ground
+            self._default_robot_model_joint_pos = np.concatenate(
+                ([0.86, -0.27, -0.68, -1.52, -0.18, 1.29, 1.72], np.zeros(12))
+            )
+            self._teleop_rotation_offset = np.array([0, 0, 0.707, 0.707])
+            # TODO: add ag support for inspire hand
+            self._ag_start_points = [
+                # GraspingPoint(link_name=f"base_link", position=[0, -0.025, 0.035]),
+                # GraspingPoint(link_name=f"base_link", position=[0, 0.03, 0.035]),
+                # GraspingPoint(link_name=f"link14", position=[-0.0115, -0.07, -0.015]),
+            ]
+            self._ag_end_points = [
+                # GraspingPoint(link_name=f"link22", position=[-0.0115, -0.06, 0.015]),
+                # GraspingPoint(link_name=f"link32", position=[-0.0115, -0.06, 0.015]),
+                # GraspingPoint(link_name=f"link42", position=[-0.0115, -0.06, 0.015]),
+                # GraspingPoint(link_name=f"link52", position=[-0.0115, -0.06, 0.015]),
+            ]
+        else:
+            raise ValueError(f"End effector {end_effector} not supported for FrankaPanda")
+
         # Run super init
         super().__init__(
             prim_path=prim_path,
@@ -108,12 +200,15 @@ def __init__(
             proprio_obs=proprio_obs,
             sensor_config=sensor_config,
             grasping_mode=grasping_mode,
+            grasping_direction=(
+                "lower" if end_effector == "gripper" else "upper"
+            ),  # gripper grasps in the opposite direction
             **kwargs,
         )
 
     @property
     def model_name(self):
-        return "FrankaPanda"
+        return self._model_name
 
     @property
     def discrete_action_list(self):
@@ -139,9 +234,18 @@ def _default_controllers(self):
         controllers["gripper_{}".format(self.default_arm)] = "MultiFingerGripperController"
         return controllers
 
+    @property
+    def _default_gripper_multi_finger_controller_configs(self):
+        conf = super()._default_gripper_multi_finger_controller_configs
+        # If the end effector is not a gripper, set the mode to independent
+        if self.end_effector != "gripper":
+            conf[self.default_arm]["mode"] = "independent"
+            conf[self.default_arm]["command_input_limits"] = None
+        return conf
+
     @property
     def _default_joint_pos(self):
-        return np.array([0.00, -1.3, 0.00, -2.87, 0.00, 2.00, 0.75, 0.00, 0.00])
+        return self._default_robot_model_joint_pos
 
     @property
     def finger_lengths(self):
@@ -153,7 +257,11 @@ def arm_control_idx(self):
 
     @property
     def gripper_control_idx(self):
-        return {self.default_arm: np.arange(7, 9)}
+        return {
+            self.default_arm: np.array(
+                [list(self.joints.keys()).index(name) for name in self.finger_joint_names[self.default_arm]]
+            )
+        }
 
     @property
     def arm_link_names(self):
@@ -165,48 +273,50 @@ def arm_joint_names(self):
 
     @property
     def eef_link_names(self):
-        return {self.default_arm: "panda_hand"}
+        return {self.default_arm: self._eef_link_names}
 
     @property
     def finger_link_names(self):
-        return {self.default_arm: ["panda_leftfinger", "panda_rightfinger"]}
+        return {self.default_arm: self._finger_link_names}
 
     @property
     def finger_joint_names(self):
-        return {self.default_arm: ["panda_finger_joint1", "panda_finger_joint2"]}
+        return {self.default_arm: self._finger_joint_names}
 
     @property
     def usd_path(self):
-        return os.path.join(gm.ASSET_PATH, "models/franka/franka_panda.usd")
+        return os.path.join(gm.ASSET_PATH, f"models/franka/{self.model_name}.usd")
 
     @property
     def robot_arm_descriptor_yamls(self):
-        return {self.default_arm: os.path.join(gm.ASSET_PATH, "models/franka/franka_panda_description.yaml")}
+        return {self.default_arm: os.path.join(gm.ASSET_PATH, f"models/franka/{self.model_name}_description.yaml")}
 
     @property
     def urdf_path(self):
-        return os.path.join(gm.ASSET_PATH, "models/franka/franka_panda.urdf")
+        return os.path.join(gm.ASSET_PATH, f"models/franka/{self.model_name}.urdf")
 
     @property
     def eef_usd_path(self):
-        return {self.default_arm: os.path.join(gm.ASSET_PATH, "models/franka/franka_panda_eef.usd")}
+        return {self.default_arm: os.path.join(gm.ASSET_PATH, f"models/franka/{self.model_name}_eef.usd")}
 
     @property
     def teleop_rotation_offset(self):
-        return {self.default_arm: euler2quat([-np.pi, 0, 0])}
+        return {self.default_arm: self._teleop_rotation_offset}
 
     @property
     def assisted_grasp_start_points(self):
-        return {
-            self.default_arm: [
-                GraspingPoint(link_name="panda_rightfinger", position=[0.0, 0.001, 0.045]),
-            ]
-        }
+        return {self.default_arm: self._ag_start_points}
 
     @property
     def assisted_grasp_end_points(self):
-        return {
-            self.default_arm: [
-                GraspingPoint(link_name="panda_leftfinger", position=[0.0, 0.001, 0.045]),
-            ]
-        }
+        return {self.default_arm: self._ag_start_points}
+
+    @property
+    def disabled_collision_pairs(self):
+        # some dexhand has self collisions that needs to be filtered out
+        if self.end_effector == "allegro":
+            return [["link_12_0", "part_studio_link"]]
+        elif self.end_effector == "inspire":
+            return [["base_link", "link12"]]
+        else:
+            return []