如何用LCM消息初始化ManipulationStationHardwareInterface而非默认值？

问题重现 #

在使用Drake控制KUKA IIWA机器人、通过ManipulationStationHardwareInterface做LCM通信，并以mock_station_simulation测试时，会遇到初始化后硬件接口输出默认零向量的问题：即便给iiwa_position输入端口设置了current_position，调用simulator.Initialize()后，iiwa_position_measured的输出仍是零向量，而非预设的位置值。示例代码如下：

drake::systems::DiagramBuilder<double> builder;
auto *interface = builder.AddSystem<ManipulationStationHardwareInterface>();
interface->Connect();
auto diagram = builder.Build();

drake::systems::Simulator<double> simulator(*diagram);
auto &simulator_context = simulator.get_mutable_context();
auto &interface_context = interface->GetMyMutableContextFromRoot(&simulator_context);
interface->GetInputPort("iiwa_position").FixValue(&interface_context, current_position);

simulator.set_publish_every_time_step(false);
simulator.set_target_realtime_rate(1.0);
simulator.Initialize();
auto q = interface->GetOutputPort("iiwa_position_measured").Eval(interface_context);
std::cout << "after initialization, interface think that the position of robot is" << q << std::endl;

该问题会导致DifferentialInverseKinematicsIntegrator用默认零向量初始化内部状态，触发机器人剧烈运动；同时LogVectorOutput的第一条记录也会是无效的默认值。

原因分析 #

问题根源在于ManipulationStationHardwareInterface内部依赖的LcmSubscriberSystem：系统初始化阶段，订阅器不会自动读取或处理已有的LCM消息，而是使用端口的默认值（零向量）。即便给输入端口设置了位置，硬件接口的测量输出是从LCM订阅器获取的，而订阅器在初始化时还未处理任何消息，因此输出默认值。

解决方案 #

1. 手动给内部LCM订阅器设置初始消息 #

直接找到ManipulationStationHardwareInterface内部的LCM订阅器，手动传入预设的位置消息，更新其输出状态：

simulator.Initialize();

// 获取硬件接口内部的IIWA状态订阅器
const auto& iiwa_state_sub = interface->GetSubsystemByName<drake::lcm::LcmSubscriberSystem>("iiwa_state_subscriber");
auto& iiwa_sub_context = iiwa_state_sub.GetMyMutableContextFromRoot(&simulator_context);

// 构造对应的LCM状态消息
lcmt_iiwa_status iiwa_status_msg;
iiwa_status_msg.utime = 0;
iiwa_status_msg.num_joints = current_position.size();
iiwa_status_msg.joint_position_measured = current_position;
// 按需填充其他必要字段（比如关节速度、力矩等）

// 让订阅器手动处理这条消息，更新输出
iiwa_state_sub.HandleMessage(iiwa_sub_context, &iiwa_status_msg);

// 此时再获取测量输出就是预设的current_position了
auto q = interface->GetOutputPort("iiwa_position_measured").Eval(interface_context);

2. 改进逆运动学积分器的初始化逻辑 #

使用DifferentialInverseKinematicsIntegrator时，无需断开robot_state输入端口，只需在初始化后手动同步其内部状态：

// 假设integrator是你创建的DifferentialInverseKinematicsIntegrator实例
auto& integrator_context = integrator->GetMyMutableContextFromRoot(&simulator_context);
integrator->SetPositions(&integrator_context, current_position);

这样既保留了输入端口的连接，又确保积分器用实际位置初始化，避免运动突变。

3. 初始化后触发一次LCM消息发布（仅适用于mock环境） #

在mock测试场景中，可在初始化后手动发布一次包含current_position的LCM消息，让订阅器自动接收处理：

// 初始化模拟器后
simulator.Initialize();

// 构造并发布IIWA状态消息
lcmt_iiwa_status iiwa_status_msg;
iiwa_status_msg.utime = drake::systems::Simulator::Now(&simulator_context) * 1e6;
iiwa_status_msg.num_joints = current_position.size();
iiwa_status_msg.joint_position_measured = current_position;

// 假设你有一个LCM实例
lcm_.publish("IIWA_STATUS", &iiwa_status_msg);

// 让模拟器步进来处理消息
simulator.AdvanceTo(simulator_context.get_time() + 1e-3);

补充说明 #

目前Drake的LcmSubscriberSystem默认不会在初始化阶段自动拉取最新LCM消息，必须通过手动干预来设置初始状态。上述方法可有效解决初始化后输出默认值的问题，让硬件接口和依赖模块使用正确的机器人初始位置。

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