feat: 将机械臂坐标系改为 z 轴朝上
主要修改: - 坐标系统: z=0 为顶部改为底部，z 轴朝上 - 高度范围: d1 ∈ [-290, 0] mm (-290=底部, 0=顶部) - Z4 偏移: 80.0 (原 -80.0), Z4_OPEN=55, Z4_CLOSED=-100 - 运动学: 更新 tcp_to_joint4_center 和 forward_kinematics 的 z 坐标变换 - 夹爪逻辑: 更新 resolve_gripper_from_z 的高度判断范围 - 插值优化: 先完成末端运动，再处理 grip/release - 移除所有坐标转换中的负号，统一为正向 z 轴朝上系统
This commit is contained in:
@@ -27,8 +27,8 @@ from pathlib import Path
|
||||
DEFAULT_UDP_IP = "192.168.4.1"
|
||||
DEFAULT_UDP_PORT = 8888
|
||||
|
||||
DEFAULT_HEIGHT_MIN = 0 # 用户坐标系：顶部，单位 mm
|
||||
DEFAULT_HEIGHT_MAX = 290 # 用户坐标系：底部，单位 mm
|
||||
DEFAULT_HEIGHT_MIN = -290 # 内部 d1 坐标：底部，单位 mm（z 轴朝上）
|
||||
DEFAULT_HEIGHT_MAX = 0 # 内部 d1 坐标：顶部，单位 mm（z 轴朝上）
|
||||
DEFAULT_JOINT_MIN = -180
|
||||
DEFAULT_JOINT_MAX = 180
|
||||
DEFAULT_J2_MIN = -110
|
||||
@@ -39,21 +39,21 @@ DEFAULT_J4_MIN = -90
|
||||
DEFAULT_J4_MAX = 130
|
||||
J5_OPEN = 81
|
||||
J5_CLOSED = -100
|
||||
Z4_OPEN = -55
|
||||
Z4_CLOSED = 100
|
||||
Z4_OPEN = 55
|
||||
Z4_CLOSED = -100
|
||||
DEFAULT_FIXED_J5 = J5_OPEN
|
||||
DEFAULT_FIXED_J6 = 0
|
||||
# ==== 抓取/释放（由 J6 控制）====
|
||||
# 请填写实际角度值：
|
||||
GRIP_ANGLE = -5 # TODO: 填写抓取时 J6 的角度
|
||||
RELEASE_ANGLE = 30 # TODO: 填写释放时 J6 的角度
|
||||
DEFAULT_FIXED_J6 = RELEASE_ANGLE
|
||||
DEFAULT_ZERO_J2 = 3
|
||||
DEFAULT_ZERO_J3 = 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
|
||||
DEFAULT_INTERP_DURATION = 1.0
|
||||
DEFAULT_INTERP_RATE = 20.0
|
||||
STATE_FILE = Path(__file__).with_name(".udp_control_state.json")
|
||||
@@ -133,7 +133,7 @@ class Joint4Center:
|
||||
|
||||
def default_command_state() -> ArmJointState:
|
||||
return ArmJointState(
|
||||
height=-DEFAULT_HEIGHT_MIN,
|
||||
height=-DEFAULT_HEIGHT_MAX,
|
||||
j2=DEFAULT_ZERO_J2,
|
||||
j3=DEFAULT_ZERO_J3,
|
||||
j4=DEFAULT_ZERO_J4,
|
||||
@@ -177,7 +177,7 @@ def tcp_to_joint4_center(geometry: ArmGeometry, pose: ArmPose) -> Joint4Center:
|
||||
return Joint4Center(
|
||||
x=pose.x - geometry.x4 * math.cos(phi),
|
||||
y=pose.y - geometry.x4 * math.sin(phi),
|
||||
z=pose.z - geometry.z4,
|
||||
z=pose.z + geometry.z4,
|
||||
)
|
||||
|
||||
|
||||
@@ -196,7 +196,7 @@ def forward_kinematics(geometry: ArmGeometry, state: ArmMathState) -> ArmPose:
|
||||
)
|
||||
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 - geometry.z4
|
||||
return ArmPose(x=x, y=y, z=z, phi_deg=math.degrees(phi))
|
||||
|
||||
|
||||
@@ -266,8 +266,8 @@ def math_to_command_state(
|
||||
j6: int,
|
||||
) -> ArmJointState:
|
||||
return ArmJointState(
|
||||
height=-clamp_int(
|
||||
round(-math_state.d1),
|
||||
height=clamp_int(
|
||||
round(math_state.d1),
|
||||
limits.height_min,
|
||||
limits.height_max,
|
||||
"height(cmd)",
|
||||
@@ -302,7 +302,7 @@ def load_cached_command_state(limits: ArmLimits) -> ArmJointState | None:
|
||||
try:
|
||||
payload = json.loads(STATE_FILE.read_text(encoding="utf-8"))
|
||||
return ArmJointState(
|
||||
height=-clamp_int(-payload["height"], limits.height_min, limits.height_max, "height(cmd)"),
|
||||
height=clamp_int(payload["height"], limits.height_min, limits.height_max, "height(cmd)"),
|
||||
j2=clamp_int(payload["j2"], limits.j2_min, limits.j2_max, "J2"),
|
||||
j3=clamp_int(payload["j3"], limits.j3_min, limits.j3_max, "J3"),
|
||||
j4=clamp_int(payload["j4"], limits.j4_min, limits.j4_max, "J4"),
|
||||
@@ -401,7 +401,11 @@ def build_pose_command_path(
|
||||
)
|
||||
return [math_to_command_state(math_state, zero_offsets, limits, j5, j6)]
|
||||
|
||||
# Interpolate pose first (movement)
|
||||
path: list[ArmJointState] = []
|
||||
start_command_j5 = None
|
||||
start_command_j6 = None
|
||||
|
||||
for step_index in range(1, steps + 1):
|
||||
t = step_index / steps
|
||||
pose = interpolate_pose(start_pose, target_pose, t)
|
||||
@@ -413,7 +417,35 @@ def build_pose_command_path(
|
||||
j5=j5,
|
||||
j6=j6,
|
||||
)
|
||||
path.append(math_to_command_state(math_state, zero_offsets, limits, j5, j6))
|
||||
command = math_to_command_state(math_state, zero_offsets, limits, j5, j6)
|
||||
|
||||
# Store the first command's j5/j6 values
|
||||
if start_command_j5 is None:
|
||||
start_command_j5 = command.j5
|
||||
start_command_j6 = command.j6
|
||||
|
||||
# For all movement steps, use the starting j5/j6 values
|
||||
path.append(ArmJointState(
|
||||
height=command.height,
|
||||
j2=command.j2,
|
||||
j3=command.j3,
|
||||
j4=command.j4,
|
||||
j5=start_command_j5,
|
||||
j6=start_command_j6,
|
||||
))
|
||||
|
||||
# Add final grip/release adjustment if j5 or j6 changed
|
||||
final = path[-1]
|
||||
if final.j5 != j5 or final.j6 != j6:
|
||||
path.append(ArmJointState(
|
||||
height=final.height,
|
||||
j2=final.j2,
|
||||
j3=final.j3,
|
||||
j4=final.j4,
|
||||
j5=j5,
|
||||
j6=j6,
|
||||
))
|
||||
|
||||
return path
|
||||
|
||||
|
||||
@@ -484,13 +516,13 @@ def build_parser() -> argparse.ArgumentParser:
|
||||
"--height-min",
|
||||
type=int,
|
||||
default=DEFAULT_HEIGHT_MIN,
|
||||
help=f"高度下限 (用户坐标, mm)，默认 {DEFAULT_HEIGHT_MIN}",
|
||||
help=f"高度下限 (内部 d1 坐标, mm)，默认 {DEFAULT_HEIGHT_MIN}",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--height-max",
|
||||
type=int,
|
||||
default=DEFAULT_HEIGHT_MAX,
|
||||
help=f"高度上限 (用户坐标, mm)，默认 {DEFAULT_HEIGHT_MAX}",
|
||||
help=f"高度上限 (内部 d1 坐标, mm)，默认 {DEFAULT_HEIGHT_MAX}",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--joint-min",
|
||||
@@ -571,7 +603,7 @@ def build_parser() -> argparse.ArgumentParser:
|
||||
action="store_true",
|
||||
help="不读取或更新上次发送的关节命令缓存",
|
||||
)
|
||||
joints.add_argument("--height", type=int, required=True, help="升降高度 mm (0=顶部, 290=底部)")
|
||||
joints.add_argument("--height", type=int, required=True, help="内部 d1 坐标 mm (-290=底部, 0=顶部，z 轴朝上)")
|
||||
joints.add_argument("--j2", type=int, required=True, help="UDP 指令里的 J2 命令值")
|
||||
joints.add_argument("--j3", type=int, required=True, help="UDP 指令里的 J3 命令值")
|
||||
joints.add_argument("--j4", type=int, required=True, help="UDP 指令里的 J4 命令值")
|
||||
@@ -617,7 +649,7 @@ def build_parser() -> argparse.ArgumentParser:
|
||||
)
|
||||
pose.add_argument("--x", type=float, required=True, help="TCP X 坐标 mm")
|
||||
pose.add_argument("--y", type=float, required=True, help="TCP Y 坐标 mm")
|
||||
pose.add_argument("--z", type=float, required=True, help="TCP Z 坐标 mm (0=顶部, 正值向下)")
|
||||
pose.add_argument("--z", type=float, required=True, help="TCP Z 坐标 mm (-290=底部, 0=顶部, z 轴朝上)")
|
||||
pose.add_argument("--phi", type=float, required=True, help="TCP 偏航角，单位度；等于 J2+J3+J4")
|
||||
pose.add_argument("--l1", type=float, default=DEFAULT_L1, help=f"J2 到 J3 的连杆长度 mm (默认 {DEFAULT_L1})")
|
||||
pose.add_argument("--l2", type=float, default=DEFAULT_L2, help=f"J3 到 J4 的连杆长度 mm (默认 {DEFAULT_L2})")
|
||||
@@ -682,13 +714,13 @@ def resolve_z4(up: bool) -> float:
|
||||
|
||||
|
||||
def resolve_gripper_from_z(z: float) -> bool:
|
||||
"""Auto-select gripper state based on TCP z coordinate (user mm).
|
||||
"""Auto-select gripper state based on TCP z coordinate (user mm, z-up).
|
||||
|
||||
- z in [55, 345] -> down (gripper open, J5=81, Z4=-55)
|
||||
- z in [-100, 190] -> up (gripper closed, J5=-100, Z4=100)
|
||||
- Overlap [55, 190] -> down (prefer down in intersection)
|
||||
- z in [-345, -55] -> down (gripper open, J5=81, Z4=55)
|
||||
- z in [-190, 110] -> up (gripper closed, J5=-100, Z4=-100)
|
||||
- Overlap [-190, -55] -> down (prefer down in intersection)
|
||||
"""
|
||||
return z < 55 # True = up, False = down
|
||||
return z > -55 # True = up, False = down
|
||||
|
||||
|
||||
def resolve_j6(grip: bool, release: bool, fallback: int) -> int:
|
||||
@@ -702,7 +734,7 @@ def resolve_j6(grip: bool, release: bool, fallback: int) -> int:
|
||||
def state_from_joint_args(args: argparse.Namespace, limits: ArmLimits) -> ArmJointState:
|
||||
up = args.up if args.up is not None else False
|
||||
return ArmJointState(
|
||||
height=-clamp_int(args.height, limits.height_min, limits.height_max, "height(cmd)"),
|
||||
height=clamp_int(args.height, limits.height_min, limits.height_max, "height(cmd)"),
|
||||
j2=clamp_int(args.j2, limits.j2_min, limits.j2_max, "J2"),
|
||||
j3=clamp_int(args.j3, limits.j3_min, limits.j3_max, "J3"),
|
||||
j4=clamp_int(args.j4, limits.j4_min, limits.j4_max, "J4"),
|
||||
@@ -714,7 +746,7 @@ def state_from_joint_args(args: argparse.Namespace, limits: ArmLimits) -> ArmJoi
|
||||
def print_joint_summary(state: ArmJointState) -> None:
|
||||
print(
|
||||
"UDP command joints:",
|
||||
f"height={-state.height}mm",
|
||||
f"height={state.height}mm",
|
||||
f"J2={state.j2}",
|
||||
f"J3={state.j3}",
|
||||
f"J4={state.j4}",
|
||||
@@ -728,7 +760,7 @@ def print_pose_summary(pose: ArmPose) -> None:
|
||||
"TCP pose:",
|
||||
f"x={pose.x:.3f}mm",
|
||||
f"y={pose.y:.3f}mm",
|
||||
f"z={-pose.z:.3f}mm",
|
||||
f"z={pose.z:.3f}mm",
|
||||
f"phi={pose.phi_deg:.3f}deg",
|
||||
)
|
||||
|
||||
@@ -738,14 +770,14 @@ def print_joint4_center_summary(center: Joint4Center) -> None:
|
||||
"J4 center:",
|
||||
f"x={center.x:.3f}mm",
|
||||
f"y={center.y:.3f}mm",
|
||||
f"z={-center.z:.3f}mm",
|
||||
f"z={center.z:.3f}mm",
|
||||
)
|
||||
|
||||
|
||||
def print_math_summary(state: ArmMathState) -> None:
|
||||
print(
|
||||
"Math joints:",
|
||||
f"d1={-state.d1:.3f}mm",
|
||||
f"d1={state.d1:.3f}mm",
|
||||
f"J2={state.theta2_deg:.3f}",
|
||||
f"J3={state.theta3_deg:.3f}",
|
||||
f"J4={state.theta4_deg:.3f}",
|
||||
@@ -819,7 +851,7 @@ def main() -> int:
|
||||
target_pose = ArmPose(
|
||||
x=args.x,
|
||||
y=args.y,
|
||||
z=-args.z,
|
||||
z=args.z,
|
||||
phi_deg=args.phi,
|
||||
)
|
||||
joint4_center = tcp_to_joint4_center(geometry, target_pose)
|
||||
|
||||
Reference in New Issue
Block a user