自定义机器人复现ManipulationStation推块任务时控制器未连接导致机器人自由下落的问题求助

我正在尝试搭建一个包含机器人和方块的仿真场景，核心任务是让机器人将方块推到指定区域。为此，我打算参照Teleop Example (2D)的示例逻辑来实现，但遇到了棘手的问题：当我运行自己写的代码时，机器人会直接自由下落，明显是控制器没有和场景中的机器人正确连接上。

我之前看过相关的问答帖子，大家都提到可以参考ManipulationStation类的实现思路，但我照着这个方向写出来的代码就是不工作。下面是我的问题代码：

def AddXarm(plant, scene_graph=None):
    if scene_graph is None:
        parser = Parser(plant, scene_graph)
    else:
        parser = Parser(plant)
    xarm = parser.AddModels(temp_urdf)[0]
    return xarm

def add_block(plant, scene_graph=None):
    parser = Parser(plant, scene_graph)
    tblock = parser.AddModels(urdf_path)[0]
    return tblock

def MakeHardwareStation():
    robot_builder = RobotDiagramBuilder(time_step=time_step)
    builder = robot_builder.builder()
    sim_plant = robot_builder.plant()
    scene_graph = robot_builder.scene_graph()
    parser = Parser(sim_plant)
    xarm = AddXarm(sim_plant, scene_graph)
    sim_plant.Finalize()
    xarm_positions = sim_plant.num_positions(xarm)
    xarm_velocities = sim_plant.num_velocities(xarm)
    controller_plant = sim_plant
    control_num_positions = controller_plant.num_positions()
    xarm_controller = builder.AddSystem(
        InverseDynamicsController(
            controller_plant,
            kp=[100.0] * control_num_positions,
            ki=[0.0] * control_num_positions,
            kd=[20.0] * control_num_positions,
            has_reference_acceleration=False,
        )
    )
    builder.Connect(
        sim_plant.get_state_output_port(),
        xarm_controller.get_input_port_estimated_state(),
    )
    builder.Connect(
        xarm_controller.get_output_port_control(),
        sim_plant.get_actuation_input_port(),
    )
    desired_state_from_position = builder.AddSystem(
        StateInterpolatorWithDiscreteDerivative(
            xarm_positions, time_step, suppress_initial_transient=True
        )
    )
    desired_state_from_position.set_name("desired_state_from_position")
    builder.Connect(
        desired_state_from_position.get_output_port(),
        xarm_controller.get_input_port_desired_state(),
    )

    builder.ExportOutput(sim_plant.get_state_output_port(), "xarm_state_estimated")
    builder.ExportInput(
        desired_state_from_position.get_input_port(), "xarm_state_desired"
    )

    builder.ExportInput(
        sim_plant.get_applied_generalized_force_input_port(),
        "applied_generalized_force",
    )
    builder.ExportInput(
        sim_plant.get_applied_spatial_force_input_port(),
        "applied_spatial_force",
    )
    builder.ExportOutput(scene_graph.get_query_output_port(), "query_object")

    diagram = robot_builder.Build()
    diagram.set_name("station")
    return diagram

class MultibodyPoseToConfig(LeafSystem):
    def __init__(self, plant: MultibodyPlant, frame: Frame):
        LeafSystem.__init__(self)
        self.plant = plant
        self.frame = frame
        self.plant_context = plant.CreateDefaultContext()
        self.DeclareAbstractInputPort(
            "pose",
            AbstractValue.Make(RigidTransform()),
        )
        self.DeclareVectorOutputPort(
            "config",
            6,
            self._CalcOutput,
        )

    def _CalcOutput(self, context, output):
        pose = self.get_input_port().Eval(context)
        ik = InverseKinematics(self.plant, self.plant_context)
        ik.AddPositionConstraint(
            frameB=self.frame,
            p_BQ=[0, 0, 0],
            frameA=self.plant.world_frame(),
            p_AQ_lower=pose.translation() - 1e-4,
            p_AQ_upper=pose.translation() + 1e-4,
        )
        ik.AddOrientationConstraint(
            frameAbar=self.frame,
            R_AbarA=pose.rotation(),
            frameBbar=self.plant.world_frame(),
            R_BbarB=RotationMatrix(),
            theta_bound=0.01,
        )
        prog = ik.prog()
        result = Solve(prog)
        q_desired = result.GetSolution(ik.q())
        output.set_value(q_desired[:6])

def teleop_3d():
    meshcat = StartMeshcat()
    builder = DiagramBuilder()
    plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=time_step)
    block = add_block(plant, scene_graph)
    xarm = AddXarm(plant, scene_graph)
    add_ground_with_friction(plant)
    plant.Finalize()
    station = builder.AddSystem(MakeHardwareStation())
    meshcat.DeleteAddedControls()
    teleop = builder.AddSystem(
        MeshcatPoseSliders(
            meshcat,
            initial_pose=RigidTransform(
                RotationMatrix(RollPitchYaw(3.14, 0, 0)), np.array([0.2, 0.2, 0.2])
            ),
            lower_limit=[0, -0.5, -np.pi, -0.6, -0.8, 0.0],
            upper_limit=[2 * np.pi, np.pi, np.pi, 0.8, 0.3, 1.1],
        )
    )
    ik_sys = builder.AddSystem(
        MultibodyPoseToConfig(
            plant, plant.GetBodyByName("push_gripper_base_link").body_frame()
        )
    )
    builder.Connect(teleop.get_output_port(), ik_sys.get_input_port())
    builder.Connect(
        ik_sys.get_output_port(), station.GetInputPort("xarm_state_desired")
    )

    AddDefaultVisualization(builder, meshcat)
    diagram = builder.Build()
    simulator = Simulator(diagram)
    simulator.get_mutable_context()
    simulator.set_target_realtime_rate(0)
    simulator.AdvanceTo(np.inf)

teleop_3d()

我理解理论上应该是用“控制侧机器人模型”来驱动“仿真场景里的机器人”，但实际代码里到底哪里出了问题，我完全摸不着头绪，有没有大佬能帮忙排查一下？

备注：内容来源于stack exchange，提问作者corehammer