双臂 IK 和 Viewport 控制

本例加载 G1-D 机器人，在左右夹爪的工具中心各创建一个 RT Gizmo。拖动箭头修改目标位置，拖动圆环修改目标姿态，机器人通过双臂 Direction-Up IK 跟随目标。

整个过程由 Gizmo 的 on_change 事件驱动，无需播放时间轴，也不创建动力学 Solver。本文只分两步：设置双臂 IK → 绑定 Viewport Gizmo。

示例文件：G1_D_dual_arm_direction_up_IK_ gizmo.py

1. 设置创建 IK

先创建 Scene，加载仓库中的 G1-D URDF，再通过 RobotKinematics.from_scene_object() 配置两个末端。tcp_local 定义夹爪基座到工具中心的局部变换，初始 Gizmo 和 IK 都使用同一个工具坐标系。

import sys

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

import ektro as ek
import lambdasim.kinematics
import lambdasim.scene.scene
from lambdasim.scene import scene_object

scene = lambdasim.scene.scene.Scene()
g1_d = scene.add_urdf__ektro(
    "tests/assets/urdf/G1D/g1_d_with_dex1_1.urdf", name="g1_d",
)
g1_d.visualization = scene_object.VisualizationSetting(mode="surface")
# 沿用双臂示例的夹持工具坐标系。Use the existing dual-arm gripper TCP calibration.
tcp_local = ek.mat44([
    [1, 0, 0, 0],
    [0, 0, 1, 0.10],
    [0, -1, 0, 0],
    [0, 0, 0, 1],
])
robot = lambdasim.kinematics.RobotKinematics.from_scene_object(
    g1_d,
    end_effector_bodies=("left_dex1_1_base_link", "right_dex1_1_base_link"),
    end_effector_local_xforms=(tcp_local, tcp_local),
)


def ik_target(ee_id: int, world_xform: ek.mat44):
    """将 Gizmo 坐标系交给 Direction-Up IK。Describe a complete TCP frame for IK."""
    return lambdasim.kinematics.DirectionUpTarget(
        ee_id=ee_id,
        position_world=ek.math.transform_point(world_xform, ek.vec3(0, 0, 0)),
        direction_world=ek.math.transform_vector(world_xform, ek.vec3(0, 0, 1)),
        up_world=ek.math.transform_vector(world_xform, ek.vec3(0, 1, 0)),
        local_direction=None,
        local_up=None,
    )

这段代码建立了以下对应关系：

配置	含义
end_effector_bodies[0]	左夹爪 left_dex1_1_base_link，对应 ee_id=0。
end_effector_bodies[1]	右夹爪 right_dex1_1_base_link，对应 ee_id=1。
end_effector_local_xforms	每个末端各自的工具坐标系。本例两侧使用同一个 tcp_local 标定值。
ik_target()	将一个 Gizmo 世界变换转换为一个末端的 Direction-Up IK 目标。

ik_target() 中，位置取工具坐标原点，方向取局部 Z 轴，Up 取局部 Y 轴。local_direction=None 与 local_up=None 成对使用，表示采用完整 TCP 坐标系约定。这里的 Up 是工具坐标轴，不是固定的世界竖直方向。

初始目标通过 ee_world_xform(robot, ee_id=...) 读取真实 TCP 世界变换，因此操作柄从夹爪当前位置开始，不会因为初始化目标不同而主动跳到其它位置。

2. 绑定到 Viewport Gizmo

通过 lambda_engine.create_application() 启动应用，再从 application.get_system_module() 获取视口管理器。创建视口和主窗口后，为左右目标分别注册 Gizmo。

以下代码接在第一步后面。Demo 保存机器人、两侧目标和求解结果；两个 Gizmo 共用 on_target_changed()，用 functools.partial() 分别绑定 ee_id=0 和 ee_id=1。

import functools
import itertools
import lambda_engine

class Demo:
    """由 Gizmo 事件驱动，不推进仿真时间。React to Gizmo events without simulation steps."""

    def __init__(self, robot, viewport, window) -> None:
        self.robot = robot
        self.viewport = viewport
        self.window = window
        self.last_result = None
        starts = dict(zip(robot.model.joint_label, robot.model.joint_q_start.to_list()))
        finger_start = int(starts["left_dex1_1_joint1_1"])
        # 本资产的双臂关节连续排列；其余关节保持固定。Only the contiguous arm DOFs are active.
        self.active_dof_ids = tuple(range(int(starts["left_shoulder_pitch_joint"]), finger_start))
        ek.fill(robot.joint_q[finger_start : finger_start + 4],
                   float(robot.model.joint_limit_lower[finger_start]))
        robot.update_fk()
        robot.sync_geo_pose()
        self.targets = [
            lambdasim.kinematics.ee_world_xform(robot, ee_id=0),
            lambdasim.kinematics.ee_world_xform(robot, ee_id=1),
        ]
        # Gizmo 使用固定 16 分量快照；只在 UI 边界转换小矩阵。Flatten only fixed-size UI matrices.
        self.left_gizmo = viewport.add_gizmo(
            "left_gripper_ik",
            world_xform=tuple(itertools.chain.from_iterable(self.targets[0])),
            mode="rt", space="local", size_pixels=40.0,
            on_change=functools.partial(self.on_target_changed, 0),
        )
        self.right_gizmo = viewport.add_gizmo(
            "right_gripper_ik",
            world_xform=tuple(itertools.chain.from_iterable(self.targets[1])),
            mode="rt", space="local", size_pixels=40.0,
            on_change=functools.partial(self.on_target_changed, 1),
        )

    def on_target_changed(self, ee_id: int, world_xform: tuple) -> None:
        """修改一侧目标，同时求解两侧。Change one target and solve both arms together."""
        self.targets[ee_id] = ek.mat44(world_xform)
        self.last_result = lambdasim.kinematics.robo_solve_ik_direction_up_multi_batch(
            self.robot,
            candidates=((ik_target(0, self.targets[0]), ik_target(1, self.targets[1])),),
            device="cpu", active_dof_ids=self.active_dof_ids,
            max_position_error=0.03, max_iters=32,
            position_tol=0.002, frame_tol_rad=0.02, max_step_norm=0.1,
        )
        # 显示本次迭代姿态，收敛标记保留在结果中。Display the iterate and retain its convergence flag.
        ek.copy(self.robot.joint_q, self.last_result.joint_q[0, :])
        self.robot.update_fk()
        self.robot.sync_geo_pose()


# 启动应用并获取已有的系统服务。Start the application and access its services.
application = lambda_engine.create_application([sys.argv[0]])
system = application.get_system_module()
instance = system.viewport_manager.create_viewport(
    viewport_id="g1_d_dual_arm_ik_gizmo", scene=scene, render_mode="opengl",
)
window = lambda_engine.create_main_window(viewport_id=instance.viewport_id)
demo = Demo(robot, instance.viewport, window)
window.setWindowTitle("G1-D · 双臂 Direction-Up IK Gizmo")
window.statusBar().showMessage("拖动左右夹爪的箭头或圆环求解 IK，无需播放时间轴")
window.resize(1440, 900)
window.show()
application.exec()

add_gizmo() 的关键参数如下：

参数	本例配置与作用
world_xform	末端初始世界变换。Gizmo 接口接收固定 16 分量快照，因此只在这个 UI 边界将一个 4×4 小矩阵展开。
mode="rt"	同时显示平移箭头和平面手柄、旋转圆环。
space="local"	操作轴跟随当前目标的局部坐标系。
size_pixels=40.0	操作柄的屏幕尺寸参数。
on_change	拖动修改目标时调用的函数，接收更新后的世界变换。

回调中的数据流为：

拖动左侧或右侧 Gizmo
    → 更新 self.targets[ee_id]
    → 将左右两个末端目标放进同一次 IK 求解
    → 将结果复制到 robot.joint_q
    → update_fk() 更新连杆世界变换
    → sync_geo_pose() 更新场景姿态，视口显示机器人运动

本例使用批量 IK 接口，但只传入一组候选，组内包含两个末端目标。外层 candidates=(...,) 表示这一组候选，内层两个 DirectionUpTarget 分别约束左右末端。移动一侧时，另一侧仍保留原目标并参与求解。

批量 IK 不会自动修改机器人。必须执行 ek.copy() 应用本次结果，再更新 FK 和场景姿态。仅移动 Gizmo 或仅取得 IK 返回值，都不会完成机器人显示更新。

last_result.success[0] 表示本次求解是否达到容差。当前 demo 会显示本次迭代得到的姿态，即使目标尚未收敛；它没有在未收敛时自动回滚，也没有碰撞规避。目标超出可达范围时，操作柄与实际夹爪位置会存在差距。

本例的位置容差为 0.002 m，方向容差为 0.02 rad，每次回调最多迭代 32 次。关节索引范围针对当前 G1-D 资产；替换机器人时，需要重新核对末端名称、工具标定和活动自由度。

完整代码

下面保留当前 demo 的 IK、Gizmo 和启动检查逻辑，将应用启动入口统一写为 lambda_engine.create_application()，视口管理器从 SystemModule 获取；使用的均是已有接口。

"""拖动两个夹爪 Gizmo 求解双臂 IK。Drag both gripper Gizmos to solve dual-arm IK.

从仓库根目录运行 / Run from the repository root:
    .venv/bin/python "examples/G1_D_dual_arm_direction_up_IK_ gizmo.py"
"""
from __future__ import annotations

import argparse
import functools
import itertools
import sys

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

import ektro as ek
import lambda_engine
import lambdasim.kinematics
import lambdasim.scene.scene
from lambdasim.scene import scene_object
import PySide6.QtCore


def ik_target(ee_id: int, world_xform: ek.mat44):
    """将 Gizmo 坐标系交给 Direction-Up IK。Describe a complete TCP frame for IK."""
    return lambdasim.kinematics.DirectionUpTarget(
        ee_id=ee_id,
        position_world=ek.math.transform_point(world_xform, ek.vec3(0, 0, 0)),
        direction_world=ek.math.transform_vector(world_xform, ek.vec3(0, 0, 1)),
        up_world=ek.math.transform_vector(world_xform, ek.vec3(0, 1, 0)),
        local_direction=None,
        local_up=None,
    )


class Demo:
    """由 Gizmo 事件驱动，不推进仿真时间。React to Gizmo events without simulation steps."""

    def __init__(self, robot, viewport, window) -> None:
        self.robot = robot
        self.viewport = viewport
        self.window = window
        self.last_result = None
        starts = dict(zip(robot.model.joint_label, robot.model.joint_q_start.to_list()))
        finger_start = int(starts["left_dex1_1_joint1_1"])
        # 本资产的双臂关节连续排列；其余关节保持固定。Only the contiguous arm DOFs are active.
        self.active_dof_ids = tuple(range(int(starts["left_shoulder_pitch_joint"]), finger_start))
        ek.fill(robot.joint_q[finger_start : finger_start + 4],
                   float(robot.model.joint_limit_lower[finger_start]))
        robot.update_fk()
        robot.sync_geo_pose()
        self.targets = [
            lambdasim.kinematics.ee_world_xform(robot, ee_id=0),
            lambdasim.kinematics.ee_world_xform(robot, ee_id=1),
        ]
        # Gizmo 使用固定 16 分量快照；只在 UI 边界转换小矩阵。Flatten only fixed-size UI matrices.
        self.left_gizmo = viewport.add_gizmo(
            "left_gripper_ik",
            world_xform=tuple(itertools.chain.from_iterable(self.targets[0])),
            mode="rt", space="local", size_pixels=40.0,
            on_change=functools.partial(self.on_target_changed, 0),
        )
        self.right_gizmo = viewport.add_gizmo(
            "right_gripper_ik",
            world_xform=tuple(itertools.chain.from_iterable(self.targets[1])),
            mode="rt", space="local", size_pixels=40.0,
            on_change=functools.partial(self.on_target_changed, 1),
        )

    def on_target_changed(self, ee_id: int, world_xform: tuple) -> None:
        """修改一侧目标，同时求解两侧。Change one target and solve both arms together."""
        self.targets[ee_id] = ek.mat44(world_xform)
        self.last_result = lambdasim.kinematics.robo_solve_ik_direction_up_multi_batch(
            self.robot,
            candidates=((ik_target(0, self.targets[0]), ik_target(1, self.targets[1])),),
            device="cpu", active_dof_ids=self.active_dof_ids,
            max_position_error=0.03, max_iters=32,
            position_tol=0.002, frame_tol_rad=0.02, max_step_norm=0.1,
        )
        # 显示本次迭代姿态，收敛标记保留在结果中。Display the iterate and retain its convergence flag.
        ek.copy(self.robot.joint_q, self.last_result.joint_q[0, :])
        self.robot.update_fk()
        self.robot.sync_geo_pose()



def build_demo(system):
    scene = lambdasim.scene.scene.Scene()
    g1_d = scene.add_urdf__ektro(
        "tests/assets/urdf/G1D/g1_d_with_dex1_1.urdf", name="g1_d",
    )
    g1_d.visualization = scene_object.VisualizationSetting(mode="surface")
    # 沿用双臂示例的夹持工具坐标系。Use the existing dual-arm gripper TCP calibration.
    tcp_local = ek.mat44([
        [1, 0, 0, 0],
        [0, 0, 1, 0.10],
        [0, -1, 0, 0],
        [0, 0, 0, 1],
    ])
    robot = lambdasim.kinematics.RobotKinematics.from_scene_object(
        g1_d,
        end_effector_bodies=("left_dex1_1_base_link", "right_dex1_1_base_link"),
        end_effector_local_xforms=(tcp_local, tcp_local),
    )
    instance = system.viewport_manager.create_viewport(
        viewport_id="g1_d_dual_arm_ik_gizmo", scene=scene, render_mode="opengl",
    )
    window = lambda_engine.create_main_window(viewport_id=instance.viewport_id)
    demo = Demo(robot, instance.viewport, window)

    window.setWindowTitle("G1-D · 双臂 Direction-Up IK Gizmo")
    window.statusBar().showMessage("拖动左右夹爪的箭头或圆环求解 IK，无需播放时间轴")
    window.resize(1440, 900)
    return window, demo


def main() -> int:
    parser = argparse.ArgumentParser(description="G1-D dual-arm IK controlled by two Qt Gizmos.")
    parser.add_argument("--smoke-test", action="store_true", help="show the editor briefly and exit")
    args = parser.parse_args()
    application = lambda_engine.create_application([sys.argv[0]])
    window, demo = build_demo(application.get_system_module())
    window.show()
    # 启动检查只验证窗口创建，不替代拖动测试。Startup smoke does not replace drag validation.
    if args.smoke_test:
        PySide6.QtCore.QTimer.singleShot(200, application.quit)
    return application.exec()


# 直接运行此文件时打开编辑器。Open the editor when this file is executed directly.
if __name__ == "__main__":
    raise SystemExit(main())

在已配置好依赖的环境中，从 LambdaSim 仓库根目录运行现有 demo：

.venv/bin/python "examples/G1_D_dual_arm_direction_up_IK_ gizmo.py"

也可以将上面的完整代码保存为独立脚本，从同一仓库根目录运行。初始取景沿用编辑器默认相机；需要自动框住机器人时，可在创建 Demo 后调用现有的 instance.viewport.frame_scene()。