在初始化期间从机器人硬件姿势更新 drake 系统状态

Question

我一直在尝试在 drake 中使用 Kuka IIWA 硬件设置自定义操作站。我得到了硬件接口工作。当运行一个关节遥操作代码（改编自drake/examples/manipulation_station/joint_teleop.py）时，机器人会先剧烈跳动（所有关节都试图移动到0位置），然后继续正常运行。深入挖掘后，我发现这是由 FirstOrderLowPassFilter 系统引起的。在稍微推进模拟 (simulator.AdvanceTo(1e-6)) 以评估 LCM 消息以设置初始 GUI 滑块-filter_initial_output_value-植物关节位置等以匹配硬件时，FirstOrderLowPassFilter 输出一个瞬时值0. 这会将实例的 IIWA_COMMAND 位置设置为零并导致抖动。 我怎样才能避免这种行为？

作为解决方法，我在初始化 drake 系统之前分别订阅来自硬件的原始 LCM 消息，并在推进模拟之前设置 filter_initial_output_value。这是推荐的方式吗？

Answer 1

FirstOrderLowPassFilter有一个设置初始值的方法。 https://drake.mit.edu/doxygen_cxx/classdrake_1_1systems_1_1_first_order_low_pass_filter.html#aaef7539cfbf1acfa0cf487c371bc5360

它在您复制的示例中使用： https://github.com/RobotLocomotion/drake/blob/master/examples/manipulation_station/joint_teleop.py#L146

Answer 2

我认为你所做的（手动读取 LCM 消息）很好。

或者，看看 DiscreteDerivative 如何提供 suppress_initial_transient = true 选项。也许我们可以向 FirstOrderLowPassFilter 添加一个类似的选项（通过不受限制的更新事件），以便在 t == 0 时从输入中采样初始输出值。但是启动的事件排序可能仍然很困难。我们基本上需要按照数据流顺序初始化系统，包括在事件触发时刷新输出端口，这在本地不受支持。

在另一种选择中，也许我们可以将 IIWA_COMMAND 发布者配置为在 t == 0 时不发布，而是仅在 t >= 0.005 时发布。