逆运动学与运动规划#

本教程用 Franka 机械臂构建一个完整的抓取-放置任务：为目标末端执行器位姿求解逆运动学（IK），规划一条到达该构型的无碰撞路径，然后闭合夹爪并抬起一个立方体。教程还会介绍 IK 所期望的位姿约定，以及为什么接近阶段在位置控制下运行、而抓取阶段在力控制下运行。

完整脚本见 examples/tutorials/IK_motion_planning_grasp.py。

在 Genesis World 查看器中，一台 Franka 机械臂位于地平面上的一个小立方体上方。

运动规划运行在 Genesis World 内置的基于采样的规划器上，因此该示例不需要额外依赖。规划器及其参数见 路径规划。

场景与机器人设置#

加载地平面、一个用于抓取的小立方体和 Franka 机械臂，然后构建场景：

cube = scene.add_entity(
    gs.morphs.Box(
        size=(0.04, 0.04, 0.04),
        pos=(0.65, 0.0, 0.02),  # meters, Z-up
    )
)
franka = scene.add_entity(
    gs.morphs.MJCF(file="xml/franka_emika_panda/panda.xml"),
)
scene.build()

Franka 有九个自由度（dof）：七个手臂关节和两个夹爪手指。把它们拆成两个索引数组，就能独立地控制手臂和手指：

motors_dof = np.arange(7)
fingers_dof = np.arange(7, 9)

位置控制是一个 PD 控制器，因此需要为每个自由度设置刚度（kp）和阻尼（kv）增益，外加一个力范围。下面的数值是针对 Franka 调好的；换用其他机器人时需要它自己的数值，而编写良好的 URDF 或 MJCF 可能已经提供了这些数值。

franka.set_dofs_kp(
    np.array([4500, 4500, 3500, 3500, 2000, 2000, 2000, 100, 100]),
)
franka.set_dofs_kv(
    np.array([450, 450, 350, 350, 200, 200, 200, 10, 10]),
)
franka.set_dofs_force_range(
    np.array([-87, -87, -87, -87, -12, -12, -12, -100, -100]),
    np.array([87, 87, 87, 87, 12, 12, 12, 100, 100]),
)

求解逆运动学#

IK 回答的问题是“哪些关节角能让末端执行器到达这个位姿？”在 Genesis World 中，它是机器人实体上的一个方法：指定充当末端执行器的连杆，给它一个目标位姿，它就返回一整套全身构型（qpos）。

end_effector = franka.get_link("hand")

qpos = franka.inverse_kinematics(
    link=end_effector,
    pos=np.array([0.65, 0.0, 0.25]),  # world-frame position, meters
    quat=np.array([0, 1, 0, 0]),  # w-x-y-z; 180 deg about X, gripper points down
)

目标 pos 和 quat 位于世界坐标系中，使用右手系、Z 轴向上的坐标系，以及标量在前的 (w, x, y, z) 四元数约定。这里的 (0, 1, 0, 0) 表示绕世界 X 轴旋转 180 度，使夹爪朝向正下方、指向桌面。

返回的 qpos 覆盖所有自由度，包括手指。设置手指对应的项，就在接近之前打开了夹爪：

qpos[-2:] = 0.04  # open gripper, meters per finger

规划通往该构型的路径#

IK 给出一个目标构型，但没有给出如何到达那里。plan_path 从当前构型到 qpos_goal 找出一条无碰撞轨迹，并返回一组路点，每个仿真步对应一个：

path = franka.plan_path(
    qpos_goal=qpos,
    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()

# let the PD controller settle onto the final waypoint
for i in range(100):
    scene.step()

执行路径时，每个路点推进一步仿真。末尾多出的 100 步很关键：PD 控制器会让手臂略微滞后于指令目标，多步进一会儿，它就能在下一阶段开始前收敛到最后一个路点。

Tip

scene.draw_debug_path(path, franka) 会在查看器中把规划出的轨迹可视化出来，scene.clear_debug_object(...) 随后将其移除。示例中两者都用上了，在手臂跟随路径的同时把路径画了出来。

抓取与抬起#

任务剩余的部分是一连串 IK 求解和控制命令。要向下够到立方体，就为更低的目标求解 IK，并只对臂部自由度做位置控制：

qpos = franka.inverse_kinematics(
    link=end_effector,
    pos=np.array([0.65, 0.0, 0.130]),
    quat=np.array([0, 1, 0, 0]),
)
franka.control_dofs_position(qpos[:-2], motors_dof)  # arm only; leave fingers as-is
for i in range(100):
    scene.step()

抓取时，把手指从位置控制切换到力控制。位置控制下达的是目标开合度，因此它取决于立方体的精确宽度，也取决于手指能否准确到达；力控制则施加稳定的夹紧力，手指会一直闭合，直到被立方体挡住：

franka.control_dofs_position(qpos[:-2], motors_dof)
franka.control_dofs_force(np.array([-0.5, -0.5]), fingers_dof)  # 0.5 N inward per finger
for i in range(100):
    scene.step()

最后，为抬高后的目标求解 IK，并在手臂抬起的过程中保持抓取：

qpos = franka.inverse_kinematics(
    link=end_effector,
    pos=np.array([0.65, 0.0, 0.28]),
    quat=np.array([0, 1, 0, 0]),
)
franka.control_dofs_position(qpos[:-2], motors_dof)
for i in range(200):
    scene.step()

从抓取这一步起，手指一直处于力控制之下，因此立方体会随夹爪一起上升。

另请参阅#