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

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

@@ -7,7 +7,7 @@ class rm75_kine_api():
     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)
-        return ret, [ q/180*math.pi for q in q_out]
+        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,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
+
+
+
+class KinematicsSolver():
+    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.")
+
+
+        acc_lmt = self.declare_parameter('max_joint_acc', 600.0).value
+        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_ctrl/setup.py

@@ -24,7 +24,7 @@ setup(
     },
     entry_points={
         'console_scripts': [
-            'arm_ctrl = harvest_arm_ctrl.arm_ctrl:main'
+            'kinematics_arm = harvest_arm_ctrl.run_kinematics_arm:main',
         ],
     },
 )

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
+/**:
+  ros__parameters:
+
+    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]
+
+    interval: 0.02
+
+
+harvest_arm_ctrl:
+  ros__parameters:
 
     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]
+    qp_iteration: 200
-
-    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
-
-    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
-    ]
-
 
     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()
@@ -73,13 +75,27 @@ def generate_launch_description():
 
 
 '''
+
+
 ros2 launch harvest_arm_rm rm_arm_rviz.launch.py
 
-ros2 topic pub /action/arm sensor_msgs/msg/JointState "
+
+
+while true; do
+  ros2 topic pub /action/arm sensor_msgs/msg/JointState "
 header:
   stamp:
-    sec: 2
+    sec: $(date +%s)
-    nanosec: 0
+    nanosec: $(date +%N)
+  frame_id: ''
+name: []
 position: [1.0, 82.6, -20.28, 32.15, -50.28, -17.28, -64.055, 20.62, 1.0]
+velocity: []
+effort: []
 " --once
+  sleep 1
+done
+
+
+
 '''