add tool cfg

harvest_arm_rm/config/config.yaml

@@ -7,6 +7,8 @@ harvest_arm_rm:
 
     tool_name: "scissor"
 
+    tool_names: ["scissor", "omnipic", "minisci", "no_tool"]
+
     initial_joint: [-82.60,   20.28,  -32.15,   50.28,   17.28,   64.055,   -20.62]
 
     max_joint: [155.0, 115.0, 172.0, 130.0, 175.0, 125.0, 179.0]
@@ -20,13 +22,17 @@ harvest_arm_rm:
     max_cartesian_acc: 1.0
     max_cartesian_angular_acc: 2.0
 
+    bound_cartesian: [
+      -0.3, -0.5, -0.1, 0.3, 0.5, 0.6
+    ]
+
 
     arm_installation: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
 
 #   0~2 position, x, y, z, m
 #   3~6 orientation, quaternion, x, y, z, w
 #   7 mass, kg
-#   8~11 center of mass, x, y, z, m
+#   8~10 center of mass, x, y, z, m
     tools_in_ee.scissor: [
       0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0,
       0.66, 0.0, 0.0, 0.06
@@ -47,14 +53,19 @@ harvest_arm_rm:
       0.0, 0.0, 0.0, 0.0
     ]
 
-    joint_cmd_topic: "/joint_cmd"
+    action_topic: "/action/arm"
-    joint_state_topic: "/joint_sts"
+    observation_topic: "/observation/arm"
-    arm_enable_topic: "/arm_enable"
+
-    tool_cmd_topic: "/tool_cmd"
+    interval: 0.02
-
+    kp_j : 1.0
-    
+
-
+    arm_ctrl_voltage: 2 # 0: 0V, 2: 12V, 3: 24V
-
+
+    # https://develop.realman-robotics.com/en/robot/apipython/classes/controllerIOConfig/#set-the-io-mode-rm-set-io-mode
+    arm_ctrl_io_mode: [0, 0, 1, 1] #  0: input, 1: output
+
+    arm_tool_io_voltage: 2 
+    arm_tool_io_mode: [1, 1] #  0: input, 1: output
 
 
 

harvest_arm_rm/harvest_arm_rm/run_realman_arm.py

@@ -4,9 +4,6 @@ from rclpy.node import Node
 from rclpy.executors import MultiThreadedExecutor
 import time
 import numpy as np
-from arm_pkg.arm_realman.arg_cfg import *
-from arm_pkg.arm_realman.fun_basic import *
-from arm_pkg.arm_realman.fun_savefile import *
 from tf2_msgs.msg import TFMessage
 from sensor_msgs.msg import JointState
 from geometry_msgs.msg import PoseStamped, TransformStamped
@@ -15,9 +12,6 @@ from std_msgs.msg import Float32MultiArray
 import math
 
 from Robotic_Arm.rm_robot_interface import *
-from arm_pkg.arm_realman.arg_cfg import *
-from arm_pkg.arm_realman.fun_rviz import Rviz_single_arm as Rviz_arm
-from arm_pkg.arm_realman.fun_peripheral import *
 
 
 class RMArmNode(Node):
@@ -33,25 +27,34 @@ class RMArmNode(Node):
         super().__init__('harvest_arm_rm')
         self.config, self.sts, self.arm_arg = self.get_arg()
 
-        self.arm_topic()
 
         try:
             self.arm_arg['arm'] = RoboticArm(rm_thread_mode_e(mode))
             self.arm_arg['arm_handle'] = self.arm_arg['arm'].rm_create_robot_arm(self.arm_arg['robot_ip'], self.arm_arg['port'], level)
             if self.arm_arg['arm_handle'].id == -1:
-                print("Failed to connect to the real robot arm (socket error)!")
+                self.get_logger().error("Failed to connect to the real robot arm (socket error)!")
             else:
                 self.get_logger().info(f"Successfully connected to the real robot arm: {self.arm_arg['arm_handle'].id}")
                 # if real robot connected, initialize following cfg
-                self.armcfg()
+                self.arm_motion_lmt()
                 self.udp_config()
-                self.arm_arg['arm'] = peripheral_cfg(self.arm_arg['arm'], self.config)
+                self.arm_arg['arm'] = self.peripheral_cfg(self.arm_arg['arm'], self.config)
-                self.init_done = True
             # self.move_to_init()
         except Exception as e:
-            self.get_logger().error(f"Robot arm hardware init error: {e}, enter RViz simulation mode!")
+            self.get_logger().error(f"Robot arm hardware init error: {e} !")
             self.arm_arg['arm'] = None
 
+        # Create publisher for joint states
+        self.observation_pub = self.create_publisher(JointState, self.config["observation_topic"], 10)
+        
+        #   Create subcriber for target joint
+        self.action_sub = self.create_subscription(JointState, self.config["action_topic"], self.arm_action_callback, 10)
+
+        self.timer = self.create_timer(
+            self.config["interval"],
+            self.rm_movej_timer
+        )
+
 
     def get_arg(self):
         """
@@ -59,46 +62,61 @@ class RMArmNode(Node):
         Returns:
             dict: Configuration dictionary containing robot IP, host IP, prefix, left/right status, port, savefile flag, and scissor gripper status.
         """
-        tool_offsets = {}
-        tool_offsets['scissor'] = self.declare_parameter('tools_in_ee.scissor', 0.0).value
-        tool_offsets['omnipick'] = self.declare_parameter('tools_in_ee.omnipick', 0.0).value
-        tool_offsets['minisci'] = self.declare_parameter('tools_in_ee.minisci', 0.0).value
-        tool_offsets['no_tool'] = self.declare_parameter('tools_in_ee.no_tool', 0.0).value
         tool_name = self.declare_parameter('tool_name', "scissor").value
-        tool_offset_list = tool_offsets[tool_name]
+        tool_name_param = 'tools_in_ee.' + tool_name
+        try: 
+            tool_offset_list = self.declare_parameter(tool_name_param, [0.0] * 11).value
+        except:
+            self.get_logger().error(f"Tool name '{tool_name}' not found in parameters. Please check the configuration.")
+
+
+        acc_lmt = self.declare_parameter('max_joint_acc', 600.0).value
+        interval = self.declare_parameter('interval', 0.02).value
+
 
         config = {
             "robot_ip": self.declare_parameter('robot_ip', "192.168.1.18").value,
             "host_ip": self.declare_parameter('host_ip', "192.168.1.101").value,
             "port": self.declare_parameter('port', 8089).value,
 
-            "max_joint": self.declare_parameter('max_joint', joint_max).value,
+            "max_joint": self.declare_parameter('max_joint', [0.0] * 7).value,
-            "min_joint": self.declare_parameter('min_joint', joint_min).value,
+            "min_joint": self.declare_parameter('min_joint', [0.0] * 7).value,
             "max_joint_speed": self.declare_parameter('max_joint_speed', 180.0).value,
-            "max_joint_acc": self.declare_parameter('max_joint_acc', 600.0).value,
+            "att_2": acc_lmt * interval * interval * 0.5,
 
             "max_cartesian_speed": self.declare_parameter('max_cartesian_speed', 1.0).value,
             "max_cartesian_angular_speed": self.declare_parameter('max_cartesian_angular_speed', 1.5).value,
             "max_cartesian_acc": self.declare_parameter('max_cartesian_acc', 1.5).value,
             "max_cartesian_angular_acc": self.declare_parameter('max_cartesian_angular_acc', 2.0).value,
+            "bound_cartesian": self.declare_parameter('bound_cartesian', [-0.3, -0.5, -0.1, 0.3, 0.5, 0.6]).value,
 
             "arm_installation": self.declare_parameter('arm_installation', [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]).value,
+            "tool_name": tool_name,
             "tool_offset": tool_offset_list,
+            "tool_names": self.declare_parameter('tool_names', ["scissor", "omnipic", "minisci", "no_tool"]).value,
             
-            "joint_cmd_topic": self.declare_parameter('joint_cmd_topic', "/joint_cmd").value,
-            "joint_state_topic": self.declare_parameter('joint_state_topic', "/joint_sts").value,
-            "arm_enable_topic": self.declare_parameter('arm_enable_topic', "/arm_enable").value,
-            "tool_cmd_topic": self.declare_parameter('tool_cmd_topic', "/tool_cmd").value,
             "initial_joint": self.declare_parameter('initial_joint', [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]).value,
+            "kp_j": self.declare_parameter('kp_j', 1.0).value,
+
+            "interval": interval,
+            "action_topic": self.declare_parameter('action_topic', "/action/arm").value,
+            "observation_topic": self.declare_parameter('observation_topic', "/observation/arm").value,
+
+            "arm_ctrl_voltage": self.declare_parameter('arm_ctrl_voltage', 2).value,
+            "arm_ctrl_io_mode": self.declare_parameter('arm_ctrl_io_mode', [0, 0, 1, 1]).value,
+            "arm_tool_io_voltage": self.declare_parameter('arm_tool_io_voltage', 2).value,
+            "arm_tool_io_mode": self.declare_parameter('arm_tool_io_mode', [1, 1]).value
         }
 
         sts = {
             "joint_cmd": [0.0] * 7,
-            "joint_sts": [0.0] * 7,
+            "joint_target": [0.0] * 7,
-            "joint_speed": [0.0] * 7,
+            "joint_pos_sts": [0.0] * 7,
-            "arm_enable": False,
+            "joint_speed_sts": [0.0] * 7,
-            "tool_cmd": False,
+            "arm_enable":  int(0),
+            "tool_cmd":  int(0),
             "pose_sts": [0.0] * 6,
+            "timer_cnt": 0,
         }
 
         arm_arg = {
@@ -108,52 +126,29 @@ class RMArmNode(Node):
 
         return config, sts, arm_arg
     
-    def arm_topic(self):
-        """
-        Initialize ROS2 topics for the robotic arm.
-        """
-        self.subname = self.config["joint_cmd_topic"]
 
-        # Create publisher for joint states
-        self.joint_sts_pub = self.create_publisher(JointState, self.config["joint_state_topic"], 10)
-        # Create subscriber for arm enable state
-        self.arm_en_sub = self.create_subscription(Bool, self.config["arm_enable_topic"], self.arm_enable_callback, 10)
-        # Create subscriber for tool command
-        self.tool_sub = self.create_subscription(Bool, self.config["tool_cmd_topic"], self.tool_callback, 10)
-        #   Create subcriber for target joint
-        self.joint_cmd_sub = self.create_subscription(JointState, self.config["joint_cmd_topic"], self.joint_cmd_callback, 10)
 
-    def arm_enable_callback(self, msg):
+    def arm_action_callback(self, msg):
-        """
-        Callback function for arm enable state.
-        Args:
-            msg (Bool): Message containing the arm enable state.
-        """
-        self.sts["arm_enable"] = msg.data
-        if self.sts["arm_enable"]:
-            self.get_logger().info(f'\n------------------Arm Enabled ----------------------')
-        else:
-            self.get_logger().info(f'\n------------------Arm Disabled ----------------------')
-
-    def tool_callback(self, msg):
-        """
-        Callback function for tool command.
-        Args:
-            msg (Bool): Message containing the tool command state.
-        """
-        self.sts["tool_cmd"] = msg.data
-        if self.sts["tool_cmd"]:
-            self.get_logger().info(f'\n------------------Tool Command Enabled ----------------------')
-        else:
-            self.get_logger().info(f'\n------------------Tool Command Disabled ----------------------')
-
-    def joint_cmd_callback(self, msg):
         """
         Callback function for joint command.
         Args:
             msg (JointState): Message containing the joint command.
         """
-        self.sts["joint_cmd"] = list(msg.position)
+        action_list = list(msg.position)
+        self.sts["arm_enable"] = int(action_list[0])
+        self.sts["joint_target"] = action_list[1:8]
+        self.sts["tool_cmd"] = int(action_list[8])
+
+    def arm_observation_pub(self):
+        """
+        Publish the current state of the robotic arm.
+        """
+        msg = JointState()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.position = [float(self.sts["arm_enable"])] + list(self.sts["joint_pos_sts"]) + [float(self.sts["tool_cmd"])] + [float(self.sts["timer_cnt"])]
+        msg.velocity = self.sts["joint_speed_sts"]
+        msg.effort = []
+        self.observation_pub.publish(msg)
 
 
     def udp_config(self):
@@ -168,9 +163,9 @@ class RMArmNode(Node):
         self.arm_state_callback = rm_realtime_arm_state_callback_ptr(self.udp_callback)
         # Use the stored callback
         self.config["arm"].rm_realtime_arm_state_call_back(self.arm_state_callback)
-        while sum(abs(self.sts["pose_sts"])) < 0.001 or sum(abs(np.array(self.sts["joint_sts"]))) < 0.0001:
+        while sum(abs(self.sts["pose_sts"])) < 0.001 or sum(abs(np.array(self.sts["joint_pos_sts"]))) < 0.0001:
             time.sleep(1)
-            print(f'p_measured = {self.sts["pose_sts"]}, j_measured = {self.sts["joint_sts"]}')
+            self.get_logger().info("Waiting for initial robot state...")
 
     def udp_callback(self, data):
         #   extract the pose data from the UDP data
@@ -206,10 +201,7 @@ class RMArmNode(Node):
                 self.get_logger().warn(f"Warning: Expected 7 joint positions, got {len(joint_position)}: {joint_position}")
                 # Pad with zeros or truncate to 7
             self.sts["joint_sts"] = joint_position
-            self.sts["joint_speed"] = joint_speed
+            self.sts["joint_speed_sts"] = joint_speed
-
-#           ------------- send cmd to the Rviz  ----------------------------------
-            self.rviz.pub_target_j(self.sts["joint_sts"])
 
         except Exception as e:
             self.get_logger().error(f"Error in UDP callback: {e}")
@@ -220,10 +212,9 @@ class RMArmNode(Node):
         if not self.initialized:
             # Perform movej motion for  robot arm
             self.movej(self.config["initial_joint"])
-
             self.get_logger().info('\n------------------- ready ------------------')
 
-    def armcfg(self ):
+    def arm_motion_lmt(self ):
         #   configure the motion limitations for the robot
         self.arm_arg['arm'].rm_set_avoid_singularity_mode(True)
         self.arm_arg['arm'].rm_set_arm_max_line_speed(self.config["max_cartesian_speed"])
@@ -231,266 +222,152 @@ class RMArmNode(Node):
         self.arm_arg['arm'].rm_set_arm_max_line_acc(self.config["max_cartesian_acc"])
         self.arm_arg['arm'].rm_set_arm_max_angular_acc(self.config["max_cartesian_angular_acc"])
         for i in range(1,8):
-            self.robot.rm_set_joint_max_speed(i, self.config["max_joint_speed"])
+            self.arm_arg['arm'].rm_set_joint_max_speed(i, self.config["max_joint_speed"])
-            self.robot.rm_set_joint_max_acc(i, self.config["max_joint_acc"])
+            self.arm_arg['arm'].rm_set_joint_max_acc(i, self.config["max_joint_acc"])
 
     def disconnect(self):
         """
         Disconnect from the robot arm.
         """
-        handle = self.robot.rm_delete_robot_arm()
+        handle = self.arm_arg['arm'].rm_delete_robot_arm()
         if handle == 0:
             self.get_logger().info("\nSuccessfully disconnected from the robot arm\n")
         else:
             self.get_logger().info("\nFailed to disconnect from the robot arm\n")
 
-    def movej(self, joint, v=20, r=0, connect=0, block=1):
-        """
-        Perform movej motion.
-        Args:
-            joint (list of float): Joint positions.
-            v (float, optional): Speed of the motion. Defaults to 20.
-            connect (int, optional): Trajectory connection flag. Defaults to 0.
-            block (int, optional): Whether the function is blocking (1 for blocking, 0 for non-blocking). Defaults to 1.
-            r (float, optional): Blending radius. Defaults to 0.
-        """
-        if not self.robot_handle.id == -1 :
-            movej_result = self.robot.rm_movej(joint, v, r, connect, block)
-            if movej_result == 0:
-                self.get_logger().info("\n real robot movej motion succeeded\n")
-            else:
-                self.get_logger().error(f"\n real robot movej motion failed, Error code: {movej_result}\n")
-        else:
-        # ---------------  movej in RViz start ---------------------------
-            self.j_t, self.j_measured  = self.rviz.movej(joint , self.j_measured)
-            self.p_measured = np.array( self.algo_handle.rm_algo_forward_kinematics(self.j_measured, flag=1) )
-
-        # ---------------  movej in RViz end ---------------------------
-
-
-    def movel(self, pose, v=20, r=0, connect=0, block=1):
-        """
-        Perform movel motion.
-        Args:
-            pose (list of float): End position [x, y, z, rx, ry, rz].
-            v (float, optional): Speed of the motion. Defaults to 20.
-            connect (int, optional): Trajectory connection flag. Defaults to 0.
-            block (int, optional): Whether the function is blocking (1 for blocking, 0 for non-blocking). Defaults to 1.
-            r (float, optional): Blending radius. Defaults to 0.
-        """
-        if not self.robot_handle.id == -1 :
-            self.get_logger().info(f"\n Entering the movel() with pose = {pose} \n")
-            movel_result = self.robot.rm_movel(pose, v, r, connect, block)
-            if movel_result == 0:
-                self.get_logger().info("\n movel motion succeeded\n")
-            else:
-                self.get_logger().error(f"\n real robot movel motion failed, Error code: {movel_result}  \n")
-
-            self.get_logger().info("\n Starting the Rviz movel() \n")
-        else:
-        # ---------------  movel in RViz start ---------------------------
-            self.j_measured = self.rviz.movel(p_t=pose,j_m=self.j_measured,p_m=self.p_measured)
-
-        self.get_logger().info("Rviz movel() done\n")
-        # ---------------  movel in RViz end ---------------------------
 
 
     def movej_canfd(self, j):
-        if self.robot_handle.id == -1:
+        movej_canfd_result = self.sts["arm"].rm_movej_canfd(j, False)
-            self.rviz.pub_target_j(j)
-            self.j_measured = j
-        else:
-            movej_canfd_result = self.robot.rm_movej_canfd(j, False)
         if movej_canfd_result != 0:
             self.get_logger().error("\n movej_canfd motion failed, Error code: ", movej_canfd_result, "\n")
 
 
-
+    def rm_movej_timer(self):
-
-class ArmCtlNode(RMArmNode):
-    def __init__(self, ik = True):
-        super().__init__()
-
-        self.dof = 7
-        self.a_max = acc_max_j
-        self.v_max = vel_max_j
-        #   initialize the state of the inverse kinematics
-        self.k = k_j
-
-        self.interval = 0.02
-
-        self.lmt_05att = np.array([[-1]*self.dof , [1]*self.dof ]) * self.interval ** 2 * 0.5 * self.a_max * lmt_scale
-
-        #   initialize the target joint positions and former target joint positions
-        self.j_t_f = self.j_measured
-        self.p_measured_f = self.p_measured
-
-        #   state of whether the target pose has already been received
-        self.p_t_rcvd = False
-        #   define the state of whether the teleoperation is enabled
-        self.tele_en = False
-        #   define the former state of whether the teleoperation is enabled
-        self.tele_en_f = False
-        #   define the state of the scissor. True: open; False: close
-        self.scissor_en = False
-
-        self.scale = np.ones(self.dof)
-
-        self.frame_id = 0
-
-        # -------------------------- create the timer -----------------------------
-        #   create timer for main controller
-        self.timer = self.create_timer(
-            self.interval,
-            self.rm_move_arm
-        )
-        self.timer_count = 0
-        #   create the subscriber to receive target joints
-        self.pose_sub = self.create_subscription(
-            JointState,
-            self.subname,
-            self.target_update_callback,
-            2)
-        # Create subscription for Bool topic to receive teleoperation enable state
-        self.en_sub = self.create_subscription(
-            Bool,
-            en_topic,
-            self.tele_en_callback,
-            2)
-
-        # Create subscription for Bool topic to receive the scissor state
-        self.sci_sub = self.create_subscription(
-            Bool,
-            self.scissor_topic,
-            self.scissor_callback,
-            2)
-
-        self.tool_flg = 0
-        self.tool_timer = self.create_timer(
-            0.2,
-            self.rm_tool_timer
-        )
-
-        #   handle for the data saving
-        self.file_hd = SaveFile( self.savefile_flg , self.left)
-
-        print('---------------- End of ArmCtlNode initial -----------------------')
-
-
-    # -------------------------- timer call fun --------------------------
-    def rm_move_arm(self):
         """
         50Hz timer callback function, independent thread, not in main loop
-        send pose/joint command to the robot
+        send joint command to the arm
         """
-        if self.tele_en:
+        if self.sts["arm_enable"] == 1:
-            if not self.tele_en_f:
+            dis_max_vt = self.config["max_joint_speed"] * self.config["interval"]
-                self.p_t_f = self.p_measured
+            dis_vt = np.array(self.sts["joint_speed_sts"]) * self.config["interval"]
-                self.j_t_f = self.j_measured
+            d_j = np.clip(a=(np.array(self.sts["joint_target"]) - np.array(self.sts["joint_pos_sts"])) * self.config["kp_j"], 
-            if self.p_t_rcvd:
+                            a_min= dis_vt - self.config["lmt_05att"] , 
+                            a_max=dis_vt + self.config["lmt_05att"])
+            d_j = np.clip(d_j, -dis_max_vt, dis_max_vt)
+            self.sts["joint_cmd"] = (d_j + np.array(self.sts["joint_pos_sts"])).tolist()
+            self.movej_canfd(self.sts["joint_cmd"])
 
-                j_cmd = self.cal_j_t(self.j_t)
-                self.movej_canfd(j_cmd)
-                t = self.j_t
 
-                self.file_hd.write(t, self.j_measured, self.p_measured)
+        self.arm_observation_pub()
+
+        self.sts["timer_cnt"] += 1
+
+        if self.sts["timer_cnt"] % (50 * 2 ) == 0:
+            self.get_logger().info(f'at {self.sts["timer_cnt"] * self.config["interval"] * 2} s, the en_state is {self.sts["arm_enable"]}, pose = {self.sts["pose_sts"]}, j cmd = {self.sts["joint_cmd"]}\n\n')
+
+
+    def peripheral_cfg(self, arm, config):
+        """
+        Configure the peripheral settings of the robotic arm.
+        Args:
+            arm (RoboticArm): Instance of the RoboticArm class.
+            config (dict): Configuration dictionary containing tool offsets.
+        """
+        self.tool_cfg(arm, config)
+        self.connector_cfg(arm, config)
+
+        return arm
+    
+    def tool_cfg(self, arm, config):
+        """
+        Configure the tool settings of the robotic arm.
+        Args:
+            arm (RoboticArm): Instance of the RoboticArm class.
+            config (dict): Configuration dictionary containing tool offsets.
+        """
+        tool_offset = config["tool_offset"]
+        tool_name = config["tool_name"]
+        eu = arm.rm_algo_quanternion2euler([tool_offset[6], tool_offset[3], tool_offset[4], tool_offset[5]])
+        tool_frame = rm_frame_t(tool_name,[tool_offset[0], tool_offset[1], tool_offset[2], eu[0], eu[1], eu[2]],
+                                tool_offset[7], [tool_offset[8], tool_offset[9], tool_offset[10]])
+        info = arm.rm_get_total_tool_frame()
+        if tool_name in info["tool_names"]:
+            ret = arm.rm_update_tool_frame(tool_frame)
         else:
-                self.p_t_f = self.p_measured
+            ret = arm.rm_set_manual_tool_frame(tool_frame)
-                self.j_t_f = self.j_measured
 
-        if self.robot_handle.id == -1:
+        ret = arm.rm_change_tool_frame(tool_name)
-            self.rviz.pub_target_j(self.j_measured)
+        if ret != 0:
+            raise RuntimeError(f"Failed to change tool frame to {tool_name}, ret={ret}")
         
-        self.timer_count += 1
+    def connector_cfg(self, arm, config):
-        self.tele_en_f = self.tele_en
+        """
+        Configure the connector settings of the robotic arm, tool connector and controller board connector.
+        Args:
+            arm (RoboticArm): Instance of the RoboticArm class.
+            config (dict): Configuration dictionary containing connector settings.
+        """
+        # Implement connector configuration logic here
+        arm.rm_set_voltage(config["arm_ctrl_voltage"])
+        arm.rm_set_io_mode(1, config["arm_ctrl_io_mode"][0], 50, 2)
+        arm.rm_set_io_mode(2, config["arm_ctrl_io_mode"][1], 50, 2)
+        arm.rm_set_io_mode(3, config["arm_ctrl_io_mode"][2], 50, 2)
+        arm.rm_set_io_mode(4, config["arm_ctrl_io_mode"][3], 50, 2)
 
-        self.p_measured_f = self.p_measured
+        if config["tool_name"] == config["tool_names"][0]:  # scissor
-        self.j_measured_f = self.j_measured
+            arm.rm_set_tool_voltage(config["arm_tool_io_voltage"]) 
 
-        if self.timer_count % (50 * 2 ) == 0:
+            arm.rm_set_tool_IO_mode(1, config["arm_tool_io_mode"][0])
-            print(f'at {self.timer_count * self.interval * 2} s, the en_state is {self.tele_en}, p_measure = {self.p_measured}, j cmd = {self.j_t_f}\n\n')
+            arm.rm_set_tool_IO_mode(2, config["arm_tool_io_mode"][1])
-
+        elif config["tool_name"] == config["tool_names"][1]:  # omnipic
-
+            modbus_sts = arm.rm_set_modbus_mode(port=1, baudrate=115200, timeout=2)
-
+            if modbus_sts != 0:
-    def cal_j_t(self, j_t):
+                self.get_logger().warn(f"Warning: Failed to set Modbus mode: {modbus_sts}")
-        '''
-        adjust the target joint positions to comply with limitations
-        '''
-        d_cmd = motion_lmt(target=j_t, measure=self.j_measured, k=self.scale * self.k, lmt=np.array(self.v_measured)*self.interval + self.lmt_05att, v_max=self.v_max, interval=self.interval)
-        self.j_t_f = (d_cmd + np.array(self.j_measured)).tolist()
-        return self.j_t_f
-
-    def target_update_callback(self, msg):
-        """Callback for target joint update"""
-        if not self.tele_en:
-            return
-        '''
-                @cnt:       count, len = 1
-                @sts:       ik calculation return value, 0: success, <0: fail, len = 1
-                @suc:       to distinguish which solver finish the ik, len = 1
-                @init_guess:current joint angles, degreee, len = 7
-                @pose:      forward kinematics calculation result based on current joint angles, len = 6, unit: m, rad in euler
-                @ik_j:      ik calculation results, joint angles, rad, len = 7
-                @p_t:       target pose, x y z qw qx qy qz (quaternion), len = 7, quaternion
-                @p_t_euler: the target pose in euler 
-        '''
-        data = msg.position
-
-        cnt = int(data[0])
-        ik_state = int(data[1])
-        suc = int(data[2])
-        j = data[3:10]
-        pose = data[10:16]
-        ik_j = data[16:23]
-        p_t = data[23:30]
-        p_t_euler = data[30:36]
-
-        frame_id = cnt
-        # print(f'in left = {self.left} target_update_callback function, frame_id = {frame_id}')
-        if frame_id > self.frame_id:
-            self.frame_id = frame_id
-
-            if ik_state < -0.5:
-                print('error in ik cal, unreachable,  ret = ', ik_state)
-                if not hasattr(self, 'p_t_rcvd') or not self.p_t_rcvd:
-                    # j_t has never been set by a successful IK, hold measured joints to avoid commanding zero position
-                    self.j_t = list(self.j_measured)
             else:
-                self.j_t = [j* 180 / math.pi for j in ik_j]
+                self.get_logger().info("Modbus mode configured successfully")
-                self.p_t_rcvd = True
-                # print(f'j_t: {self.j_t} in target_update_callback')
 
+            addr = 1
+            #   reg-11: target vel, reg-12: target torque, reg-13: target acc, reg14: target dea
+            #   value:      255,                70,                 255,                255
+            reg_value = [255, 60, 255, 255]
+            for i, reg_addr in enumerate([11, 12, 13, 14]) :
+                write_params = rm_peripheral_read_write_params_t(1, reg_addr, addr, 1)
+                arm.rm_write_single_register(write_params, reg_value[i])
+                time.sleep(0.5)
+        elif config["tool_name"] == config["tool_names"][2]:  # minisci
+            pass
 
+    def set_tool_position(arm, config, percent, device = 1, tool_name = 0):
+        """
+        Set gripper position (0.0 = closed, 1.0 = fully open)
+        """
+        if tool_name == config["tool_names"][1]:  # scissor
+            pos_value = int(percent * 255)
+            write_params = rm_peripheral_read_write_params_t(1, 10, device, 1)
+            arm.rm_write_single_register(write_params, pos_value)
+            #   trigger
+            write_params = rm_peripheral_read_write_params_t(1, 15, device, 1)
+            time.sleep(0.5)
+            arm.rm_write_single_register(write_params, 0x01)
+            time.sleep(0.5)
 
-    def tele_en_callback(self, msg):
+        elif tool_name == config["tool_names"][0]:
-        """Callback for Bool topic"""
+            #scissor gripper only has two states, fully open and fully closed, so we can set a threshold to determine the state
-        self.tele_en = msg.data
+            if percent == 0:
-        if not self.tele_en:
+                arm.rm_set_tool_do_state(1, 1)
-            self.p_t_rcvd = False
+                arm.rm_set_tool_do_state(2, 0)
-        else:
+            elif percent == 1:
-            self.p_t_rcvd = False
+                arm.rm_set_tool_do_state(1, 0)
-            self.j_t = list(self.j_measured)
+                arm.rm_set_tool_do_state(2, 1)
-            self.j_t_f = list(self.j_measured)
+        elif tool_name == config["tool_names"][2]:  # minisci
-            self.p_t_f = np.array(self.p_measured)
+            #   mini scissor
-            self.frame_id = -1
+            if percent == 0:
-        if self.tele_en:
+                arm.rm_set_do_state(3, 1)
-            self.get_logger().info(f'\n------------------Teleoperation Enabled ----------------------')
+                arm.rm_set_do_state(4, 0)
-        else:
+            elif percent == 1:
-            self.get_logger().info(f'\n------------------Teleoperation Disabled ----------------------')
+                arm.rm_set_do_state(3, 0)
-
+                arm.rm_set_do_state(4, 1)
-    def scissor_callback(self, msg):
-        ''' for scissor state '''
-        self.scissor_en = msg.data
-        tool_exe(self.robot, self.scissorgripper, self.scissor_en)
-
-    def rm_tool_timer(self):
-        if self.init_done and ( not self.scissorgripper == -1):
-            tool_button = self.robot.rm_get_io_input()[1][0]
-            if tool_button == 1 and self.tool_flg == 0:
-                tool_exe(self.robot, self.scissorgripper, True)
-            elif tool_button == 0 and self.tool_flg == 1:
-                tool_exe(self.robot, self.scissorgripper, False)
-            self.tool_flg =  tool_button