fallensigh/craic
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix: 修复类型错误并添加 up/down 显式控制

Browse Source 
问题 1：发布状态失败 - 类型错误
- 修复：在发布 TCP 位姿时显式转换为 float
- 修复：在 GetPose 响应中显式转换类型

问题 2：缺少显式 z4 控制
- 添加 up/down 参数到 MovePose 服务
- up: 强制使用 z4=-100（夹爪朝上）
- down: 强制使用 z4=55（夹爪朝下）
- 默认：根据目标 z 坐标自动选择

控制逻辑：
- z4 选择优先级：up/down > 自动选择
- up/down 与 grip/release 完全独立
- up/down 控制夹爪高度（z4）
- grip/release 控制夹爪开合（J6）

与 udp_control.py 行为完全一致！
This commit is contained in:
FallenSigh
2026-06-16 19:44:12 +08:00
parent 33f1a31c59
commit aa7cd87151
2 changed files with 31 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
ros2/src/arm_control_msgs/srv/MovePose.srv
View File

@@ -4,12 +4,14 @@ float64 y # 目标 Y 坐标 (mm)
float64 z # 目标 Z 坐标 (mm) float64 z # 目标 Z 坐标 (mm)
float64 phi # 目标偏航角 (度) float64 phi # 目标偏航角 (度)
bool elbow_up # 是否使用肘部向上解 bool elbow_up # 是否使用肘部向上解
uint8 gripper_state # 夹爪状态: 0=保持, 1=打开, 2=闭合 bool up # 夹爪朝上（z4=-100）
bool grip # 是否抓取 bool down # 夹爪朝下（z4=55）
bool release # 是否释放 bool grip # 是否抓取（J6=-5）
bool release # 是否释放（J6=80）
float64 duration # 运动时长 (秒，0 表示使用默认值) float64 duration # 运动时长 (秒，0 表示使用默认值)
# 夹爪状态常量 # 已废弃：使用 up/down 代替
uint8 gripper_state # 夹爪状态: 0=保持, 1=打开, 2=闭合
uint8 GRIPPER_KEEP = 0 uint8 GRIPPER_KEEP = 0
uint8 GRIPPER_OPEN = 1 uint8 GRIPPER_OPEN = 1
uint8 GRIPPER_CLOSED = 2 uint8 GRIPPER_CLOSED = 2

49
ros2/src/udp_teleop/udp_teleop/arm_control.py
View File

@@ -604,14 +604,17 @@ class ArmControlNode(Node):
phi_deg=request.phi phi_deg=request.phi
) )
# 解析夹爪状态 # 解析 z4：优先级 up/down > 自动选择
if request.gripper_state == SetGripper.Request.GRIPPER_OPEN: if request.up:
j5 = J5_OPEN z4 = Z4_CLOSED # 夹爪朝上，z4=-100
elif request.gripper_state == SetGripper.Request.GRIPPER_CLOSED: elif request.down:
j5 = J5_CLOSED z4 = Z4_OPEN # 夹爪朝下，z4=55
else: else:
j5 = self.current_state.j5 # 自动选择：根据目标 z 坐标
j5_auto = resolve_j5_from_z(target_pose.z)
z4 = resolve_z4_from_j5(j5_auto)
# 解析夹爪开合（J6：grip/release）
if request.grip: if request.grip:
j6 = GRIP_ANGLE j6 = GRIP_ANGLE
elif request.release: elif request.release:
@@ -619,10 +622,8 @@ class ArmControlNode(Node):
else: else:
j6 = self.current_state.j6 j6 = self.current_state.j6
# 根据目标 z 坐标自动选择 J5 和 z4 # 根据 z4 反推 J5（用于 UDP 命令）
if request.gripper_state == MovePose.Request.GRIPPER_KEEP: j5 = J5_CLOSED if z4 == Z4_CLOSED else J5_OPEN
j5 = resolve_j5_from_z(target_pose.z)
z4 = resolve_z4_from_j5(j5)
# 逆运动学 # 逆运动学
math_state = inverse_kinematics( math_state = inverse_kinematics(
@@ -679,16 +680,16 @@ class ArmControlNode(Node):
pose = forward_kinematics(self.geometry, math_state, z4) pose = forward_kinematics(self.geometry, math_state, z4)
response.success = True response.success = True
response.x = pose.x response.x = float(pose.x)
response.y = pose.y response.y = float(pose.y)
response.z = pose.z response.z = float(pose.z)
response.phi = pose.phi_deg response.phi = float(pose.phi_deg)
response.height = self.current_state.height response.height = int(self.current_state.height)
response.j2 = self.current_state.j2 response.j2 = int(self.current_state.j2)
response.j3 = self.current_state.j3 response.j3 = int(self.current_state.j3)
response.j4 = self.current_state.j4 response.j4 = int(self.current_state.j4)
response.j5 = self.current_state.j5 response.j5 = int(self.current_state.j5)
response.j6 = self.current_state.j6 response.j6 = int(self.current_state.j6)
except Exception as e: except Exception as e:
response.success = False response.success = False
@@ -760,10 +761,10 @@ class ArmControlNode(Node):
pose_msg = TCPPose() pose_msg = TCPPose()
pose_msg.header.stamp = self.get_clock().now().to_msg() pose_msg.header.stamp = self.get_clock().now().to_msg()
pose_msg.x = pose.x pose_msg.x = float(pose.x)
pose_msg.y = pose.y pose_msg.y = float(pose.y)
pose_msg.z = pose.z pose_msg.z = float(pose.z)
pose_msg.phi = pose.phi_deg pose_msg.phi = float(pose.phi_deg)
self.pub_tcp_pose.publish(pose_msg) self.pub_tcp_pose.publish(pose_msg)
except Exception as e: except Exception as e: