diff --git a/harvest_arm_ctrl/harvest_arm_ctrl/rm75_kine_rm.py b/harvest_arm_ctrl/harvest_arm_ctrl/rm75_kine_rm.py index a7e4362..e05365f 100644 --- a/harvest_arm_ctrl/harvest_arm_ctrl/rm75_kine_rm.py +++ b/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 -------') diff --git a/harvest_arm_ctrl/harvest_arm_ctrl/run_kinematics_arm.py b/harvest_arm_ctrl/harvest_arm_ctrl/run_kinematics_arm.py index 7f9b5f4..4125b3c 100644 --- a/harvest_arm_ctrl/harvest_arm_ctrl/run_kinematics_arm.py +++ b/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, - - "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, + "tools_in_ee": tools_in_ee, - "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 + + 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 - return config, sts, arm_arg + 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) diff --git a/harvest_arm_ctrl/launch/kine_arm.launch.py b/harvest_arm_ctrl/launch/kine_arm.launch.py new file mode 100644 index 0000000..8dbde77 --- /dev/null +++ b/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 + + + +''' \ No newline at end of file diff --git a/harvest_arm_ctrl/setup.py b/harvest_arm_ctrl/setup.py index 93ba02c..1bf732e 100644 --- a/harvest_arm_ctrl/setup.py +++ b/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', ], }, -) +) \ No newline at end of file diff --git a/harvest_arm_rm/config/config.yaml b/harvest_arm_rm/config/config.yaml index 3913110..b514d5c 100644 --- a/harvest_arm_rm/config/config.yaml +++ b/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"