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

init config

Browse Source
This commit is contained in:
LiuzhengSJ
2026-06-23 17:12:47 +01:00
parent 2d8ef78b8b
commit a14e2ff8bb
2 changed files with 166 additions and 88 deletions
Download Patch File Download Diff File Expand all files Collapse all files

51
harvest_arm_rm/config/config.yaml
View File

@ -3,46 +3,61 @@ harvest_arm_rm:
robot_ip: "192.168.1.19" robot_ip: "192.168.1.19"
host_ip: "192.168.1.101" host_ip: "192.168.1.101"
robot_port: 8899
port: 8089 port: 8089
level: 3
mode: 2
prefix: ""
left: true
savefile_flg: true
scissorgripper: -1
tool_name: "scissor" tool_name: "scissor"
max_velocity: 0.2 initial_joint: [-82.60, 20.28, -32.15, 50.28, 17.28, 64.055, -20.62]
max_acceleration: 0.5
max_joint: [155.0, 115.0, 172.0, 130.0, 175.0, 125.0, 179.0]
min_joint: [-155.0, -30.0, -172.0, -130.0, -175.0, -125.0, -179.0]
max_joint_speed: 180.0 # deg/s
max_joint_acc: 600.0
max_cartesian_speed: 1.0 # m/s
max_cartesian_angular_speed: 1.5 # rad/s
max_cartesian_acc: 1.0
max_cartesian_angular_acc: 2.0
use_sim_time: false
arm_installation: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] arm_installation: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
# 0~2 position, x, y, z, m
# 3~6 orientation, quaternion, x, y, z, w
# 7 mass, kg
# 8~11 center of mass, x, y, z, m
tools_in_ee.scissor: [ tools_in_ee.scissor: [
0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0, 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 0.66, 0.0, 0.0, 0.06
] ]
tools_in_ee.omnipic: [ tools_in_ee.omnipic: [
0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0, 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 0.43, 0.0, 0.0, 0.06
] ]
tools_in_ee.minisci: [ tools_in_ee.minisci: [
0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0, 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 0.46, 0.0, 0.0, 0.06
] ]
tools_in_ee.no_tool: [ 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, 1.0,
0.0, 0.0, 0.0, 0.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_cmd_topic: "/joint_cmd"
joint_min: [-155.0, -30.0, -172.0, -130.0, -175.0, -125.0, -179.0] joint_state_topic: "/joint_sts"
arm_enable_topic: "/arm_enable"
tool_cmd_topic: "/tool_cmd"
bounds_p_right: [-0.35, 0.7, -0.7, 0.4, 0.32, 0.7]

203
harvest_arm_rm/harvest_arm_rm/run_realman_arm.py
View File

@ -31,58 +31,129 @@ class RMArmNode(Node):
mode (int, optional): Thread mode (0: single, 1: dual, 2: triple). Defaults to 2. mode (int, optional): Thread mode (0: single, 1: dual, 2: triple). Defaults to 2.
""" """
super().__init__('harvest_arm_rm') super().__init__('harvest_arm_rm')
self.init_done = False self.config, self.sts, self.arm_arg = self.get_arg()
self.arm_topic()
try: try:
self.get_logger().info(f"---Robot IP: {robot_ip}") self.arm_arg['arm'] = RoboticArm(rm_thread_mode_e(mode))
except: self.arm_arg['arm_handle'] = self.arm_arg['arm'].rm_create_robot_arm(self.arm_arg['robot_ip'], self.arm_arg['port'], level)
robot_ip = self.declare_parameter('robot_ip', "192.168.1.18").value if self.arm_arg['arm_handle'].id == -1:
self.get_logger().info(f"Robot ----IP: {robot_ip}") print("Failed to connect to the real robot arm (socket error)!")
self.host_ip = self.declare_parameter('host_ip', "192.168.1.101").value else:
self.prefix = self.declare_parameter('prefix', "").value self.get_logger().info(f"Successfully connected to the real robot arm: {self.arm_arg['arm_handle'].id}")
self.left = self.declare_parameter('left',True).value # if real robot connected, initialize following cfg
self.port = self.declare_parameter('port',8089).value self.armcfg()
self.savefile_flg = self.declare_parameter('savefile_flg',True).value self.udp_config()
self.scissorgripper = self.declare_parameter('scissorgripper',-1).value self.arm_arg['arm'] = peripheral_cfg(self.arm_arg['arm'], self.config)
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.arm_arg['arm'] = None
def get_arg(self):
"""
Get the configuration of the robotic arm.
Returns:
dict: Configuration dictionary containing robot IP, host IP, prefix, left/right status, port, savefile flag, and scissor gripper status.
"""
tool_offsets = {}
tool_offsets['scissor'] = self.declare_parameter('tools_in_ee.scissor', 0.0).value
tool_offsets['omnipick'] = self.declare_parameter('tools_in_ee.omnipick', 0.0).value
tool_offsets['minisci'] = self.declare_parameter('tools_in_ee.minisci', 0.0).value
tool_offsets['no_tool'] = self.declare_parameter('tools_in_ee.no_tool', 0.0).value
tool_name = self.declare_parameter('tool_name', "scissor").value
tool_offset_list = tool_offsets[tool_name]
self.leftright = 0 if self.left else 1 config = {
self.subname = rm_l_act_topic if self.left else rm_r_act_topic "robot_ip": self.declare_parameter('robot_ip', "192.168.1.18").value,
self.scissor_topic = left_scissor_topic if self.left else right_scissor_topic "host_ip": self.declare_parameter('host_ip', "192.168.1.101").value,
"port": self.declare_parameter('port', 8089).value,
self.robot_handle = None "max_joint": self.declare_parameter('max_joint', joint_max).value,
self.robot = None "min_joint": self.declare_parameter('min_joint', joint_min).value,
# measured pose, joint, joint-velocity "max_joint_speed": self.declare_parameter('max_joint_speed', 180.0).value,
self.p_measured = np.zeros(6, dtype=np.float32) "max_joint_acc": self.declare_parameter('max_joint_acc', 600.0).value,
self.j_measured = [1] * 7
self.j_t = [0.0] * 7
self.v_measured = [0.0] * 7
self.initialized = False "max_cartesian_speed": self.declare_parameter('max_cartesian_speed', 1.0).value,
self.algo_handle = alg_init(scissorgripper=self.scissorgripper, tools_in_ee = tools_in_ee, joint_max=joint_max, joint_min=joint_min) "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,
self.rviz = Rviz_arm(node=self, rviz_flg=self.declare_parameter('rviz', True).value, prefix=self.prefix, left_flg = self.left) "arm_installation": self.declare_parameter('arm_installation', [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]).value,
"tool_offset": tool_offset_list,
if robot_enable_sts[self.leftright]: "joint_cmd_topic": self.declare_parameter('joint_cmd_topic', "/joint_cmd").value,
try: "joint_state_topic": self.declare_parameter('joint_state_topic', "/joint_sts").value,
self.robot = RoboticArm(rm_thread_mode_e(mode)) "arm_enable_topic": self.declare_parameter('arm_enable_topic', "/arm_enable").value,
self.robot_handle = self.robot.rm_create_robot_arm(robot_ip, port, level) "tool_cmd_topic": self.declare_parameter('tool_cmd_topic', "/tool_cmd").value,
if self.robot_handle.id == -1: "initial_joint": self.declare_parameter('initial_joint', [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]).value,
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}") sts = {
# if real robot connected, initialize following cfg "joint_cmd": [0.0] * 7,
self.limitcfg() "joint_sts": [0.0] * 7,
self.udp_config() "joint_speed": [0.0] * 7,
self.robot = peripheral_cfg(self.robot, scissorgripper=self.scissorgripper, tools_in_ee = tools_in_ee) "arm_enable": False,
self.init_done = True "tool_cmd": False,
# self.move_to_init() "pose_sts": [0.0] * 6,
except Exception as e: }
self.get_logger().error(f"Robot arm hardware init error: {e}, enter RViz simulation mode!")
self.robot = None arm_arg = {
"arm" : None,
"arm_handle" : None,
}
return config, sts, arm_arg
def arm_topic(self):
"""
Initialize ROS2 topics for the robotic arm.
"""
self.subname = self.config["joint_cmd_topic"]
# Create publisher for joint states
self.joint_sts_pub = self.create_publisher(JointState, self.config["joint_state_topic"], 10)
# Create subscriber for arm enable state
self.arm_en_sub = self.create_subscription(Bool, self.config["arm_enable_topic"], self.arm_enable_callback, 10)
# Create subscriber for tool command
self.tool_sub = self.create_subscription(Bool, self.config["tool_cmd_topic"], self.tool_callback, 10)
# Create subcriber for target joint
self.joint_cmd_sub = self.create_subscription(JointState, self.config["joint_cmd_topic"], self.joint_cmd_callback, 10)
def arm_enable_callback(self, msg):
"""
Callback function for arm enable state.
Args:
msg (Bool): Message containing the arm enable state.
"""
self.sts["arm_enable"] = msg.data
if self.sts["arm_enable"]:
self.get_logger().info(f'\n------------------Arm Enabled ----------------------')
else: else:
self.get_logger().error(f"Robot arm enable sts: {robot_enable_sts[self.leftright]} with leftright = {self.leftright}, enter RViz simulation mode!") self.get_logger().info(f'\n------------------Arm Disabled ----------------------')
self.robot = None
def tool_callback(self, msg):
"""
Callback function for tool command.
Args:
msg (Bool): Message containing the tool command state.
"""
self.sts["tool_cmd"] = msg.data
if self.sts["tool_cmd"]:
self.get_logger().info(f'\n------------------Tool Command Enabled ----------------------')
else:
self.get_logger().info(f'\n------------------Tool Command Disabled ----------------------')
def joint_cmd_callback(self, msg):
"""
Callback function for joint command.
Args:
msg (JointState): Message containing the joint command.
"""
self.sts["joint_cmd"] = list(msg.position)
def udp_config(self): def udp_config(self):
@ -91,15 +162,15 @@ class RMArmNode(Node):
custom.joint_speed = 1 custom.joint_speed = 1
custom.lift_state = 1 custom.lift_state = 1
custom.expand_state = 1 custom.expand_state = 1
config = rm_realtime_push_config_t(3, True, self.port, 0, self.host_ip, custom) config = rm_realtime_push_config_t(3, True, self.config["port"], 0, self.config["host_ip"], custom)
self.robot.rm_set_realtime_push(config) self.config["arm"].rm_set_realtime_push(config)
# Store the callback as an instance variable to prevent garbage collection # Store the callback as an instance variable to prevent garbage collection
self.arm_state_callback = rm_realtime_arm_state_callback_ptr(self.udp_callback) self.arm_state_callback = rm_realtime_arm_state_callback_ptr(self.udp_callback)
# Use the stored callback # Use the stored callback
self.robot.rm_realtime_arm_state_call_back(self.arm_state_callback) self.config["arm"].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: while sum(abs(self.sts["pose_sts"])) < 0.001 or sum(abs(np.array(self.sts["joint_sts"]))) < 0.0001:
time.sleep(1) time.sleep(1)
print(f'self.prefix = {self.prefix}, p_measured = {self.p_measured}, j_measured = {self.j_measured}') print(f'p_measured = {self.sts["pose_sts"]}, j_measured = {self.sts["joint_sts"]}')
def udp_callback(self, data): def udp_callback(self, data):
# extract the pose data from the UDP data # extract the pose data from the UDP data
@ -117,7 +188,7 @@ class RMArmNode(Node):
return 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'] ] 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 ) self.sts["pose_sts"] = pose
#--------------------------------------- joint 1*7 ---------------------------------- #--------------------------------------- joint 1*7 ----------------------------------
# Convert to dict safely # Convert to dict safely
@ -134,11 +205,11 @@ class RMArmNode(Node):
if len(joint_position) != 7: if len(joint_position) != 7:
self.get_logger().warn(f"Warning: Expected 7 joint positions, got {len(joint_position)}: {joint_position}") self.get_logger().warn(f"Warning: Expected 7 joint positions, got {len(joint_position)}: {joint_position}")
# Pad with zeros or truncate to 7 # Pad with zeros or truncate to 7
self.j_measured = joint_position self.sts["joint_sts"] = joint_position
self.v_measured = joint_speed self.sts["joint_speed"] = joint_speed
# ------------- send cmd to the Rviz ---------------------------------- # ------------- send cmd to the Rviz ----------------------------------
self.rviz.pub_target_j(self.j_measured) self.rviz.pub_target_j(self.sts["joint_sts"])
except Exception as e: except Exception as e:
self.get_logger().error(f"Error in UDP callback: {e}") self.get_logger().error(f"Error in UDP callback: {e}")
@ -147,29 +218,21 @@ class RMArmNode(Node):
def move_to_init(self): def move_to_init(self):
if not self.initialized: 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 # Perform movej motion for robot arm
self.movej(points["movej"]) self.movej(self.config["initial_joint"])
# Perform movel motion
self.movel(points["movel"])
self.initialized = True
self.get_logger().info('\n------------------- ready ------------------') self.get_logger().info('\n------------------- ready ------------------')
def limitcfg(self ): def armcfg(self ):
# configure the motion limitations for the robot # configure the motion limitations for the robot
self.robot.rm_set_avoid_singularity_mode(True) self.arm_arg['arm'].rm_set_avoid_singularity_mode(True)
self.robot.rm_set_arm_max_line_speed(1) self.arm_arg['arm'].rm_set_arm_max_line_speed(self.config["max_cartesian_speed"])
self.robot.rm_set_arm_max_angular_speed(1.5) self.arm_arg['arm'].rm_set_arm_max_angular_speed(self.config["max_cartesian_angular_speed"])
self.robot.rm_set_arm_max_line_acc(1) self.arm_arg['arm'].rm_set_arm_max_line_acc(self.config["max_cartesian_acc"])
self.robot.rm_set_arm_max_angular_acc(2) self.arm_arg['arm'].rm_set_arm_max_angular_acc(self.config["max_cartesian_angular_acc"])
for i in range(1,8): for i in range(1,8):
self.robot.rm_set_joint_max_speed(i, 180) self.robot.rm_set_joint_max_speed(i, self.config["max_joint_speed"])
self.robot.rm_set_joint_max_acc(i, 180) self.robot.rm_set_joint_max_acc(i, self.config["max_joint_acc"])
def disconnect(self): def disconnect(self):
""" """
@ -282,7 +345,7 @@ class ArmCtlNode(RMArmNode):
self.rm_move_arm self.rm_move_arm
) )
self.timer_count = 0 self.timer_count = 0
# create the subscriber to receive target pose # create the subscriber to receive target joints
self.pose_sub = self.create_subscription( self.pose_sub = self.create_subscription(
JointState, JointState,
self.subname, self.subname,