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

Question

目前，我正试图想象德雷克的多个机器人。这意味着动力将不被考虑，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);

或在场景图中)

端口重新排序如下:7自由度的，每个机器人的基础位置(4四元数，3×z)首先列出。然后对关节进行随机排序。

例：

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 z位置之后，向robotA联合1发送一个状态。然而，重新排序使得它成为robotB qw (在本例中)。

我试着重新安排端口线路以配合接头。但是当我这么做的时候，港口就会换订单。

我的问题是，透视图或MultibodyPositionToGeometryPose是否重新排序位置位置？(也许是为了效率什么的？)

如果是的话，是否有办法防止这种情况发生？

谢谢

Answer 1

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

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

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

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

但是，您似乎摆脱了MBP (知道这些映射的MBP)，而只关心viz？也许你可以用两台MultibodyPositionToGeometryPose，每个机器人一台？

希望能帮上忙。