use yaml file to config robot arm

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]

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()

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],
+        )
+    ])

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',
+    ("share/ament_index/resource_index/packages", ["resource/harvest_arm_rm"]),
-            ['resource/' + package_name]),
+    ("share/harvest_arm_rm", ["package.xml"]),
-        ('share/' + package_name, ['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',
         ],
     },
 )