pdm_closed_planner trajectory's states have no dynamic information

[CristianGariboldi]

Hello, I was inspecting the trajectory generated by the pdm_closed_planner and I realized that it contains just static information and not dynamic states (I think it would be the same for the other planners as well):

InterpolatedTrajectory with 81 states
(wrapped_fn pid=1814060) EgoState(time=1633419573.0001209), Position=(365882.03552731016, 143116.11716887038, -1.80036579096818), Velocity=(0.0, 0.0), Acceleration=(0.0, 0.0), Steering_Angle=0.0)

This is the output (I showed just one state) I get when I print the states of the trajectory with this code (in ego_state.py):

def __str__(self):
        return (
            f"EgoState(time={self.time_point.time_s}), "
            f"Position=({self.rear_axle.x}, {self.rear_axle.y}, {self.rear_axle.heading}), "
            f"Velocity=({self.dynamic_car_state.rear_axle_velocity_2d.x}, {self.dynamic_car_state.rear_axle_velocity_2d.y}), "
            f"Acceleration=({self.dynamic_car_state.rear_axle_acceleration_2d.x}, {self.dynamic_car_state.rear_axle_acceleration_2d.y}), "
            f"Steering_Angle={self.tire_steering_angle})"
        )

and this (in interpolated_trajectory.py):

def __str__(self):
        return f"InterpolatedTrajectory with {len(self._trajectory)} states"

    def print_states(self):
        for state in self._trajectory:
            print(state)

and finally this (in pdm_closed_planner.py):

def compute_planner_trajectory():
     ......................................
     ......................................
      print(trajectory)
      trajectory.print_states()         

      return trajectory 

So, my question is: do you also generate a velocity profile for every state of the generated trajectory? If yes, how do you do it and how can you have access to it?

Also, do you take into account the left and right bounds of the road when computing the trajectory? If yes, how do you extract/generate them and how can you have access to it?

Thank you very much in advance, very appreciated!

[DanielDauner]

Hi @CristianGariboldi

The LQR controller of nuPlan extracts the target acceleration and steering based on the position values of the trajectory. Thus, the dynamic states of the output trajectory have zero effect, and we left the values blank.

There are multiple options for our planners to retrieve the dynamic states. PDM-Closed calculates the velocity and acceleration in each propagating step of the IDM policy. Another option is to calculate the intended acceleration and velocity from the positions. You may find some functions in pdm_comfort_metrics.py or nuPlan’s state_extractors.py helpful.

We don’t explicitly consider the left and right bounds of the road. However, we use an occupancy map (initialized here) to determine if geometries are inside or outside the drivable area. You can get the boundaries from the nuPlan lane object or lane connector object.

Let me know if you need any further details or clarification.

[DanielDauner]

I am closing this issue for now. Feel free to re-open if you have further questions!

[CristianGariboldi]

@DanielDauner sorry for my late reply, busy times. Anyway, thank you very much for your feedback, very helpful!