MuJoCo-Ektro Solver 双臂抓取案例

本案例使用 G1-D 双臂机器人接近料箱、闭合夹爪并抬升，通过 LambdaEngine 的时间轴播放仿真。IK 负责生成机器人关节目标，MujocoEktroSolver 根据驱动、重力和接触求解实际运动，OpenGL 视口显示计算结果。

对应代码：G1_D_dual_arm_direction_up_IK_mujoco.py。

运行示例

场景组成

对象	来源	本例设置
G1-D 与双侧 DEX1-1 夹爪	tests/assets/urdf/G1D/g1_d_with_dex1_1.urdf	场景名称为 g1_d，关节驱动以资产配置为基础，另覆盖四个夹爪主动关节的刚度。 example code 中路径变量名为 G1_D_URDF
料箱	tests/asset/factory/lx_with_gripper_slots.usda	场景名称为 box，平移至 (0.5, 0, 0.5)，绕 Z 轴旋转 90°，质量覆盖为 0.1 kg。 example code 中路径变量名为 BOX_USD
桌面	scene.add_box__ektro() 程序化创建	静态盒体，中心为 (0.5, 0, 0.48)，半尺寸为 (0.25, 0.20, 0.02)，完整尺寸为 0.50 × 0.40 × 0.04 m。

构建场景

build_scene() 加载机器人和料箱，设置物性、桌面及末端坐标系

scene = Scene()

# 添加机器人
robot_object = scene.add_urdf__ektro(str(G1_D_URDF), name="g1_d")

# 添加料箱 
box = scene.add_usd__ektro(str(BOX_USD), name="box")
box.set_RT(translate=ek.vec3(0.5, 0.0, 0.5),rotate=ek.vec3(0.0, 0.0, 90.0))

# 固定薄桌面的顶面贴住料箱底部。The fixed tabletop meets the bin's underside.
scene.add_box__ektro(
    path="/World/Tabletop",
    pos=(0.5, 0.0, 0.48),
    half_extents=(0.25, 0.20, 0.02),
    static=True,
)

使用 Scene PhysicsProperties API 设置物体物理属性

加载资产后，可以通过 Scene PhysicsProperties API 覆盖资产中的物理配置。PhysicsProperties 以 *_physics_properties(...) 结尾

配置内容	Scene PhysicsProperties API	本例用途
接触属性	scene.set_contact_physics_properties()	设置夹指摩擦。
刚体属性	scene.set_body_physics_properties()	设置料箱质量。
关节驱动	scene.set_actuators_physics_properties()	一次设置左右夹爪四个主动关节的刚度。

设置夹指摩擦

下面以左侧一根夹指为例：

scene.set_contact_physics_properties(
    body_paths=(
        "/root/obj/g1_d/G1_D_with_DEX1_1/left_dex1_1_link1_1",
    ),
    sliding_friction=2.0,
    torsional_friction=1.005,
    rolling_friction=0.0001,
)

body_paths 指定需要修改的刚体，设置应用到这些刚体关联的碰撞几何。完整 demo 对左右两侧共 12 个夹指连杆设置了相同参数。

设置料箱质量

直接指定料箱刚体的完整场景路径，将质量配置设为 0.1 kg：

# 直接指定料箱刚体的完整场景路径。Address the bin body by its full scene path.
scene.set_body_physics_properties("/root/obj/box/Box", mass=0.1)

/root/obj/box 是 SceneObject，Box 是它内部的刚体节点。路径直接对应 USD 层级，不再重复拼接 /root/obj。

设置夹爪驱动刚度

左右手各有一只夹爪，每只夹爪有两个主动滑动关节，共四个驱动。用一次批量调用设置这四个关节的刚度：

scene.set_actuators_physics_properties(
    joint_paths=[
        "/root/obj/g1_d/G1_D_with_DEX1_1/joints/left_dex1_1_joint1_1",
        "/root/obj/g1_d/G1_D_with_DEX1_1/joints/left_dex1_1_joint2_1",
        "/root/obj/g1_d/G1_D_with_DEX1_1/joints/right_dex1_1_joint1_1",
        "/root/obj/g1_d/G1_D_with_DEX1_1/joints/right_dex1_1_joint2_1",
    ],
    stiffness=80.0,
)

stiffness 控制位置驱动根据目标位置误差产生作用力的强度。列表中的四个主动关节统一设置为 80.0，未指定的阻尼等参数沿用各自的资产配置。批次中的目标和参数全部通过校验后才写入，校验失败时整批不修改。

创建 MuJoCo-Ektro Solver 求解器

from lambdasim.solvers.solver_mujoco_ektro import (
    MujocoEktroSolver,
    MujocoOptions,
    SolverSchedule,
)

...

solver = MujocoEktroSolver(
    mujoco_options=MujocoOptions(
        integrator="implicitfast",
        cone="elliptic",
        impratio=10.0,
        disableflags=0,
    ),
    njmax=512,
    schedule=SolverSchedule(substep_count=4),
    use_graph=True,
)

solver.src_paths = ["/G1_D_with_DEX1_1", "/box/Box", "/World/Tabletop"]
scene.add_solver__ektro(solver)

MujocoEktroSolver.src_paths 按 GeoDomain 内部的 USD 路径选择输入，料箱为 /box/Box。Scene 属性接口和公共 rigid.body_paths 使用完整场景路径 /root/obj/box/Box。

ComponentDocument.for_scene(scene).restore_components()

samples = robotics.sample_gripper_groups(
    robotics.read_gripper_slots(box), ee_ids=(0, 1), count=50, seed=7,
)
demo = Demo(scene, robot, solver, candidates=samples.candidates)

restore_components() 将资产保存的组件配置恢复成当前场景可用的组件数据。这个料箱的抓取槽依赖该步骤，应在 read_gripper_slots() 前执行。

机器人两侧末端使用 left_dex1_1_base_link 和 right_dex1_1_base_link，并通过 tcp_local 定义夹持工具坐标系。抓取姿态由料箱抓取槽、候选采样和工具坐标系共同决定。

时间轴中的三个阶段

阶段	更新目标的方式	切换条件
approach：接近	每次更新执行一次双臂 Direction-Up IK 迭代，仅调整 active_dof_ids 中的自由度。	IK 返回 success 后，下一次推进开始闭合。
close：闭合	在 2.5 秒仿真时间内，将四个夹爪目标从关节下限插值到上限。	closure 达到 1.0 后，下一次推进开始抬升。
lift：抬升	两个升降关节目标以 0.025 m/s 增加，到目标位置后保持。	保持在 lift；当前没有额外的松爪或放置阶段。

升降目标取第一个升降关节初值增加 0.05 m，并受该关节上限约束；当前代码将这个目标同时用于连续的两个升降关节。它描述关节目标变化，不保证料箱世界坐标恰好上升 0.05 m。

接近阶段使用 position_tol=0.003、frame_tol_rad=0.04，每次 IK 的最大关节更新范数为 0.6 × dt。这里的范数限制不是末端笛卡尔速度。

同一时间的重复请求不会再次推进阶段。时间归零或回退时，Demo 恢复初始关节目标、阶段和闭合进度；物理状态重置由 Scene 与 Solver 的时间管理处理。

IK 目标如何交给 MuJoCo

运行顺序为：

Demo.update()
    → IK 计算更新输出: robot.joint_q
    → 发布到 SimData["control"]["joint_target_pos"]
    → MuJoCo-Ektro Solver 读取 SimData["control"]["joint_target_pos"]字段进行目标姿态推进