末端执行器

末端执行器

P7 的 EEF 控制模式独立于机械臂主体的 planning、Servo 或 MIT 模式。发送夹爪或其他末端执行器目标 前，先确认设备存在、运行时自由度、EEF 模式和该产品型号的单位契约。

确认 EEF 是否可用

组合读取以下信息：

• get_firmware_info().eef_type：整机报告的 EEF 型号；空字符串表示未提供型号；
• get_eef_mode()：服务是否识别 EEF、当前控制模式和活动 EEF controller；
• get_eef_joint_state()：状态存在时，可用数组长度核对运行时 eef_dof；线性 EEF 的位置和速度 已按 SDK 公开的 mm、mm/s 返回。

move_eef() 会从运行时信息解析 eef_dof。自由度为 0 时返回 False；所有适用的目标和选项数组 都必须与该自由度完全一致。不要假设每个 EEF 都是单轴夹爪。

get_eef_mode()

client.get_eef_mode() -> dict[str, object] | None

gRPC 和 DDS 均支持，不需要控制权，也不会切换模式或触发运动。成功时返回：

键	类型	含义
has_eef	bool	当前运行时是否报告已配置 EEF
current_mode	int	0 idle、1 MIT、2 CSP；其他值按未知处理
current_mode_name	str	idle、mit、csp 或 unknown(n)
active_eef_controller_id	int	当前活动 EEF controller 的运行时编号
active_eef_controller_name	str	当前活动 EEF controller 名称；不是稳定枚举

通信失败时返回 None。has_eef=False、未知模式或空 controller 名称都应阻止后续 EEF 运动，而不是 用默认设备类型继续。

当前 get_eef_mode() 不会把查询结果写入 SDK 的本地 EEF 模式缓存。即使返回 csp 或 mit，当前 客户端仍需在取得控制权后成功调用一次 switch_eef_control_mode()；查询不能代替切换。

EEF 控制模式

EEFControlMode	值	move_eef() 使用的目标	EEFMoveOptions 生效字段
idle	0	不执行 EEF 控制命令	无；调用返回 False
mit	1	position、当前 EEF speed、torque、kp、kd	torque、kp、kd；blocking 被忽略
csp	2	position、当前 EEF speed、current threshold	eff、blocking

MIT 是面向专项控制方案的低层入口。参数范围不等于稳定或安全的推荐值；缺少所用 EEF 的标定参数和 单位说明时，不要启用。

switch_eef_control_mode()

client.switch_eef_control_mode(
    target_mode: EEFControlMode,
    timeout_ms: int = 6000,
) -> bool

gRPC 和 DDS 均支持。调用要求有效控制租约；传入非 EEFControlMode 值会返回 False。成功后 SDK 才 更新本地 EEF 模式缓存。

每个新建或重连的控制客户端都应显式执行这一步。其他客户端在板端完成的模式切换不会可靠建立当前 客户端的本地缓存。

机械臂主体和 EEF 是两个模式维度：把机械臂切到 Controller.idle 不等于把 EEF 切到 EEFControlMode.idle，反之亦然。正常结束时分别确认两者的停止状态，再释放控制权。

set_eef_speed()

client.set_eef_speed(eef_speed: float) -> bool

该方法只更新当前 SDK 客户端保存的 EEF 目标速度，随后由 move_eef() 使用；它本身不发运动请求， 也不需要控制权。

项目	当前规则
单位	mm/s
允许范围	[10, 1000] mm/s
客户端初始值	1000 mm/s
越界行为	返回 False，不更新缓存

当前范围比较没有显式拒绝 NaN，非有限值可能进入客户端缓存并在后续命令中发送。调用前先检查：

import math

eef_speed = 50.0
if not math.isfinite(eef_speed) or not 10.0 <= eef_speed <= 1000.0:
    raise ValueError("eef_speed must be finite and within [10, 1000] mm/s")
if not client.set_eef_speed(eef_speed):
    raise RuntimeError("EEF speed was rejected")

1000 mm/s 是 SDK 默认值，不是对所有 EEF、负载和工位的推荐速度。应采用对应整机资料批准的速度。

move_eef()

client.move_eef(
    pos: list[float],
    options: EEFMoveOptions,
    timeout_ms: int = 1000,
) -> bool

gRPC 和 DDS 均支持。方法需要控制权。模式缓存为空时 SDK 会尝试读取当前模式，但当前实现不会用 查询结果更新缓存；因此发送前必须由当前客户端成功调用 switch_eef_control_mode()。

参数	类型	当前公开约定
pos	list[float]	长度必须等于运行时 eef_dof；SDK 入口按 mm 接收并换算后发送
options	EEFMoveOptions	CSP 使用 eff；MIT 使用 torque、kp、kd
timeout_ms	int	route 调用等待上限，单位 ms；不是 EEF 动作时长

发送前对 pos 和当前模式使用的全部 options 数组逐项执行 math.isfinite()。当前 SDK 只统一检查 长度；对于已知夹爪，NaN 或无穷位置甚至可能被夹紧到行程端点，而不是返回 False。

SDK 会把已知夹爪型号的 pos 夹紧到内部行程表后发送，而不是拒绝越界目标；未知型号不做位置范围 检查。当前 G2P 的 SDK 上限为 95 mm，整机产品配置中的关节上限为 0.096 m。两个来源尚未统一， 因此 95 mm 只能视为当前 SDK 的兼容上限，不能当作 G2P 的最终机械行程规格。

状态值仍需经过目标检查

线性 EEF 的 eef_pos 已与 move_eef(pos) 统一使用 mm，eef_vel 与 set_eef_speed() 统一 使用 mm/s，SDK 会负责与 route 的 m、m/s 转换。把状态作为下一次目标前，仍须确认 EEF 型号、 eef_dof、行程和有限值；不要把状态快照当作已经批准的控制目标。

eff、协议中的 current_threshold 和运行配置中的 max_torque 尚未形成统一的物理单位说明。 不同 EEF 还可能包含平移或转动关节；不要把 eff 一概解释为 N、N·m 或 A。MIT 的 torque、kp、 kd 也应以对应 EEF 的已批准控制参数为准。

CSP 返回

EEFControlMode.csp 发送 position、speed 和 eff。blocking=False 时，True 表示位置控制请求被 接受；blocking=True 时，服务端等待 EEF 的完成判据。客户端超时后动作可能仍在继续。

MIT 返回

EEFControlMode.mit 发送 position、speed、torque、kp 和 kd。请求中没有 blocking 字段， options.blocking 被忽略；True 只表示低层请求被接受，不表示到达位置或稳定保持。

当前可运行的只读示例

下面的 inspect_cartesian_and_eef.py 调用 get_eef_mode() 和 get_firmware_info()，不会把 EEF 状态快照直接作为控制目标，也不会申请控制权或调用 move_eef()：

完整代码：inspect_cartesian_and_eef.py

#!/usr/bin/env python3
"""Inspect Cartesian and EEF inputs without acquiring control or moving P7."""

from __future__ import annotations

import argparse
import math
from collections.abc import Callable, Sequence
from typing import Any


def validate_cartesian_pose(pose: Any, *, norm_tolerance: float = 1e-3) -> None:
    """Reject malformed, non-finite, or non-unit Cartesian poses."""
    position = tuple(pose.position)
    orientation = tuple(pose.orientation)
    if len(position) != 3 or len(orientation) != 4:
        raise ValueError("pose must contain 3 position and 4 quaternion values")

    values = position + orientation
    if not all(math.isfinite(float(value)) for value in values):
        raise ValueError("pose values must be finite")

    quaternion_norm = math.sqrt(sum(float(value) ** 2 for value in orientation))
    if not math.isclose(quaternion_norm, 1.0, abs_tol=norm_tolerance):
        raise ValueError(
            f"quaternion must be normalized; received norm={quaternion_norm:.6f}"
        )


def make_offset_target(
    current: Any,
    offset_m: Sequence[float],
    pose_factory: Callable[..., Any],
) -> Any:
    """Build a preview target in base_link while preserving orientation."""
    validate_cartesian_pose(current)
    if len(offset_m) != 3 or not all(math.isfinite(float(v)) for v in offset_m):
        raise ValueError("offset_m must contain 3 finite values")

    target = pose_factory(
        position=tuple(
            float(value) + float(delta)
            for value, delta in zip(current.position, offset_m)
        ),
        orientation=tuple(float(value) for value in current.orientation),
    )
    validate_cartesian_pose(target)
    return target


def inspect_inputs(client: Any, pose_factory: Callable[..., Any]) -> dict[str, Any]:
    """Read current metadata and prepare, but never send, a Cartesian target."""
    current = client.get_end_pose()
    if current is None:
        raise RuntimeError("current Cartesian pose is unavailable")

    # The 10 mm offset is an input-format example, not an approved motion target.
    target = make_offset_target(current, (0.01, 0.0, 0.0), pose_factory)
    return {
        "current_pose": current,
        "preview_target": target,
        "eef_mode": client.get_eef_mode(),
        "firmware_info": client.get_firmware_info(),
        "motion_sent": False,
    }


def parse_args() -> argparse.Namespace:
    parser = argparse.ArgumentParser(description=__doc__)
    parser.add_argument("--backend", choices=("grpc", "dds"), default="grpc")
    parser.add_argument("--host", help="P7 gRPC host; required for --backend grpc")
    parser.add_argument("--port", type=int, default=50071)
    parser.add_argument("--domain-id", type=int, help="required for --backend dds")
    parser.add_argument("--side", choices=("none", "left", "right"), default="none")
    return parser.parse_args()


def client_options_from_args(args: argparse.Namespace) -> dict[str, Any]:
    """Build explicit gRPC or DDS client options from command-line arguments."""
    backend = args.backend
    if backend == "dds":
        if args.domain_id is None:
            raise ValueError("--domain-id is required for --backend dds")
        return {
            "backend": "dds",
            "domain_id": args.domain_id,
            "side": args.side,
        }
    if not args.host:
        raise ValueError("--host is required for --backend grpc")
    return {
        "backend": "grpc",
        "host": args.host,
        "port": args.port,
    }


def main() -> None:
    from arm_p7_sdk import AirbotClient, CartesianPose

    try:
        with AirbotClient(**client_options_from_args(parse_args())) as client:
            result = inspect_inputs(client, CartesianPose)
    except (KeyError, ValueError) as error:
        raise SystemExit(f"input validation failed: {error}") from error
    except (ConnectionError, RuntimeError) as error:
        raise SystemExit(f"inspection failed: {error}") from error

    print("read-only inspection; no control lease or motion command was used")
    for name, value in result.items():
        print(f"{name}: {value}")


if __name__ == "__main__":
    main()

python inspect_cartesian_and_eef.py \
  --backend grpc --host P7_IP_ADDRESS --port 50071

预期输出包含 eef_mode、firmware_info 和 motion_sent: False。如果 eef_mode 为 None、 has_eef=False 或模式未知，先检查整机配置和版本，不要尝试切换或运动。

位置和速度的 SDK 单位已经统一，但不同 EEF 的行程、可用自由度和驱动原生 effort 参数仍可能不同。 实际项目应使用交付资料中明确列出的 EEF 型号、行程、速度和控制参数，并先在受控环境验证。 SDK 最小可执行用例只在显式提供这些参数后调用 move_eef()，不会从状态 回灌未经范围检查的目标或填入猜测值。

正常停止和故障处理

正常结束：

1. 使用阻塞调用，或通过设备状态确认非阻塞命令已经完成；
2. 调用 switch_eef_control_mode(EEFControlMode.idle) 并再次读取模式；
3. 如机械臂主体也参与运动，分别切回 Controller.idle 并确认 FSM；
4. 两部分都停止后再释放控制权并关闭客户端。

move_eef() 返回 False、超时或状态丢失时，停止发送新目标，不要假定释放租约会停止已接受的命令。 出现夹持异常、工具掉落风险或机械臂意外运动时，按工位风险评估执行硬件安全停止。

clear_eef_motor_err() 虽然是公开方法，但当前 gRPC 和 DDS 后端均抛出 UnsupportedOperationError，不能作为恢复步骤。保存 EEF 型号、固件版本、原始错误码和日志，按 整机交付流程处理。机械臂清错与软件急停见安全与恢复命令。