move tool frame/task frame to ctrl pkg

harvest_arm_ctrl/harvest_arm_ctrl/rm75_kine_rm.py

@@ -3,7 +3,7 @@ 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 -------')

harvest_arm_ctrl/harvest_arm_ctrl/run_kinematics_arm.py

@@ -16,20 +16,29 @@ 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()
+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='/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)
+        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(tools_in_ee)
-        self.robot_kine_rm.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
+        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)
+
+        self.action_arm_pub = self.create_publisher(JointState, self.config["observation_topic"], 10)
+        
+        #   Create subcriber for target joint
+        self.action_pose_sub = self.create_subscription(PoseStamped, self.config["action_pose_topic"], self.pose_callback, 10)
+        self.action_tool_sub = self.create_subscription(Bool, self.config["action_tool_topic"], self.tool_callback, 10)
+        self.action_enable_sub = self.create_subscription(Bool, self.config["action_arm_enable_topic"], self.arm_enable_callback, 10)
+
+        self.get_logger().info(f'the config is {self.config}')
 
     def get_arg(self):
         """
@@ -38,77 +47,187 @@ class KinematicsSolver():
             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
-
+        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 = {
-            "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,
+            "tools_in_ee": tools_in_ee,
 
-            "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,
+            "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_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
+            "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_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_target": [0.0] * 7,
+            "joint_solved": [0.0] * 7,
+            "arm_enable":  0,
+            "tool_cmd":  0,
             "pose_sts": [0.0] * 6,
-            "timer_cnt": 0,
-            "action_timestamp": 0.0,
+            "pose_target_timestamp": 0.0,
+            "ik_ret": False,
         }
 
-        arm_arg = {
-            "arm" : None,
-            "arm_handle" : None,
-        }
+        return config, sts
     
-        return config, sts, arm_arg
+    def update_msg(self):
+        msg = JointState()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.name = ['joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7']
+        msg.position = [float(self.sts["arm_enable"])] + self.sts["joint_solved"] + [float(self.sts["tool_cmd"])]
+        return 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 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"] = True
+        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"] = True
+            else:
+                self.sts["ik_ret"] = False
+
+    def pose_transform(self, pos, quat, pos_frame, quat_frame):
+        '''
+        pos: position in the task frame
+        quat: quaternion in the task frame
+        pos_frame: position in the arm base frame
+        quat_frame: quaternion in the arm base frame
+        '''
+        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 = RM_Arm()
+    arm_node = KinematicsSolver()
     executor = MultiThreadedExecutor()
     executor.add_node(arm_node)
 

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': [
-            'kinematics_arm = harvest_arm_ctrl.run_kinematics_arm:main',
+            'kine_arm = harvest_arm_ctrl.run_kinematics_arm:main',
         ],
     },
 )

harvest_arm_rm/config/config.yaml

@@ -6,12 +6,23 @@
     min_joint: [-155.0, -30.0, -172.0, -130.0, -175.0, -125.0, -179.0]
 
     interval: 0.02
+    tool_name: "scissor"
+
+    action_topic: "/action/arm"
+    observation_topic: "/observation/arm"
+
+    action_pose_topic: "/action/pose"
+    action_tool_topic: "/action/tool"
+    action_arm_enable_topic: "/action/arm_enable"
     
 
 harvest_arm_ctrl:
   ros__parameters:
 
-    tool_name: "scissor"
+    pos_task_in_base = [-0.1, 0.2, -0.025]
+    quat_task_in_base = [-0.5, -0.5, 0.5, 0.5]
+
+    work_frame: ["base", "task"]
 
     tool_names: ["scissor", "omnipic", "minisci", "vacuum", "no_tool"]
     
@@ -43,6 +54,9 @@ harvest_arm_ctrl:
       0.0, 0.0, 0.0, 0.0
     ]
 
+    bound_cartesian: [  -0.3, -0.5, -0.1, 0.3, 0.5, 0.6 ]
+
+
 
 harvest_arm_rm:
   ros__parameters:
@@ -60,15 +74,6 @@ harvest_arm_rm:
     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"