ZhengLiu-cart/HarvestingRealman
1
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

use yaml file to config robot arm

Browse Source
This commit is contained in:
LiuzhengSJ
2026-06-23 16:10:03 +01:00
parent 2bf7b4885e
commit 2d8ef78b8b
4 changed files with 531 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

48
harvest_arm_rm/config/config.yaml Normal file
View File

@ -0,0 +1,48 @@
harvest_arm_rm:
ros__parameters:
robot_ip: "192.168.1.19"
host_ip: "192.168.1.101"
robot_port: 8899
port: 8089
level: 3
mode: 2
prefix: ""
left: true
savefile_flg: true
scissorgripper: -1
tool_name: "scissor"
max_velocity: 0.2
max_acceleration: 0.5
use_sim_time: false
arm_installation: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
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, 0.0, 0.0, 0.0
]
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, 0.0, 0.0, 0.0
]
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, 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, 0.0, 0.0, 0.0
]
joint_max: [155.0, 115.0, 172.0, 130.0, 175.0, 125.0, 179.0]
joint_min: [-155.0, -30.0, -172.0, -130.0, -175.0, -125.0, -179.0]
bounds_p_right: [-0.35, 0.7, -0.7, 0.4, 0.32, 0.7]

448
harvest_arm_rm/harvest_arm_rm/run_realman_arm.py Normal file
View File

@ -0,0 +1,448 @@
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from rclpy.executors import MultiThreadedExecutor
import time
import numpy as np
from arm_pkg.arm_realman.arg_cfg import *
from arm_pkg.arm_realman.fun_basic import *
from arm_pkg.arm_realman.fun_savefile import *
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 Robotic_Arm.rm_robot_interface import *
from arm_pkg.arm_realman.arg_cfg import *
from arm_pkg.arm_realman.fun_rviz import Rviz_single_arm as Rviz_arm
from arm_pkg.arm_realman.fun_peripheral import *
class RMArmNode(Node):
def __init__(self, port=8080, level=3, mode=2, node_name="harvest_arm_rm"):
"""
Initialize and connect to the robotic arm.
Args:
ip (str): IP address of the robot arm.
port (int): Port number.
level (int, optional): Connection level. Defaults to 3.
mode (int, optional): Thread mode (0: single, 1: dual, 2: triple). Defaults to 2.
"""
super().__init__('harvest_arm_rm')
self.init_done = False
try:
self.get_logger().info(f"---Robot IP: {robot_ip}")
except:
robot_ip = self.declare_parameter('robot_ip', "192.168.1.18").value
self.get_logger().info(f"Robot ----IP: {robot_ip}")
self.host_ip = self.declare_parameter('host_ip', "192.168.1.101").value
self.prefix = self.declare_parameter('prefix', "").value
self.left = self.declare_parameter('left',True).value
self.port = self.declare_parameter('port',8089).value
self.savefile_flg = self.declare_parameter('savefile_flg',True).value
self.scissorgripper = self.declare_parameter('scissorgripper',-1).value
self.leftright = 0 if self.left else 1
self.subname = rm_l_act_topic if self.left else rm_r_act_topic
self.scissor_topic = left_scissor_topic if self.left else right_scissor_topic
self.robot_handle = None
self.robot = None
# measured pose, joint, joint-velocity
self.p_measured = np.zeros(6, dtype=np.float32)
self.j_measured = [1] * 7
self.j_t = [0.0] * 7
self.v_measured = [0.0] * 7
self.initialized = False
self.algo_handle = alg_init(scissorgripper=self.scissorgripper, tools_in_ee = tools_in_ee, joint_max=joint_max, joint_min=joint_min)
self.rviz = Rviz_arm(node=self, rviz_flg=self.declare_parameter('rviz', True).value, prefix=self.prefix, left_flg = self.left)
if robot_enable_sts[self.leftright]:
try:
self.robot = RoboticArm(rm_thread_mode_e(mode))
self.robot_handle = self.robot.rm_create_robot_arm(robot_ip, port, level)
if self.robot_handle.id == -1:
print("Failed to connect to the real robot arm (socket error), enter RViz simulation mode!")
else:
self.get_logger().info(f"Successfully connected to the real robot arm: {self.robot_handle.id}")
# if real robot connected, initialize following cfg
self.limitcfg()
self.udp_config()
self.robot = peripheral_cfg(self.robot, scissorgripper=self.scissorgripper, tools_in_ee = tools_in_ee)
self.init_done = True
# self.move_to_init()
except Exception as e:
self.get_logger().error(f"Robot arm hardware init error: {e}, enter RViz simulation mode!")
self.robot = None
else:
self.get_logger().error(f"Robot arm enable sts: {robot_enable_sts[self.leftright]} with leftright = {self.leftright}, enter RViz simulation mode!")
self.robot = None
def udp_config(self):
# configure the periodical UDP data transmission from the robot to the computer
custom = rm_udp_custom_config_t()
custom.joint_speed = 1
custom.lift_state = 1
custom.expand_state = 1
config = rm_realtime_push_config_t(3, True, self.port, 0, self.host_ip, custom)
self.robot.rm_set_realtime_push(config)
# Store the callback as an instance variable to prevent garbage collection
self.arm_state_callback = rm_realtime_arm_state_callback_ptr(self.udp_callback)
# Use the stored callback
self.robot.rm_realtime_arm_state_call_back(self.arm_state_callback)
while sum(abs(self.p_measured )) < 0.001 or sum(abs(np.array(self.j_measured))) < 0.0001:
time.sleep(1)
print(f'self.prefix = {self.prefix}, p_measured = {self.p_measured}, j_measured = {self.j_measured}')
def udp_callback(self, data):
# extract the pose data from the UDP data
try:
# Check if data is valid
if data is None or data.joint_status is None:
self.get_logger().warn("Warning: Received None data in UDP callback")
return
# Convert to dict safely
#-------------------------------- pose 1*6 ------------------------------------
pose_dict = data.waypoint.to_dict()
if (pose_dict is None) or ('position' not in pose_dict) or ('quaternion' not in pose_dict) :
self.get_logger().warn(f"Warning: no 'position' or 'quaternion' key. Available keys: {list(pose_dict.keys())}")
return
pose = [ pose_dict['position']['x'], pose_dict['position']['y'], pose_dict['position']['z'], pose_dict['euler']['rx'], pose_dict['euler']['ry'], pose_dict['euler']['rz'] ]
self.p_measured = np.array( pose )
#--------------------------------------- joint 1*7 ----------------------------------
# Convert to dict safely
joint_dict = data.joint_status.to_dict()
if (joint_dict is None) or ('joint_position' not in joint_dict):
self.get_logger().warn(f"Warning: 'joint_position' not in joint_dict. Available keys: {list(joint_dict.keys())}")
return
# Now safe to access
joint_position = joint_dict['joint_position']
joint_speed = joint_dict.get('joint_speed', [0.0] * 7)
# Ensure we have exactly 7 joints
if len(joint_position) != 7:
self.get_logger().warn(f"Warning: Expected 7 joint positions, got {len(joint_position)}: {joint_position}")
# Pad with zeros or truncate to 7
self.j_measured = joint_position
self.v_measured = joint_speed
# ------------- send cmd to the Rviz ----------------------------------
self.rviz.pub_target_j(self.j_measured)
except Exception as e:
self.get_logger().error(f"Error in UDP callback: {e}")
import traceback
self.get_logger().error( traceback.print_exc() )
def move_to_init(self):
if not self.initialized:
# run initial motions before ready
self.j_t = np.array( self.j_measured )
side = "left" if self.left else "right"
points = arm_init_points.get("RM_75", {}).get(side, {})
# Perform movej motion for robot arm
self.movej(points["movej"])
# Perform movel motion
self.movel(points["movel"])
self.initialized = True
self.get_logger().info('\n------------------- ready ------------------')
def limitcfg(self ):
# configure the motion limitations for the robot
self.robot.rm_set_avoid_singularity_mode(True)
self.robot.rm_set_arm_max_line_speed(1)
self.robot.rm_set_arm_max_angular_speed(1.5)
self.robot.rm_set_arm_max_line_acc(1)
self.robot.rm_set_arm_max_angular_acc(2)
for i in range(1,8):
self.robot.rm_set_joint_max_speed(i, 180)
self.robot.rm_set_joint_max_acc(i, 180)
def disconnect(self):
"""
Disconnect from the robot arm.
"""
handle = self.robot.rm_delete_robot_arm()
if handle == 0:
self.get_logger().info("\nSuccessfully disconnected from the robot arm\n")
else:
self.get_logger().info("\nFailed to disconnect from the robot arm\n")
def movej(self, joint, v=20, r=0, connect=0, block=1):
"""
Perform movej motion.
Args:
joint (list of float): Joint positions.
v (float, optional): Speed of the motion. Defaults to 20.
connect (int, optional): Trajectory connection flag. Defaults to 0.
block (int, optional): Whether the function is blocking (1 for blocking, 0 for non-blocking). Defaults to 1.
r (float, optional): Blending radius. Defaults to 0.
"""
if not self.robot_handle.id == -1 :
movej_result = self.robot.rm_movej(joint, v, r, connect, block)
if movej_result == 0:
self.get_logger().info("\n real robot movej motion succeeded\n")
else:
self.get_logger().error(f"\n real robot movej motion failed, Error code: {movej_result}\n")
else:
# --------------- movej in RViz start ---------------------------
self.j_t, self.j_measured = self.rviz.movej(joint , self.j_measured)
self.p_measured = np.array( self.algo_handle.rm_algo_forward_kinematics(self.j_measured, flag=1) )
# --------------- movej in RViz end ---------------------------
def movel(self, pose, v=20, r=0, connect=0, block=1):
"""
Perform movel motion.
Args:
pose (list of float): End position [x, y, z, rx, ry, rz].
v (float, optional): Speed of the motion. Defaults to 20.
connect (int, optional): Trajectory connection flag. Defaults to 0.
block (int, optional): Whether the function is blocking (1 for blocking, 0 for non-blocking). Defaults to 1.
r (float, optional): Blending radius. Defaults to 0.
"""
if not self.robot_handle.id == -1 :
self.get_logger().info(f"\n Entering the movel() with pose = {pose} \n")
movel_result = self.robot.rm_movel(pose, v, r, connect, block)
if movel_result == 0:
self.get_logger().info("\n movel motion succeeded\n")
else:
self.get_logger().error(f"\n real robot movel motion failed, Error code: {movel_result} \n")
self.get_logger().info("\n Starting the Rviz movel() \n")
else:
# --------------- movel in RViz start ---------------------------
self.j_measured = self.rviz.movel(p_t=pose,j_m=self.j_measured,p_m=self.p_measured)
self.get_logger().info("Rviz movel() done\n")
# --------------- movel in RViz end ---------------------------
def movej_canfd(self, j):
if self.robot_handle.id == -1:
self.rviz.pub_target_j(j)
self.j_measured = j
else:
movej_canfd_result = self.robot.rm_movej_canfd(j, False)
if movej_canfd_result != 0:
self.get_logger().error("\n movej_canfd motion failed, Error code: ", movej_canfd_result, "\n")
class ArmCtlNode(RMArmNode):
def __init__(self, ik = True):
super().__init__()
self.dof = 7
self.a_max = acc_max_j
self.v_max = vel_max_j
# initialize the state of the inverse kinematics
self.k = k_j
self.interval = 0.02
self.lmt_05att = np.array([[-1]*self.dof , [1]*self.dof ]) * self.interval ** 2 * 0.5 * self.a_max * lmt_scale
# initialize the target joint positions and former target joint positions
self.j_t_f = self.j_measured
self.p_measured_f = self.p_measured
# state of whether the target pose has already been received
self.p_t_rcvd = False
# define the state of whether the teleoperation is enabled
self.tele_en = False
# define the former state of whether the teleoperation is enabled
self.tele_en_f = False
# define the state of the scissor. True: open; False: close
self.scissor_en = False
self.scale = np.ones(self.dof)
self.frame_id = 0
# -------------------------- create the timer -----------------------------
# create timer for main controller
self.timer = self.create_timer(
self.interval,
self.rm_move_arm
)
self.timer_count = 0
# create the subscriber to receive target pose
self.pose_sub = self.create_subscription(
JointState,
self.subname,
self.target_update_callback,
2)
# Create subscription for Bool topic to receive teleoperation enable state
self.en_sub = self.create_subscription(
Bool,
en_topic,
self.tele_en_callback,
2)
# Create subscription for Bool topic to receive the scissor state
self.sci_sub = self.create_subscription(
Bool,
self.scissor_topic,
self.scissor_callback,
2)
self.tool_flg = 0
self.tool_timer = self.create_timer(
0.2,
self.rm_tool_timer
)
# handle for the data saving
self.file_hd = SaveFile( self.savefile_flg , self.left)
print('---------------- End of ArmCtlNode initial -----------------------')
# -------------------------- timer call fun --------------------------
def rm_move_arm(self):
"""
50Hz timer callback function, independent thread, not in main loop
send pose/joint command to the robot
"""
if self.tele_en:
if not self.tele_en_f:
self.p_t_f = self.p_measured
self.j_t_f = self.j_measured
if self.p_t_rcvd:
j_cmd = self.cal_j_t(self.j_t)
self.movej_canfd(j_cmd)
t = self.j_t
self.file_hd.write(t, self.j_measured, self.p_measured)
else:
self.p_t_f = self.p_measured
self.j_t_f = self.j_measured
if self.robot_handle.id == -1:
self.rviz.pub_target_j(self.j_measured)
self.timer_count += 1
self.tele_en_f = self.tele_en
self.p_measured_f = self.p_measured
self.j_measured_f = self.j_measured
if self.timer_count % (50 * 2 ) == 0:
print(f'at {self.timer_count * self.interval * 2} s, the en_state is {self.tele_en}, p_measure = {self.p_measured}, j cmd = {self.j_t_f}\n\n')
def cal_j_t(self, j_t):
'''
adjust the target joint positions to comply with limitations
'''
d_cmd = motion_lmt(target=j_t, measure=self.j_measured, k=self.scale * self.k, lmt=np.array(self.v_measured)*self.interval + self.lmt_05att, v_max=self.v_max, interval=self.interval)
self.j_t_f = (d_cmd + np.array(self.j_measured)).tolist()
return self.j_t_f
def target_update_callback(self, msg):
"""Callback for target joint update"""
if not self.tele_en:
return
'''
@cnt: count, len = 1
@sts: ik calculation return value, 0: success, <0: fail, len = 1
@suc: to distinguish which solver finish the ik, len = 1
@init_guess:current joint angles, degreee, len = 7
@pose: forward kinematics calculation result based on current joint angles, len = 6, unit: m, rad in euler
@ik_j: ik calculation results, joint angles, rad, len = 7
@p_t: target pose, x y z qw qx qy qz (quaternion), len = 7, quaternion
@p_t_euler: the target pose in euler
'''
data = msg.position
cnt = int(data[0])
ik_state = int(data[1])
suc = int(data[2])
j = data[3:10]
pose = data[10:16]
ik_j = data[16:23]
p_t = data[23:30]
p_t_euler = data[30:36]
frame_id = cnt
# print(f'in left = {self.left} target_update_callback function, frame_id = {frame_id}')
if frame_id > self.frame_id:
self.frame_id = frame_id
if ik_state < -0.5:
print('error in ik cal, unreachable, ret = ', ik_state)
if not hasattr(self, 'p_t_rcvd') or not self.p_t_rcvd:
# j_t has never been set by a successful IK, hold measured joints to avoid commanding zero position
self.j_t = list(self.j_measured)
else:
self.j_t = [j* 180 / math.pi for j in ik_j]
self.p_t_rcvd = True
# print(f'j_t: {self.j_t} in target_update_callback')
def tele_en_callback(self, msg):
"""Callback for Bool topic"""
self.tele_en = msg.data
if not self.tele_en:
self.p_t_rcvd = False
else:
self.p_t_rcvd = False
self.j_t = list(self.j_measured)
self.j_t_f = list(self.j_measured)
self.p_t_f = np.array(self.p_measured)
self.frame_id = -1
if self.tele_en:
self.get_logger().info(f'\n------------------Teleoperation Enabled ----------------------')
else:
self.get_logger().info(f'\n------------------Teleoperation Disabled ----------------------')
def scissor_callback(self, msg):
''' for scissor state '''
self.scissor_en = msg.data
tool_exe(self.robot, self.scissorgripper, self.scissor_en)
def rm_tool_timer(self):
if self.init_done and ( not self.scissorgripper == -1):
tool_button = self.robot.rm_get_io_input()[1][0]
if tool_button == 1 and self.tool_flg == 0:
tool_exe(self.robot, self.scissorgripper, True)
elif tool_button == 0 and self.tool_flg == 1:
tool_exe(self.robot, self.scissorgripper, False)
self.tool_flg = tool_button
def main(args=None):
rclpy.init(args=args)
arm_node = RMArmNode()
executor = MultiThreadedExecutor()
executor.add_node(arm_node)
try:
executor.spin()
except KeyboardInterrupt:
pass
finally:
arm_node.disconnect()
arm_node.destroy_node()
rclpy.shutdown()

25
harvest_arm_rm/launch/rm_arm.launch.py Normal file
View File

@ -0,0 +1,25 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import os
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 = get_package_share_directory('harvest_arm_rm')
config_file = os.path.join(package_share_dir, 'config', 'config.yaml')
return LaunchDescription([
Node(
package="harvest_arm_rm",
executable="realman_run",
name="harvest_arm_rm",
output="screen",
parameters=[config_file],
)
])

13
harvest_arm_rm/setup.py
View File

@ -1,4 +1,6 @@
from setuptools import find_packages, setup
import os
from glob import glob
package_name = 'harvest_arm_rm'
@ -7,9 +9,11 @@ setup(
version='0.0.0',
packages=find_packages(exclude=['test']),
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
("share/ament_index/resource_index/packages", ["resource/harvest_arm_rm"]),
("share/harvest_arm_rm", ["package.xml"]),
(os.path.join("share", "harvest_arm_rm", "config"), glob("config/*.yaml")),
(os.path.join("share", "harvest_arm_rm", "launch"), glob("launch/*.launch.py")),
],
install_requires=['setuptools'],
zip_safe=True,
@ -24,7 +28,8 @@ setup(
},
entry_points={
'console_scripts': [
'arm_cfg = harvest_arm_rm.arm_cfg:main'
'arm_cfg = harvest_arm_rm.arm_cfg:main',
'realman_run = harvest_arm_rm.run_realman_arm:main',
],
},
)