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_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]

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 *
class RMArmNode(Node):
class RM_Arm(Node):
def __init__(self, port=8080, level=3, mode=2, node_name="harvest_arm_rm"):
"""
Initialize and connect to the robotic arm.
@ -30,7 +30,7 @@ class RMArmNode(Node):
try:
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:
self.get_logger().error("Failed to connect to the real robot arm (socket error)!")
else:
@ -38,7 +38,7 @@ class RMArmNode(Node):
# if real robot connected, initialize following cfg
self.arm_motion_lmt()
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()
except Exception as 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.config["interval"],
self.rm_movej_timer
self.rm_timer
)
@ -62,7 +62,7 @@ class RMArmNode(Node):
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', "scissor").value
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
@ -82,7 +82,7 @@ class RMArmNode(Node):
"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,
"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_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_speed_sts": [0.0] * 7,
"arm_enable": int(0),
"tool_cmd": int(0),
"tool_cmd": [int(0), int(0)],
"pose_sts": [0.0] * 6,
"timer_cnt": 0,
"action_timestamp": 0.0,
}
arm_arg = {
@ -134,10 +135,17 @@ class RMArmNode(Node):
Args:
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)
self.sts["arm_enable"] = int(action_list[0])
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):
"""
@ -145,7 +153,7 @@ class RMArmNode(Node):
"""
msg = JointState()
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.effort = []
self.observation_pub.publish(msg)
@ -238,16 +246,18 @@ class RMArmNode(Node):
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:
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
send joint command to the arm
"""
############# movej --> arm
if self.sts["arm_enable"] == 1:
dis_max_vt = self.config["max_joint_speed"] * 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.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.sts["timer_cnt"] += 1
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.
Args:
arm (RoboticArm): Instance of the RoboticArm class.
config (dict): Configuration dictionary containing tool offsets.
"""
self.tool_cfg(arm, config)
self.connector_cfg(arm, config)
self.frame_cfg()
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:
arm (RoboticArm): Instance of the RoboticArm class.
config (dict): Configuration dictionary containing tool offsets.
"""
tool_offset = config["tool_offset"]
tool_name = config["tool_name"]
eu = arm.rm_algo_quanternion2euler([tool_offset[6], tool_offset[3], tool_offset[4], tool_offset[5]])
tool_offset = self.config["tool_offset"]
tool_name = self.config["tool_name"]
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_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"]:
ret = arm.rm_update_tool_frame(tool_frame)
ret = self.arm_arg['arm'].rm_update_tool_frame(tool_frame)
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:
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.
Args:
@ -309,19 +337,19 @@ class RMArmNode(Node):
config (dict): Configuration dictionary containing connector settings.
"""
# Implement connector configuration logic here
arm.rm_set_voltage(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)
self.arm_arg['arm'].rm_set_voltage(self.config["arm_ctrl_voltage"])
if config["tool_name"] == config["tool_names"][0]: # scissor
arm.rm_set_tool_voltage(config["arm_tool_io_voltage"])
self.arm_arg['arm'].rm_set_io_mode(1, self.config["arm_ctrl_io_mode"][0], 50, 2)
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])
elif config["tool_name"] == config["tool_names"][1]: # omnipic
modbus_sts = arm.rm_set_modbus_mode(port=1, baudrate=115200, timeout=2)
if self.config["tool_name"] == 'scissor': # scissor
self.arm_arg['arm'].rm_set_tool_voltage(self.config["arm_tool_io_voltage"])
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:
self.get_logger().warn(f"Warning: Failed to set Modbus mode: {modbus_sts}")
else:
@ -333,48 +361,50 @@ class RMArmNode(Node):
reg_value = [255, 60, 255, 255]
for i, reg_addr in enumerate([11, 12, 13, 14]) :
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)
elif config["tool_name"] == config["tool_names"][2]: # minisci
pass
elif self.config["tool_name"] == 'minisci': # minisci
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)
"""
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)
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
write_params = rm_peripheral_read_write_params_t(1, 15, device, 1)
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)
elif tool_name == config["tool_names"][0]:
#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
elif tool_name == 'minisci':
# mini scissor
if percent == 0:
arm.rm_set_do_state(3, 1)
arm.rm_set_do_state(4, 0)
self.arm_arg['arm'].rm_set_do_state(3, 1)
self.arm_arg['arm'].rm_set_do_state(4, 0)
elif percent == 1:
arm.rm_set_do_state(3, 0)
arm.rm_set_do_state(4, 1)
self.arm_arg['arm'].rm_set_do_state(3, 0)
self.arm_arg['arm'].rm_set_do_state(4, 1)
def main(args=None):
rclpy.init(args=args)
arm_node = RMArmNode()
arm_node = RM_Arm()
executor = MultiThreadedExecutor()
executor.add_node(arm_node)