scene_graph.get_source_pose_port() 是否混淆了输入的顺序？

Question

目前，我正在尝试在 drake 中可视化多个机器人。这意味着将不考虑动态，MBP 将不是一个系统。 MBP只是用来给世界添加urdf模型，辅助做场景图

我是如何移除动态并使其成为纯粹的可视化工具的，如下所示：

我把机器人关节位置发给了

builder.AddSystem<MultibodyPositionToGeometryPose<double>>(plant);

然后将其连接到

scene_graph.get_source_pose_port(plant.get_source_id().value()));

部分代码如下：

// Two robots are in the simulations. Their position states will be muxed into one
// in order to send to MultibodyPositionToGeometryPose

// For now, robotA's position states will come from a constant zero vector, 
// while robotB's will come from a receiver. 

std::vector<int> mux_sizes = {  14,27 };

auto plant_states_mux = 
        builder.AddSystem<systems::Multiplexer>(mux_sizes);    
auto robots_to_pose =
        builder.AddSystem<MultibodyPositionToGeometryPose<double>>(plant);

VectorX<double> constant_vector = VectorX<double>::Zero(14);
auto constant_zero_source =
      builder.template AddSystem<systems::ConstantVectorSource<double>>(
      constant_vector);

builder.Connect(constant_zero_source->get_output_port(),
                plant_states_mux->get_input_port(robotA_id));
builder.Connect(robotB_state_receiver->get_position_measured_output_port(),
                plant_states_mux->get_input_port(robotB_id));
builder.Connect(plant_states_mux->get_output_port(0), 
                robots_to_pose->get_input_port());
builder.Connect(robots_to_pose->get_output_port(), 
                scene_graph.get_source_pose_port(plant.get_source_id().value()));

drake::geometry::ConnectDrakeVisualizer(&builder, scene_graph);

直到 MultibodyPositionToGeometryPose 端口，我才能够检查关节位置的顺序是否正确。这是通过查看函数“CalcGeometryPose”并将 plant_context_ 值输出到终端来完成的。

但是，当我运行模拟并将关节位置发送到 robot_B 时，它移动了错误的关节。此外，即使我没有向它发送任何状态，robot_A 也会移动它的关节。

我发现这是由于机器人端口的重新排序（在内部

plant_.get_geometry_poses_output_port().Calc(*plant_context_, output);

或在场景图中）

端口重新排序如下：每个机器人的基本位置（4 个四元数，3 个 xyz）的 7 个自由度列在最前面。然后关节之后随机排序。

EX:

robotA qw, robotA qx, robotA qy, robotA qz, robotA x, robotA y, robotA z,

robotB qw, robotB qx, robotB qy, robotB qz, robotB x, robotB y, robotB z,

robotA joint 1, robotA joint 2, robotB joint 1, robotB joint 2, robotA joint 3 ... etc

所以，我认为向 robotA 关节 1 发送状态会在 robotA 的 z 位置之后。但是，重新排序使其成为 robotB qw（对于此示例）。

我试图重新排序端口布线以匹配接头。但是当我这样做时，端口会切换命令。

我的问题是，场景图或 MultibodyPositionToGeometryPose 是否重新排序位置状态位置？（也许是为了效率什么的？）

如果是这样，有没有办法防止这种情况发生？

谢谢

Answer 1

我没有详细查看您的代码，但使用多路复用器基本上假设机器人 2 的所有状态立即且连续地跟随机器人 1 的状态。一个很好的假设，但这不是 MBP 所做的。一般而言，该假设是不正确的。但是，MBP 确实提供了说明某些特定顺序的 API。

如果您确实想要向量形式的单个机器人的状态，最好使用模型实例。例如，您可以执行 GetPositionsAndVelocities(cotext, model_instace)，这基本上完成了上面的多路复用器工作，但我们需要注意的是 MBP 确实知道状态是如何存储的。

另一个推荐的选项是使用文档化的 public API。一般来说，如果我们记录某些东西，我们是认真的（当然除非有错误，但我们会尝试）。因此，我建议您查看 Joint::position_start() 之类的内容和相关方法，以了解事物的索引方式，并可能构建您自己的地图以了解您希望如何对事物进行索引。

最后，对于自由体，你可以说MBP::SetFreeBodyPose()来设置姿势（参见Body::is_free()和Body:中的相关方法）。

但是，您似乎摆脱了 MBP（知道这些映射的那个）而只关心可视化？也许你可以使用两个 MultibodyPositionToGeometryPose，每个机器人一个？

希望对您有所帮助。

scene_graph.get_source_pose_port() 是否混淆了输入的顺序？

Does scene_graph.get_source_pose_port() mix up the order of the inputs?

drake