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

add kine pkg pub/sub

Browse Source
This commit is contained in:
LiuzhengSJ
2026-07-16 17:11:56 +01:00
parent 120a252ccc
commit 9119837294
4 changed files with 86 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

29
harvest_arm_rm/config/config.yaml
View File

@ -19,21 +19,24 @@
harvest_arm_ctrl:
ros__parameters:
pos_task_in_base = [-0.1, 0.2, -0.025]
quat_task_in_base = [-0.5, -0.5, 0.5, 0.5]
work_frame: ["base", "task"]
tool_names: ["scissor", "omnipic", "minisci", "vacuum", "no_tool"]
# set the position and quaternion of the task frame in the base frame.
arm_installation : [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
tool_names: ["scissor", "omnipic", "minisci", "camera", "gripper_bh", "no_tool"]
qp_iteration: 200
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~10 center of mass, x, y, z, m
# The tool parameters are defined in the following order:
# 0~2 position, x, y, z, m
# 3~6 orientation, quaternion, x, y, z, w
# 7 mass, kg
# 8~10 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
@ -49,6 +52,16 @@ harvest_arm_ctrl:
0.46, 0.0, 0.0, 0.06
]
tools_in_ee.camera: [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0,
0.0, 0.0, 0.0, 0.0
]
tools_in_ee.gripper_bh: [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0,
0.0, 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

43
harvest_arm_rm/harvest_arm_rm/run_realman_arm.py
View File

@ -66,11 +66,11 @@ class RM_Arm(Node):
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', "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
except:
self.get_logger().error(f"Tool name '{tool_name}' not found in parameters. Please check the configuration.")
# tool_name_param = 'tools_in_ee.' + tool_name
# try:
# tool_offset_list = self.declare_parameter(tool_name_param, [0.0] * 11).value
# except:
# self.get_logger().error(f"Tool name '{tool_name}' not found in parameters. Please check the configuration.")
acc_lmt = self.declare_parameter('max_joint_acc', 600.0).value
@ -91,12 +91,12 @@ class RM_Arm(Node):
"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,
"bound_cartesian": self.declare_parameter('bound_cartesian', [-0.3, -0.5, -0.1, 0.3, 0.5, 0.6]).value,
# "bound_cartesian": self.declare_parameter('bound_cartesian', [-0.3, -0.5, -0.1, 0.3, 0.5, 0.6]).value,
"arm_installation": self.declare_parameter('arm_installation', [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]).value,
# "arm_installation": self.declare_parameter('arm_installation', [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]).value,
"tool_name": tool_name,
"tool_offset": tool_offset_list,
"tool_names": self.declare_parameter('tool_names', ["scissor", "omnipic", "minisci", "no_tool"]).value,
# "tool_offset": tool_offset_list,
# "tool_names": self.declare_parameter('tool_names', ["scissor", "omnipic", "minisci", "no_tool"]).value,
"initial_joint": self.declare_parameter('initial_joint', [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]).value,
"kp_j": self.declare_parameter('kp_j', 1.0).value,
@ -113,12 +113,20 @@ class RM_Arm(Node):
"arm_tool_io_mode": self.declare_parameter('arm_tool_io_mode', [1, 1]).value
}
# joint_cmd: the joint command to the arm, unit: degree;
# joint_target: the joint target from the /action/arm, unit: degree;
# joint_pos_sts: the joint position from the arm udp (real robot) or from the /observation/arm_rviz (if no real robot), unit: degree;
# joint_speed_sts: the joint speed from the arm udp (real robot) or from the /observation/arm_rviz (if no real robot), unit: degree/s;
# arm_enable: 1 or 0, the arm enable status from the /action/arm;
# tool_cmd: the tool command from the /action/arm, 1 or 0;
# pose_sts: the pose from the arm udp (real robot), unit: m and rad;
sts = {
"joint_cmd": [0.0] * 7,
"joint_target": [0.0] * 7,
"joint_pos_sts": [0.0] * 7,
"joint_speed_sts": [0.0] * 7,
"arm_enable": int(0),
"ik_ret": int(0),
"tool_cmd": [int(0), int(0)],
"pose_sts": [0.0] * 6,
"timer_cnt": 0,
@ -157,13 +165,14 @@ class RM_Arm(Node):
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"][0] = int(action_list[8])
self.sts["ik_ret"] = int(action_list[1])
self.sts["joint_target"] = action_list[2:9]
self.sts["tool_cmd"][0] = int(action_list[9])
def arm_observation_relay_pub(self):
"""
Publish the current state of the robotic arm.
Publish the current state of the robotic arm to rviz.
"""
msg0= JointState()
msg0.header.stamp = self.get_clock().now().to_msg()
@ -173,7 +182,9 @@ class RM_Arm(Node):
self.action_rviz_pub.publish(msg0)
'''
publish the joint state to the harvest_arm_ctrl pkg.
'''
msg = JointState()
msg.header.stamp = self.get_clock().now().to_msg()
arm_real = self.arm_arg['arm_handle'].id
@ -283,9 +294,9 @@ class RM_Arm(Node):
send joint command to the arm
"""
############# movej --> arm
if self.sts["arm_enable"] == 1:
if self.sts["arm_enable"] == 1 and self.sts["ik_ret"] == 1:
dis_max_vt = self.config["max_joint_speed"] * self.config["interval"]
dis_vt = np.array(self.sts["joint_speed_sts"]) * self.config["interval"] * 0.75
dis_vt = np.array(self.sts["joint_speed_sts"]) * self.config["interval"] * 0.8
d_j = np.clip(a=(np.array(self.sts["joint_target"]) - np.array(self.sts["joint_pos_sts"])) * self.config["kp_j"],
a_min= dis_vt - self.config["lmt_05att"] ,
a_max=dis_vt + self.config["lmt_05att"])
@ -314,7 +325,7 @@ class RM_Arm(Node):
arm (RoboticArm): Instance of the RoboticArm class.
config (dict): Configuration dictionary containing tool offsets.
"""
self.frame_cfg()
# self.frame_cfg()
self.connector_cfg()

2
harvest_arm_rm/launch/rm_arm_rviz.launch.py
View File

@ -89,7 +89,7 @@ header:
nanosec: $(date +%N)
frame_id: ''
name: []
position: [1.0, 82.6, -20.28, 32.15, -50.28, -17.28, -64.055, 20.62, 1.0]
position: [1.0, 1.0, 82.6, -20.28, 32.15, -50.28, -17.28, -64.055, 20.62, 1.0]
velocity: []
effort: []
" --once