diff --git a/harvest_arm_rm/config/config.yaml b/harvest_arm_rm/config/config.yaml index ea7e432..5f35fa4 100644 --- a/harvest_arm_rm/config/config.yaml +++ b/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" - joint_state_topic: "/joint_sts" - arm_enable_topic: "/arm_enable" - tool_cmd_topic: "/tool_cmd" - - - + action_topic: "/action/arm" + observation_topic: "/observation/arm" + 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 diff --git a/harvest_arm_rm/harvest_arm_rm/run_realman_arm.py b/harvest_arm_rm/harvest_arm_rm/run_realman_arm.py index e16eb25..8093385 100644 --- a/harvest_arm_rm/harvest_arm_rm/run_realman_arm.py +++ b/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.init_done = True + self.arm_arg['arm'] = self.peripheral_cfg(self.arm_arg['arm'], self.config) # 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, - "min_joint": self.declare_parameter('min_joint', joint_min).value, + "max_joint": self.declare_parameter('max_joint', [0.0] * 7).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_speed": [0.0] * 7, - "arm_enable": False, - "tool_cmd": False, + "joint_target": [0.0] * 7, + "joint_pos_sts": [0.0] * 7, + "joint_speed_sts": [0.0] * 7, + "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): - """ - 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): + def arm_action_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 - -# ------------- send cmd to the Rviz ---------------------------------- - self.rviz.pub_target_j(self.sts["joint_sts"]) + self.sts["joint_speed_sts"] = joint_speed 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.robot.rm_set_joint_max_acc(i, self.config["max_joint_acc"]) + self.arm_arg['arm'].rm_set_joint_max_speed(i, self.config["max_joint_speed"]) + 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: - 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") + movej_canfd_result = self.sts["arm"].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") - - -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): + def rm_movej_timer(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 not self.tele_en_f: - self.p_t_f = self.p_measured - self.j_t_f = self.j_measured - if self.p_t_rcvd: - - 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) - else: - self.p_t_f = self.p_measured - self.j_t_f = self.j_measured - - if self.robot_handle.id == -1: - self.rviz.pub_target_j(self.j_measured) - - self.timer_count += 1 - self.tele_en_f = self.tele_en - - self.p_measured_f = self.p_measured - self.j_measured_f = self.j_measured - - if self.timer_count % (50 * 2 ) == 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') + if self.sts["arm_enable"] == 1: + dis_max_vt = self.config["max_joint_speed"] * self.config["interval"] + dis_vt = np.array(self.sts["joint_speed_sts"]) * self.config["interval"] + d_j = np.clip(a=(np.array(self.sts["joint_target"]) - np.array(self.sts["joint_pos_sts"])) * self.config["kp_j"], + 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"]) + self.arm_observation_pub() - def cal_j_t(self, j_t): - ''' - 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 + self.sts["timer_cnt"] += 1 - 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.p_t_rcvd = True - # print(f'j_t: {self.j_t} in target_update_callback') + 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) - def tele_en_callback(self, msg): - """Callback for Bool topic""" - self.tele_en = msg.data - if not self.tele_en: - self.p_t_rcvd = False + 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_rcvd = False - self.j_t = list(self.j_measured) - self.j_t_f = list(self.j_measured) - self.p_t_f = np.array(self.p_measured) - self.frame_id = -1 - if self.tele_en: - self.get_logger().info(f'\n------------------Teleoperation Enabled ----------------------') - else: - self.get_logger().info(f'\n------------------Teleoperation Disabled ----------------------') + ret = arm.rm_set_manual_tool_frame(tool_frame) - def scissor_callback(self, msg): - ''' for scissor state ''' - self.scissor_en = msg.data - tool_exe(self.robot, self.scissorgripper, self.scissor_en) + ret = arm.rm_change_tool_frame(tool_name) + if ret != 0: + raise RuntimeError(f"Failed to change tool frame to {tool_name}, ret={ret}") + + def connector_cfg(self, arm, config): + """ + 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) - 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 + if config["tool_name"] == config["tool_names"][0]: # scissor + arm.rm_set_tool_voltage(config["arm_tool_io_voltage"]) + + arm.rm_set_tool_IO_mode(1, config["arm_tool_io_mode"][0]) + 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: + self.get_logger().warn(f"Warning: Failed to set Modbus mode: {modbus_sts}") + else: + self.get_logger().info("Modbus mode configured successfully") + + 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) + + elif tool_name == config["tool_names"][0]: + #scissor gripper only has two states, fully open and fully closed, so we can set a threshold to determine the state + if percent == 0: + arm.rm_set_tool_do_state(1, 1) + arm.rm_set_tool_do_state(2, 0) + elif percent == 1: + arm.rm_set_tool_do_state(1, 0) + arm.rm_set_tool_do_state(2, 1) + elif tool_name == config["tool_names"][2]: # minisci + # mini scissor + if percent == 0: + arm.rm_set_do_state(3, 1) + arm.rm_set_do_state(4, 0) + elif percent == 1: + arm.rm_set_do_state(3, 0) + arm.rm_set_do_state(4, 1)