如何在Leaf System中解决MultibodyPlant自由体姿态设置的上下文问题？

针对你遇到的在Leaf System内部无法调用SetFreeBodyPose的问题，有以下几种可行方案：

方案1：从Leaf System上下文获取根上下文，再拿到Plant的上下文 #

在Leaf System的Update方法中，传入的context是当前Wrapper系统的上下文，你可以通过它获取整个Diagram的根上下文，再用根上下文拿到Plant的专属上下文，之后就能正常调用SetFreeBodyPose。

修改后的Update方法代码：

def Update(self, context, discrete_state):
    # 获取Diagram的根上下文
    root_context = context.get_root_context()
    # 从根上下文提取Plant的上下文
    plant_context = self.plant.GetMyContextFromRoot(root_context)
    # 设置自由体姿态
    self.plant.SetFreeBodyPose(plant_context, body,
                               RigidTransform(p=np.zeros(3),
                                              quaternion=Quaternion([1., 0., 0., 0.])))

方案2：通过输入端口传递姿态指令（符合Drake设计规范的推荐方式） #

直接在其他系统中修改MultibodyPlant的状态并非Drake的最佳实践，更规范的方式是给Wrapper添加输入端口传递目标姿态，再在更新逻辑中通过根上下文完成姿态设置。

示例代码（简化版）：

class MBPWrapper(LeafSystem):
    def __init__(self, plant, adaptive_vr_system, internal_object_list):
        LeafSystem.__init__(self)
        self.plant = plant
        self.body = internal_object_list[0]  # 假设此处为目标刚体
        # 声明接收目标姿态的抽象输入端口
        self.target_pose_port = self.DeclareAbstractInputPort(
            "target_pose", AbstractValue.Make(RigidTransform())
        )
        self.DeclarePeriodicDiscreteUpdateEvent(
            period_sec=2e-2,
            offset_sec=0.,
            update=self.Update)

    def Update(self, context, discrete_state):
        # 读取输入的目标姿态
        target_pose = self.target_pose_port.Eval(context)
        root_context = context.get_root_context()
        plant_context = self.plant.GetMyContextFromRoot(root_context)
        self.plant.SetFreeBodyPose(plant_context, self.body, target_pose)

方案3：初始化阶段提前设置姿态 #

如果仅需在仿真启动前设置初始姿态，可在Diagram构建完成、Simulator初始化之前，用你已获取的plant_context直接设置：

self.diagram = builder.Build()
self.diagram_context = self.diagram.CreateDefaultContext()
self.plant_context = self.plant.GetMyContextFromRoot(self.diagram_context)
# 在这里设置初始姿态
self.plant.SetFreeBodyPose(self.plant_context, body,
                           RigidTransform(p=np.zeros(3),
                                          quaternion=Quaternion([1., 0., 0., 0.])))
# 再初始化仿真器
self.simulator = Simulator(self.diagram, self.diagram_context)
self.simulator.Initialize()

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