轮式机器人：AGV 地面直行

本例使用仓库自带的 AGV（自动导引车）资产，在地面上以固定速度直行，并在 LambdaEngine 视口中循环播放。差速底盘将车体的线速度、转向角速度转换为左右轮的转速，MobileRobotObject 负责更新车体位姿和车轮关节。

1. 运行示例

在已经配置好 LambdaSim 和 LambdaEngine GUI 依赖的 Python 环境中，从 LambdaSim 仓库根目录运行：

.venv/bin/python examples/AGV_move.py

启动后，标题为 AGV · 地面直行 的窗口自动播放时间轴。AGV 从世界位置 (0, 1.5, 0) 出发，以 0.5 m/s 沿世界 -Y 方向行驶，6 秒后到达 (0, -1.5, 0)，随后回到起点重新播放。播放时，车体平移和左右车轮旋转同时更新。

本例采用运动学驱动，直接根据速度和时间更新位姿。地面用于提供可见参考，车辆高度由资产的初始坐标确定；本例不计算地面接触、轮胎摩擦或障碍物碰撞响应。

2. 分步构建

以下代码块按操作顺序展示示例中的各个部分。片段使用前文创建的变量；脚本中的 build_scene()、update_agv() 和 main() 的完整组织方式见文末。

2.1 初始化应用

从仓库源码加载所需模块，并创建 Qt 应用。create_application() 必须在创建窗口前调用。

import sys
from functools import partial

# 从仓库根目录加载源码。Load source modules from the repository root.
sys.path[:0] = ["src", "lambda_engine/src"]

import ektro as ek
import lambda_engine.app
import lambda_engine.ui.main_window
import lambdasim.robotics
import lambdasim.robotics.differential_drive
from lambda_engine.ui import ViewportManager
from lambdasim.scene.scene import Scene
from lambdasim.scene.scene_object import VisualizationSetting
from lambdasim.scene.step_context import StepContext

application = lambda_engine.app.create_application()

lambdasim.robotics.MobileRobotObject 是本例使用的包级入口，调用时无需经过 mobile_robot_object 子模块。

2.2 创建地面并加载 AGV

创建一个 8 m × 8 m 的地面，再加载已经声明轮式机器人关节的 USD 资产。

scene = Scene()
scene.add_plane__ektro(width=8.0, length=8.0, color=(0.32, 0.35, 0.38))
agv = scene.add_usd__ektro(
    "assets/demo/storage/assets/agv_navigation.usda",
    name="agv",
    transform=ek.vec3(0, 1.5, 0),
)
agv.visualization = VisualizationSetting(mode="surface")

add_usd__ektro() 返回场景对象 agv。本例使用的 agv_navigation.usda 包含底盘和两侧车轮的 3 个刚体，以及 2 个连续旋转的轮关节。VisualizationSetting(mode="surface") 将模型以实体表面显示。

资产的底盘根原点位于两轮轴线中点的地面投影处。将根节点放在 z=0 时，车轮处于地面高度，无需再把整车沿 Z 轴抬高一个轮半径。

2.3 绑定差速底盘

图中为底盘坐标的简化三维示意。坐标轴按 X 红、Y 绿、Z 蓝标记；本资产的车头朝局部 -Y，上方为局部 +Z。左右轮以车辆朝向为准。本例初始放置没有旋转，因此局部轴与世界轴同向。

从 AGV 的几何域读取轮关节、轮半径和轮距：

base = lambdasim.robotics.differential_drive.DifferentialDriveMobileBase.from_geo_domain(
    agv.model.geo_domain,
    left_wheel_joint="left_wheel_joint",
    right_wheel_joint="right_wheel_joint",
    local_forward_axis=ek.vec3(0, -1, 0),
    local_up_axis=ek.vec3(0, 0, 1),
    left_joint_sign=1.0,
    right_joint_sign=1.0,
)

配置	本例取值	含义
left_wheel_joint	"left_wheel_joint"	左侧驱动轮的关节名称。
right_wheel_joint	"right_wheel_joint"	右侧驱动轮的关节名称。
local_forward_axis	(0, -1, 0)	车体局部前进方向；正线速度沿此方向行驶。
local_up_axis	(0, 0, 1)	车体局部上方向；正转向角速度按此轴的右手定则旋转。
left_joint_sign、right_joint_sign	均为 +1.0	将底盘计算的轮速转换为资产关节正方向。合法取值为 +1.0 或 -1.0。

底盘从资产解析得到轮胎几何半径约 0.132 m、轮距约 0.77 m，调用方不再手填这两个尺寸。正向直行时，两轮的驱动轮速相同；转向时，两轮产生速度差。

这份资产的 left_wheel_joint 绑定到 USD 节点 Wheels/Right，right_wheel_joint 绑定到 Wheels/Left。绑定依据是关节及其刚体关系，不按几何节点的名称推断左右。

2.4 创建运动运行时

将场景对象和底盘配置交给 MobileRobotObject，得到可接收速度命令的机器人实例。

mobile = lambdasim.robotics.MobileRobotObject.from_scene_object(
    agv, base=base,
)

这三个变量各自承担以下职责：

对象	职责
agv	保存场景中的模型和节点变换，是视口显示的对象。
base	保存左右轮绑定、轮尺寸和底盘轴向，将车体速度转换为轮速。
mobile	保存运动状态，接收速度命令，推进车体和轮关节，并将结果写回 agv。

mobile.set_command() 设置线速度和转向角速度；mobile.step(dt) 推进正时长 dt，同步车体变换、车轮关节和正运动学结果；mobile.reset() 恢复创建时的初始位姿和轮角，并清零速度命令与运行时间。

2.5 注册时间轴运动回调

时间轴为场景提供目标时刻 context.time，单位是秒。本例每次先恢复初始状态，再用固定速度计算从起点到目标时刻的运动，因此重复选择同一帧会得到相同位置，向前或向后拖动时间轴都能直接查看对应的姿态。

def update_agv(
    mobile: lambdasim.robotics.MobileRobotObject,
    context: StepContext,
) -> None:
    """将时间轴目标时间交给运动学 API。Evaluate a timeline target with kinematics APIs."""
    # 从初始状态求目标时刻，支持回退和重复求值。
    # Evaluate from the initial state to support seeking and repeated evaluation.
    mobile.reset()
    mobile.set_command(linear_velocity=0.5, angular_velocity=0.0)
    # 零时刻仅恢复起点，step 要求正时长。At zero, only reset; step requires positive time.
    if context.time > 0.0:
        mobile.step(context.time)


scene.add_pre_sim_callback(partial(update_agv, mobile))

add_pre_sim_callback() 接收一个参数为 StepContext 的回调。partial(update_agv, mobile) 预先绑定机器人实例，场景求值时只需传入当前请求的 context。

这里传给 step() 的是 恢复初始状态后的整段时长。例如，选择 3 秒时刻就执行 reset()，再按 0.5 m/s 推进 3 秒。context.time=0 时只恢复起点，跳过要求正时长的 step()。

这种求值方式用于本例的固定速度运动。若速度命令随时间变化，需要按对应的命令历史推进；仅修改当前命令后再推进整段时长，会把该命令应用于从起点开始的整个区间。

2.6 创建视口和窗口

将场景绑定到 OpenGL 视口，设置能看清整段直线路径的相机，再创建主窗口。

application.aboutToQuit.connect(scene.close)

# 用 OpenGL 查看整段直线路径。Display the whole straight route with OpenGL.
instance = ViewportManager.get_instance().create_viewport(
    viewport_id="agv_move", scene=scene, render_mode="opengl",
)
instance.viewport.camera.position = (4.0, -5.0, 3.2)
instance.viewport.camera.target = (0.0, 0.0, 0.25)
instance.viewport.camera.up = (0.0, 0.0, 1.0)
instance.viewport.camera.near = 0.01
instance.viewport.camera_controller.sync_from_camera()

window = lambda_engine.ui.main_window.create_main_window(
    viewport_id=instance.viewport_id,
)
window.setWindowTitle("AGV · 地面直行")
window.resize(1200, 800)

视口和主窗口使用相同的 viewport_id。sync_from_camera() 让相机控制器采用刚设置的观察位置；aboutToQuit.connect(scene.close) 在退出应用时释放场景资源。

2.7 设置时间轴并开始播放

将时间轴设为 30 帧/秒，播放第 1 至第 181 帧，并开启循环。

timeline = window.editor_layout.time_view_controller
timeline.set_fps(30)
timeline.set_time_range(1, 181)
timeline.set_frame_range(1, 181)
timeline.set_loop_enabled(True)
window.show()
timeline.play()
application.exec()

set_time_range() 设置时间轴可用的帧范围，set_frame_range() 设置其中用于播放的帧区间；两者的参数均为帧号。当前帧通过下式转换为目标秒数：

目标时刻（秒） =（当前帧号 - 1）/ fps

第 1 帧对应 0 秒，第 181 帧对应 6 秒。到达播放区间末尾后，时间轴回到起始帧，运动回调相应恢复起点。

3. 检查结果与调整参数

本例初始朝向固定，直行时世界位置满足 y = 1.5 - 0.5 × t，x 和 z 均为 0。

时间轴帧号	目标时刻	AGV 世界位置（m）
1	0 s	(0, 1.5, 0)
31	1 s	(0, 1.0, 0)
91	3 s	(0, 0, 0)
181	6 s	(0, -1.5, 0)

两轮各自的转速约为 0.5 / 0.132 = 3.788 rad/s，6 秒时相对初始轮角各转过约 22.727 rad。mobile.state 提供当前运行时间、根部平面位姿、左右轮角和轮速。其 root_pose_xy_yaw 使用创建时的运动平面坐标：前进方向为平面 x，横向为平面 y，第三项为绕上轴的转角；它与上表的世界 XYZ 位置含义不同。

调整位置	参数	单位与效果
update_agv()	linear_velocity	m/s；正值前进，负值后退。
update_agv()	angular_velocity	rad/s；0 为直行，非零值使车辆绕上轴转向。
build_scene()	transform	m；设置 AGV 创建时的世界位置，也是回放起点。
main()	set_frame_range(1, 181)	帧号；与 fps 一起决定回放时长，必须位于 time_range 内。

修改固定速度或播放时长后，应同时检查行驶路径是否仍在地面和相机覆盖范围内。

4. 常见运行问题

现象	检查与处理
找不到源码模块或 AGV 文件。	从仓库根目录运行，并使用已配置 GUI 依赖的解释器。源码目录和资产路径均以该目录为基准。
出现轮关节绑定错误。	使用 agv_navigation.usda，并保持本例的两个关节名称。原始 agv.usd 只有外观等源数据，没有本例要求的轮关节。
车辆不随播放更新。	检查当前帧是否变化，以及 scene.add_pre_sim_callback(partial(update_agv, mobile)) 是否注册到视口使用的同一个场景。
修改轴向后车辆行驶方向与车头不符。	本资产使用 local_forward_axis=(0,-1,0)、local_up_axis=(0,0,1)。替换资产时应依据新资产的坐标和关节方向配置这些参数。

5. 完整代码

以下代码与 examples/AGV_move.py 一致，可保存为脚本后从仓库根目录运行。

"""一台 AGV 在地面上直行，六秒循环。Drive one AGV on a floor in a six-second loop.

从仓库根目录运行。Run from the repository root:
    .venv/bin/python examples/AGV_move.py

使用 MobileRobotObject 运动学驱动车体和车轮。
MobileRobotObject updates the chassis and wheels kinematically.
"""

import sys
from functools import partial

# 从仓库根目录加载源码。Load source modules from the repository root.
sys.path[:0] = ["src", "lambda_engine/src"]

import ektro as ek
import lambda_engine.app
import lambda_engine.ui.main_window
import lambdasim.robotics
import lambdasim.robotics.differential_drive
from lambda_engine.ui import ViewportManager
from lambdasim.scene.scene import Scene
from lambdasim.scene.scene_object import VisualizationSetting
from lambdasim.scene.step_context import StepContext


def update_agv(
    mobile: lambdasim.robotics.MobileRobotObject,
    context: StepContext,
) -> None:
    """将时间轴目标时间交给运动学 API。Evaluate a timeline target with kinematics APIs."""
    # 从初始状态求目标时刻，支持回退和重复求值。
    # Evaluate from the initial state to support seeking and repeated evaluation.
    mobile.reset()
    mobile.set_command(linear_velocity=0.5, angular_velocity=0.0)
    # 零时刻仅恢复起点，step 要求正时长。At zero, only reset; step requires positive time.
    if context.time > 0.0:
        mobile.step(context.time)


def build_scene() -> tuple[
    Scene,
    lambdasim.robotics.MobileRobotObject,
]:
    """创建地面和一台 AGV。Create a floor and one AGV."""
    scene = Scene()
    scene.add_plane__ektro(width=8.0, length=8.0, color=(0.32, 0.35, 0.38))
    agv = scene.add_usd__ektro(
        "assets/demo/storage/assets/agv_navigation.usda",
        name="agv",
        transform=ek.vec3(0, 1.5, 0),
    )
    agv.visualization = VisualizationSetting(mode="surface")

    # 车头朝局部 -Y，轮轴数据直接读取资产。Read authored wheel data; the AGV faces local -Y.
    base = lambdasim.robotics.differential_drive.DifferentialDriveMobileBase.from_geo_domain(
        agv.model.geo_domain,
        left_wheel_joint="left_wheel_joint",
        right_wheel_joint="right_wheel_joint",
        local_forward_axis=ek.vec3(0, -1, 0),
        local_up_axis=ek.vec3(0, 0, 1),
        left_joint_sign=1.0,
        right_joint_sign=1.0,
    )
    mobile = lambdasim.robotics.MobileRobotObject.from_scene_object(
        agv, base=base,
    )
    scene.add_pre_sim_callback(partial(update_agv, mobile))
    return scene, mobile


def main() -> int:
    application = lambda_engine.app.create_application()
    scene, mobile = build_scene()
    application.aboutToQuit.connect(scene.close)

    # 用 OpenGL 查看整段直线路径。Display the whole straight route with OpenGL.
    instance = ViewportManager.get_instance().create_viewport(
        viewport_id="agv_move", scene=scene, render_mode="opengl",
    )
    instance.viewport.camera.position = (4.0, -5.0, 3.2)
    instance.viewport.camera.target = (0.0, 0.0, 0.25)
    instance.viewport.camera.up = (0.0, 0.0, 1.0)
    instance.viewport.camera.near = 0.01
    instance.viewport.camera_controller.sync_from_camera()

    window = lambda_engine.ui.main_window.create_main_window(
        viewport_id=instance.viewport_id,
    )
    window.setWindowTitle("AGV · 地面直行")
    window.resize(1200, 800)
    timeline = window.editor_layout.time_view_controller
    timeline.set_fps(30)
    timeline.set_time_range(1, 181)
    timeline.set_frame_range(1, 181)
    timeline.set_loop_enabled(True)
    window.show()
    timeline.play()
    return application.exec()


# 直接运行此文件时启动示例。Start the example when this file is run directly.
if __name__ == "__main__":
    raise SystemExit(main())

6. 运行导航示例

导航示例在直行示例的基础上加入地面导航网格和一座货架。程序先根据货架占地生成禁行区域，再从起点规划到终点的路径，最后驱动 AGV 沿路径移动。车体平移、原地转向和左右轮旋转由同一个运动学运行时更新。

图中橙色车体为 AGV，货架周围的浅红区域表示为车体预留的安全范围，绿色折线表示规划结果。箭头表示行驶顺序；车辆在折点附近停车转向，再驶向下一个路点。

从仓库根目录运行 AGV_Navigation.py：

.venv/bin/python examples/AGV_Navigation.py

本例默认通过 LambdaRender 托管视口，要求已经安装相应渲染依赖，并具有可用的 cuda:0。场景只有一盏矩形面光源，不启用内置 HDR，环境光强度为 0。

启动后可以看到 8 m × 8 m 的地面网格、中央货架和一台 AGV。车辆从世界位置 (-3, 0, 0) 出发，绕过货架，到达 (3, 0, 0) 后停车；30 秒播放区间结束后回到起点重新运行。

阶段	使用的对象或接口	结果
创建导航地面	create_grid_mesh()	带共享顶点的平面几何域。
标记静态障碍	build_point_obstacle_data_from_AABB_list()	导航顶点上的 obstacle 属性。
构建导航图	NavigationMesh.from_geo_view()	过滤障碍面后的导航快照。
查询路径	find_path_by_points()	包含三维路点的 NavigationPath。
跟随路径	PathFollowController	当前子步的线速度与转向角速度。
更新车辆	MobileRobotObject	车体位姿、车轮关节及显示几何同步更新。

以下章节依次说明这些操作。片段对应新的导航脚本，所需导入及函数组织见第 12 节。

7. 地面导航网格

导航网格描述车辆中心允许经过的区域。它由相邻的凸多边形组成，寻路时沿多边形之间的连接查询通道。本例使用规则四边形网格，地面位于世界 Z=0。

navigation_domain = lambdasim.procedural_content.create_grid_mesh(
    center=ek.vec3(0, 0, 0), size=ek.vec3(8, 8, 0),
    resX=41, resY=41, device="cpu",
)

create_grid_mesh() 返回 ek.GeoDomain。center 是地面中心的世界位置，size 是完整宽度和长度，单位为米；平面网格要求 size.z=0。resX、resY 表示两个方向的顶点数量。

本例每个方向有 41 个顶点、40 个网格单元，因此单元边长为 8 / 40 = 0.2 m。整个地面共有 1681 个顶点、1600 个四边形，覆盖 X、Y ∈ [-4, 4]。

网格越细，障碍边界的离散表达越细；同时需要处理的顶点和多边形也越多。本例的分辨率用于当前尺寸的 AGV 和货架，替换为更小障碍物时应重新检查标记结果。

8. 货架静态避障

8.1 加载并放置货架

使用测试资产 shelf_basic.usd。该资产以 Y 轴朝上，加载到本例的 Z 向上场景时，绕 X 轴旋转 90°。

scene = Scene()
shelf = scene.add_usd__ektro(
    "tests/asset/factory/shelf_basic.usd",
    name="shelf",
    euler=ek.vec3(90, 0, 0),
)
shelf.visualization = VisualizationSetting(mode="surface")

shelf.world_bounds 返回放置后的世界空间轴对齐包围盒（AABB），其 min、max 分别为各坐标轴上的最小值和最大值。本例中的货架约占 1.957 m × 0.661 m 的地面范围，高约 1.8 m。

8.2 为车体预留安全间距

路径点描述的是车辆根原点的位置。AGV 的外壳和车轮占据一定面积，因此需要为整个车体预留空间，包括车辆在折点处原地转向时扫过的区域。

bounds = shelf.world_bounds
padding = ek.vec3(0.65, 0.65, 0.1)
lambdasim.navigation.build_point_obstacle_data_from_AABB_list(
    navigation_domain,
    [ek.AABB(bounds.min - padding, bounds.max + padding)],
)

这里将货架包围盒的 X、Y 两侧各扩张 0.65 m，Z 两侧各扩张 0.1 m。0.65 m 是本示例明确选定的车体避让间距，和车轮的 0.132 m 几何半径含义不同。更换车辆、货架位置或导航分辨率后，应重新检查整车沿路径移动和转向时的覆盖范围。

标记函数在几何域中写入标量整型点属性 obstacle：位于任意输入 AABB 内部或边界上的顶点记为 1，其余顶点记为 0。函数返回 None，不直接返回导航图。

该判断使用三维坐标。导航点与包围盒必须处于同一坐标系；离地包围盒只有在 Z 范围覆盖地面时才会标记地面顶点。本例的货架底部接近 Z=0，Z 扩张后包含地面。

8.3 过滤障碍面并显示地面

navigation_mesh = lambdasim.navigation.NavigationMesh.from_geo_view(
    navigation_domain.get_view_handle(0), point_obstacle_attr="obstacle",
)
ground = scene.add_geo_domain__ektro(navigation_domain, name="navigation")
ground.visualization = VisualizationSetting(
    mode="wire_surface", color=(0.32, 0.35, 0.38),
)

本例的程序化几何域只有一个网格 View，因此使用 get_view_handle(0)。构建导航图时，一个面只要有任意顶点被标记为 1，整个面就被排除。这个规则会额外排除障碍边缘的关联单元，使可行走区域的边界沿网格单元展开。

按当前配置，153 个顶点被标记，过滤后保留 1420 个可行走四边形。地面显示仍使用原始几何域，wire_surface 同时显示表面和网格线；过滤只作用于导航图，不会从显示模型中删除那些面。

本例将整座货架的包围盒作为禁行范围，货架下方的空隙也不参与通行。障碍标记是顶点与包围盒的包含判断，不进行精确的面盒相交检测；小障碍若没有覆盖任何网格顶点，需要调整网格分辨率或明确配置的障碍范围。

本节实现的是静态路径避障。车辆执行预先规划的路径，不计算碰撞响应、轮地摩擦，也不会自动感知或绕开后来进入路径的物体。移动货架后，需要重新生成障碍属性、构建导航图并规划路径；修改场景显示位置不会自动更新既有导航快照。

9. 寻路与路径跟随

9.1 查询起点到终点的路径

path = navigation_mesh.find_path_by_points(
    ek.vec3(-3, 0, 0), ek.vec3(3, 0, 0),
)
# 不可达时停止创建任务。Stop task creation when the goal is unreachable.
if not path.found:
    raise ValueError("AGV navigation goal is unreachable")

查询先在导航多边形之间使用 A* 搜索通道，再通过 Funnel 漏斗算法沿通道提取路径点。函数只返回结果，不移动 AGV，也不创建路径跟随控制器。

NavigationPath 字段	含义
found	是否找到连接起终点的通道。不可达时为 False。
polygon_ids	当前导航快照中，路径通道经过的多边形编号。
points	按行驶顺序排列的世界空间 ek.vec3 路点。不可达时为空。

find_path_by_points() 会将查询点解析到最近的可行走位置，包括先投影到导航平面。更换起终点后，应检查 path.points 的首尾；请求位置处于障碍或网格外时，实际首尾不保证等于原始输入。本例的两个输入点已经位于可行走区域内。

查询的 boundary_clearance 默认值为 0。本例已通过包围盒扩张表达车体安全间距，查询沿用这个默认值。

9.2 对齐导航坐标与车辆运动平面

导航路径使用世界 XYZ 坐标，MobileRobotObject 的平面位姿使用创建时的锚点、前进方向和横向方向。导航示例将 AGV 绕世界 Z 轴旋转 90°，使资产的局部前进方向 -Y 对齐世界 +X，横向对齐世界 +Y。

agv = scene.add_usd__ektro(
    "assets/demo/storage/assets/agv_navigation.usda",
    name="agv",
    euler=ek.vec3(0, 0, 90),
)
agv.visualization = VisualizationSetting(mode="surface")
base = lambdasim.robotics.differential_drive.DifferentialDriveMobileBase.from_geo_domain(
    agv.model.geo_domain,
    left_wheel_joint="left_wheel_joint",
    right_wheel_joint="right_wheel_joint",
    local_forward_axis=ek.vec3(0, -1, 0),
    local_up_axis=ek.vec3(0, 0, 1),
    left_joint_sign=1.0,
    right_joint_sign=1.0,
)
mobile = lambdasim.robotics.MobileRobotObject.from_scene_object(
    agv, base=base, initial_root_pose_xy_yaw=(-3.0, 0.0, 0.0),
)

AGV 创建时未设置平移，运动平面的锚点位于世界原点；initial_root_pose_xy_yaw 将车辆初始化到平面 (-3, 0)，初始转角为 0。这样，导航路径的世界 X、Y 数值与车辆的平面位置数值一致。

这个对齐关系是本例直接取路径 XY 的前提。改变创建时的平移或朝向后，需要先把世界路点转换到相应的车辆运动平面，不能直接沿用下面的二维取值。

9.3 创建路径跟随控制器

from operator import itemgetter

controller = lambdasim.robotics.PathFollowController(
    path=lambdasim.robotics.WaypointPath2D(
        points=tuple(map(itemgetter(0, 1), path.points)),
    ),
    config=lambdasim.robotics.PathFollowConfig(
        drive_speed=0.5, turn_rate=1.0,
        waypoint_tolerance=0.01, heading_tolerance=0.005,
    ),
)

itemgetter(0, 1) 取得一个路点的 X、Y 分量，map() 将这个操作应用于全部路点，tuple() 固定结果序列。这里的 WaypointPath2D 使用当前对齐后的运动平面坐标。

控制器依次处理各个路点：朝向误差超过容差时，线速度为 0，车辆原地转向；朝向满足要求后，沿当前方向驶向目标路点。进入路点位置容差后切换下一段，最后到达终点并停车。控制器只产生命令，车辆状态由下一步的 mobile.step() 更新。

9.4 按时间轴子步推进

导航过程中的命令随路段和转向阶段变化，因此每个子步都需要重新读取车辆状态并计算速度。导航回调替换直行示例中的固定速度回调：

def update_agv(
    mobile: lambdasim.robotics.MobileRobotObject,
    controller: lambdasim.robotics.PathFollowController,
    context: StepContext,
) -> None:
    """跟随路径，支持时间轴回退。Follow the path and support timeline rewind."""
    # 回退或循环时同时重置机器人和控制器。Reset both on rewind or playback loops.
    if context.time < mobile.state.time - 1.0e-9:
        mobile.reset()
        controller.reset()
    # 按时间轴步长追到目标时间，重复求值不推进。Substep to the target; repeated times do not advance.
    while context.time - mobile.state.time > 1.0e-9:
        dt = min(context.dt, context.time - mobile.state.time)
        command = controller.step(mobile.state, dt)
        mobile.set_command(
            linear_velocity=command.linear_velocity,
            angular_velocity=command.angular_velocity,
        )
        mobile.step(dt)


scene.add_pre_sim_callback(partial(update_agv, mobile, controller))

context.time 是目标时刻，context.dt 是建议的积分子步；拖动时间轴跨过一段时间时，回调依次补算所需子步。重复求值同一时刻不会再次推进。回退时同时重置车辆与控制器，再从任务起点重新计算。

不能将当前路段的速度命令直接应用到从 0 秒开始的整段时长，否则会跳过此前的转向和行驶阶段。

10. 导航场景的渲染与回放

10.1 设置场景灯光

在 build_scene() 中为场景添加一盏矩形面光源。灯光参数与本例的视口参数面板一致，旋转使用角度制。

scene.add_light(lambdasim.scene.light_object.RectLightObject(
    "navigation_key",
    size=(5.0, 5.0),
    world_xform=ek.math.transform_from_matrix(ek.math.transform_from_euler(
        ek.vec3(0, -0.917, 5), ek.vec3(19.127, 2.110, 0.732),
    )),
    intensity=4.0,
    cast_shadow=True,
))

灯光使用默认白色，开启阴影，关闭色温换色。矩形灯沿自身局部 -Z 方向发射光线。

10.2 托管 LambdaRender

以下片段放在 main() 中，承接已经创建的 application 和 scene。

delegate = lambda_engine.integrations.LambdaRenderViewportDelegate(
    device="cuda:0", device_id=0, spp=4,
)
# 不启用内置 HDR，只使用场景面光源。Use only the scene area light, without builtin HDR.
delegate.set_render_parameter("lambda_render.env_light_intensity", 0.0)
application.aboutToQuit.connect(delegate.close)
instance = ViewportManager.get_instance().create_viewport(
    viewport_id="agv_navigation", scene=scene,
    render_mode="lambda_render", render_callback=delegate,
)
instance.viewport.frame_scene(viewport_aspect=1.5)

渲染委托负责把场景和视口相机交给 LambdaRender，并同步车辆姿态变化。渲染器默认环境模式为 dome，本例没有 Dome 灯光，环境光强度又设为 0，因此背景为黑色，不使用内置 HDR 补光。

frame_scene() 根据场景范围自动取景。LambdaRender 显示地面实体及资产材质；地面的 wire_surface 网格线属于 OpenGL 的显示设置，LambdaRender 不叠加这些网格线。

创建窗口后，注册渲染参数面板，并设置 30 秒的循环播放区间：

window = lambda_engine.ui.main_window.create_main_window(
    viewport_id=instance.viewport_id,
)
window.setWindowTitle("AGV · 货架绕行导航")
window.resize(1200, 800)
lambda_engine.integrations.register_lambda_render_parameters(
    application.get_system_module().render_parameters,
    delegate,
    request_repaint=instance.task.request_repaint,
    set_continuous_rendering=window.viewport_widget.set_continuous_rendering,
)
timeline = window.editor_layout.time_view_controller
timeline.set_fps(30)
timeline.set_time_range(1, 901)
timeline.set_frame_range(1, 901)
timeline.set_loop_enabled(True)
window.show()
timeline.play()
application.exec()

第 1 帧对应 0 秒，第 901 帧对应 30 秒。车辆到达终点后保持停车，直到循环回到起始帧。

11. 导航结果与参数参考

11.1 检查规划与执行结果

当前网格、障碍和起终点配置下，实测规划得到以下世界路点，单位为米：

(-3,  0, 0)   起点
(-3, -1, 0)   第一段结束
(2.8, -1, 0)  货架另一侧
(3,   0, 0)   终点

这些中间路点由路径查询生成，没有写入脚本。修改障碍或网格后，应以新的 path.points 为准。当前导航示例也没有额外绘制路径折线；第 6 节图中的绿色线用于解释规划结果。

在 30 fps 配置下，车辆约在 20.4 秒到达终点，终点位置误差小于 0.01 m。当前配置下，AGV 与货架的世界 XY 包围盒在整段行驶和转向中保持分离。更换车辆或路线后，需要重新检查整车覆盖范围；本例不计算物理接触响应。

11.2 调整导航参数

调整位置	本例值	影响
create_grid_mesh() 的 size	(8, 8, 0) m	导航地面范围。
resX、resY	均为 41 个顶点	对应 0.2 m 单元边长；改变后需重新检查障碍标记。
padding.x、padding.y	均为 0.65 m	扩张货架水平禁行范围，给整车移动与转向留空间。
padding.z	0.1 m	包围盒的竖直扩张量，不是车辆离地高度。
find_path_by_points()	(-3,0,0) → (3,0,0)	请求的世界起终点；同时核对实际返回的路径首尾。
initial_root_pose_xy_yaw	(-3,0,0)	运行时起始平面位置和转角，需与路径首点一致。
drive_speed	0.5 m/s	控制器直行速度上限。
turn_rate	1.0 rad/s	控制器原地转向角速度上限。
waypoint_tolerance	0.01 m	切换路点和判断到达的位置容差。
heading_tolerance	0.005 rad	结束原地转向、进入直行的朝向容差。
时间轴播放区间	第 1～901 帧，30 fps	30 秒循环；速度降低后需要检查是否仍能在区间内到达终点。

11.3 常见导航问题

现象	检查与处理
货架横躺或占地异常。	源资产为 Y 向上，检查加载时的 euler=(90,0,0)，并在变换后读取 world_bounds。
路径经过货架占地。	先生成 obstacle，再构建 NavigationMesh，并传入 point_obstacle_attr="obstacle"；确认包围盒和地面点使用同一坐标系，Z 范围覆盖地面。
粗网格没有标记小障碍。	检查是否有顶点落在障碍 AABB 内。当前函数不做面盒相交测试，需提高分辨率或调整明确配置的障碍范围。
出现 no walkable polygons remain。	全部导航面都被障碍标记排除，检查包围盒、单位、扩张量及地面大小。
出现 AGV navigation goal is unreachable。	起终点解析后没有连通通道，检查障碍是否封住通路，以及避让间距是否使通道消失。
路径首尾与请求坐标不同。	查询会吸附到最近可行走位置，检查实际 path.points，再设置一致的车辆初始状态。
出现首个车辆状态不在路径起点容差内的错误。	检查运动平面锚点、车辆初始旋转、二维路点和 initial_root_pose_xy_yaw 的坐标一致性。
移动货架后路线没有变化。	导航图是构建时快照；重新标记障碍、构图和寻路，再创建对应的跟随任务。
拖动时间轴后车辆走错路段。	使用第 9.4 节的子步回调；回退时同时重置 mobile 和 controller，不要用当前速度推进整段历史时间。
LambdaRender 中没有地面网格线或绿色路径线。	OpenGL 网格线不属于路径追踪表面；本例没有添加路径折线显示。这不影响导航图和路径执行。

12. 导航示例完整代码

以下代码与当前 examples/AGV_Navigation.py 一致，包含场景灯光、导航、车轮运动和 LambdaRender 托管。

"""通过 LambdaRender 查看 AGV 绕过货架。View AGV shelf navigation with LambdaRender.

从仓库根目录运行。Run from the repository root:
    .venv/bin/python examples/AGV_Navigation.py

复用 106.2.2 障碍标记与 105.2/105.3、106.4 的规划、控制、运动学调用链。
Reuse the existing obstacle marking, path planning, control, and kinematics APIs.
"""

import sys
from functools import partial
from operator import itemgetter

# 从仓库根目录加载源码。Load source modules from the repository root.
sys.path[:0] = ["src", "lambda_engine/src"]

import ektro as ek
import lambda_engine.app
import lambda_engine.integrations
import lambda_engine.ui.main_window
import lambdasim.navigation
import lambdasim.procedural_content
import lambdasim.robotics
import lambdasim.robotics.differential_drive
import lambdasim.scene.light_object
from lambda_engine.ui import ViewportManager
from lambdasim.scene.scene import Scene
from lambdasim.scene.scene_object import VisualizationSetting
from lambdasim.scene.step_context import StepContext


def update_agv(
    mobile: lambdasim.robotics.MobileRobotObject,
    controller: lambdasim.robotics.PathFollowController,
    context: StepContext,
) -> None:
    """跟随路径，支持时间轴回退。Follow the path and support timeline rewind."""
    # 回退或循环时同时重置机器人和控制器。Reset both on rewind or playback loops.
    if context.time < mobile.state.time - 1.0e-9:
        mobile.reset()
        controller.reset()
    # 按时间轴步长追到目标时间，重复求值不推进。Substep to the target; repeated times do not advance.
    while context.time - mobile.state.time > 1.0e-9:
        dt = min(context.dt, context.time - mobile.state.time)
        command = controller.step(mobile.state, dt)
        mobile.set_command(
            linear_velocity=command.linear_velocity,
            angular_velocity=command.angular_velocity,
        )
        mobile.step(dt)


def build_scene() -> tuple[
    Scene,
    lambdasim.robotics.MobileRobotObject,
    lambdasim.robotics.PathFollowController,
]:
    """创建网格、货架和绕行任务。Create the mesh, shelf, and navigation task."""
    scene = Scene()

    # 按参数面板截图设置面光源，旋转单位为度。Match the light panel settings; rotation is in degrees.
    scene.add_light(lambdasim.scene.light_object.RectLightObject(
        "navigation_key",
        size=(5.0, 5.0),
        world_xform=ek.math.transform_from_matrix(ek.math.transform_from_euler(
            ek.vec3(0, -0.917, 5), ek.vec3(19.127, 2.110, 0.732),
        )),
        intensity=4.0,
        cast_shadow=True,
    ))

    # 货架源资产 Y 向上，旋转到本场景的 Z 向上。Rotate the Y-up shelf into this Z-up scene.
    shelf = scene.add_usd__ektro(
        "tests/asset/factory/shelf_basic.usd",
        name="shelf",
        euler=ek.vec3(90, 0, 0),
    )
    shelf.visualization = VisualizationSetting(mode="surface")

    # 0.2 米网格；货架周围留 0.65 米供完整车体通过和原地转向。
    # Use 0.2 m cells and 0.65 m clearance around the shelf for the full chassis and turns.
    navigation_domain = lambdasim.procedural_content.create_grid_mesh(
        center=ek.vec3(0, 0, 0), size=ek.vec3(8, 8, 0),
        resX=41, resY=41, device="cpu",
    )
    bounds = shelf.world_bounds
    padding = ek.vec3(0.65, 0.65, 0.1)
    lambdasim.navigation.build_point_obstacle_data_from_AABB_list(
        navigation_domain,
        [ek.AABB(bounds.min - padding, bounds.max + padding)],
    )
    navigation_mesh = lambdasim.navigation.NavigationMesh.from_geo_view(
        navigation_domain.get_view_handle(0), point_obstacle_attr="obstacle",
    )
    path = navigation_mesh.find_path_by_points(ek.vec3(-3, 0, 0), ek.vec3(3, 0, 0))
    # 不可达直接报告，不改成直线或预设绕行点。Reject unreachable goals without a substitute route.
    if not path.found:
        raise ValueError("AGV navigation goal is unreachable")
    ground = scene.add_geo_domain__ektro(navigation_domain, name="navigation")
    ground.visualization = VisualizationSetting(mode="wire_surface", color=(0.32, 0.35, 0.38))

    # 将车头旋转到世界 +X，使运动学平面坐标与导航世界 XY 一致。
    # Rotate the forward axis to world +X so the motion frame matches navigation XY.
    agv = scene.add_usd__ektro(
        "assets/demo/storage/assets/agv_navigation.usda",
        name="agv",
        euler=ek.vec3(0, 0, 90),
    )
    agv.visualization = VisualizationSetting(mode="surface")

    # 车头朝局部 -Y，轮轴数据直接读取资产。Read authored wheel data; the AGV faces local -Y.
    base = lambdasim.robotics.differential_drive.DifferentialDriveMobileBase.from_geo_domain(
        agv.model.geo_domain,
        left_wheel_joint="left_wheel_joint",
        right_wheel_joint="right_wheel_joint",
        local_forward_axis=ek.vec3(0, -1, 0),
        local_up_axis=ek.vec3(0, 0, 1),
        left_joint_sign=1.0,
        right_joint_sign=1.0,
    )
    mobile = lambdasim.robotics.MobileRobotObject.from_scene_object(
        agv, base=base, initial_root_pose_xy_yaw=(-3.0, 0.0, 0.0),
    )
    controller = lambdasim.robotics.PathFollowController(
        path=lambdasim.robotics.WaypointPath2D(
            points=tuple(map(itemgetter(0, 1), path.points)),
        ),
        config=lambdasim.robotics.PathFollowConfig(
            drive_speed=0.5, turn_rate=1.0,
            waypoint_tolerance=0.01, heading_tolerance=0.005,
        ),
    )
    scene.add_pre_sim_callback(partial(update_agv, mobile, controller))
    return scene, mobile, controller


def main() -> int:
    application = lambda_engine.app.create_application()
    scene, mobile, controller = build_scene()
    application.aboutToQuit.connect(scene.close)

    # 托管 LambdaRender，沿用视口的相机与场景同步。Delegate rendering with viewport camera and scene sync.
    delegate = lambda_engine.integrations.LambdaRenderViewportDelegate(
        device="cuda:0", device_id=0, spp=4,
    )
    # 不启用内置 HDR，关闭环境光，只使用场景面光源。Disable HDR and environment light; use the scene area light.
    delegate.set_render_parameter("lambda_render.env_light_intensity", 0.0)
    application.aboutToQuit.connect(delegate.close)
    instance = ViewportManager.get_instance().create_viewport(
        viewport_id="agv_navigation", scene=scene,
        render_mode="lambda_render", render_callback=delegate,
    )
    instance.viewport.frame_scene(viewport_aspect=1.5)

    window = lambda_engine.ui.main_window.create_main_window(
        viewport_id=instance.viewport_id,
    )
    window.setWindowTitle("AGV · 货架绕行导航")
    window.resize(1200, 800)
    # 参数面板控制采样、环境光和降噪。Expose sampling, environment light, and denoising controls.
    lambda_engine.integrations.register_lambda_render_parameters(
        application.get_system_module().render_parameters,
        delegate,
        request_repaint=instance.task.request_repaint,
        set_continuous_rendering=window.viewport_widget.set_continuous_rendering,
    )
    timeline = window.editor_layout.time_view_controller
    timeline.set_fps(30)
    timeline.set_time_range(1, 901)
    timeline.set_frame_range(1, 901)
    timeline.set_loop_enabled(True)
    window.show()
    timeline.play()
    return application.exec()


# 直接运行此文件时启动示例。Start the example when this file is run directly.
if __name__ == "__main__":
    raise SystemExit(main())