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

48
harvest_arm_ctrl/harvest_arm_ctrl/run_kinematics_arm.py
View File

@ -31,12 +31,22 @@ class KinematicsSolver(Node):
self.robot_kine_rm.add_tool_frames(self.config['tools_in_ee']) self.robot_kine_rm.add_tool_frames(self.config['tools_in_ee'])
self.robot_kine_rm.cfg_j_limit(min_j=self.config['min_joint'], max_j=self.config['max_joint'], rad_flag=False) self.robot_kine_rm.cfg_j_limit(min_j=self.config['min_joint'], max_j=self.config['max_joint'], rad_flag=False)
self.action_arm_pub = self.create_publisher(JointState, self.config["observation_topic"], 10) # pub, arm joint command
self.action_arm_pub = self.create_publisher(JointState, self.config["action_arm_topic"], 10)
# Create subcriber for target joint # sub, get the desired pose command in task frame
self.action_pose_sub = self.create_subscription(PoseStamped, self.config["action_pose_topic"], self.pose_callback, 10) self.action_pose_sub = self.create_subscription(PoseStamped, self.config["action_pose_topic"], self.pose_callback, 10)
# sub, get the desired tool command
self.action_tool_sub = self.create_subscription(Bool, self.config["action_tool_topic"], self.tool_callback, 10) self.action_tool_sub = self.create_subscription(Bool, self.config["action_tool_topic"], self.tool_callback, 10)
# sub, get the teleoperation enable command
self.action_enable_sub = self.create_subscription(Bool, self.config["action_arm_enable_topic"], self.arm_enable_callback, 10) self.action_enable_sub = self.create_subscription(Bool, self.config["action_arm_enable_topic"], self.arm_enable_callback, 10)
# sub, get the joint sts from arm
self.obser_arm_sub = self.create_subscription(JointState, self.config["observation_topic"], self.arm_observation_callback, 10)
self.timer = self.create_timer(
self.config["interval"],
self.kine_timer
)
self.get_logger().info(f'the config is {self.config}') self.get_logger().info(f'the config is {self.config}')
@ -65,13 +75,15 @@ class KinematicsSolver(Node):
"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, 1.0]).value,
"tool_name": tool_name, "tool_name": tool_name,
"tools_in_ee": tools_in_ee, "tools_in_ee": tools_in_ee,
"interval": self.declare_parameter('interval', 0.02).value, "interval": self.declare_parameter('interval', 0.02).value,
"urdf_path": self.declare_parameter('urdf_path', "/home/zl/Downloads/urdf_rm75/RM75-B.urdf").value, "urdf_path": self.declare_parameter('urdf_path', "/home/zl/Downloads/urdf_rm75/RM75-B.urdf").value,
"mesh_dir": self.declare_parameter('mesh_dir', "/home/zl/Downloads/urdf_rm75").value, "mesh_dir": self.declare_parameter('mesh_dir', "/home/zl/Downloads/urdf_rm75").value,
"action_arm_topic": self.declare_parameter('action_topic', "/action/arm").value, "action_arm_topic": self.declare_parameter('action_topic', "/action/arm").value,
"observation_topic": self.declare_parameter('observation_topic', "/observation/arm").value, "observation_topic": self.declare_parameter('observation_topic', "/observation/arm").value,
"action_pose_topic": self.declare_parameter('action_pose_topic', "/action/pose").value, "action_pose_topic": self.declare_parameter('action_pose_topic', "/action/pose").value,
@ -87,7 +99,7 @@ class KinematicsSolver(Node):
"tool_cmd": 0, "tool_cmd": 0,
"pose_sts": [0.0] * 6, "pose_sts": [0.0] * 6,
"pose_target_timestamp": 0.0, "pose_target_timestamp": 0.0,
"ik_ret": False, "ik_ret": 0,
} }
return config, sts return config, sts
@ -95,10 +107,13 @@ class KinematicsSolver(Node):
def update_msg(self): def update_msg(self):
msg = JointState() msg = JointState()
msg.header.stamp = self.get_clock().now().to_msg() msg.header.stamp = self.get_clock().now().to_msg()
msg.name = ['joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7'] msg.position = [float(self.sts["arm_enable"])] + [float(self.sts["ik_ret"])] + self.sts["joint_solved"] + [float(self.sts["tool_cmd"])]
msg.position = [float(self.sts["arm_enable"])] + self.sts["joint_solved"] + [float(self.sts["tool_cmd"])]
return msg return msg
def kine_timer(self):
msg = self.update_msg()
self.action_arm_pub.publish(msg)
def arm_enable_callback(self, msg): def arm_enable_callback(self, msg):
if msg.data: if msg.data:
self.sts["arm_enable"] = 1 self.sts["arm_enable"] = 1
@ -111,6 +126,12 @@ class KinematicsSolver(Node):
else: else:
self.sts["tool_cmd"] = 0 self.sts["tool_cmd"] = 0
def arm_observation_callback(self, msg):
if isinstance(msg, JointState):
self.sts["joint_pos_sts"] = list(msg.position[2:9])
else:
self.get_logger().error("Received message of unsupported type.")
def pose_callback(self, msg): def pose_callback(self, msg):
if isinstance(msg, PoseStamped): if isinstance(msg, PoseStamped):
timestamp = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9 timestamp = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
@ -136,22 +157,27 @@ class KinematicsSolver(Node):
ret_rm_ik, q_out = self.robot_kine_rm.inverse_kinematics(target_position, target_rpy, initial_guess, tool) ret_rm_ik, q_out = self.robot_kine_rm.inverse_kinematics(target_position, target_rpy, initial_guess, tool)
if ret_rm_ik == 0: if ret_rm_ik == 0:
self.sts["joint_solved"] = q_out self.sts["joint_solved"] = q_out
self.sts["ik_ret"] = True self.sts["ik_ret"] = 1
else: else:
ret_qp_ik, q_out = self.robot_kine_qp.inverse_kinematics(target_position= target_position, target_rpy=target_rpy, initial_guess=initial_guess, tool=tool, max_iter=300, work=work) ret_qp_ik, q_out = self.robot_kine_qp.inverse_kinematics(target_position= target_position, target_rpy=target_rpy, initial_guess=initial_guess, tool=tool, max_iter=300, work=work)
if ret_qp_ik == 0: if ret_qp_ik == 0:
self.sts["joint_solved"] = q_out self.sts["joint_solved"] = q_out
self.sts["ik_ret"] = True self.sts["ik_ret"] = 1
else: else:
self.sts["ik_ret"] = False self.sts["ik_ret"] = 0
def pose_transform(self, pos, quat, pos_frame, quat_frame): def pose_transform(self, pos, quat, pos_frame, quat_frame):
''' '''
pos: position in the task frame pos: position in the task frame
quat: quaternion in the task frame quat: quaternion in the task frame
pos_frame: position in the arm base frame pos_frame: task frame position in the arm base frame
quat_frame: quaternion in the arm base frame quat_frame: task framequaternion in the arm base frame
''' '''
pos_frame = np.asarray(pos_frame, dtype=np.float32)
quat_frame = np.asarray(quat_frame, dtype=np.float32)
pos = np.asarray(pos, dtype=np.float32)
quat = np.asarray(quat, dtype=np.float32)
pos_arm = rotate_vector_by_quaternion( quat=quat_frame, v=pos) + pos_frame pos_arm = rotate_vector_by_quaternion( quat=quat_frame, v=pos) + pos_frame
quat_arm = quaternion_multiply(quat_frame, quat) quat_arm = quaternion_multiply(quat_frame, quat)
return pos_arm, quat_arm return pos_arm, quat_arm

29
harvest_arm_rm/config/config.yaml
View File

@ -19,21 +19,24 @@
harvest_arm_ctrl: harvest_arm_ctrl:
ros__parameters: 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 qp_iteration: 200
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 # The tool parameters are defined in the following order:
# 3~6 orientation, quaternion, x, y, z, w # 0~2 position, x, y, z, m
# 7 mass, kg # 3~6 orientation, quaternion, x, y, z, w
# 8~10 center of mass, x, y, z, m # 7 mass, kg
# 8~10 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.66, 0.0, 0.0, 0.06
@ -49,6 +52,16 @@ harvest_arm_ctrl:
0.46, 0.0, 0.0, 0.06 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: [ 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

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. 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 = 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
except: # except:
self.get_logger().error(f"Tool name '{tool_name}' not found in parameters. Please check the configuration.") # 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 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_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_acc": self.declare_parameter('max_cartesian_acc', 1.5).value,
"max_cartesian_angular_acc": self.declare_parameter('max_cartesian_angular_acc', 2.0).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_name": tool_name,
"tool_offset": tool_offset_list, # "tool_offset": tool_offset_list,
"tool_names": self.declare_parameter('tool_names', ["scissor", "omnipic", "minisci", "no_tool"]).value, # "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, "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, "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 "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 = { sts = {
"joint_cmd": [0.0] * 7, "joint_cmd": [0.0] * 7,
"joint_target": [0.0] * 7, "joint_target": [0.0] * 7,
"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),
"ik_ret": int(0),
"tool_cmd": [int(0), int(0)], "tool_cmd": [int(0), int(0)],
"pose_sts": [0.0] * 6, "pose_sts": [0.0] * 6,
"timer_cnt": 0, "timer_cnt": 0,
@ -157,13 +165,14 @@ class RM_Arm(Node):
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["ik_ret"] = int(action_list[1])
self.sts["tool_cmd"][0] = int(action_list[8]) self.sts["joint_target"] = action_list[2:9]
self.sts["tool_cmd"][0] = int(action_list[9])
def arm_observation_relay_pub(self): 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= JointState()
msg0.header.stamp = self.get_clock().now().to_msg() msg0.header.stamp = self.get_clock().now().to_msg()
@ -173,7 +182,9 @@ class RM_Arm(Node):
self.action_rviz_pub.publish(msg0) self.action_rviz_pub.publish(msg0)
'''
publish the joint state to the harvest_arm_ctrl pkg.
'''
msg = JointState() msg = JointState()
msg.header.stamp = self.get_clock().now().to_msg() msg.header.stamp = self.get_clock().now().to_msg()
arm_real = self.arm_arg['arm_handle'].id arm_real = self.arm_arg['arm_handle'].id
@ -283,9 +294,9 @@ class RM_Arm(Node):
send joint command to the arm send joint command to the arm
""" """
############# movej --> 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_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"], 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_min= dis_vt - self.config["lmt_05att"] ,
a_max=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. arm (RoboticArm): Instance of the RoboticArm class.
config (dict): Configuration dictionary containing tool offsets. config (dict): Configuration dictionary containing tool offsets.
""" """
self.frame_cfg() # self.frame_cfg()
self.connector_cfg() self.connector_cfg()

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

@ -89,7 +89,7 @@ header:
nanosec: $(date +%N) nanosec: $(date +%N)
frame_id: '' frame_id: ''
name: [] 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: [] velocity: []
effort: [] effort: []
" --once " --once