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

the basic version for the arm driver pkg

Browse Source
This commit is contained in:
LiuzhengSJ
2026-06-25 13:48:40 +01:00
parent 88545a7af0
commit 45b25e6133
2 changed files with 103 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
harvest_arm_rm/config/config.yaml
View File

@ -7,7 +7,7 @@ harvest_arm_rm:
tool_name: "scissor" tool_name: "scissor"
tool_names: ["scissor", "omnipic", "minisci", "no_tool"] tool_names: ["scissor", "omnipic", "minisci", "vacuum", "no_tool"]
initial_joint: [-82.60, 20.28, -32.15, 50.28, 17.28, 64.055, -20.62] initial_joint: [-82.60, 20.28, -32.15, 50.28, 17.28, 64.055, -20.62]

146
harvest_arm_rm/harvest_arm_rm/run_realman_arm.py
View File

@ -14,7 +14,7 @@ import math
from Robotic_Arm.rm_robot_interface import * from Robotic_Arm.rm_robot_interface import *
class RMArmNode(Node): class RM_Arm(Node):
def __init__(self, port=8080, level=3, mode=2, node_name="harvest_arm_rm"): def __init__(self, port=8080, level=3, mode=2, node_name="harvest_arm_rm"):
""" """
Initialize and connect to the robotic arm. Initialize and connect to the robotic arm.
@ -30,7 +30,7 @@ class RMArmNode(Node):
try: try:
self.arm_arg['arm'] = RoboticArm(rm_thread_mode_e(mode)) self.arm_arg['arm'] = RoboticArm(rm_thread_mode_e(mode))
self.arm_arg['arm_handle'] = self.arm_arg['arm'].rm_create_robot_arm(self.arm_arg['robot_ip'], self.arm_arg['port'], level) self.arm_arg['arm_handle'] = self.arm_arg['arm'].rm_create_robot_arm(self.config['robot_ip'], self.config['port'], level)
if self.arm_arg['arm_handle'].id == -1: if self.arm_arg['arm_handle'].id == -1:
self.get_logger().error("Failed to connect to the real robot arm (socket error)!") self.get_logger().error("Failed to connect to the real robot arm (socket error)!")
else: else:
@ -38,7 +38,7 @@ class RMArmNode(Node):
# if real robot connected, initialize following cfg # if real robot connected, initialize following cfg
self.arm_motion_lmt() self.arm_motion_lmt()
self.udp_config() self.udp_config()
self.arm_arg['arm'] = self.peripheral_cfg(self.arm_arg['arm'], self.config) self.arm_arg['arm'] = self.peripheral_cfg()
# self.move_to_init() # self.move_to_init()
except Exception as e: except Exception as e:
self.get_logger().error(f"Robot arm hardware init error: {e} !") self.get_logger().error(f"Robot arm hardware init error: {e} !")
@ -52,7 +52,7 @@ class RMArmNode(Node):
self.timer = self.create_timer( self.timer = self.create_timer(
self.config["interval"], self.config["interval"],
self.rm_movej_timer self.rm_timer
) )
@ -62,7 +62,7 @@ class RMArmNode(Node):
Returns: Returns:
dict: Configuration dictionary containing robot IP, host IP, prefix, left/right status, port, savefile flag, and scissor gripper status. 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', "scissor").value tool_name = self.declare_parameter('tool_name', "no_tool").value
tool_name_param = 'tools_in_ee.' + tool_name tool_name_param = 'tools_in_ee.' + tool_name
try: try:
tool_offset_list = self.declare_parameter(tool_name_param, [0.0] * 11).value tool_offset_list = self.declare_parameter(tool_name_param, [0.0] * 11).value
@ -82,7 +82,7 @@ class RMArmNode(Node):
"max_joint": self.declare_parameter('max_joint', [0.0] * 7).value, "max_joint": self.declare_parameter('max_joint', [0.0] * 7).value,
"min_joint": self.declare_parameter('min_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, "max_joint_speed": self.declare_parameter('max_joint_speed', 180.0).value,
"att_2": acc_lmt * interval * interval * 0.5, "lmt_05att": acc_lmt * interval * interval * 0.5,
"max_cartesian_speed": self.declare_parameter('max_cartesian_speed', 1.0).value, "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_angular_speed": self.declare_parameter('max_cartesian_angular_speed', 1.5).value,
@ -114,9 +114,10 @@ class RMArmNode(Node):
"joint_pos_sts": [0.0] * 7, "joint_pos_sts": [0.0] * 7,
"joint_speed_sts": [0.0] * 7, "joint_speed_sts": [0.0] * 7,
"arm_enable": int(0), "arm_enable": int(0),
"tool_cmd": int(0), "tool_cmd": [int(0), int(0)],
"pose_sts": [0.0] * 6, "pose_sts": [0.0] * 6,
"timer_cnt": 0, "timer_cnt": 0,
"action_timestamp": 0.0,
} }
arm_arg = { arm_arg = {
@ -134,10 +135,17 @@ class RMArmNode(Node):
Args: Args:
msg (JointState): Message containing the joint command. msg (JointState): Message containing the joint command.
""" """
self.sts["tool_cmd"][1] = self.sts["tool_cmd"][0]
total_seconds = msg.header.stamp.sec + (msg.header.stamp.nanosec * 1e-9)
if total_seconds > self.sts["action_timestamp"]:
self.sts["action_timestamp"] = total_seconds
action_list = list(msg.position) action_list = list(msg.position)
self.sts["arm_enable"] = int(action_list[0]) self.sts["arm_enable"] = int(action_list[0])
self.sts["joint_target"] = action_list[1:8] self.sts["joint_target"] = action_list[1:8]
self.sts["tool_cmd"] = int(action_list[8]) self.sts["tool_cmd"][0] = int(action_list[8])
def arm_observation_pub(self): def arm_observation_pub(self):
""" """
@ -145,7 +153,7 @@ class RMArmNode(Node):
""" """
msg = JointState() msg = JointState()
msg.header.stamp = self.get_clock().now().to_msg() msg.header.stamp = self.get_clock().now().to_msg()
msg.position = [float(self.sts["arm_enable"])] + list(self.sts["joint_pos_sts"]) + [float(self.sts["tool_cmd"])] + [float(self.sts["timer_cnt"])] msg.position = [float(self.sts["arm_enable"])] + list(self.sts["joint_pos_sts"]) + [float(self.sts["tool_cmd"][0])] + [float(self.sts["timer_cnt"])]
msg.velocity = self.sts["joint_speed_sts"] msg.velocity = self.sts["joint_speed_sts"]
msg.effort = [] msg.effort = []
self.observation_pub.publish(msg) self.observation_pub.publish(msg)
@ -238,16 +246,18 @@ class RMArmNode(Node):
def movej_canfd(self, j): def movej_canfd(self, j):
movej_canfd_result = self.sts["arm"].rm_movej_canfd(j, False) if self.arm_arg['arm_handle'].id != -1:
movej_canfd_result = self.arm_arg["arm"].rm_movej_canfd(j, False)
if movej_canfd_result != 0: if movej_canfd_result != 0:
self.get_logger().error("\n movej_canfd motion failed, Error code: ", movej_canfd_result, "\n") self.get_logger().error("\n movej_canfd motion failed, Error code: ", movej_canfd_result, "\n")
def rm_movej_timer(self): def rm_timer(self):
""" """
50Hz timer callback function, independent thread, not in main loop 50Hz timer callback function, independent thread, not in main loop
send joint command to the arm send joint command to the arm
""" """
############# movej --> arm
if self.sts["arm_enable"] == 1: if self.sts["arm_enable"] == 1:
dis_max_vt = self.config["max_joint_speed"] * self.config["interval"] dis_max_vt = self.config["max_joint_speed"] * self.config["interval"]
dis_vt = np.array(self.sts["joint_speed_sts"]) * self.config["interval"] dis_vt = np.array(self.sts["joint_speed_sts"]) * self.config["interval"]
@ -258,50 +268,68 @@ class RMArmNode(Node):
self.sts["joint_cmd"] = (d_j + np.array(self.sts["joint_pos_sts"])).tolist() self.sts["joint_cmd"] = (d_j + np.array(self.sts["joint_pos_sts"])).tolist()
self.movej_canfd(self.sts["joint_cmd"]) self.movej_canfd(self.sts["joint_cmd"])
############# tool
if self.sts["tool_cmd"][0] == 1 and self.sts["tool_cmd"][1] == 0:
self.set_tool_position(self.arm_arg['arm'], self.config, percent=1, device=1, tool_name=self.config["tool_name"])
elif self.sts["tool_cmd"][0] == 0 and self.sts["tool_cmd"][1] == 1:
self.set_tool_position(self.arm_arg['arm'], self.config, percent=0, device=1, tool_name=self.config["tool_name"])
self.arm_observation_pub() self.arm_observation_pub()
self.sts["timer_cnt"] += 1 self.sts["timer_cnt"] += 1
if self.sts["timer_cnt"] % (50 * 2 ) == 0: if self.sts["timer_cnt"] % (50 * 2 ) == 0:
self.get_logger().info(f'at {self.sts["timer_cnt"] * self.config["interval"] * 2} s, the en_state is {self.sts["arm_enable"]}, pose = {self.sts["pose_sts"]}, j cmd = {self.sts["joint_cmd"]}\n\n') self.get_logger().info(f'at {self.sts["timer_cnt"] * self.config["interval"] * 1} s, the en_state is {self.sts["arm_enable"]}, pose = {self.sts["pose_sts"]}, j cmd = {self.sts["joint_cmd"]}\n\n')
def peripheral_cfg(self, arm, config): def peripheral_cfg(self):
""" """
Configure the peripheral settings of the robotic arm. Configure the peripheral settings of the robotic arm.
Args: Args:
arm (RoboticArm): Instance of the RoboticArm class. arm (RoboticArm): Instance of the RoboticArm class.
config (dict): Configuration dictionary containing tool offsets. config (dict): Configuration dictionary containing tool offsets.
""" """
self.tool_cfg(arm, config) self.frame_cfg()
self.connector_cfg(arm, config) self.connector_cfg()
return arm
def tool_cfg(self, arm, config): def frame_cfg(self):
""" """
Configure the tool settings of the robotic arm. Configure the tool/work frame settings of the robotic arm.
Args: Args:
arm (RoboticArm): Instance of the RoboticArm class. arm (RoboticArm): Instance of the RoboticArm class.
config (dict): Configuration dictionary containing tool offsets. config (dict): Configuration dictionary containing tool offsets.
""" """
tool_offset = config["tool_offset"] tool_offset = self.config["tool_offset"]
tool_name = config["tool_name"] tool_name = self.config["tool_name"]
eu = arm.rm_algo_quanternion2euler([tool_offset[6], tool_offset[3], tool_offset[4], tool_offset[5]]) eu = self.arm_arg['arm'].rm_algo_quanternion2euler([tool_offset[6], tool_offset[3], tool_offset[4], tool_offset[5]])
tool_frame = rm_frame_t(tool_name,[tool_offset[0], tool_offset[1], tool_offset[2], eu[0], eu[1], eu[2]], tool_frame = rm_frame_t(tool_name,[tool_offset[0], tool_offset[1], tool_offset[2], eu[0], eu[1], eu[2]],
tool_offset[7], [tool_offset[8], tool_offset[9], tool_offset[10]]) tool_offset[7], [tool_offset[8], tool_offset[9], tool_offset[10]])
info = arm.rm_get_total_tool_frame() info = self.arm_arg['arm'].rm_get_total_tool_frame()
if tool_name in info["tool_names"]: if tool_name in info["tool_names"]:
ret = arm.rm_update_tool_frame(tool_frame) ret = self.arm_arg['arm'].rm_update_tool_frame(tool_frame)
else: else:
ret = arm.rm_set_manual_tool_frame(tool_frame) ret = self.arm_arg['arm'].rm_set_manual_tool_frame(tool_frame)
ret = arm.rm_change_tool_frame(tool_name) ret = self.arm_arg['arm'].rm_change_tool_frame(tool_name)
if ret != 0: if ret != 0:
raise RuntimeError(f"Failed to change tool frame to {tool_name}, ret={ret}") raise RuntimeError(f"Failed to change tool frame to {tool_name}, ret={ret}")
def connector_cfg(self, arm, config):
work_offset = self.config["arm_installation"]
ret, info, n = self.arm_arg['arm'].rm_get_total_work_frame()
if ret != 0:
raise RuntimeError(f"Failed to get total work frame, ret={ret}")
if n > 0:
if 'b' in info["tool_names"]:
self.arm_arg['arm'].rm_update_work_frame('b', self.config["arm_installation"])
else:
self.arm_arg['arm'].rm_set_manual_work_frame('b', self.config["arm_installation"])
self.arm_arg['arm'].rm_change_work_frame('b')
def connector_cfg(self):
""" """
Configure the connector settings of the robotic arm, tool connector and controller board connector. Configure the connector settings of the robotic arm, tool connector and controller board connector.
Args: Args:
@ -309,19 +337,19 @@ class RMArmNode(Node):
config (dict): Configuration dictionary containing connector settings. config (dict): Configuration dictionary containing connector settings.
""" """
# Implement connector configuration logic here # Implement connector configuration logic here
arm.rm_set_voltage(config["arm_ctrl_voltage"]) self.arm_arg['arm'].rm_set_voltage(self.config["arm_ctrl_voltage"])
arm.rm_set_io_mode(1, config["arm_ctrl_io_mode"][0], 50, 2)
arm.rm_set_io_mode(2, config["arm_ctrl_io_mode"][1], 50, 2)
arm.rm_set_io_mode(3, config["arm_ctrl_io_mode"][2], 50, 2)
arm.rm_set_io_mode(4, config["arm_ctrl_io_mode"][3], 50, 2)
if config["tool_name"] == config["tool_names"][0]: # scissor self.arm_arg['arm'].rm_set_io_mode(1, self.config["arm_ctrl_io_mode"][0], 50, 2)
arm.rm_set_tool_voltage(config["arm_tool_io_voltage"]) self.arm_arg['arm'].rm_set_io_mode(2, self.config["arm_ctrl_io_mode"][1], 50, 2)
arm.rm_set_tool_IO_mode(1, config["arm_tool_io_mode"][0])
arm.rm_set_tool_IO_mode(2, config["arm_tool_io_mode"][1]) if self.config["tool_name"] == 'scissor': # scissor
elif config["tool_name"] == config["tool_names"][1]: # omnipic self.arm_arg['arm'].rm_set_tool_voltage(self.config["arm_tool_io_voltage"])
modbus_sts = arm.rm_set_modbus_mode(port=1, baudrate=115200, timeout=2)
self.arm_arg['arm'].rm_set_tool_IO_mode(1, self.config["arm_tool_io_mode"][0])
self.arm_arg['arm'].rm_set_tool_IO_mode(2, self.config["arm_tool_io_mode"][1])
elif self.config["tool_name"] == 'omnipic': # omnipic
modbus_sts = self.arm_arg['arm'].rm_set_modbus_mode(port=1, baudrate=115200, timeout=2)
if modbus_sts != 0: if modbus_sts != 0:
self.get_logger().warn(f"Warning: Failed to set Modbus mode: {modbus_sts}") self.get_logger().warn(f"Warning: Failed to set Modbus mode: {modbus_sts}")
else: else:
@ -333,48 +361,50 @@ class RMArmNode(Node):
reg_value = [255, 60, 255, 255] reg_value = [255, 60, 255, 255]
for i, reg_addr in enumerate([11, 12, 13, 14]) : for i, reg_addr in enumerate([11, 12, 13, 14]) :
write_params = rm_peripheral_read_write_params_t(1, reg_addr, addr, 1) write_params = rm_peripheral_read_write_params_t(1, reg_addr, addr, 1)
arm.rm_write_single_register(write_params, reg_value[i]) self.arm_arg['arm'].rm_write_single_register(write_params, reg_value[i])
time.sleep(0.5) time.sleep(0.5)
elif config["tool_name"] == config["tool_names"][2]: # minisci elif self.config["tool_name"] == 'minisci': # minisci
pass self.arm_arg['arm'].rm_set_io_mode(3, self.config["arm_ctrl_io_mode"][2], 50, 2)
self.arm_arg['arm'].rm_set_io_mode(4, self.config["arm_ctrl_io_mode"][3], 50, 2)
def set_tool_position(arm, config, percent, device = 1, tool_name = 0): def set_tool_position(self, percent, device = 1, tool_name = 'scissor'):
""" """
Set gripper position (0.0 = closed, 1.0 = fully open) Set gripper position (0.0 = closed, 1.0 = fully open)
""" """
if tool_name == config["tool_names"][1]: # scissor if self.arm_arg['arm_handle'].id != -1:
if tool_name == 'scissor':
#scissor gripper only has two states, fully open and fully closed, so we can set a threshold to determine the state
if percent == 0:
self.arm_arg['arm'].rm_set_tool_do_state(1, 1)
self.arm_arg['arm'].rm_set_tool_do_state(2, 0)
elif percent == 1:
self.arm_arg['arm'].rm_set_tool_do_state(1, 0)
self.arm_arg['arm'].rm_set_tool_do_state(2, 1)
elif tool_name == 'omnipic':
pos_value = int(percent * 255) pos_value = int(percent * 255)
write_params = rm_peripheral_read_write_params_t(1, 10, device, 1) write_params = rm_peripheral_read_write_params_t(1, 10, device, 1)
arm.rm_write_single_register(write_params, pos_value) self.arm_arg['arm'].rm_write_single_register(write_params, pos_value)
# trigger # trigger
write_params = rm_peripheral_read_write_params_t(1, 15, device, 1) write_params = rm_peripheral_read_write_params_t(1, 15, device, 1)
time.sleep(0.5) time.sleep(0.5)
arm.rm_write_single_register(write_params, 0x01) self.arm_arg['arm'].rm_write_single_register(write_params, 0x01)
time.sleep(0.5) time.sleep(0.5)
elif tool_name == config["tool_names"][0]: elif tool_name == 'minisci':
#scissor gripper only has two states, fully open and fully closed, so we can set a threshold to determine the state
if percent == 0:
arm.rm_set_tool_do_state(1, 1)
arm.rm_set_tool_do_state(2, 0)
elif percent == 1:
arm.rm_set_tool_do_state(1, 0)
arm.rm_set_tool_do_state(2, 1)
elif tool_name == config["tool_names"][2]: # minisci
# mini scissor # mini scissor
if percent == 0: if percent == 0:
arm.rm_set_do_state(3, 1) self.arm_arg['arm'].rm_set_do_state(3, 1)
arm.rm_set_do_state(4, 0) self.arm_arg['arm'].rm_set_do_state(4, 0)
elif percent == 1: elif percent == 1:
arm.rm_set_do_state(3, 0) self.arm_arg['arm'].rm_set_do_state(3, 0)
arm.rm_set_do_state(4, 1) self.arm_arg['arm'].rm_set_do_state(4, 1)
def main(args=None): def main(args=None):
rclpy.init(args=args) rclpy.init(args=args)
arm_node = RMArmNode() arm_node = RM_Arm()
executor = MultiThreadedExecutor() executor = MultiThreadedExecutor()
executor.add_node(arm_node) executor.add_node(arm_node)