Update self-collision barrier unit test

[stephane-caron]

This PR investigates #130.

Failing unit test

The test considers the following collision pair:

        self.colliding_objects_pair_id = 24  # Between second and third bodies

Indeed, pair 24 is between bodies iiwa_link_2_0 and iiwa_link_3_1 of the iiwa14 description:

In [12]: first = self.configuration.collision_model.collisionPairs[24].first

In [13]: second = self.configuration.collision_model.collisionPairs[24].second

In [14]: self.configuration.collision_model.geometryObjects[first].name
Out[14]: 'iiwa_link_2_0'

In [15]: self.configuration.collision_model.geometryObjects[second].name
Out[15]: 'iiwa_link_3_1'

However, the test checks h[self.colliding_objects_pair_id], where the ordering of the barrier vector h depends on sorted distances:

        closest_pairs_idxs = np.argpartition(-distances, -self.dim)[
            -self.dim :
        ]

        return distances[closest_pairs_idxs]

If this analysis is correct, h[self.colliding_objects_pair_id] may not correspond to the distance for pair 24 after sorting, so it will not necessarily be negative 🤔

Proposed update

This PR updates the test to:

1. Put the robot in the following configuration:

2. Check that the lowest value in h is negative (as there is a self-collision)

[stephane-caron]

@domrachev03 Do you agree that this update tests what we want to test here?