Question

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

自从我第一次进行不带名称的检查以来，我注意到了这一点，并注意到每个模型实例的每个列表末尾都有一个奇怪的高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";
            }
        };

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

Answer 1

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

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

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

更多的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";
  }
};

希望这段代码没什么。

世界身体BodyIndex是否为非零？

1 个答案: