路点运动

路点运动#

路点接口把相邻目标组成多个规划段，并可在段与段的边界设置混合。关节路点直接给出 7 轴角度；笛卡尔路点给出末端在 base_link 下的位姿。两种接口都用于 planning，不是 Servo 流式控制。

路点、规划段和边界#

对 N 个路点：

至少需要 2 个路点；
规划段数量为 N - 1；
可混合的内部边界数量为 N - 2；
第一项是序列起点，最后一项是序列终点。

例如 [P0, P1, P2, P3] 形成 P0→P1、P1→P2、P2→P3 三段，并在 P1、P2 形成两个内部边界。 segments[0] 配置第一段，同时用它的 enable_blend 和 min_blend_radius 配置 P1 边界；最后一段没有后续边界，因此它的混合字段不会产生新的边界。

共同前置条件#

工位、工具、负载、线缆和实体急停已完成现场检查；
ServiceState、关节状态和电机状态可用；
显式取得控制权并切换到 Controller.planning_control；
通过 ServiceState.fsm_state 确认进入 PLANNING_CONTROL；
每个路点以及相邻路点之间的路径都已评估可达性和碰撞风险；
选择能覆盖规划与执行等待的 timeout_ms，并准备异常停止流程。

路点序列不是“只检查几个离散点”。即使每个路点单独可达，相邻段仍可能因逆运动学构型变化、奇异、关节限位或碰撞而失败。

`move_joint_waypoints()`#

client.move_joint_waypoints(
    waypoints: list[list[float]],
    options: JointWaypointsMoveOptions,
    timeout_ms: int = 1000,
) -> bool

gRPC 和 DDS 均支持。每个 waypoint 必须包含运行时 arm_dof 个有限关节角，当前 P7 为 7 项，单位 rad。数组长度错误或非有限值会返回 False；角度越过 SDK 命令限位时会被逐轴夹紧后发送，而不是拒绝整个序列。

公开段类型为：

"ptp"：关节空间点到点段；
"ompl"：带模型碰撞检查的关节路径搜索。

未知 motion_type 当前可能落入 OMPL 分支，但不属于受支持行为。不要传 "lin"、planner 具体名称或拼写错误的字符串。

`move_end_pose_waypoints()`#

client.move_end_pose_waypoints(
    waypoints: list[CartesianPose],
    options: CartesianWaypointsMoveOptions,
    timeout_ms: int = 1000,
) -> bool

gRPC 和 DDS 均支持。每个 waypoint 由 3 个位置值和 4 个四元数值组成；位置单位为 m，姿态顺序为 (qx, qy, qz, qw)，坐标系为 base_link。应用还必须检查有限值和四元数归一化，因为 SDK 的长度检查不会完成这些验证。

公开段类型为：

"ptp"：到达下一末端位姿，不约束 TCP 为直线；
"lin"：相邻笛卡尔路点之间采用直线段；
"ompl"：使用 OMPL 搜索，并启用模型碰撞检查。

笛卡尔 waypoint 选项包含 circ_is_center，但公开段类型不包含 CIRCLE，也没有单独的圆弧路径参考数组。不要用 waypoint 接口表达圆弧；使用 move_end_pose_circle() 给出明确的 path。

顶层配置和逐段配置#

不需要逐段差异时，直接设置顶层 options：

from arm_p7_sdk import JointWaypointsMoveOptions

options = JointWaypointsMoveOptions(
    motion_type="ptp",
    velocity_scaling_factor=0.1,
    acceleration_scaling_factor=0.1,
    enable_blend=False,
    blocking=True,
)

需要逐段差异时，segments 的长度必须恰好等于 len(waypoints) - 1：

from arm_p7_sdk import CartesianWaypointsMoveOptions

segments = [
    CartesianWaypointsMoveOptions(
        motion_type="ptp",
        velocity_scaling_factor=0.10,
        enable_blend=True,
        min_blend_radius=0.005,
    ),
    CartesianWaypointsMoveOptions(
        motion_type="lin",
        velocity_scaling_factor=0.05,
        enable_blend=False,
    ),
]
options = CartesianWaypointsMoveOptions(segments=segments, blocking=True)

错误数量的 segments 不会被拒绝

当前 SDK 只有在 len(options.segments) 与规划段数量完全相等时才使用逐段配置。空列表或任何其他长度都会静默改用顶层 options 配置所有段。调用前显式断言 not segments or len(segments) == len(waypoints) - 1，不要依赖 SDK 纠正配置。

以下字段来自每个 segment：

motion_type、use_collision；
速度和加速度缩放、sampling_time、allow_planning_time、max_retry_attempts；
OMPL planner 和插值参数；
has_seed_start/seed_start、has_seed_goal/seed_goal；
对应内部边界的 enable_blend 和 min_blend_radius。

顶层 eff 用于整个序列，顶层 blocking 决定方法何时返回。segment 内的 eff 和 blocking 不会分别发送。segments 是同类型选项的列表；不要在 segment 内继续嵌套 segments。

sampling time 必须一致#

逐段配置可以写不同的 sampling_time，但当前 P7 运行时要求序列内所有段使用相同采样时间，不一致会拒绝请求。构造逐段选项时使用一个共享值，并在发送前检查：

sampling_times = {segment.sampling_time for segment in options.segments}
if len(sampling_times) > 1:
    raise ValueError("all waypoint segments must use the same sampling_time")

混合和停顿#

enable_blend=True 请求在当前段终点与下一段起点的内部边界连续过渡，min_blend_radius 是该边界的最小混合半径，单位 m，默认 0.005 m，允许范围 [0, 10] m。关节和笛卡尔序列都在末端的笛卡尔空间构造混合圆弧，因此关节路点的这一字段也不是 rad。接口不承诺实际混合轨迹一定达到请求半径。

需要在某个中间路点明确停顿时，把进入该边界的 segment 设为 enable_blend=False。这表示不请求边界混合，不等于创建一个带指定停留时间的动作；接口没有 dwell time 参数。

半径过大、相邻段过短、路径几何不适合或运行时约束不满足时，混合可能失败。不要通过增大半径来 “保证平滑”，应先在目标工位验证轨迹和速度连续性。

seed 和构型连续性#

每个 segment 可以分别启用起点和终点 seed。启用后数组必须为 7 个有限关节角，单位 rad；越过命令限位的项会夹紧。未启用时对应数组内容被忽略。

seed 可以帮助规划器偏向某个关节构型，但不会保证相邻笛卡尔段选中同一 IK 分支。需要保持腕部方向或避开构型翻转时，应同时检查各路点的最终关节解、段间连续性和实际执行反馈。

返回值、超时和部分执行风险#

blocking=False 等待序列规划结果后返回，后续执行可能仍在进行；blocking=True 等待运行时的序列完成判据。True 表示当前等待方式下请求成功，不是永久安全状态。

timeout_ms 是一次 route 调用的等待上限。客户端返回 False 或超时，不能证明序列没有开始，也不能撤销已接受的段。应用不得把整个 sequence 当作可由客户端超时原子回滚的事务。

失败时停止发送新序列，读取 FSM、关节状态和电机状态，确认停在哪个实际位置。不能确认已停止或出现意外运动时执行实体急停。只有确认运动停止并切回 idle 后，才能释放控制权。

构造预览，不发送序列#

下面只演示数据结构，不创建客户端：

from arm_p7_sdk import CartesianPose, CartesianWaypointsMoveOptions

waypoints = [
    CartesianPose((0.40, 0.00, 0.30), (0.0, 0.0, 0.0, 1.0)),
    CartesianPose((0.41, 0.00, 0.30), (0.0, 0.0, 0.0, 1.0)),
]
options = CartesianWaypointsMoveOptions(
    motion_type="lin",
    velocity_scaling_factor=0.05,
    acceleration_scaling_factor=0.05,
    enable_blend=False,
    blocking=True,
)

assert len(waypoints) >= 2
assert not options.segments or len(options.segments) == len(waypoints) - 1

这些数值只用于展示 m 和四元数顺序，不是已验证的工位路径。完整字段见运动参数参考。

路点与单段 PTP/LIN/OMPL 的取舍见选择规划模式。关节和笛卡尔路点的同名运行入口见 SDK 最小可执行用例。