如何在Drake(pydrake)控制环中获取图像实现视觉伺服？

问题 #

我希望使用pydrake搭建视觉伺服仿真场景，但尚未明确如何为自定义LeafSystem的端口接线，以在控制环中读取相机图像。当前我的关节位置控制器结构如下：

class MyController(LeafSystem):

    def __init__(self, n_joints=7):
        super().__init__()

        self.position_cmd_port = self.DeclareVectorOutputPort(
            "my_command",
            BasicVector(2 * n_joints),
            self._get_position_command
        )

    def _get_position_command(self, context, command_vec):        
        # TODO I need to get an RGB image here from my camera
        rgb_img = None

        command = command_vec.get_mutable_value()
        # TODO Do the image processing and control command computation
        # needed here to populate joint position (and/or velocity) command 

我注意到pydrake.systems.sensors.ImageWriter类拥有DeclareImageInputPort方法，且该类继承自LeafSystem，因此考虑让控制器继承ImageWriter类：

class MyController(ImageWriter):
   ...

再通过DeclareImageInputPort接入相机图像，从而在_get_position_command函数中获取图像。但从概念上看，让控制器继承ImageWriter并不合理，请问是否有更优的方法在控制环中获取图像？此外，是否有Drake中实现视觉伺服的示例可供参考？

解决方案 #

1. 直接在自定义LeafSystem中声明图像输入端口 #

完全不需要继承ImageWriter，你可以直接在自己的MyController类里通过DeclareAbstractInputPort声明图像输入端口——因为Drake中的图像属于抽象值类型（AbstractValue）。

修改后的控制器代码示例：

from pydrake.systems.sensors import ImageRgba8U  # 根据实际使用的图像类型选择，比如ImageRgb8U、ImageDepth32F等

class MyController(LeafSystem):

    def __init__(self, n_joints=7):
        super().__init__()
        # 声明图像输入端口，指定对应图像类型
        self.image_input_port = self.DeclareAbstractInputPort(
            name="camera_image",
            model_value=ImageRgba8U()  # 用默认尺寸图像作为模型，实际会由相机输出覆盖
        )

        self.position_cmd_port = self.DeclareVectorOutputPort(
            "my_command",
            BasicVector(2 * n_joints),
            self._get_position_command
        )

    def _get_position_command(self, context, command_vec):        
        # 从输入端口读取图像
        rgb_img = self.image_input_port.Eval(context)

        # 可直接获取图像数据进行处理：
        # img_data = rgb_img.data  # 对应numpy数组格式
        # height = rgb_img.height()
        # width = rgb_img.width()

        command = command_vec.get_mutable_value()
        # 在这里完成图像处理与控制指令计算逻辑

后续搭建仿真系统时，只需用builder.Connect()将相机的图像输出端口与控制器的camera_image输入端口连接即可。

2. Drake中的视觉伺服参考示例 #

Drake的官方模块与示例中有不少可参考的视觉伺服实现：

• Manipulation Station框架：该框架集成了机器人、相机与控制器逻辑，可参考其中如何将相机数据接入控制环的流程。
• Pick-and-Place示例：examples/manipulation/pick_and_place目录下的代码展示了视觉识别目标+伺服抓取的完整流程，包含视觉数据读取与处理环节。
• 官方视觉伺服教程：Drake内置教程中有专门的视觉伺服章节，覆盖从相机数据获取到控制律设计的全流程实现。

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