From 33f1a31c59084709da82f307043f65487197edf1 Mon Sep 17 00:00:00 2001 From: FallenSigh Date: Tue, 16 Jun 2026 19:37:42 +0800 Subject: [PATCH] =?UTF-8?q?feat(arm=5Fcontrol):=20=E5=AE=9E=E7=8E=B0?= =?UTF-8?q?=E5=8A=A8=E6=80=81=20z4=20=E8=87=AA=E5=8A=A8=E9=80=82=E9=85=8D?= =?UTF-8?q?=E6=9C=BA=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 问题根源： - udp_control.py 使用动态 z4（根据目标 z 自动选择） - arm_control.py 使用固定 z4=80 - 导致相同目标位姿在两个工具中行为不一致 解决方案： - 添加 resolve_j5_from_z(): 根据目标 z 自动选择 J5 - 添加 resolve_z4_from_j5(): 根据 J5 确定 z4 值 - 更新正/逆运动学接受动态 z4 参数 - 自动适配规则： * z > -55mm: J5=-100, z4=-100 (闭合, 工作范围 [-190, 110]) * z ≤ -55mm: J5=81, z4=55 (张开, 工作范围 [-345, -55]) 现在 arm_control 与 udp_control.py 行为一致！ --- ros2/README.md | 3 + ros2/src/udp_teleop/udp_teleop/arm_control.py | 73 ++++++++++++++++--- 2 files changed, 64 insertions(+), 12 deletions(-) diff --git a/ros2/README.md b/ros2/README.md index 60f66e6..d0913c3 100644 --- a/ros2/README.md +++ b/ros2/README.md @@ -23,6 +23,9 @@ CRAIC 项目的 ROS 2 机械臂控制和视觉抓取系统。 **功能**： - 关节空间和笛卡尔空间运动控制 - 完整的逆运动学和正运动学 +- **自动 z4 适配**：根据目标 z 坐标自动选择夹爪状态 + - `z > -55mm`: J5=-100（闭合），z4=-100mm，工作范围 z ∈ [-190, 110]mm + - `z ≤ -55mm`: J5=81（张开），z4=55mm，工作范围 z ∈ [-345, -55]mm - UDP 通信（与 ESP32） - 状态发布（10Hz） diff --git a/ros2/src/udp_teleop/udp_teleop/arm_control.py b/ros2/src/udp_teleop/udp_teleop/arm_control.py index 03cf8dc..02f1eb7 100644 --- a/ros2/src/udp_teleop/udp_teleop/arm_control.py +++ b/ros2/src/udp_teleop/udp_teleop/arm_control.py @@ -50,6 +50,10 @@ J5_OPEN = 81 J5_CLOSED = -100 DEFAULT_FIXED_J5 = J5_OPEN +# Z4 值根据夹爪状态变化 +Z4_OPEN = 55 # 夹爪张开（J5=81） +Z4_CLOSED = -100 # 夹爪闭合（J5=-100） + GRIP_ANGLE = -5 RELEASE_ANGLE = 80 DEFAULT_FIXED_J6 = RELEASE_ANGLE @@ -61,7 +65,7 @@ DEFAULT_ZERO_J4 = 25 DEFAULT_L1 = 125.0 DEFAULT_L2 = 125.0 DEFAULT_X4 = 110.0 -DEFAULT_Z4 = 80.0 +DEFAULT_Z4 = 80.0 # 仅用于配置默认值，实际使用动态 z4 DEFAULT_INTERP_DURATION = 1.0 DEFAULT_INTERP_RATE = 20.0 @@ -165,13 +169,37 @@ def normalize_angle_deg(angle_deg: float) -> float: return normalized -def forward_kinematics(geometry: ArmGeometry, state: ArmMathState) -> ArmPose: - """正运动学：关节角度 → TCP 位姿""" +def resolve_z4_from_j5(j5: int) -> float: + """根据 J5 状态确定 z4 值 + + - J5 > 0 (张开): z4 = 55mm + - J5 < 0 (闭合): z4 = -100mm + """ + return Z4_OPEN if j5 > 0 else Z4_CLOSED + + +def resolve_j5_from_z(z: float) -> int: + """根据目标 z 坐标自动选择夹爪状态 + + - z > -55: 使用闭合状态 (J5=-100, z4=-100) + - z <= -55: 使用张开状态 (J5=81, z4=55) + """ + return J5_CLOSED if z > -55 else J5_OPEN + + +def forward_kinematics(geometry: ArmGeometry, state: ArmMathState, z4: float) -> ArmPose: + """正运动学：关节角度 → TCP 位姿 + + Args: + geometry: 机械臂几何参数 + state: 数学坐标系的关节状态 + z4: J4 到 TCP 的 Z 偏移（根据 J5 状态确定） + """ theta2 = math.radians(state.theta2_deg) theta3 = math.radians(state.theta3_deg) theta4 = math.radians(state.theta4_deg) phi = theta2 + theta3 + theta4 - + j4_center_x = ( geometry.l1 * math.cos(theta2) + geometry.l2 * math.cos(theta2 + theta3) @@ -180,11 +208,11 @@ def forward_kinematics(geometry: ArmGeometry, state: ArmMathState) -> ArmPose: geometry.l1 * math.sin(theta2) + geometry.l2 * math.sin(theta2 + theta3) ) - + x = j4_center_x + geometry.x4 * math.cos(phi) y = j4_center_y + geometry.x4 * math.sin(phi) - z = state.d1 - geometry.z4 - + z = state.d1 - z4 # 使用动态 z4 + return ArmPose(x=x, y=y, z=z, phi_deg=math.degrees(phi)) @@ -195,14 +223,25 @@ def inverse_kinematics( elbow_up: bool, j5: int, j6: int, + z4: float, ) -> ArmMathState: - """逆运动学：TCP 位姿 → 关节角度""" + """逆运动学：TCP 位姿 → 关节角度 + + Args: + geometry: 机械臂几何参数 + pose: 目标 TCP 位姿 + limits: 关节限位 + elbow_up: 肘部朝上/朝下 + j5: J5 角度 + j6: J6 角度 + z4: J4 到 TCP 的 Z 偏移（根据 J5 状态确定） + """ # 计算 J4 中心位置 phi = math.radians(pose.phi_deg) j4_x = pose.x - geometry.x4 * math.cos(phi) j4_y = pose.y - geometry.x4 * math.sin(phi) - j4_z = pose.z + geometry.z4 - + j4_z = pose.z + z4 # 使用动态 z4 + d1 = j4_z # 计算平面距离 @@ -580,6 +619,11 @@ class ArmControlNode(Node): else: j6 = self.current_state.j6 + # 根据目标 z 坐标自动选择 J5 和 z4 + if request.gripper_state == MovePose.Request.GRIPPER_KEEP: + j5 = resolve_j5_from_z(target_pose.z) + z4 = resolve_z4_from_j5(j5) + # 逆运动学 math_state = inverse_kinematics( geometry=self.geometry, @@ -588,6 +632,7 @@ class ArmControlNode(Node): elbow_up=request.elbow_up, j5=j5, j6=j6, + z4=z4, ) # 转换为命令状态 @@ -628,7 +673,10 @@ class ArmControlNode(Node): return response math_state = command_to_math_state(self.current_state, self.zero_offsets) - pose = forward_kinematics(self.geometry, math_state) + + # 根据当前 J5 状态确定 z4 + z4 = resolve_z4_from_j5(self.current_state.j5) + pose = forward_kinematics(self.geometry, math_state, z4) response.success = True response.x = pose.x @@ -707,7 +755,8 @@ class ArmControlNode(Node): # 计算并发布 TCP 位姿 math_state = command_to_math_state(self.current_state, self.zero_offsets) - pose = forward_kinematics(self.geometry, math_state) + z4 = resolve_z4_from_j5(self.current_state.j5) + pose = forward_kinematics(self.geometry, math_state, z4) pose_msg = TCPPose() pose_msg.header.stamp = self.get_clock().now().to_msg()