add kine pkg pub/sub

harvest_arm_ctrl/harvest_arm_ctrl/arm_ctrl.py

@@ -1,6 +0,0 @@
-def main():
-    print('Hi from harvest_arm_ctrl.')
-
-
-if __name__ == '__main__':
-    main()

harvest_arm_ctrl/harvest_arm_ctrl/rm75_kine_qp.py

@@ -12,7 +12,7 @@ import threading
 
 
 
-class KinematicsSolver():
+class KinematicsSolver_QP():
     def __init__(self,urdf_path="urdf_rm75/RM75-B.urdf", mesh_dir="urdf_rm75"):
         """
         for realman 75b
@@ -712,7 +712,7 @@ def main():
     """Main test function"""
 
 
-    rm75 = KinematicsSolver()
+    rm75 = KinematicsSolver_QP()
 
     # Test 1: Forward Kinematics
     print("\n1. Forward Kinematics Test")

harvest_arm_ctrl/harvest_arm_ctrl/rm75_kine_rm.py

@@ -3,11 +3,11 @@ from Robotic_Arm.rm_robot_interface import *
 import numpy as np
 import math
 
-class rm75_kine_api():
+class KinematicsSolver_RM():
     def __init__(self):
         #   ----------      rm75 official algorithm         -----------
         print(f'-------  the realman official kinematic initialising -------')
-        arm_model = rm_robot_arm_model_e.RM_MODEL_RM_75_E  # RM_65 Robotic arm
+        arm_model = rm_robot_arm_model_e.RM_MODEL_RM_75_E  # RM_75 Robotic arm
         force_type = rm_force_type_e.RM_MODEL_RM_B_E  # Standard version
         # Initialize the robotic arm model and sensor type in the algorithm
         self.robot_kine_rm = Algo(arm_model, force_type)
@@ -120,14 +120,40 @@ class rm75_kine_api():
             self.work_name = work
             self.cfg_work_frame(work)
 
-        target = target_position + target_rpy
+        target = list(target_position) + list(target_rpy)
 
         if initial_guess is not None:
             q_ref = [ 180/math.pi * ig for ig in  initial_guess ]
         else:
             q_ref = [0.0, 110.0, 20.0, 40.0, 30.0, 180.0, 20.0]
         ret, phi = self.robot_kine_rm.rm_algo_calculate_arm_angle_from_config_rm75(q_ref)
+        # print(f'the arm angle is ret = {ret}, and phi = {phi}')
         params = rm_inverse_kinematics_params_t(q_ref,
                                                 target, 1)
         ret, q_out = self.robot_kine_rm.rm_algo_inverse_kinematics_rm75_for_arm_angle(params, phi)
+        p_fk = self.robot_kine_rm.rm_algo_forward_kinematics(joint=q_out, flag=1)
+        pose_dis = cal_pose_deviation(p_fk, target)
+
+        # print(f'target pose is {target}, fk pose is {p_fk}, dis of poses is {pose_dis}')
+        #
+        # print(f'\nin the rm75_kine_rm, l133, inverse_kinematics, q_ref = {q_ref}, target = {target}  phi = {phi}, q_out = {q_out}, ret = {ret}\n\n')
+        # print(f'the tool frame is {self.robot_kine_rm.rm_algo_get_curr_toolframe()}')
+        if int(ret) < 0:
+            #   fail in ik calculation
             return ret, [ q/180*math.pi for q in q_out]
+        elif pose_dis >= 0.01:
+            #   success in ik calculation, but the pose deviation is too large
+            return -10, [ q/180*math.pi for q in q_out]
+        else:
+            #   success in ik calculation
+            return ret, [ q/180*math.pi for q in q_out]
+
+def cal_pose_deviation(pose1, pose2):
+    d_fk_p1 = np.array(pose1) - np.array(pose2)
+    for j in [3, 4, 5]:
+        while d_fk_p1[j] > math.pi:
+            d_fk_p1[j] -= 2 * math.pi
+        while d_fk_p1[j] < -math.pi:
+            d_fk_p1[j] += 2 * math.pi
+    d_fk = np.linalg.norm(d_fk_p1)
+    return d_fk

harvest_arm_ctrl/harvest_arm_ctrl/run_kinematics.py

@@ -1,6 +0,0 @@
-def main():
-    print('Hi from harvest_arm_ctrl.')
-
-
-if __name__ == '__main__':
-    main()

harvest_arm_ctrl/harvest_arm_ctrl/run_kinematics_arm.py

@@ -0,0 +1,267 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from rclpy.executors import MultiThreadedExecutor
+import time
+import numpy as np
+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 harvest_arm_ctrl.rm75_kine_qp import KinematicsSolver_QP as kine_qp
+from harvest_arm_ctrl.rm75_kine_rm import KinematicsSolver_RM as kine_rm
+
+
+
+class KinematicsSolver(Node):
+    def __init__(self):
+        super().__init__('kine_arm_node')
+        self.config, self.sts= self.get_arg()
+
+        #  -----------      rm75 qp based kine          ------------
+        self.robot_kine_qp = kine_qp(urdf_path=self.config['urdf_path'], mesh_dir=self.config['mesh_dir'])
+        self.robot_kine_qp.add_tool_frames(self.config['tools_in_ee'])
+        self.robot_kine_qp.cfg_j_limit(min_j=self.config['min_joint'], max_j=self.config['max_joint'], rad_flag=False)
+
+        #   ----------      rm75 official algorithm         -----------
+        self.robot_kine_rm = kine_rm()
+        self.robot_kine_rm.add_tool_frames(self.config['tools_in_ee'])
+        self.robot_kine_rm.cfg_j_limit(min_j=self.config['min_joint'], max_j=self.config['max_joint'], rad_flag=False)
+
+        #   pub, arm joint command
+        self.action_arm_pub = self.create_publisher(JointState, self.config["action_arm_topic"], 10)
+        
+        #   sub, get the desired pose command in task frame
+        self.action_pose_sub = self.create_subscription(PoseStamped, self.config["action_pose_topic"], self.pose_callback, 10)
+        #   sub, get the desired tool command
+        self.action_tool_sub = self.create_subscription(Bool, self.config["action_tool_topic"], self.tool_callback, 10)
+        #   sub, get the teleoperation enable command
+        self.action_enable_sub = self.create_subscription(Bool, self.config["action_arm_enable_topic"], self.arm_enable_callback, 10)
+        #   sub, get the joint sts from arm
+        self.obser_arm_sub = self.create_subscription(JointState, self.config["observation_topic"], self.arm_observation_callback, 10)
+
+        self.timer = self.create_timer(
+            self.config["interval"],
+            self.kine_timer
+        )
+
+        self.get_logger().info(f'the config is {self.config}')
+
+    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_name = self.declare_parameter('tool_name', "no_tool").value
+        tools_in_ee = {}
+        tool_names = self.declare_parameter('tool_names', ["scissor", "omnipic", "minisci", "no_tool"]).value
+        for name in tool_names:
+            param_name = 'tools_in_ee.' + name
+            try:
+                tool_frame = self.declare_parameter(param_name, [0.0] * 11).value
+                tools_in_ee[name] = np.zeros((2,7))
+                tools_in_ee[name][0, :] = tool_frame[0:7]
+                tools_in_ee[name][1, 0:4] = tool_frame[7:11]
+            except:
+                self.get_logger().error(f"Tool name '{name}' not found in parameters. Please check the configuration.")
+
+        config = {
+            "max_joint": self.declare_parameter('max_joint', [0.0] * 7).value,
+            "min_joint": self.declare_parameter('min_joint', [0.0] * 7).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, 1.0]).value,
+            "tool_name": tool_name,
+            "tools_in_ee": tools_in_ee,
+
+            "interval": self.declare_parameter('interval', 0.02).value,
+
+            "urdf_path": self.declare_parameter('urdf_path', "/home/zl/Downloads/urdf_rm75/RM75-B.urdf").value,
+            "mesh_dir": self.declare_parameter('mesh_dir', "/home/zl/Downloads/urdf_rm75").value,
+
+            "action_arm_topic": self.declare_parameter('action_topic', "/action/arm").value,
+            "observation_topic": self.declare_parameter('observation_topic', "/observation/arm").value,
+            "action_pose_topic": self.declare_parameter('action_pose_topic', "/action/pose").value,
+            "action_tool_topic": self.declare_parameter('action_tool_topic', "/action/tool").value,
+            "action_arm_enable_topic": self.declare_parameter('action_arm_enable_topic', "/action/arm_enable").value,
+        }
+
+        sts = {
+            "joint_pos_sts": [0.0] * 7,
+            "pose_target": [0.0] * 7,
+            "joint_solved": [0.0] * 7,
+            "arm_enable":  0,
+            "tool_cmd":  0,
+            "pose_sts": [0.0] * 6,
+            "pose_target_timestamp": 0.0,
+            "ik_ret": 0,
+        }
+
+        return config, sts
+    
+    def update_msg(self):
+        msg = JointState()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.position = [float(self.sts["arm_enable"])] + [float(self.sts["ik_ret"])] + self.sts["joint_solved"] + [float(self.sts["tool_cmd"])]
+        return msg
+    
+    def kine_timer(self):
+        msg = self.update_msg()
+        self.action_arm_pub.publish(msg)
+    
+    def arm_enable_callback(self, msg):
+        if msg.data:
+            self.sts["arm_enable"] = 1
+        else:
+            self.sts["arm_enable"] = 0
+
+    def tool_callback(self, msg):
+        if msg.data:
+            self.sts["tool_cmd"] = 1
+        else:
+            self.sts["tool_cmd"] = 0
+
+    def arm_observation_callback(self, msg):
+        if isinstance(msg, JointState):
+            self.sts["joint_pos_sts"] = list(msg.position[2:9])
+        else:
+            self.get_logger().error("Received message of unsupported type.")
+
+    def pose_callback(self, msg):
+        if isinstance(msg, PoseStamped):
+            timestamp = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
+            if timestamp < self.sts["pose_target_timestamp"]:
+                return
+            else:
+                #   get the desired pose command in task frame
+                self.sts["pose_target_timestamp"] = timestamp
+                t_position = [msg.pose.position.x, msg.pose.position.y, msg.pose.position.z]
+                t_quat = [msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w]
+                
+                #   transform the pose from task frame to arm base frame
+                t_pos_arm, t_quat_arm = self.pose_transform(pos=t_position, quat=t_quat, pos_frame=self.config['arm_installation'][0:3], quat_frame=self.config['arm_installation'][3:7])
+                t_rpy_arm = self.quaternion_to_euler(t_quat_arm)
+
+                #   call the inverse kinematics solver
+                self.inverse_kine(target_position=t_pos_arm, target_rpy=t_rpy_arm, initial_guess=self.sts["joint_pos_sts"], tool=self.config['tool_name'], work='base')
+
+        else:
+            self.get_logger().error("Received message of unsupported type.")
+
+    def inverse_kine(self, target_position, target_rpy, initial_guess, tool="no_tool", work='base'):
+        ret_rm_ik, q_out = self.robot_kine_rm.inverse_kinematics(target_position, target_rpy, initial_guess, tool)
+        if ret_rm_ik == 0:
+            self.sts["joint_solved"] = q_out
+            self.sts["ik_ret"] = 1
+        else:
+            ret_qp_ik, q_out = self.robot_kine_qp.inverse_kinematics(target_position= target_position, target_rpy=target_rpy, initial_guess=initial_guess, tool=tool, max_iter=300, work=work)
+            if ret_qp_ik == 0:
+                self.sts["joint_solved"] = q_out
+                self.sts["ik_ret"] = 1
+            else:
+                self.sts["ik_ret"] = 0
+
+    def pose_transform(self, pos, quat, pos_frame, quat_frame):
+        '''
+        pos: position in the task frame
+        quat: quaternion in the task frame
+        pos_frame: task frame position in the arm base frame
+        quat_frame: task framequaternion in the arm base frame
+        '''
+        pos_frame = np.asarray(pos_frame, dtype=np.float32)
+        quat_frame = np.asarray(quat_frame, dtype=np.float32)
+        pos = np.asarray(pos, dtype=np.float32)
+        quat = np.asarray(quat, dtype=np.float32)
+
+        pos_arm = rotate_vector_by_quaternion( quat=quat_frame, v=pos)  + pos_frame
+        quat_arm = quaternion_multiply(quat_frame, quat)
+        return pos_arm, quat_arm
+    
+def quaternion_to_euler(q):
+    """
+    Convert quaternion to Euler angles (roll, pitch, yaw)
+
+    Args:
+        qx, qy, qz, qw: quaternion components
+
+    Returns:
+        list: [roll, pitch, yaw] in radians
+    """
+    # Roll (x-axis rotation)
+    sinr_cosp = 2.0 * (q[3] * q[0] + q[1] * q[2])
+    cosr_cosp = 1.0 - 2.0 * (q[0] * q[0] + q[1] * q[1])
+    roll = np.arctan2(sinr_cosp, cosr_cosp)
+
+    # Pitch (y-axis rotation)
+    sinp = 2.0 * (q[3] * q[1] - q[2] * q[0])
+    if abs(sinp) >= 1:
+        pitch = np.copysign(np.pi / 2, sinp)  # Use 90 degrees if out of range
+    else:
+        pitch = np.arcsin(sinp)
+
+    # Yaw (z-axis rotation)
+    siny_cosp = 2.0 * (q[3] * q[2] + q[0] * q[1])
+    cosy_cosp = 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2])
+    yaw = np.arctan2(siny_cosp, cosy_cosp)
+
+    return [roll, pitch, yaw]
+
+
+def rotate_vector_by_quaternion(quat, v):
+    """
+    Rotate vector v by quaternion quat.
+
+    Parameters:
+    v : numpy array (3,) - vector in original frame (frame0)
+    quat : numpy array (4,) - quaternion [x, y, z, w] representing rotation from frame0 to frame1
+
+    Returns:
+    v_rotated : numpy array (3,) - vector expressed in frame1
+    """
+    # Ensure quaternion is normalized
+    q = quat / np.linalg.norm(quat)
+    x, y, z, w = q
+
+    # Extract vector part of quaternion
+    q_vec = np.array([x, y, z])
+
+    # Compute using vector formula
+    v_rotated = (w * w - np.dot(q_vec, q_vec)) * v \
+                + 2 * np.dot(q_vec, v) * q_vec \
+                + 2 * w * np.cross(q_vec, v)
+
+    return v_rotated
+
+def quaternion_multiply( q1, q2):
+    '''
+    return x, y, z, w
+    '''
+    x1, y1, z1, w1 = q1
+    x2, y2, z2, w2 = q2
+
+    w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
+    x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
+    y = w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2
+    z = w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2
+
+    return np.array([x, y, z, w], dtype=np.float32)
+
+def main(args=None):
+    rclpy.init(args=args)
+    arm_node = KinematicsSolver()
+    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_ctrl/launch/kine_arm.launch.py

@@ -0,0 +1,63 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+
+import os
+from pathlib import Path
+
+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_rm = get_package_share_directory("harvest_arm_rm")
+    package_share_dir = get_package_share_directory("harvest_arm_ctrl")
+
+    config_file = os.path.join(
+        package_share_dir_rm,
+        "config",
+        "config.yaml"
+    )
+
+    urdf_file = os.path.join(
+        package_share_dir_rm,
+        "urdf",
+        "urdf_rm75",
+        "RM75.urdf"
+    )
+    meshes_dir = str(Path(urdf_file).parent)
+
+
+    kine_arm_node = Node(
+        package="harvest_arm_ctrl",
+        executable="kine_arm",
+        name="kine_arm_node",
+        output="screen",
+        parameters=[
+            config_file,
+            {
+            "urdf_path": urdf_file,
+            "mesh_dir": meshes_dir,
+            },
+        ],
+    )
+
+
+
+
+
+    return LaunchDescription([
+        kine_arm_node,
+    ])
+
+
+
+
+'''
+
+
+colcon build --packages-select harvest_arm_ctrl --symlink-install
+
+
+
+'''

harvest_arm_ctrl/setup.py

@@ -1,4 +1,6 @@
 from setuptools import find_packages, setup
+import os
+from glob import glob
 
 package_name = 'harvest_arm_ctrl'
 
@@ -10,6 +12,9 @@ setup(
         ('share/ament_index/resource_index/packages',
             ['resource/' + package_name]),
         ('share/' + package_name, ['package.xml']),
+
+        (os.path.join('share', package_name, 'launch'),
+            glob('launch/*.launch.py')),
     ],
     install_requires=['setuptools'],
     zip_safe=True,
@@ -24,7 +29,7 @@ setup(
     },
     entry_points={
         'console_scripts': [
-            'arm_ctrl = harvest_arm_ctrl.arm_ctrl:main'
+            'kine_arm = harvest_arm_ctrl.run_kinematics_arm:main',
         ],
     },
 )

harvest_arm_rm/config/config.yaml

@@ -1,34 +1,38 @@
-harvest_arm_rm:
+
+/**:
   ros__parameters:
-    robot_ip: "192.168.1.19"
-    host_ip: "192.168.1.101"
-
-    port: 8089
-
-    tool_name: "scissor"
-
-    tool_names: ["scissor", "omnipic", "minisci", "vacuum", "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]
     min_joint: [-155.0, -30.0, -172.0, -130.0, -175.0, -125.0, -179.0]
 
-    max_joint_speed: 300.0      # deg/s
-    max_joint_acc: 10000.0
+    interval: 0.02
+    tool_name: "scissor"
 
-    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
+    action_topic: "/action/arm"
+    observation_topic: "/observation/arm"
 
-    bound_cartesian: [
-      -0.3, -0.5, -0.1, 0.3, 0.5, 0.6
-    ]
+    action_pose_topic: "/action/pose"
+    action_tool_topic: "/action/tool"
+    action_arm_enable_topic: "/action/arm_enable"
     
 
+harvest_arm_ctrl:
+  ros__parameters:
+
+    
+
+
+    # set the position and quaternion of the task frame in the base frame.
+    arm_installation : [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
+
+
+    tool_names: ["scissor", "omnipic", "minisci", "camera", "gripper_bh", "no_tool"]
+    
+    qp_iteration: 200
+
     arm_installation: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
 
+    # The tool parameters are defined in the following order:
     #   0~2 position, x, y, z, m
     #   3~6 orientation, quaternion, x, y, z, w
     #   7 mass, kg
@@ -48,17 +52,44 @@ harvest_arm_rm:
       0.46, 0.0, 0.0, 0.06
     ]
 
+    tools_in_ee.camera: [
+      0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0,
+      0.0, 0.0, 0.0, 0.0
+    ]
+
+    tools_in_ee.gripper_bh: [
+      0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0,
+      0.0, 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
     ]
 
-    action_topic: "/action/arm"
-    observation_topic: "/observation/arm"
+    bound_cartesian: [  -0.3, -0.5, -0.1, 0.3, 0.5, 0.6 ]
+
+
+
+harvest_arm_rm:
+  ros__parameters:
+    robot_ip: "192.168.1.19"
+    host_ip: "192.168.1.101"
+    port: 8089
+
+    initial_joint: [-82.60,   20.28,  -32.15,   50.28,   17.28,   64.055,   -20.62]
+
+    max_joint_speed: 300.0      # deg/s
+    max_joint_acc: 10000.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
+
     action_rviz_topic: "/action/arm_rviz"
     observation_rviz_topic: "/observation/arm_rviz"
 
-    interval: 0.02
     kp_j : 1.0
 
     arm_ctrl_voltage: 2 # 0: 0V, 2: 12V, 3: 24V

harvest_arm_rm/harvest_arm_rm/run_realman_arm.py

@@ -66,11 +66,11 @@ class RM_Arm(Node):
             dict: Configuration dictionary containing robot IP, host IP, prefix, left/right status, port, savefile flag, and scissor gripper status.
         """
         tool_name = self.declare_parameter('tool_name', "no_tool").value
-        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.")
+        # 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
@@ -91,12 +91,12 @@ class RM_Arm(Node):
             "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,
+            # "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,
+            # "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,
+            # "tool_offset": tool_offset_list,
+            # "tool_names": self.declare_parameter('tool_names', ["scissor", "omnipic", "minisci", "no_tool"]).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,
@@ -113,12 +113,20 @@ class RM_Arm(Node):
             "arm_tool_io_mode": self.declare_parameter('arm_tool_io_mode', [1, 1]).value
         }
 
+        #   joint_cmd:  the joint command to the arm, unit: degree;
+        #   joint_target: the joint target from the /action/arm, unit: degree;
+        #   joint_pos_sts: the joint position from the arm udp (real robot) or from the /observation/arm_rviz (if no real robot), unit: degree;
+        #   joint_speed_sts: the joint speed from the arm udp (real robot) or from the /observation/arm_rviz (if no real robot), unit: degree/s;
+        #   arm_enable: 1 or 0, the arm enable status from the /action/arm;
+        #   tool_cmd: the tool command from the /action/arm, 1 or 0;
+        #   pose_sts: the pose from the arm udp (real robot), unit: m and rad;
         sts = {
             "joint_cmd": [0.0] * 7,
             "joint_target": [0.0] * 7,
             "joint_pos_sts": [0.0] * 7,
             "joint_speed_sts": [0.0] * 7,
             "arm_enable":  int(0),
+            "ik_ret":  int(0),
             "tool_cmd":  [int(0), int(0)],
             "pose_sts": [0.0] * 6,
             "timer_cnt": 0,
@@ -157,13 +165,14 @@ class RM_Arm(Node):
 
             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"][0] = int(action_list[8])
+            self.sts["ik_ret"] = int(action_list[1])
+            self.sts["joint_target"] = action_list[2:9]
+            self.sts["tool_cmd"][0] = int(action_list[9])
         
 
     def arm_observation_relay_pub(self):
         """
-        Publish the current state of the robotic arm.
+        Publish the current state of the robotic arm to rviz.
         """
         msg0= JointState()
         msg0.header.stamp = self.get_clock().now().to_msg()
@@ -173,7 +182,9 @@ class RM_Arm(Node):
         self.action_rviz_pub.publish(msg0)
 
 
-
+        '''
+        publish the joint state to the harvest_arm_ctrl pkg.
+        '''
         msg = JointState()
         msg.header.stamp = self.get_clock().now().to_msg()
         arm_real = self.arm_arg['arm_handle'].id
@@ -283,9 +294,9 @@ class RM_Arm(Node):
         send joint command to the arm
         """
         #############       movej --> arm
-        if self.sts["arm_enable"] == 1:
+        if self.sts["arm_enable"] == 1 and self.sts["ik_ret"] == 1:
             dis_max_vt = self.config["max_joint_speed"] * self.config["interval"]
-            dis_vt = np.array(self.sts["joint_speed_sts"]) * self.config["interval"]  * 0.75
+            dis_vt = np.array(self.sts["joint_speed_sts"]) * self.config["interval"]  * 0.8
             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"])
@@ -314,7 +325,7 @@ class RM_Arm(Node):
             arm (RoboticArm): Instance of the RoboticArm class.
             config (dict): Configuration dictionary containing tool offsets.
         """
-        self.frame_cfg()
+        # self.frame_cfg()
         self.connector_cfg()
 
     

harvest_arm_rm/launch/rm_arm_rviz.launch.py

@@ -2,6 +2,7 @@
 # -*- coding: utf-8 -*-
 
 import os
+from pathlib import Path
 
 from launch import LaunchDescription
 from launch_ros.actions import Node
@@ -23,6 +24,7 @@ def generate_launch_description():
         "urdf_rm75",
         "RM75.urdf"
     )
+    meshes_dir = str(Path(urdf_file).parent)
 
     with open(urdf_file, "r") as f:
         robot_description = f.read()
@@ -73,13 +75,27 @@ def generate_launch_description():
 
 
 '''
+
+
 ros2 launch harvest_arm_rm rm_arm_rviz.launch.py
 
+
+
+while true; do
   ros2 topic pub /action/arm sensor_msgs/msg/JointState "
 header:
   stamp:
-    sec: 2
-    nanosec: 0
-position: [1.0, 82.6, -20.28, 32.15, -50.28, -17.28, -64.055, 20.62, 1.0]
+    sec: $(date +%s)
+    nanosec: $(date +%N)
+  frame_id: ''
+name: []
+position: [1.0, 1.0, 82.6, -20.28, 32.15, -50.28, -17.28, -64.055, 20.62, 1.0]
+velocity: []
+effort: []
 " --once
+  sleep 1
+done
+
+
+
 '''