Missing conversion of position from center to rear axle?

CommonRoad-Search
1

In SearchBaseClass.is_collision_free() the position of the vehicle is shifted from the center of the rear axle to the of the vehicle. This is explained with the following comment:

# positions of states in input path have to be shifted to the center of the vehicle, since the positions
# refer to the rear axis (reference point) when using motion primitives for the KS model
# the collision checker requires the center point position to create the collision object

Similar conversions are also done in other places (e.g. create_trajectory_from_list_states()), but I think the inverse transformation is missing in at least one place.
When constructing the initial node in all of the provided motion planners, the initial state of the planning problem is simply copied to initial node. This initial state seems to use the center of the car, but it is now interpreted as the center of the rear axle.

An easy place to test this is PRI_Barceloneta-2_1_T-1:
The ego vehicle starts directly behind an obstacle. Due to the bug, the initial node uses a position about 1.5m too far forward which means that the initial node is always colliding (checked with is_collision_free(SearchBaseClass.node_initial.list_paths[-1])).
When converting the initial state correctly, this error does not happen (copied from is_collision_free() but with with a negative sign):

new_state_initial = self.state_initial.translate_rotate(
    -np.array([self.rear_ax_dist * np.cos(self.state_initial.orientation),
               self.rear_ax_dist * np.sin(self.state_initial.orientation)]), 0)

Screenshot from 2022-11-21 18-52-45
Screenshot from 2022-11-21 18-52-45809×762 79.8 KB

2

Hi ge86sug,

thanks for this hint. We are currently looking into this and check whether we will have to modify the provided code. In case we do so we’ll let you know and announce it on Moodle separately again.

Best,
Gerald

3

Hi ge86sug,

you were right, there was a bug with the initial state, thanks a lot for spotting this. We have fixed it and pushed the updated code. Please note, that you will also have to re-install the requirements as the version of one dependency (commonroad-drivability-checker) has changed due to a minor change which was necessary in this package. We have also announced this separately on Moodle .

Best,
Gerald

4

Thanks for fixing the issue!

But to be honest I am not really happy with the solution. There are now approximately 10 places where the position is transformed. That is very prone to errors in the future. But more importantly, it is very unintuitive for students who haven’t spend hours debugging this. They could very easily forget to add such a transformation to their own code and introduce a bug which they might never be able to fix because they don’t realize this transformation is necessary.
I think its fundamentally a bad pattern to have an attribute (KSState.position, InitialState.position) which has a different meaning (car center vs rear axle center) depending only on context external to the object. Maybe there should be a new attribute position_rear_axle so that the position attribute is only used with the center of car meaning?

There is also still at least one scenario with a colliding initial state: FRA_Anglet-2_1_T-1

Which leads to the question why this repo does not seem to have any kind of tests. Both the initial bug and the bug which still seems to exist somewhere could have been caught with a very simple test (iterate over all scenarios and check that the initial state is collision free).
I like the core idea of this exercise a lot, but its quite exhausting that many of the problems I am having are not related to my code at all. I think the exercise would be greatly improved if we knew it were possible to solve all 500 scenarios without changing any internal code (only your own planner and config files).