路径规划#

plan_path 在机器人的关节空间中找出一条从当前构型到目标构型的无碰撞轨迹。它是一个运动学规划器：它考虑的是哪些关节角会与场景发生碰撞，而不涉及力或动力学。结果是若干路点组成的列表，随后由你选择的控制器来执行。

本页单独介绍 plan_path。把它与逆运动学、夹爪控制配合起来的完整抓取-放置工作流，见 逆运动学与运动规划，下文摘录以其可运行脚本（examples/tutorials/IK_motion_planning_grasp.py）为准。

心智模型#

规划在构型空间中进行：空间中的一个点是一个完整的关节构型（qpos），而不是笛卡尔位姿。给规划器一个目标 qpos，它就会搜索一串构型，把起点与目标连接起来，且其中任何一个构型都不让机器人发生碰撞。

由此有两点推论：

  • 通常要先做逆运动学。 任务通常以世界坐标系中的末端执行器位姿给出，而规划器接受的是关节构型。逆运动学与运动规划 负责把前者转换为后者；把它的 qpos 输出作为 qpos_goal 传入。

  • 规划不会移动机器人。 plan_path 返回路点，只有当你把这些路点发送给控制器并调用 scene.step() 时，机器人才会移动。

规划器是基于采样的（RRT 和 RRTConnect）。它们是概率完备的（只要时间足够，存在路径就一定能找到），但它们是随机化的，因此对同一问题跑两次可能返回不同的路径，难题也可能在重试预算内失败。

最简示例#

为目标位姿求解 IK，规划一条通往该构型的路径，然后沿路径驱动机器人：

end_effector = franka.get_link("hand")

qpos_goal = franka.inverse_kinematics(
    link=end_effector,
    pos=np.array([0.65, 0.0, 0.25]),  # world-frame target, meters, Z-up
    quat=np.array([0, 1, 0, 0]),  # w-x-y-z; gripper points down
)

path = franka.plan_path(
    qpos_goal=qpos_goal,
    num_waypoints=200,  # 200 steps at dt=0.01 s -> 2 s of motion
)

for waypoint in path:
    franka.control_dofs_position(waypoint)
    scene.step()

num_waypoints 决定 plan_path 对找到的路径重采样得多细，每个仿真步对应一个路点，因此 num_waypoints * dt 就是运动的仿真时长。对单个环境来说，路点是形状为 (num_waypoints, n_qs) 的完整 qpos 向量。

Tip

在最后一个路点之后让控制器稳定下来。位置控制是 PD 控制器，手臂会略微滞后于指令目标；多步进一会儿，它就能在下一阶段开始前收敛。

for i in range(100):
    scene.step()

在执行前先在查看器中预览规划出的路径：

path_debug = scene.draw_debug_path(path, franka)  # renders the trajectory
# ... execute the path ...
scene.clear_debug_object(path_debug)  # remove it afterward

规划器与参数#

plan_path 提供两个基于采样的规划器，用 planner 选择：

规划器

说明

"RRTConnect"（默认）

从起点和终点各生长一棵树，并把它们连接起来。通常更快。

"RRT"

从起点生长一棵树。更简单，连接通常更慢。

其余参数控制搜索与输出：

参数

默认值

含义

qpos_goal

（必填）

目标构型。

qpos_start

None

起始构型；为 None 时使用机器人当前的 qpos。

resolution

0.05

关节空间中的步长（弧度），各段按此间距进行碰撞检查。取值更小能发现更细的障碍物，代价也更高。

max_nodes

2000

单次尝试放弃前搜索树大小的上限。

timeout

None

每次尝试的墙钟预算，单位为秒（近似值）。

max_retry

1

规划失败后，在首次尝试之外额外重试的次数。

smooth_path

True

重采样前对原始路径做捷径平滑。

num_waypoints

300

路径重采样后的路点数。为 None 时跳过插值。

ignore_collision

False

跳过所有碰撞检查。

Warning

resolution 决定两个构型相距多远时，规划器才会把它们之间的线段视为已做碰撞检查。如果它大于构型空间中最细的障碍物，规划器就可能一步跨过碰撞，返回一条穿过几何体的路径。当路径看起来有效却穿过了障碍物时，把它调小。

携带物体时规划#

机器人抓住物体后，该物体就成为扫掠体积的一部分，因此规划器也必须对它做碰撞检查。在规划的整个过程中，通过指定夹爪连杆并传入实体来附上它：

path = franka.plan_path(
    qpos_goal=place_qpos,
    ee_link_name="hand",  # link the object is rigidly attached to
    with_entity=cube,
)

规划器在搜索时会把这一附着关系考虑进去，但不会创建任何物理约束。一次附着只覆盖一个非关节体实体。要在仿真本身中把连杆焊接在一起，见 刚体约束。

检查规划是否成功#

设置 return_valid_mask=True，就能在路径之外额外得到一个成功标志。对单个环境来说，它是一个标量布尔值：

path, is_valid = franka.plan_path(qpos_goal=qpos_goal, return_valid_mask=True)
if not is_valid:
    print("no collision-free path found within the retry budget")

未请求掩码时，失败的规划仍会返回一个路径形状的张量。请检查标志，而不要假定路点可用。

跨并行环境规划#

在 批处理场景 中，plan_path 一次性为每个环境规划。返回的张量会在路点维度和关节维度之间多出一个环境维度：

scene.build(n_envs=16)

path = franka.plan_path(qpos_goal=qpos_goal)
print(path.shape)  # (num_waypoints, n_envs, n_qs)

传入 envs_idx 可只对子集规划；此时结果的环境维度大小会与索引数量一致。把它与 return_valid_mask 结合使用，可以看出哪些环境成功了，因为随机化规划可能只解出其中一部分：

path, valid = franka.plan_path(
    qpos_goal=qpos_goal,
    envs_idx=[0, 5, 10],
    return_valid_mask=True,
)  # path: (num_waypoints, 3, n_qs); valid: (3,)

注意事项与坑#

  • 不支持自由关节和球关节。 规划器会拒绝含有自由关节或球关节的实体，因此用自由关节建模的移动底盘无法直接对它做规划。

  • 规划是随机化的。 要在难题上获得可靠性，请用 timeout 和 max_retry 给规划器留出余量，而不要指望第一次尝试就成功。

  • ignore_collision=True 会跳过所有碰撞检查。 它产生关节空间中的直线插值，只有在你已经确定空间畅通、只需要一条快速轨迹时才有用。

另请参阅#