remove kine parameters, like tool/work frames, from rm pkg to ctrl pkg

harvest_arm_ctrl/config/config.yaml

@@ -1,37 +0,0 @@
-
-harvest_arm_ctrl:
-  ros__parameters:
-
-    urdf_path:  '/home/zl/ws_Harvest/src/harvest_arm_rm/urdf/urdf_rm75/RM75.urdf'
-    meshes_path: '/home/zl/ws_Harvest/src/harvest_arm_rm/urdf/urdf_rm75'
-    
-    qp_iteration: 200
-    joint_upper : [155.0, 115.0, 172.0,  130, 175.0, 125.0,  350.0 ]
-    joint_lower : [-155.0, -30.0, -172.0, -130, -175.0, -125.0, -350.0 ]
-
-    tool_name: "scissor"
-
-    #   0~2 position, x, y, z, m
-#   3~6 orientation, quaternion, x, y, z, w
-#   7 mass, kg
-#   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
-    ]
-
-    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
-    ]
-
-    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
-    ]
-
-    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
-    ]
-

harvest_arm_ctrl/harvest_arm_ctrl/rm75_kine_rm.py

@@ -139,11 +139,14 @@ class rm75_kine_api():
         # 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:
+        elif pose_dis >= 0.01:
-            return ret, [ q/180*math.pi for q in q_out]
+            #   success in ik calculation, but the pose deviation is too large
-        else:
             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)

harvest_arm_ctrl/harvest_arm_ctrl/run_kinematics_arm.py

@@ -1,11 +1,122 @@
+#!/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
 
 
 
-def main():
+class KinematicsSolver():
-    print('Hi from harvest_arm_ctrl.')
+    def __init__(self, node_name="kine_run_node"):
+        super().__init__('realman_run_node')
+        self.config, self.sts, self.arm_arg = self.get_arg()
+
+        #  -----------      rm75 qp based kine          ------------
+        self.robot_kine_qp = kine_qp(urdf_path='/home/zl/Downloads/urdf_rm75/RM75-B.urdf', mesh_dir='/home/zl/Downloads/urdf_rm75')
+        self.robot_kine_qp.add_tool_frames(tools_in_ee)
+        self.robot_kine_qp.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
+
+        #   ----------      rm75 official algorithm         -----------
+        self.robot_kine_rm = kine_rm()
+        self.robot_kine_rm.add_tool_frames(tools_in_ee)
+        self.robot_kine_rm.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
+
+    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
+        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.")
 
 
-if __name__ == '__main__':
+        acc_lmt = self.declare_parameter('max_joint_acc', 600.0).value
-    main()
+        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', [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,
+            "lmt_05att": 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,
+            
+            "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,
+            "action_rviz_topic": self.declare_parameter('action_rviz_topic', "/action/arm_rviz").value,
+            "observation_rviz_topic": self.declare_parameter('observation_rviz_topic', "/observation/arm_rviz").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_target": [0.0] * 7,
+            "joint_pos_sts": [0.0] * 7,
+            "joint_speed_sts": [0.0] * 7,
+            "arm_enable":  int(0),
+            "tool_cmd":  [int(0), int(0)],
+            "pose_sts": [0.0] * 6,
+            "timer_cnt": 0,
+            "action_timestamp": 0.0,
+        }
+
+        arm_arg = {
+            "arm" : None,
+            "arm_handle" : None,
+        }
+
+        return config, sts, arm_arg
+
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    arm_node = RM_Arm()
+    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/config/config.yaml

@@ -1,31 +1,21 @@
-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
+    interval: 0.02
-    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
 
-    bound_cartesian: [
+harvest_arm_ctrl:
-      -0.3, -0.5, -0.1, 0.3, 0.5, 0.6
+  ros__parameters:
-    ]
 
+    tool_name: "scissor"
+
+    tool_names: ["scissor", "omnipic", "minisci", "vacuum", "no_tool"]
+    
+    qp_iteration: 200
 
     arm_installation: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
 
@@ -53,12 +43,35 @@ harvest_arm_rm:
       0.0, 0.0, 0.0, 0.0
     ]
 
+
+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
+
+    bound_cartesian: [
+      -0.3, -0.5, -0.1, 0.3, 0.5, 0.6
+    ]
+
+
+
+
     action_topic: "/action/arm"
     observation_topic: "/observation/arm"
     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/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()