From a14e2ff8bb7aa15d58402b2dea41f329212659e8 Mon Sep 17 00:00:00 2001 From: LiuzhengSJ Date: Tue, 23 Jun 2026 17:12:47 +0100 Subject: [PATCH] init config --- harvest_arm_rm/config/config.yaml | 51 +++-- .../harvest_arm_rm/run_realman_arm.py | 203 ++++++++++++------ 2 files changed, 166 insertions(+), 88 deletions(-) diff --git a/harvest_arm_rm/config/config.yaml b/harvest_arm_rm/config/config.yaml index 94127bb..ea7e432 100644 --- a/harvest_arm_rm/config/config.yaml +++ b/harvest_arm_rm/config/config.yaml @@ -3,46 +3,61 @@ harvest_arm_rm: robot_ip: "192.168.1.19" host_ip: "192.168.1.101" - robot_port: 8899 port: 8089 - level: 3 - mode: 2 - - prefix: "" - left: true - savefile_flg: true - scissorgripper: -1 tool_name: "scissor" - max_velocity: 0.2 - max_acceleration: 0.5 + 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] + min_joint: [-155.0, -30.0, -172.0, -130.0, -175.0, -125.0, -179.0] + + max_joint_speed: 180.0 # deg/s + max_joint_acc: 600.0 + + max_cartesian_speed: 1.0 # m/s + max_cartesian_angular_speed: 1.5 # rad/s + max_cartesian_acc: 1.0 + max_cartesian_angular_acc: 2.0 - use_sim_time: false 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 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, 0.0, 0.0, 0.0 + 0.66, 0.0, 0.0, 0.06 ] tools_in_ee.omnipic: [ 0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0, - 0.43, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0 + 0.43, 0.0, 0.0, 0.06 ] tools_in_ee.minisci: [ 0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0, - 0.46, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0 + 0.46, 0.0, 0.0, 0.06 ] tools_in_ee.no_tool: [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 + 0.0, 0.0, 0.0, 0.0 ] - joint_max: [155.0, 115.0, 172.0, 130.0, 175.0, 125.0, 179.0] - joint_min: [-155.0, -30.0, -172.0, -130.0, -175.0, -125.0, -179.0] + joint_cmd_topic: "/joint_cmd" + joint_state_topic: "/joint_sts" + arm_enable_topic: "/arm_enable" + tool_cmd_topic: "/tool_cmd" + + + + + + + + + - bounds_p_right: [-0.35, 0.7, -0.7, 0.4, 0.32, 0.7] \ No newline at end of file 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 ce930f4..e16eb25 100644 --- a/harvest_arm_rm/harvest_arm_rm/run_realman_arm.py +++ b/harvest_arm_rm/harvest_arm_rm/run_realman_arm.py @@ -31,58 +31,129 @@ class RMArmNode(Node): mode (int, optional): Thread mode (0: single, 1: dual, 2: triple). Defaults to 2. """ super().__init__('harvest_arm_rm') - self.init_done = False + self.config, self.sts, self.arm_arg = self.get_arg() + + self.arm_topic() + try: - self.get_logger().info(f"---Robot IP: {robot_ip}") - except: - robot_ip = self.declare_parameter('robot_ip', "192.168.1.18").value - self.get_logger().info(f"Robot ----IP: {robot_ip}") - self.host_ip = self.declare_parameter('host_ip', "192.168.1.101").value - self.prefix = self.declare_parameter('prefix', "").value - self.left = self.declare_parameter('left',True).value - self.port = self.declare_parameter('port',8089).value - self.savefile_flg = self.declare_parameter('savefile_flg',True).value - self.scissorgripper = self.declare_parameter('scissorgripper',-1).value + 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)!") + 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.udp_config() + self.arm_arg['arm'] = 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.arm_arg['arm'] = None + def get_arg(self): + """ + Get the configuration of the robotic arm. + 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] - self.leftright = 0 if self.left else 1 - self.subname = rm_l_act_topic if self.left else rm_r_act_topic - self.scissor_topic = left_scissor_topic if self.left else right_scissor_topic + 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, - self.robot_handle = None - self.robot = None - # measured pose, joint, joint-velocity - self.p_measured = np.zeros(6, dtype=np.float32) - self.j_measured = [1] * 7 - self.j_t = [0.0] * 7 - self.v_measured = [0.0] * 7 + "max_joint": self.declare_parameter('max_joint', joint_max).value, + "min_joint": self.declare_parameter('min_joint', joint_min).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, - self.initialized = False - self.algo_handle = alg_init(scissorgripper=self.scissorgripper, tools_in_ee = tools_in_ee, joint_max=joint_max, joint_min=joint_min) + "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, + + "arm_installation": self.declare_parameter('arm_installation', [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]).value, + "tool_offset": tool_offset_list, + + "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, + } - self.rviz = Rviz_arm(node=self, rviz_flg=self.declare_parameter('rviz', True).value, prefix=self.prefix, left_flg = self.left) + sts = { + "joint_cmd": [0.0] * 7, + "joint_sts": [0.0] * 7, + "joint_speed": [0.0] * 7, + "arm_enable": False, + "tool_cmd": False, + "pose_sts": [0.0] * 6, + } - if robot_enable_sts[self.leftright]: - try: - self.robot = RoboticArm(rm_thread_mode_e(mode)) - self.robot_handle = self.robot.rm_create_robot_arm(robot_ip, port, level) - if self.robot_handle.id == -1: - print("Failed to connect to the real robot arm (socket error), enter RViz simulation mode!") - else: - self.get_logger().info(f"Successfully connected to the real robot arm: {self.robot_handle.id}") - # if real robot connected, initialize following cfg - self.limitcfg() - self.udp_config() - self.robot = peripheral_cfg(self.robot, scissorgripper=self.scissorgripper, tools_in_ee = tools_in_ee) - 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.robot = None + arm_arg = { + "arm" : None, + "arm_handle" : None, + } + + 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().error(f"Robot arm enable sts: {robot_enable_sts[self.leftright]} with leftright = {self.leftright}, enter RViz simulation mode!") - self.robot = None + 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) def udp_config(self): @@ -91,15 +162,15 @@ class RMArmNode(Node): custom.joint_speed = 1 custom.lift_state = 1 custom.expand_state = 1 - config = rm_realtime_push_config_t(3, True, self.port, 0, self.host_ip, custom) - self.robot.rm_set_realtime_push(config) + config = rm_realtime_push_config_t(3, True, self.config["port"], 0, self.config["host_ip"], custom) + self.config["arm"].rm_set_realtime_push(config) # Store the callback as an instance variable to prevent garbage collection self.arm_state_callback = rm_realtime_arm_state_callback_ptr(self.udp_callback) # Use the stored callback - self.robot.rm_realtime_arm_state_call_back(self.arm_state_callback) - while sum(abs(self.p_measured )) < 0.001 or sum(abs(np.array(self.j_measured))) < 0.0001: + 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: time.sleep(1) - print(f'self.prefix = {self.prefix}, p_measured = {self.p_measured}, j_measured = {self.j_measured}') + print(f'p_measured = {self.sts["pose_sts"]}, j_measured = {self.sts["joint_sts"]}') def udp_callback(self, data): # extract the pose data from the UDP data @@ -117,7 +188,7 @@ class RMArmNode(Node): return pose = [ pose_dict['position']['x'], pose_dict['position']['y'], pose_dict['position']['z'], pose_dict['euler']['rx'], pose_dict['euler']['ry'], pose_dict['euler']['rz'] ] - self.p_measured = np.array( pose ) + self.sts["pose_sts"] = pose #--------------------------------------- joint 1*7 ---------------------------------- # Convert to dict safely @@ -134,11 +205,11 @@ class RMArmNode(Node): if len(joint_position) != 7: self.get_logger().warn(f"Warning: Expected 7 joint positions, got {len(joint_position)}: {joint_position}") # Pad with zeros or truncate to 7 - self.j_measured = joint_position - self.v_measured = joint_speed + self.sts["joint_sts"] = joint_position + self.sts["joint_speed"] = joint_speed # ------------- send cmd to the Rviz ---------------------------------- - self.rviz.pub_target_j(self.j_measured) + self.rviz.pub_target_j(self.sts["joint_sts"]) except Exception as e: self.get_logger().error(f"Error in UDP callback: {e}") @@ -147,29 +218,21 @@ class RMArmNode(Node): def move_to_init(self): if not self.initialized: - # run initial motions before ready - self.j_t = np.array( self.j_measured ) - - side = "left" if self.left else "right" - points = arm_init_points.get("RM_75", {}).get(side, {}) # Perform movej motion for robot arm - self.movej(points["movej"]) + self.movej(self.config["initial_joint"]) - # Perform movel motion - self.movel(points["movel"]) - self.initialized = True self.get_logger().info('\n------------------- ready ------------------') - def limitcfg(self ): + def armcfg(self ): # configure the motion limitations for the robot - self.robot.rm_set_avoid_singularity_mode(True) - self.robot.rm_set_arm_max_line_speed(1) - self.robot.rm_set_arm_max_angular_speed(1.5) - self.robot.rm_set_arm_max_line_acc(1) - self.robot.rm_set_arm_max_angular_acc(2) + 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"]) + self.arm_arg['arm'].rm_set_arm_max_angular_speed(self.config["max_cartesian_angular_speed"]) + 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, 180) - self.robot.rm_set_joint_max_acc(i, 180) + 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"]) def disconnect(self): """ @@ -282,7 +345,7 @@ class ArmCtlNode(RMArmNode): self.rm_move_arm ) self.timer_count = 0 - # create the subscriber to receive target pose + # create the subscriber to receive target joints self.pose_sub = self.create_subscription( JointState, self.subname,