world body BodyIndex is non-zero?

Question

I have a simple check once I add all my models to my plant to review every BodyIndex and its corresponding name. I noticed that the worldbody registers in each call to GetBodyIndices() regardless of model instance index, and, alarmingly, with a non-zero body index.

I noticed this since I first made the check without the names, and noticed a weird high Bodyindex at the end of each list per model instance, like so:

Robot body indices:
1  
2  
3  
4  
5  
6
21997

Box body indices:
18  
21997

Then, when I started outputting the names, it ran once through and showed this:

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

The index for the world body seems random above 20000.

In subsequent runs, it threw this error each time.

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

My code is like so now (same for the robot model instance):

        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";
            }
        };

Not sure why it's registering like this, but something curious I came across.

score 2 · Accepted Answer · answered Jul 12 '19 at 16:26

You have several bugs and style problems in that code. The biggest problem however is in the line:

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

Notice you are using "<=" instead of the classical C "<", since index i should go from 0 to size()-1.

A more C++11 way of doing this would be:

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";
  }
};

Hopefully nothing to catch in this code.

world body BodyIndex is non-zero?

1 Answers1