refractored parallel gripper utils

Author: zhigenzhao

xrobotoolkit_teleop/hardware/dual_arm_ur_controller.py

@@ -32,7 +32,7 @@
     apply_delta_pose,
     quat_diff_as_angle_axis,
 )
-from xrobotoolkit_teleop.utils.gripper_utils import calc_parallel_gripper_position
+from xrobotoolkit_teleop.utils.parallel_gripper_utils import calc_parallel_gripper_position
 from xrobotoolkit_teleop.utils.path_utils import ASSET_PATH
 from xrobotoolkit_teleop.utils.xr_client import XrClient
 
@@ -192,9 +192,7 @@ def _process_xr_pose(self, xr_pose, arm_name: str):
             delta_rot = np.array([0.0, 0.0, 0.0])  # Angle-axis
         else:
             delta_xyz = (controller_xyz - self.init_controller_xyz[arm_name]) * self.scale_factor
-            delta_rot = quat_diff_as_angle_axis(
-                self.init_controller_quat[arm_name], controller_quat
-            )
+            delta_rot = quat_diff_as_angle_axis(self.init_controller_quat[arm_name], controller_quat)
         return delta_xyz, delta_rot
 
     def calc_target_joint_position(self):
@@ -284,9 +282,7 @@ def calc_target_joint_position(self):
         except RuntimeError as e:
             print(f"IK solver failed: {e}. Returning last known good joint positions.")
         except Exception as e:
-            print(
-                f"An unexpected error occurred in IK: {e}. Returning last known good joint positions."
-            )
+            print(f"An unexpected error occurred in IK: {e}. Returning last known good joint positions.")
 
     def reset(self):
         self.left_controller.reset()

xrobotoolkit_teleop/hardware/galaxea_teleop_controller.py

@@ -15,7 +15,7 @@
     apply_delta_pose,
     quat_diff_as_angle_axis,
 )
-from xrobotoolkit_teleop.utils.gripper_utils import calc_parallel_gripper_position
+from xrobotoolkit_teleop.utils.parallel_gripper_utils import calc_parallel_gripper_position
 from xrobotoolkit_teleop.utils.path_utils import ASSET_PATH
 from xrobotoolkit_teleop.utils.xr_client import XrClient
 

xrobotoolkit_teleop/simulation/mujoco_teleop_controller.py

@@ -16,15 +16,15 @@
     apply_delta_pose,
     quat_diff_as_angle_axis,
 )
-from xrobotoolkit_teleop.utils.gripper_utils import (
-    calc_parallel_gripper_position,
-)
 from xrobotoolkit_teleop.utils.mujoco_utils import (
     calc_mujoco_ctrl_from_qpos,
     calc_mujoco_qpos_from_placo_q,
     calc_placo_q_from_mujoco_qpos,
     set_mujoco_joint_pos_by_name,
 )
+from xrobotoolkit_teleop.utils.parallel_gripper_utils import (
+    calc_parallel_gripper_position,
+)
 from xrobotoolkit_teleop.utils.xr_client import XrClient
 
 
@@ -178,9 +178,7 @@ def _setup_mocap_target(self):
         """Find and set up the mocap target body."""
         for name, config in self.end_effector_config.items():
             if "vis_target" not in config:
-                print(
-                    f"Warning: 'vis_target' not found in config for {name}. Skipping mocap setup."
-                )
+                print(f"Warning: 'vis_target' not found in config for {name}. Skipping mocap setup.")
                 continue
             vis_target = config["vis_target"]
             mocap_id = mujoco.mj_name2id(self.mj_model, mujoco.mjtObj.mjOBJ_BODY, vis_target)
@@ -215,8 +213,8 @@ def run(self):
                     # Process current pose
                     if active:
                         if self.init_ee_xyz[name] is None:
-                            self.init_ee_xyz[name], self.init_ee_quat[name] = (
-                                self._get_end_effector_info(config["link_name"])
+                            self.init_ee_xyz[name], self.init_ee_quat[name] = self._get_end_effector_info(
+                                config["link_name"]
                             )
                             print(f"{name} is activated.")
                         xr_pose = self.xr_client.get_pose_by_name(config["pose_source"])
@@ -250,9 +248,7 @@ def run(self):
 
                     if "gripper_config" in config:
                         gripper_config = config["gripper_config"]
-                        trigger_value = self.xr_client.get_key_value_by_name(
-                            gripper_config["gripper_trigger"]
-                        )
+                        trigger_value = self.xr_client.get_key_value_by_name(gripper_config["gripper_trigger"])
                         gripper_pos = calc_parallel_gripper_position(
                             gripper_config["open_pos"],
                             gripper_config["close_pos"],
@@ -298,9 +294,7 @@ def _process_xr_pose(self, xr_pose, src_name):
         else:
             # Calculate relative transformation from init pose
             delta_xyz = (controller_xyz - self.init_controller_xyz[src_name]) * self.scale_factor
-            delta_rot = quat_diff_as_angle_axis(
-                self.init_controller_quat[src_name], controller_quat
-            )
+            delta_rot = quat_diff_as_angle_axis(self.init_controller_quat[src_name], controller_quat)
 
         return delta_xyz, delta_rot