Skip to content

IDM Trajectory

IDMTrajectory

Bases: Trajectory

IDM Trajectory

Source code in commonroad_idm_planner/idm_trajectory.py 
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
class IDMTrajectory(Trajectory):
    """
    IDM Trajectory
    """

    def __init__(
        self, initial_time_step: int, state_list: List[IDMState], delta_t: float = 0.1
    ) -> None:
        """
        IDM Trajectory
        :param initial_time_step: initial state time step
        :param state_list: list of states of initial state
        :param delta_t: time step size
        """
        super().__init__(initial_time_step, state_list)

    def remove_all_states_later_than_state(self, idm_state: IDMState) -> None:
        """
        Remove all states later than given state
        :param idm_state: last state that should be kept
        """
        states = [s for s in self._state_list if s.time_step > idm_state.time_step]
        for state in states:
            self.state_list.remove(state)

    def calc_steering_angle_from_orientation(
        self, wheelbase_rear_to_cog: float, dt: float
    ) -> None:
        """
        Updates states in state list by calculating the steering angle as the difference of orientations
        of to states.
        :param wheelbase_rear_to_cog: distance of the rear wheelbase to the center of gravity
        :param delta_t: time between steps
        """
        for idx in range(len(self._state_list) - 1):
            delta_orientation: float = wrap_to_pi(
                self._state_list[idx + 1].orientation
                - self._state_list[idx].orientation
            )
            self._state_list[idx].steering_angle = math.atan2(
                delta_orientation * wheelbase_rear_to_cog,
                (dt * self._state_list[idx].velocity),
            )

        self._state_list[-1].steering_angle = 0.0

    def to_cr_dynamic_obstacle(
        self, vehicle_width: float, vehicle_length: float, vehicle_id: int
    ) -> DynamicObstacle:
        """
        Converts trajectory cr dynamic obstacle for plotting
        :param vehicle_width: vehicle width
        :param vehicle_length: vehicle length
        :param vehicle_id: vehicle id
        :return: cr dynamic obstacle
        """

        if not self._state_list:
            raise ValueError("State dict is empty")

        else:
            # convert to CR obstacle
            initial_state = self.state_list[0].convert_state_to_state(InitialState())

            shape = Rectangle(width=vehicle_width, length=vehicle_length)

            trajectory_prediction = TrajectoryPrediction(trajectory=self, shape=shape)
            # obstacle generation
            return DynamicObstacle(
                obstacle_id=30000 + vehicle_id,
                obstacle_type=ObstacleType.CAR,
                obstacle_shape=shape,
                initial_state=initial_state,
                prediction=trajectory_prediction,
            )

    def check_collision(
        self,
        collision_checker: CollisionChecker,
        scenario: Scenario,
        vehicle_width: float,
        vehicle_length: float,
    ) -> CollisionStatus:
        """
        Checks collision and returns true if so
        :param collision_checker: cr collision checker
        :param scenario: commonroad scenario
        :param vehicle_width: vehicle width
        :param vehicle_length: vehicle length
        :return: true if collision and time step
        """
        half_length: float = vehicle_length / 2
        half_width: float = vehicle_width / 2
        collide_flag: bool = False
        collision_step: int = None
        colliding_obstacles: List[int] = None
        ego_orientation: float = None
        ego_position: float = None

        for state in self.state_list:
            # check each pose for collisions
            ego = TimeVariantCollisionObject(state.time_step)
            occupancy = RectOBB(
                half_length,
                half_width,
                state.orientation,
                state.position[0],
                state.position[1],
            )
            ego.append_obstacle(occupancy)
            if collision_checker.collide(ego):
                collide_flag = True
                collision_step = state.time_step
                ego_orientation: float = state.orientation
                ego_position = state.position
                break
            else:
                pass

        if collide_flag:
            colliding_obstacles = colliding_obstacles_id_at_step(
                scenario=scenario,
                step=collision_step,
                ego_position=ego_position,
                ego_orientation=ego_orientation,
                ego_width=vehicle_width,
                ego_length=vehicle_length,
            )

        return CollisionStatus(
            collision_detected=collide_flag,
            time_step=collision_step,
            obstacle_ids=colliding_obstacles,
        )

__init__(initial_time_step, state_list, delta_t=0.1)

IDM Trajectory

Parameters:

Name Type Description Default
initial_time_step int

initial state time step

 required
state_list List[IDMState]

list of states of initial state

 required
delta_t float

time step size

 0.1
Source code in commonroad_idm_planner/idm_trajectory.py 
29
30
31
32
33
34
35
36
37
38
def __init__(
    self, initial_time_step: int, state_list: List[IDMState], delta_t: float = 0.1
) -> None:
    """
    IDM Trajectory
    :param initial_time_step: initial state time step
    :param state_list: list of states of initial state
    :param delta_t: time step size
    """
    super().__init__(initial_time_step, state_list)

calc_steering_angle_from_orientation(wheelbase_rear_to_cog, dt)

Updates states in state list by calculating the steering angle as the difference of orientations of to states.

Parameters:

Name Type Description Default
wheelbase_rear_to_cog float

distance of the rear wheelbase to the center of gravity

 required
delta_t

time between steps

 required
Source code in commonroad_idm_planner/idm_trajectory.py 
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
def calc_steering_angle_from_orientation(
    self, wheelbase_rear_to_cog: float, dt: float
) -> None:
    """
    Updates states in state list by calculating the steering angle as the difference of orientations
    of to states.
    :param wheelbase_rear_to_cog: distance of the rear wheelbase to the center of gravity
    :param delta_t: time between steps
    """
    for idx in range(len(self._state_list) - 1):
        delta_orientation: float = wrap_to_pi(
            self._state_list[idx + 1].orientation
            - self._state_list[idx].orientation
        )
        self._state_list[idx].steering_angle = math.atan2(
            delta_orientation * wheelbase_rear_to_cog,
            (dt * self._state_list[idx].velocity),
        )

    self._state_list[-1].steering_angle = 0.0

check_collision(collision_checker, scenario, vehicle_width, vehicle_length)

Checks collision and returns true if so

Parameters:

Name Type Description Default
collision_checker CollisionChecker

cr collision checker

 required
scenario Scenario

commonroad scenario

 required
vehicle_width float

vehicle width

 required
vehicle_length float

vehicle length

 required

Returns:

Type Description
CollisionStatus

true if collision and time step
Source code in commonroad_idm_planner/idm_trajectory.py 
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
def check_collision(
    self,
    collision_checker: CollisionChecker,
    scenario: Scenario,
    vehicle_width: float,
    vehicle_length: float,
) -> CollisionStatus:
    """
    Checks collision and returns true if so
    :param collision_checker: cr collision checker
    :param scenario: commonroad scenario
    :param vehicle_width: vehicle width
    :param vehicle_length: vehicle length
    :return: true if collision and time step
    """
    half_length: float = vehicle_length / 2
    half_width: float = vehicle_width / 2
    collide_flag: bool = False
    collision_step: int = None
    colliding_obstacles: List[int] = None
    ego_orientation: float = None
    ego_position: float = None

    for state in self.state_list:
        # check each pose for collisions
        ego = TimeVariantCollisionObject(state.time_step)
        occupancy = RectOBB(
            half_length,
            half_width,
            state.orientation,
            state.position[0],
            state.position[1],
        )
        ego.append_obstacle(occupancy)
        if collision_checker.collide(ego):
            collide_flag = True
            collision_step = state.time_step
            ego_orientation: float = state.orientation
            ego_position = state.position
            break
        else:
            pass

    if collide_flag:
        colliding_obstacles = colliding_obstacles_id_at_step(
            scenario=scenario,
            step=collision_step,
            ego_position=ego_position,
            ego_orientation=ego_orientation,
            ego_width=vehicle_width,
            ego_length=vehicle_length,
        )

    return CollisionStatus(
        collision_detected=collide_flag,
        time_step=collision_step,
        obstacle_ids=colliding_obstacles,
    )

remove_all_states_later_than_state(idm_state)

Remove all states later than given state

Parameters:

Name Type Description Default
idm_state IDMState

last state that should be kept

 required
Source code in commonroad_idm_planner/idm_trajectory.py 
40
41
42
43
44
45
46
47
def remove_all_states_later_than_state(self, idm_state: IDMState) -> None:
    """
    Remove all states later than given state
    :param idm_state: last state that should be kept
    """
    states = [s for s in self._state_list if s.time_step > idm_state.time_step]
    for state in states:
        self.state_list.remove(state)

to_cr_dynamic_obstacle(vehicle_width, vehicle_length, vehicle_id)

Converts trajectory cr dynamic obstacle for plotting

Parameters:

Name Type Description Default
vehicle_width float

vehicle width

 required
vehicle_length float

vehicle length

 required
vehicle_id int

vehicle id

 required

Returns:

Type Description
DynamicObstacle

cr dynamic obstacle
Source code in commonroad_idm_planner/idm_trajectory.py 
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
def to_cr_dynamic_obstacle(
    self, vehicle_width: float, vehicle_length: float, vehicle_id: int
) -> DynamicObstacle:
    """
    Converts trajectory cr dynamic obstacle for plotting
    :param vehicle_width: vehicle width
    :param vehicle_length: vehicle length
    :param vehicle_id: vehicle id
    :return: cr dynamic obstacle
    """

    if not self._state_list:
        raise ValueError("State dict is empty")

    else:
        # convert to CR obstacle
        initial_state = self.state_list[0].convert_state_to_state(InitialState())

        shape = Rectangle(width=vehicle_width, length=vehicle_length)

        trajectory_prediction = TrajectoryPrediction(trajectory=self, shape=shape)
        # obstacle generation
        return DynamicObstacle(
            obstacle_id=30000 + vehicle_id,
            obstacle_type=ObstacleType.CAR,
            obstacle_shape=shape,
            initial_state=initial_state,
            prediction=trajectory_prediction,
        )

IDMState dataclass

Bases: ExtendedPMState

IDM dataclass as extended point mass class

Parameters:

Name Type Description Default
position

2d position as array

 required
velocity

long. velocity

 required
orientation

cartesian orientaiton

 required
acceleration

long. acceleration

 required
time_step

Time step of the state

 required
steering_angle Optional[float]

steering angle

 None
Source code in commonroad_idm_planner/idm_state.py 
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
@dataclass()
class IDMState(ExtendedPMState):
    """
    IDM dataclass as extended point mass class
    :param position: 2d position as array
    :param velocity: long. velocity
    :param orientation: cartesian orientaiton
    :param acceleration: long. acceleration
    :param time_step: Time step of the state
    :param steering_angle: steering angle
    """

    steering_angle: Optional[float] = None

    def __post_init__(self):
        if (
            self.position is None
            or self.velocity is None
            or self.orientation is None
            or self.acceleration is None
            or self.time_step is None
        ):
            logger = logging.getLogger("cridm.idm_state")
            logger.warning(
                f"One or more attributes are initialized as None: \n"
                f"position={self.position}  "
                f"--  velocity={self.velocity} "
                f"-- orientation={self.orientation} "
                f"-- acceleration={self.acceleration} "
                f"-- time_step={self.time_step}"
            )

IDMInput dataclass

Parameters:

Name Type Description Default
acceleration float

long. acceleration

 required
steering_angle_velocity float

steering angle_velocity

 required
time_step int

time step at which the input is applied.

 required
Source code in commonroad_idm_planner/idm_input.py 
10
11
12
13
14
15
16
17
18
19
20
@dataclass
class IDMInput:
    """
    :param acceleration: long. acceleration
    :param steering_angle_velocity: steering angle_velocity
    :param time_step: time step at which the input is applied.
    """

    acceleration: float
    steering_angle_velocity: float
    time_step: int

IDMInputFactory

Approximates the input trajectory given a double-integrator-based state trajectory.

Source code in commonroad_idm_planner/idm_input.py 
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
class IDMInputFactory:
    """
    Approximates the input trajectory given a double-integrator-based state trajectory.
    """

    def input_from_idm_trajectory(
        self, idm_trajectory: IDMTrajectory, dt: float
    ) -> List[IDMInput]:
        """
        Approximate input sequence of idm trajectory using a double integrator model.
        :param idm_trajectory: idm trajectory
        :param dt: time steps size
        :return: sequence of inputs
        """
        input_traj: List[IDMInput] = list()
        for idx in range(len(idm_trajectory.state_list) - 1):
            input_traj.append(
                input_from_states(
                    current_state=idm_trajectory.state_list[idx],
                    next_state=idm_trajectory.state_list[idx + 1],
                    dt=dt,
                )
            )

        return input_traj

input_from_idm_trajectory(idm_trajectory, dt)

Approximate input sequence of idm trajectory using a double integrator model.

Parameters:

Name Type Description Default
idm_trajectory IDMTrajectory

idm trajectory

 required
dt float

time steps size

 required

Returns:

Type Description
List[IDMInput]

sequence of inputs
Source code in commonroad_idm_planner/idm_input.py 
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
def input_from_idm_trajectory(
    self, idm_trajectory: IDMTrajectory, dt: float
) -> List[IDMInput]:
    """
    Approximate input sequence of idm trajectory using a double integrator model.
    :param idm_trajectory: idm trajectory
    :param dt: time steps size
    :return: sequence of inputs
    """
    input_traj: List[IDMInput] = list()
    for idx in range(len(idm_trajectory.state_list) - 1):
        input_traj.append(
            input_from_states(
                current_state=idm_trajectory.state_list[idx],
                next_state=idm_trajectory.state_list[idx + 1],
                dt=dt,
            )
        )

    return input_traj

input_from_states(current_state, next_state, dt)

Calculate input via forward simulation

Parameters:

Name Type Description Default
current_state IDMState

current idm state

 required
next_state IDMState

next idm state

 required
dt float

time between two steps

 required

Returns:

Type Description
IDMInput

idm input
Source code in commonroad_idm_planner/idm_input.py 
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
def input_from_states(
    current_state: IDMState, next_state: IDMState, dt: float
) -> IDMInput:
    """
    Calculate input via forward simulation
    :param current_state: current idm state
    :param next_state: next idm state
    :param dt: time between two steps
    :return: idm input
    """
    if dt <= 0:
        logging.getLogger("cr_idm.input_generation").error(f"dt={dt} but should be >0")
        raise ValueError(f"dt={dt} but should be >0")

    return IDMInput(
        acceleration=(next_state.velocity - current_state.velocity) / dt,
        steering_angle_velocity=wrap_to_pi(
            next_state.steering_angle - current_state.steering_angle
        )
        / dt,
        time_step=current_state.time_step,
    )