Drake中DIKI导致MultibodyPlant启动时跳至固定未初始化点

针对你遇到的DIKI结合逆动力学控制器启动阶段末端执行器跳变、初始位姿设置无效的问题，可按以下步骤排查修复：

1. 正确获取子系统上下文 #

必须从Diagram的根上下文里获取Plant和DIKI的子上下文，不能使用独立创建的上下文，否则设置无法同步到整个Diagram：

diagram = builder.Build()
context = diagram.CreateDefaultContext()
# 获取Plant的子上下文
plant_context = plant.GetMyContextFromRoot(context)
# 获取DIKI的子上下文
diki_context = diki.GetMyContextFromRoot(context)

2. 完整同步DIKI与Plant的初始状态 #

仅设置Plant位姿和调用diki.finalize不足以让DIKI内部状态完全同步，需要补充以下操作：

# 确认start_joint_positions是关节位置数组（不是末端位姿）
plant.SetPositions(plant_context, model, start_joint_positions)
# 设置DIKI的初始关节位置和速度（速度设为0，避免启动时运动）
diki.SetPositions(diki_context, start_joint_positions)
diki.SetVelocities(diki_context, np.zeros(plant.num_positions(model)))
# 将初始末端位姿设为DIKI的目标输入，避免启动时目标突变
end_effector_frame = plant.GetFrameByName("iiwa_link_7")
initial_end_pose = plant.CalcRelativeTransform(plant_context, plant.world_frame(), end_effector_frame)
diki.get_input_port().FixValue(diki_context, initial_end_pose)
# 最终同步DIKI内部状态
diki.finalize(diki_context, start_joint_positions)

3. 调整Simulator初始化流程 #

创建Simulator时直接传入已配置好的上下文，无需额外调用simulator.Initialize()（该方法会重置上下文到默认状态，导致之前的设置失效）：

# 所有上下文配置完成后再创建Simulator
simulator = Simulator(diagram, context)
# 直接启动仿真即可，无需Initialize
simulator.AdvanceTo(...)

4. 检查逆动力学控制器初始输入 #

确保逆动力学控制器的初始力矩指令与机器人静止状态匹配，避免初始力矩突变：

# 假设使用InverseDynamicsController
id_controller = builder.AddSystem(InverseDynamicsController(plant, ...))
# 初始力矩设为0
id_controller.get_input_port(0).FixValue(context, np.zeros(plant.num_actuated_dofs()))

额外排查项 #

• 确认start_pose变量存储的是关节位置而非末端位姿（plant.SetPositions要求输入关节位置数组，变量名混淆会导致设置无效）
• 检查DIKI的timescale参数，过大的 timescale 会导致积分器启动时步长突变，引发跳变
• 避免在Diagram构建后修改Plant的结构（比如添加模型），这会导致已创建的上下文失效

内容的提问来源于stack exchange，提问作者Ahmed Fayed