跳转至
AIRBOT P7 七轴机械臂 AIRBOT P7 七轴机械臂
笛卡尔运动

笛卡尔运动#

笛卡尔接口用末端在 base_link 下的位置和姿态描述目标。PTP 关注起点和终点，LIN 约束末端沿直线 移动，CIRCLE 通过路径参考构造圆弧。三者都可能带动全部 7 个关节；目标位姿可达不代表整段路径 无碰撞。

关节、笛卡尔和 EEF 运动命令经过不同执行路径并返回状态

选择运动入口#

任务 推荐入口 主要路径约束
到达一个末端位姿，不要求 TCP 走直线 move_end_pose() + planning PTP 规划器选择关节运动，到达目标位姿
工具沿直线接近或退出 move_end_pose_linear() TCP 从显式起点到目标沿笛卡尔直线
工具沿圆弧移动 move_end_pose_circle() TCP 经过中间点，或围绕圆心参考运动
连续跟踪外部生成的位姿 move_end_pose() + Servo 每次发送一个在线位姿目标
经过多个已知位姿 move_end_pose_waypoints() 多段 PTP/LIN/OMPL；见路点运动

PTP、LIN 和 CIRCLE 描述路径类型，不代表速度或完成等待方式。速度缩放、规划时间和 blocking 在 CartesianMoveOptions 中设置。

位姿格式#

from arm_p7_sdk import CartesianPose

target = CartesianPose(
    position=(0.40, 0.00, 0.30),       # x, y, z；m
    orientation=(0.0, 0.0, 0.0, 1.0), # qx, qy, qz, qw
)
字段 长度 单位 顺序和坐标系
position 3 m 末端相对 base_link 的 (x, y, z)
orientation 4 无量纲 同一位姿的单位四元数 (qx, qy, qz, qw)

(0, 0, 0, 1) 表示零旋转，不是“姿态未设置”。不要把角度、欧拉角或顺序为 (qw, qx, qy, qz) 的 四元数直接填入 orientation。

SDK 当前只在部分入口检查两个数组的长度，没有统一拒绝 NaN、无穷值、零四元数或未归一化 四元数；LIN 和 CIRCLE 入口连长度检查也不完整。应用应在发送前检查：

  1. position 恰好 3 项，orientation 恰好 4 项；
  2. 7 个数都是有限浮点数；
  3. 四元数范数接近 1，且不为零；
  4. 位姿来自同一个 base_link，不是相机、工具或工件坐标系中的未转换数据。

位置和姿态约定也见状态数据模型。get_end_pose() 是关节反馈经过 正运动学计算的结果，不是末端独立传感器测量值。

运动前检查#

发送任何笛卡尔目标前，确认工位无人和障碍物、实体急停可用、工具及负载配置正确，并完成以下检查：

  1. ServiceState 可用，机械臂没有电机错误、碰撞或急停状态；
  2. 当前位姿可以读取且通过格式检查；
  3. 目标、起点和路径参考都使用 m、base_link 和 (qx, qy, qz, qw)；
  4. 应用已判断位姿可达，并评估工具、线缆、负载和现场障碍；
  5. 显式取得控制权，切换到接口要求的模式，并通过 fsm_state 确认切换结果；
  6. 选择与任务总时长相匹配的 timeout_ms，同时准备好异常停止流程。

use_collision=True 只检查规划模型中已配置的几何体，不能发现未建模的现场障碍。

move_end_pose()#

client.move_end_pose(
    pos: CartesianPose,
    options: CartesianMoveOptions,
    timeout_ms: int = 1000,
) -> bool

gRPC 和 DDS 均支持。方法需要控制权；SDK 在本地没有租约时会尝试自动申请，但生产应用应显式申请， 以便处理多客户端竞争。

当前机械臂模式 实际行为 主要生效字段 True 的含义
planning_control 从当前末端位姿规划 PTP/LIN/OMPL 请求 planning、seed、eff、blocking 规划成功，或阻塞等待的执行完成
servo_control 发送一个笛卡尔 Servo 位姿目标 eff、blocking；速度来自当前机械臂速度数组 目标帧已接受，或阻塞到达判据成功
mit_control 当前版本实际发送固定参数的笛卡尔 PTP 规划 只读取 eff 和 blocking 固定 PTP 请求成功；不是 MIT 力矩命令
idle、重力补偿或其他模式 不支持 无 返回 False

planning 起点不可用时不要继续

当前 SDK 在 planning 和上述 MIT 兼容路径中读取 get_end_pose() 作为规划起点。读取返回 None 时，它会改用 base_link 原点和单位四元数继续发送请求，而不是拒绝运动。该回退位姿通常不等于 机械臂真实位姿。在版本修复并验证前，不要在无法容忍错误起点的任务中使用这一自动起点路径。

应用在调用前成功读取位姿，不能保证 SDK 随后用于规划的第二次读取仍成功。出现状态中断、返回 False 或超时时，停止发送新目标并确认机械臂实际状态；不能确认已停止时按现场实体急停流程处理。

mit_control 分支不会发送 CartesianMoveOptions.torque、kp 或 kd，也不会采用其中的规划缩放、 seed 或碰撞设置。不要把它用于预期的 MIT 低层控制任务。

move_end_pose_linear()#

client.move_end_pose_linear(
    start: CartesianPose,
    target: CartesianPose,
    options: CartesianMoveOptions,
    timeout_ms: int = 1000,
) -> bool

LIN 请求要求调用方显式给出起点和目标。先切换到 Controller.planning_control，确认 fsm_state == "PLANNING_CONTROL"，再调用该方法。接口固定发送 LIN；options.motion_type 和 options.circ_is_center 不生效。

start 必须与调用时机械臂的实际末端位姿一致。SDK 不比较 start 与反馈，也不会自动把不一致的 起点改正。起点状态不可读、已经过期或机械臂在读取后发生移动时，不要发送请求。

直线约束可能让原本可达的终点变得不可规划，例如路径中间经过奇异区域、超出关节限位或发生模型 碰撞。失败后不要自动改用 PTP 重试；先确认任务是否允许改变 TCP 路径。

move_end_pose_circle()#

client.move_end_pose_circle(
    start: CartesianPose,
    path: CartesianPose,
    target: CartesianPose,
    options: CartesianMoveOptions,
    timeout_ms: int = 3000,
) -> bool

先切换并确认 planning_control。接口固定发送 CIRCLE；options.motion_type 不生效。

options.circ_is_center path 的位置含义 使用注意
False（默认） 圆弧经过的中间位姿 起点、路径点和目标应能确定所需圆弧，避免重合或近共线
True 圆心参考 确认起点和目标相对圆心满足任务几何约束

path.orientation 会随请求发送，但当前公开契约没有单独说明圆心模式如何使用该姿态。不要依赖未经 版本验证的圆心姿态行为。无论使用哪种模式，start 都必须与实际末端位姿一致。

seed、IK 和不可达目标#

笛卡尔位姿通常需要通过逆运动学转换成关节姿态。同一个末端位姿可能有多个关节解，也可能因为关节 限位、奇异、碰撞或工具模型而没有可用解。

  • has_seed_start=True 时，seed_start 必须是 7 个有限关节角，单位 rad；
  • has_seed_goal=True 时，seed_goal 同样必须为 7 项；
  • 超出 SDK 关节命令限位的 seed 会被逐轴夹紧后发送；
  • seed 只影响求解或搜索起点，不保证选择特定构型，也不保证规划成功。

基础任务先保持 seed 关闭。只有已经验证目标构型、关节限位和工位路径时再启用，并记录最终关节反馈。

阻塞、超时和失败#

blocking=False 不等于命令没有执行，也不保证立即返回：planning 路径仍要等待规划结果。 blocking=True 等待当前执行路径定义的完成判据。timeout_ms 是客户端 route 调用的等待上限，不是 轨迹时长；客户端超时后，服务端已经接受的运动可能继续。

返回 False 或发生超时时：

  1. 停止生成和发送新目标；
  2. 不要立即释放控制权并假定轨迹已停止；
  3. 读取 ServiceState、关节状态和电机错误，确认任务是否仍在执行；
  4. 出现意外运动或无法确认安全状态时，由现场人员操作实体急停；
  5. 确认停止且通信正常后切回 Controller.idle，再释放控制权并保存诊断信息。

完整的超时区别和停止步骤见关节运动。

只读位姿预览#

下面的 inspect_cartesian_and_eef.py 读取当前位姿，检查有限值和单位四元数，构造沿 base_link X 方向偏移 0.01 m 的预览目标，并读取 EEF 模式和固件信息。脚本不申请控制权、不切换模式，也不调用 任何运动方法。

完整代码：inspect_cartesian_and_eef.py
inspect_cartesian_and_eef.py
  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
#!/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

输出中的 motion_sent: False 表示只完成了输入检查。0.01 m 只是格式示例，不是对任何工位批准的 运动目标。需要运行 LIN、CIRCLE 或笛卡尔路点时，使用 SDK 最小可执行用例并显式提供当前工位已经批准的偏移；脚本的幅度限制 不能替代路径复核。

PTP、LIN、CIRCLE 与 OMPL 的路径差异和选择条件见 选择规划模式。