world body BodyIndex 是非零的？

Question

将所有模型添加到我的工厂后，我会进行一个简单的检查，以检查每个 BodyIndex 及其对应的名称。我注意到 worldbody 在每次调用 GetBodyIndices() 时都会注册，而不管模型实例索引如何，而且令人担忧的是，它的主体索引非零。

我注意到这一点，因为我第一次进行没有名称的检查，并注意到每个模型实例的每个列表末尾都有一个奇怪的高 Bodyindex，如下所示：

Robot body indices:
1  
2  
3  
4  
5  
6
21997

Box body indices:
18  
21997

然后，当我开始输出名字时，它运行显示了一遍：

Robot body indices:
1 : center
2 : div_link_0
3 : div_link_1
4 : div_link_2
5 : div_link_3
6 : div_link_4
22058 : WorldBody

Box body indices:
18 : box
22058 : WorldBody

world body 的指数似乎运行dom 在 20000 以上。

在随后的运行中，每次都抛出这个错误。

terminate called after throwing an instance of 'drake::detail::assertion_error'
  what():  Failure at bazel-out/k8-opt/bin/multibody/tree/_virtual_includes/multibody_tree_core/drake/multibody/tree/multibody_tree.h:648 in get_body(): condition 'body_index < num_bodies()' failed.
Aborted

我的代码现在是这样的（机器人模型实例也一样）：

        std::cout << "\nBox body indices:\n";
        for(unsigned int i=0; i<=plant.GetBodyIndices(box_model_instance).size(); i++) {
            auto body_index = plant.GetBodyIndices(box_model_instance)[i];
            try {
                std::string name = plant.get_body(plant.GetBodyIndices(box_model_instance)[i]).name();
                std::cout << body_index << " : " << name << "\n";
            } catch (...) {
                std::cout << body_index << " ! failed world body\n";
            }
        };

不确定为什么会这样注册，但我遇到了一些奇怪的事情。

Answer 1

您的代码中存在多个错误和样式问题。然而，最大的问题在于：

for(unsigned int i=0; i<=plant.GetBodyIndices(box_model_instance).size(); i++)

请注意，您使用的是“<=”而不是经典的 C 语言“<”，因为索引 i 应该从 0 到 size()-1。

一种更符合 C++11 的方法是：

std::cout << "\nBox body indices:\n";
for(auto body_index : plant.GetBodyIndices(box_model_instance)) {
  try {
    const std::string name = plant.get_body(body_index).name();
    std::cout << body_index << " : " << name << "\n";
  } catch (...) {
    std::cout << body_index << " ! failed world body\n";
  }
};

希望这段代码没有什么可捕捉的。

world body BodyIndex 是非零的？

world body BodyIndex is non-zero?

drake