0

一旦我将所有模型添加到我的工厂以查看每个 BodyIndex 及其相应的名称，我就会进行简单的检查。我注意到，GetBodyIndices()无论模型实例索引如何，worldbody 都会在每次调用中注册，而且令人震惊的是，它的主体索引非零。

我注意到这一点，因为我第一次在没有名称的情况下进行了检查，并注意到每个模型实例的每个列表末尾都有一个奇怪的高 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

世界机构的指数似乎随机高于 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";
            }
        };

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

4

1 回答 1

2

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

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

请注意，您使用的是“<=”而不是经典的 C“<”，因为索引i应该从0to 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";
  }
};

希望在这段代码中没有什么可以捕捉到的。

于 2019-07-12T16:26:26.580 回答