Add gripper IK controller

omnigibson/controllers/__init__.py

@@ -11,7 +11,7 @@
 from omnigibson.controllers.dd_controller import DifferentialDriveController
 from omnigibson.controllers.ik_controller import InverseKinematicsController
 from omnigibson.controllers.joint_controller import JointController
-from omnigibson.controllers.multi_finger_gripper_controller import MultiFingerGripperController
+from omnigibson.controllers.multi_finger_gripper_controller import MultiFingerGripperController, GripperIKController
 from omnigibson.controllers.null_joint_controller import NullJointController
 from omnigibson.utils.python_utils import assert_valid_key
 

omnigibson/controllers/ik_controller.py

@@ -15,7 +15,7 @@
 m = create_module_macros(module_path=__file__)
 m.IK_POS_TOLERANCE = 0.002
 m.IK_POS_WEIGHT = 20.0
-m.IK_ORN_TOLERANCE = 0.001
+m.IK_ORN_TOLERANCE = 0.01
 m.IK_ORN_WEIGHT = 0.05
 m.IK_MAX_ITERATIONS = 100
 

omnigibson/controllers/multi_finger_gripper_controller.py

@@ -1,4 +1,5 @@
 import numpy as np
+import pybullet as p
 
 from omnigibson.macros import create_module_macros
 from omnigibson.controllers import IsGraspingState, ControlType, GripperController
@@ -128,7 +129,6 @@ def _command_to_control(self, command, control_dict):
 
         Args:
             command (Array[float]): desired (already preprocessed) command to convert into control signals.
-                This should always be 2D command for each gripper joint
             control_dict (Dict[str, Any]): dictionary that should include any relevant keyword-mapped
                 states necessary for controller computation. Must include the following keys:
                     joint_position: Array of current joint positions
@@ -258,3 +258,103 @@ def control_type(self):
     @property
     def command_dim(self):
         return len(self.dof_idx) if self._mode == "independent" else 1
+
+
+class GripperIKController(GripperController):
+    """
+    IK controller for dexterous hand, based on Pybullet
+    """
+    def __init__(
+            self, 
+            arm_name,
+            finger_tip_info,
+            finger_joint_names,
+            eef_urdf_path,
+            control_freq,
+            control_limits,
+            dof_idx,
+            use_delta_commands=True,
+            command_input_limits=None,
+            command_output_limits=None,
+        ) -> None:
+        """
+        Initializes the Gripper IK controller
+        Args:
+            eef_urdf_path (str): path to the end effector urdf file
+            max_iter (int): maximum number of iterations for the IK solver, default is 100.
+        """
+        self.arm_name = arm_name
+        self.finger_tip_info = finger_tip_info
+        self.use_delta_commands = use_delta_commands
+        # start pybullet
+        clid = p.connect(p.SHARED_MEMORY)
+        if (clid < 0):
+            p.connect(p.GUI)
+        # load hand
+        self.hand_id = p.loadURDF(eef_urdf_path, [0, 0, 0])
+        p.resetBasePositionAndOrientation(self.hand_id, [0, 0, 0], p.getQuaternionFromEuler([0, 0, 0]))
+        # calculate finger joint remapping
+        self.joint_index_remapping = []
+        for i in range(p.getNumJoints(self.hand_id)):
+            joint_name = p.getJointInfo(self.hand_id, i)[1].decode("utf-8")
+            if joint_name in finger_joint_names:
+                self.joint_index_remapping.append(finger_joint_names.index(joint_name))
+        assert len(self.joint_index_remapping) == len(finger_joint_names), "Not every finger joint is remapped!"
+        # load balls
+        ball_radius = 0.02
+        ball_shape = p.createCollisionShape(p.GEOM_SPHERE, radius=ball_radius)
+        self.ball = p.createMultiBody(baseMass=0.05, baseCollisionShapeIndex=ball_shape)
+        p.setCollisionFilterGroupMask(self.ball, -1, 0, 0)
+
+        # Run super init
+        super().__init__(
+            control_freq=control_freq,
+            control_limits=control_limits,
+            dof_idx=dof_idx,
+            command_input_limits=command_input_limits,
+            command_output_limits=command_output_limits,
+        )
+
+    def _command_to_control(self, command, control_dict):
+        current_pos = np.array(control_dict["finger_tip_relative_poses"][self.arm_name])
+        command = np.reshape(command, (-1, 3))
+        if self.use_delta_commands:
+            target_pos = current_pos + command
+        else:
+            target_pos = command
+        p.resetBasePositionAndOrientation(self.ball, target_pos[3], [0, 0, 0, 1])
+        target_joint_poses = p.calculateInverseKinematics2(
+            self.hand_id,
+            self.finger_tip_info["tip_link_indices"],
+            target_pos,
+            solver=p.IK_DLS,
+            maxNumIterations=100,
+            residualThreshold=0.0001
+        )
+        bullet_joints = target_joint_poses[0:4] + (0.0,) + target_joint_poses[4:8] + (0.0,) + target_joint_poses[8:12] + (0.0,) + target_joint_poses[12:16]
+
+        for i in range(19):
+            p.setJointMotorControl2(bodyIndex=self.hand_id,
+                                    jointIndex=i,
+                                    controlMode=p.POSITION_CONTROL,
+                                    targetPosition=bullet_joints[i],
+                                    targetVelocity=0,
+                                    force=500,
+                                    positionGain=0.3,
+                                    velocityGain=1)
+        p.stepSimulation()
+        return np.array(target_joint_poses)[self.joint_index_remapping]
+
+    @property
+    def control_type(self):
+        return ControlType.get_type(type_str="position")
+
+    @property
+    def command_dim(self):
+        return len(self.finger_tip_info["finger_names"]) * 3
+
+    def is_grasping(self):
+        return IsGraspingState.UNKNOWN
+
+    def reset(self):
+        pass

omnigibson/robots/behaviorbot.py

@@ -336,8 +336,8 @@ def update_controller_mode(self):
         for link in self.links.values():
             link.ccd_enabled = True
         for arm in self.arm_names:
-            for link in self.finger_link_names[arm]:
-                self.links[link].mass = 0.02
+            for finger_link in self.finger_links[arm]:
+                finger_link.mass = 0.02
         self.links["base"].mass = 15
         self.links["lh_palm"].mass = 0.3
         self.links["rh_palm"].mass = 0.3

omnigibson/robots/franka_allegro.py

@@ -1,7 +1,5 @@
 import os
 import numpy as np
-from typing import Dict, Iterable
-from omni.isaac.motion_generation import LulaKinematicsSolver
 
 import omnigibson.utils.transform_utils as T
 from omnigibson.macros import gm
@@ -121,8 +119,6 @@ def __init__(
             **kwargs,
         )
 
-        self.allegro_ik_controller = AllegroIKController(self)
-
     @property
     def model_name(self):
         return "FrankaAllegro"
@@ -139,10 +135,10 @@ def _create_discrete_action_space(self):
     def update_controller_mode(self):
         super().update_controller_mode()
         # overwrite joint params here
-        for i in range(7):
-            self.joints[f"panda_joint{i+1}"].damping = 100
-            self.joints[f"panda_joint{i+1}"].stiffness = 1000
-            self.joints[f"panda_joint{i+1}"].max_effort = 100
+        # for i in range(7):
+        #     self.joints[f"panda_joint{i+1}"].damping = 100
+        #     self.joints[f"panda_joint{i+1}"].stiffness = 1000
+        #     self.joints[f"panda_joint{i+1}"].max_effort = 100
 
     @property
     def controller_order(self):
@@ -155,14 +151,6 @@ def _default_controllers(self):
         controllers["gripper_{}".format(self.default_arm)] = "MultiFingerGripperController"
         return controllers
 
-    @property
-    def _default_arm_ik_controller_configs(self):
-        conf = super()._default_arm_ik_controller_configs
-        conf[self.default_arm]["mode"] = "pose_absolute_ori"
-        conf[self.default_arm]["command_input_limits"] = None
-        conf[self.default_arm]["motor_type"] = "position"
-        return conf
-
     @property
     def _default_gripper_multi_finger_controller_configs(self):
         conf = super()._default_gripper_multi_finger_controller_configs
@@ -206,7 +194,8 @@ def finger_link_names(self):
 
     @property
     def finger_joint_names(self):
-        return {self.default_arm: [f"joint_{i}_0" for i in range(16)]}
+        # thumb.proximal, ..., thumb.tip, ..., ring.tip
+        return {self.default_arm: [f"joint_{i}_0" for i in [12, 13, 14, 15, 8, 9, 10, 11, 4, 5, 6, 7, 0, 1, 2, 3]]}
 
     @property
     def usd_path(self):
@@ -228,7 +217,7 @@ def urdf_path(self):
         return os.path.join(gm.ASSET_PATH, "models/franka/franka_allegro.urdf")
 
     @property
-    def gripper_urdf_path(self):
+    def eef_urdf_path(self):
         return os.path.join(gm.ASSET_PATH, "models/franka/allegro_hand.urdf")
 
     @property
@@ -252,6 +241,27 @@ def assisted_grasp_end_points(self):
             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]),
         ]}
+
+    def get_control_dict(self):
+        # In addition to super method, add in finger tip relative pos
+        dic = super().get_control_dict()
+        dic["finger_tip_relative_poses"] = {self.default_arm: []}
+        hand_base_pos, hand_base_orn = self.links[self.eef_link_names[self.default_arm]].get_position_orientation()
+        for finger_tip_name in self.finger_tip_info[self.default_arm]["tip_link_names"]:
+            finger_tip_pos, finger_tip_orn = self.links[finger_tip_name].get_position_orientation()
+            relative_finger_tip_pos, _ = T.relative_pose_transform(
+                finger_tip_pos, finger_tip_orn, hand_base_pos, hand_base_orn
+            )
+            dic["finger_tip_relative_poses"][self.default_arm].append(relative_finger_tip_pos)
+        return dic
+
+    @property
+    def finger_tip_info(self):
+        return {self.default_arm: {
+            "finger_names": ["thumb", "index", "middle", "ring"],   # finger name
+            "tip_link_names": ["link_15_0_tip", "link_11_0_tip", "link_7_0_tip", "link_3_0_tip"],   # fingertip link name
+            "tip_link_indices": [19, 14, 9, 4], # fingertip link index in urdf
+        }}
 
     @property
     def vr_rotation_offset(self):
@@ -269,7 +279,6 @@ def remap_thumb_to_allegro(self, coord: np.ndarray) -> np.ndarray:
             coord[i] = np.interp(coord[i], hand_bound[i], allegro_bound[i])
         return coord
 
-
     def gen_action_from_vr_data(self, vr_data: dict):
         action = np.zeros(22)
         if "hand_data" in vr_data:
@@ -302,58 +311,3 @@ def gen_action_from_vr_data(self, vr_data: dict):
             action[:6] = action_from_controller[:6]
             action[6:] = action_from_controller[6]
         return action
-
-
-class AllegroIKController:
-    """
-    IK controller for Allegro hand, based on the LulaKinematicsSolver
-    """
-    def __init__(self, robot: FrankaAllegro, max_iter=150) -> None:
-        """
-        Initializes the IK controller
-        Args:
-            robot (FrankaAllegro): the Franka Allegro robot
-            max_iter (int): maximum number of iterations for the IK solver, default is 100.
-        """
-        self.robot = robot
-        self.fingers = {
-            "ring":     ("link_3_0_tip",    np.array([7, 11, 15, 19])),     # finger name, finger tip link name, joint indices
-            "middle":   ("link_7_0_tip",    np.array([9, 13, 17, 21])),
-            "index":    ("link_11_0_tip",   np.array([10, 14, 18, 22])),
-            "thumb":    ("link_15_0_tip",   np.array([8, 12, 16, 20])), 
-        }
-        self.finger_ik_solvers = {}
-        for finger in self.fingers.keys():
-            self.finger_ik_solvers[finger]  = LulaKinematicsSolver(
-                robot_description_path = robot.robot_gripper_descriptor_yamls[finger],
-                urdf_path = robot.gripper_urdf_path
-            )
-            self.finger_ik_solvers[finger].ccd_max_iterations = max_iter
-
-    def solve(self, target_gripper_pos: Dict[str, Iterable[float]]) -> Dict[str, np.ndarray]:
-        """
-        compute the joint positions given the position of each finger tip
-        Args:
-            target_gripper_pos (Dict[str, Iterable[float]]): (finger name, the 3-array of target positions of the finger tips)
-        Returns:
-            Dict[str, np.ndarray]: (finger name, 4-array of joint positions in order of proximal to tip)
-        """
-        target_joint_positions = {}
-        # get the current finger joint positions
-        finger_joint_positions = self.robot.get_joint_positions()
-        # get current hand base pose
-        hand_base_pos, hand_base_orn = self.robot.links["base_link"].get_position_orientation()
-        for finger_name, pos in target_gripper_pos.items():
-            target_joint_positions[finger_name] = finger_joint_positions[self.fingers[finger_name][1]]
-            # Grab the finger joint positions in order to reach the desired finger pose
-            self.finger_ik_solvers[finger_name].set_robot_base_pose(hand_base_pos, T.convert_quat(hand_base_orn, "wxyz"))
-            finger_joint_pos, success = self.finger_ik_solvers[finger_name].compute_inverse_kinematics(
-                frame_name=self.fingers[finger_name][0],
-                target_position=pos,
-                target_orientation=None,
-                warm_start=finger_joint_positions[self.fingers[finger_name][1]]
-            )
-            if success:
-                target_joint_positions[finger_name] = finger_joint_pos
-
-        return target_joint_positions

omnigibson/robots/franka_leap.py

@@ -1,7 +1,6 @@
 import os
 import numpy as np
 from typing import Dict, Iterable
-from omni.isaac.motion_generation import LulaKinematicsSolver
 
 import omnigibson.utils.transform_utils as T
 from omnigibson.macros import gm
@@ -136,11 +135,6 @@ def _create_discrete_action_space(self):
 
     def update_controller_mode(self):
         super().update_controller_mode()
-        # overwrite joint params here
-        for i in range(7):
-            self.joints[f"panda_joint{i+1}"].damping = 100
-            self.joints[f"panda_joint{i+1}"].stiffness = 1000
-            self.joints[f"panda_joint{i+1}"].max_effort = 100
 
     @property
     def controller_order(self):
@@ -152,14 +146,6 @@ def _default_controllers(self):
         controllers["arm_{}".format(self.default_arm)] = "InverseKinematicsController"
         controllers["gripper_{}".format(self.default_arm)] = "MultiFingerGripperController"
         return controllers
-
-    @property
-    def _default_arm_ik_controller_configs(self):
-        conf = super()._default_arm_ik_controller_configs
-        conf[self.default_arm]["mode"] = "pose_absolute_ori"
-        conf[self.default_arm]["command_input_limits"] = None
-        conf[self.default_arm]["motor_type"] = "position"
-        return conf
 
     @property
     def _default_gripper_multi_finger_controller_configs(self):