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"
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
initial_joint: [-82.60, 20.28, -32.15, 50.28, 17.28, 64.055, -20.62]
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]
# 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: [
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: [
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: [
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: [
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_min: [-155.0, -30.0, -172.0, -130.0, -175.0, -125.0, -179.0]
joint_cmd_topic: "/joint_cmd"
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.
"""
super().__init__('harvest_arm_rm')
self.init_done = False
self.config, self.sts, self.arm_arg = self.get_arg()
self.arm_topic()
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!")
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)
if self.arm_arg['arm_handle'].id == -1:
print("Failed to connect to the real robot arm (socket error)!")
else:
self.get_logger().info(f"Successfully connected to the real robot arm: {self.robot_handle.id}")
self.get_logger().info(f"Successfully connected to the real robot arm: {self.arm_arg['arm_handle'].id}")
# if real robot connected, initialize following cfg
self.limitcfg()
self.armcfg()
self.udp_config()
self.robot = peripheral_cfg(self.robot, scissorgripper=self.scissorgripper, tools_in_ee = tools_in_ee)
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.robot = None
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]
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', joint_max).value,
"min_joint": self.declare_parameter('min_joint', joint_min).value,
"max_joint_speed": self.declare_parameter('max_joint_speed', 180.0).value,
"max_joint_acc": self.declare_parameter('max_joint_acc', 600.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_acc": self.declare_parameter('max_cartesian_acc', 1.5).value,
"max_cartesian_angular_acc": self.declare_parameter('max_cartesian_angular_acc', 2.0).value,
"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,
"joint_cmd_topic": self.declare_parameter('joint_cmd_topic', "/joint_cmd").value,
"joint_state_topic": self.declare_parameter('joint_state_topic', "/joint_sts").value,
"arm_enable_topic": self.declare_parameter('arm_enable_topic', "/arm_enable").value,
"tool_cmd_topic": self.declare_parameter('tool_cmd_topic', "/tool_cmd").value,
"initial_joint": self.declare_parameter('initial_joint', [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]).value,
}
sts = {
"joint_cmd": [0.0] * 7,
"joint_sts": [0.0] * 7,
"joint_speed": [0.0] * 7,
"arm_enable": False,
"tool_cmd": False,
"pose_sts": [0.0] * 6,
}
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:
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
self.get_logger().info(f'\n------------------Arm Disabled ----------------------')
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):
@ -91,15 +162,15 @@ class RMArmNode(Node):
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)
config = rm_realtime_push_config_t(3, True, self.config["port"], 0, self.config["host_ip"], custom)
self.config["arm"].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:
self.config["arm"].rm_realtime_arm_state_call_back(self.arm_state_callback)
while sum(abs(self.sts["pose_sts"])) < 0.001 or sum(abs(np.array(self.sts["joint_sts"]))) < 0.0001:
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):
# extract the pose data from the UDP data
@ -117,7 +188,7 @@ class RMArmNode(Node):
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 )
self.sts["pose_sts"] = pose
#--------------------------------------- joint 1*7 ----------------------------------
# Convert to dict safely
@ -134,11 +205,11 @@ class RMArmNode(Node):
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
self.sts["joint_sts"] = joint_position
self.sts["joint_speed"] = joint_speed
# ------------- 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:
self.get_logger().error(f"Error in UDP callback: {e}")
@ -147,29 +218,21 @@ class RMArmNode(Node):
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"])
self.movej(self.config["initial_joint"])
# Perform movel motion
self.movel(points["movel"])
self.initialized = True
self.get_logger().info('\n------------------- ready ------------------')
def limitcfg(self ):
def armcfg(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)
self.arm_arg['arm'].rm_set_avoid_singularity_mode(True)
self.arm_arg['arm'].rm_set_arm_max_line_speed(self.config["max_cartesian_speed"])
self.arm_arg['arm'].rm_set_arm_max_angular_speed(self.config["max_cartesian_angular_speed"])
self.arm_arg['arm'].rm_set_arm_max_line_acc(self.config["max_cartesian_acc"])
self.arm_arg['arm'].rm_set_arm_max_angular_acc(self.config["max_cartesian_angular_acc"])
for i in range(1,8):
self.robot.rm_set_joint_max_speed(i, 180)
self.robot.rm_set_joint_max_acc(i, 180)
self.robot.rm_set_joint_max_speed(i, self.config["max_joint_speed"])
self.robot.rm_set_joint_max_acc(i, self.config["max_joint_acc"])
def disconnect(self):
"""
@ -282,7 +345,7 @@ class ArmCtlNode(RMArmNode):
self.rm_move_arm
)
self.timer_count = 0
# create the subscriber to receive target pose
# create the subscriber to receive target joints
self.pose_sub = self.create_subscription(
JointState,
self.subname,