为定制机器人复制 ManipulationStation

Question

我正在尝试使用机器人+块设置模拟，任务是机器人推动块定义的区域。为此，我尝试从 Teleop 示例 (2D) 笔记本中重现该示例： https://deepnote.com/workspace/Manipulation-ac8201a1-470a-4c77-afd0-2cc45bc229ff/project/6e32ce10-e406-4026-abfb-00be78c478cb/notebook/intro-7553c700044345fe8ce 505eecfcf087f?duplicate=true&utm_medium=产品共享内容&utm_source=duplicate-cta&utm_campaign=duplicate&utm_content=6e32ce10-e406-4026-abfb-00be78c478cb

我看到其他帖子回答了看看 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()

我从概念上理解工厂中的“模拟机器人”是由“控制机器人”的模型驱动的。但我不确定实际实施中出了什么问题。

Answer 1

是的，按照我添加的方式放置两个 sim 植物是导致问题的原因。修改问题以拥有一个用于控制器的内部模拟工厂（替换控制器使用的机器人模型）和另一个用于模拟的内部模拟工厂（替换真实的机器人）解决了问题。