From 2d8ef78b8bd4e37c88a7e9d79a38b0c1ce568672 Mon Sep 17 00:00:00 2001 From: LiuzhengSJ Date: Tue, 23 Jun 2026 16:10:03 +0100 Subject: [PATCH] use yaml file to config robot arm --- harvest_arm_rm/config/config.yaml | 48 ++ .../harvest_arm_rm/run_realman_arm.py | 448 ++++++++++++++++++ harvest_arm_rm/launch/rm_arm.launch.py | 25 + harvest_arm_rm/setup.py | 15 +- 4 files changed, 531 insertions(+), 5 deletions(-) create mode 100644 harvest_arm_rm/config/config.yaml create mode 100644 harvest_arm_rm/harvest_arm_rm/run_realman_arm.py create mode 100644 harvest_arm_rm/launch/rm_arm.launch.py diff --git a/harvest_arm_rm/config/config.yaml b/harvest_arm_rm/config/config.yaml new file mode 100644 index 0000000..94127bb --- /dev/null +++ b/harvest_arm_rm/config/config.yaml @@ -0,0 +1,48 @@ +harvest_arm_rm: + ros__parameters: + 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 + + use_sim_time: false + + arm_installation: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + + 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 + ] + + 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 + ] + + 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 + ] + + 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 + ] + + 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] + + 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 new file mode 100644 index 0000000..ce930f4 --- /dev/null +++ b/harvest_arm_rm/harvest_arm_rm/run_realman_arm.py @@ -0,0 +1,448 @@ +#!/usr/bin/env python3 +import rclpy +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 +from std_msgs.msg import Bool +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): + def __init__(self, port=8080, level=3, mode=2, node_name="harvest_arm_rm"): + """ + Initialize and connect to the robotic arm. + Args: + ip (str): IP address of the robot arm. + port (int): Port number. + level (int, optional): Connection level. Defaults to 3. + mode (int, optional): Thread mode (0: single, 1: dual, 2: triple). Defaults to 2. + """ + super().__init__('harvest_arm_rm') + self.init_done = False + 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.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 + + 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 + + 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) + + self.rviz = Rviz_arm(node=self, rviz_flg=self.declare_parameter('rviz', True).value, prefix=self.prefix, left_flg = self.left) + + 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 + 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 + + + def udp_config(self): + # configure the periodical UDP data transmission from the robot to the computer + custom = rm_udp_custom_config_t() + 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) + # 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: + time.sleep(1) + print(f'self.prefix = {self.prefix}, p_measured = {self.p_measured}, j_measured = {self.j_measured}') + + def udp_callback(self, data): + # extract the pose data from the UDP data + try: + # Check if data is valid + if data is None or data.joint_status is None: + self.get_logger().warn("Warning: Received None data in UDP callback") + return + + # Convert to dict safely + #-------------------------------- pose 1*6 ------------------------------------ + pose_dict = data.waypoint.to_dict() + if (pose_dict is None) or ('position' not in pose_dict) or ('quaternion' not in pose_dict) : + self.get_logger().warn(f"Warning: no 'position' or 'quaternion' key. Available keys: {list(pose_dict.keys())}") + 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 ) + + #--------------------------------------- joint 1*7 ---------------------------------- + # Convert to dict safely + joint_dict = data.joint_status.to_dict() + if (joint_dict is None) or ('joint_position' not in joint_dict): + self.get_logger().warn(f"Warning: 'joint_position' not in joint_dict. Available keys: {list(joint_dict.keys())}") + return + + # Now safe to access + joint_position = joint_dict['joint_position'] + joint_speed = joint_dict.get('joint_speed', [0.0] * 7) + + # Ensure we have exactly 7 joints + 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 + +# ------------- send cmd to the Rviz ---------------------------------- + self.rviz.pub_target_j(self.j_measured) + + except Exception as e: + self.get_logger().error(f"Error in UDP callback: {e}") + import traceback + self.get_logger().error( traceback.print_exc() ) + + 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"]) + + # Perform movel motion + self.movel(points["movel"]) + self.initialized = True + self.get_logger().info('\n------------------- ready ------------------') + + def limitcfg(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) + for i in range(1,8): + self.robot.rm_set_joint_max_speed(i, 180) + self.robot.rm_set_joint_max_acc(i, 180) + + def disconnect(self): + """ + Disconnect from the robot arm. + """ + handle = self.robot.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") + + + + +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 pose + 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 + """ + 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') + + + + 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 + + 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') + + + + 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 + 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 ----------------------') + + 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 + + + + +def main(args=None): + rclpy.init(args=args) + arm_node = RMArmNode() + executor = MultiThreadedExecutor() + executor.add_node(arm_node) + + try: + executor.spin() + except KeyboardInterrupt: + pass + finally: + arm_node.disconnect() + arm_node.destroy_node() + rclpy.shutdown() \ No newline at end of file diff --git a/harvest_arm_rm/launch/rm_arm.launch.py b/harvest_arm_rm/launch/rm_arm.launch.py new file mode 100644 index 0000000..a596733 --- /dev/null +++ b/harvest_arm_rm/launch/rm_arm.launch.py @@ -0,0 +1,25 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +import os + +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + + + +def generate_launch_description(): + package_share_dir = get_package_share_directory('harvest_arm_rm') + + config_file = os.path.join(package_share_dir, 'config', 'config.yaml') + + return LaunchDescription([ + Node( + package="harvest_arm_rm", + executable="realman_run", + name="harvest_arm_rm", + output="screen", + parameters=[config_file], + ) + ]) \ No newline at end of file diff --git a/harvest_arm_rm/setup.py b/harvest_arm_rm/setup.py index 64d9cdb..83b6f95 100644 --- a/harvest_arm_rm/setup.py +++ b/harvest_arm_rm/setup.py @@ -1,4 +1,6 @@ from setuptools import find_packages, setup +import os +from glob import glob package_name = 'harvest_arm_rm' @@ -7,10 +9,12 @@ setup( version='0.0.0', packages=find_packages(exclude=['test']), data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - ], + ("share/ament_index/resource_index/packages", ["resource/harvest_arm_rm"]), + ("share/harvest_arm_rm", ["package.xml"]), + + (os.path.join("share", "harvest_arm_rm", "config"), glob("config/*.yaml")), + (os.path.join("share", "harvest_arm_rm", "launch"), glob("launch/*.launch.py")), +], install_requires=['setuptools'], zip_safe=True, maintainer='zl', @@ -24,7 +28,8 @@ setup( }, entry_points={ 'console_scripts': [ - 'arm_cfg = harvest_arm_rm.arm_cfg:main' + 'arm_cfg = harvest_arm_rm.arm_cfg:main', + 'realman_run = harvest_arm_rm.run_realman_arm:main', ], }, )