motion_primitive.py 11.5 KB
Newer Older
Edmond Irani Liu's avatar
Edmond Irani Liu committed
1
2
import warnings
from copy import deepcopy
Edmond Irani Liu's avatar
Edmond Irani Liu committed
3
from typing import List
Edmond Irani Liu's avatar
Edmond Irani Liu committed
4
5

import numpy as np
Edmond Irani Liu's avatar
Edmond Irani Liu committed
6
from commonroad.common.solution import VehicleModel, VehicleType
Edmond Irani Liu's avatar
Edmond Irani Liu committed
7
8
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
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
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
from commonroad.scenario.trajectory import Trajectory, State


class MotionPrimitive:
    """
    Class for motion primitives.
    """

    class PrimitiveState:
        """
        Inner class to represent the initial and final states of a motion primitive.
        """

        def __init__(self, x, y, steering_angle, velocity, orientation, time_step=0):
            """
            Initialisation of a state of a motion primitive.

            :param x: position in x axis
            :param y: position in y axis
            :param steering_angle: steering angle
            :param velocity: velocity
            :param orientation: orientation
            :param time_step: time step of the state
            """
            self.x = x
            self.y = y
            self.steering_angle = steering_angle
            self.velocity = velocity
            self.orientation = orientation
            self.time_step = time_step

        def __str__(self) -> str:
            """
            Returns the information of PrimitiveState.
            """
            return "pos: ({}, {})m, vel: {} m/s, ori: {} rad, steer: {} rad".format(round(self.x, 2),
                                                                                    round(self.y, 2),
                                                                                    round(self.velocity, 2),
                                                                                    round(self.orientation, 2),
                                                                                    round(self.steering_angle, 2))

    def __init__(self, state_initial: PrimitiveState, state_final: PrimitiveState, trajectory: Trajectory,
                 length_time_step):
        """
        Initialisation of a motion primitive.

        :param state_initial: initial state of the primitive
        :param state_final: final state of the primitive
        :param trajectory: trajectory of the primitive, which is a list of states between the initial and final states
        :param length_time_step: length of the time step of trajectory
        """
        self.state_initial = state_initial
        self.state_final = state_final
        self.trajectory = trajectory
        self.length_time_step = length_time_step

        # default vehicle type and model
        self.id_type_vehicle = 2
        self.type_vehicle = VehicleType.BMW_320i
        self.model_vehicle = VehicleModel.KS

        self._id = int(0)
        self._id_is_set = False
        # a list to store connectable successive primitives from this primitive
        self.list_successors = []
        self.list_ids_successors = []

    def __str__(self):
        return "Primitive: \n\t {}\t=>\n\t{}".format(str(self.state_initial), str(self.state_final))

    @property
    def id(self) -> int:
        """
        ID getter function.
        """
        assert self._id_is_set, f"The primitive id is not set!"
        return self._id

    @id.setter
    def id(self, primitive_id) -> None:
        """
        ID setter function.
        """
        assert isinstance(primitive_id, int), f"<ID> Provided id is not an instance of int, id = {primitive_id}"

        if not self._id_is_set:
            self._id = primitive_id
            self._id_is_set = True
        else:
            warnings.warn("Primitive ID is already set!")

    def print_info(self) -> None:
        """
        Prints the states of the primitive.
        """
        kwarg = {'position': np.array([self.state_initial.x, self.state_initial.y]),
                 'velocity': self.state_initial.velocity,
                 'steering_angle': self.state_initial.steering_angle,
                 'orientation': self.state_initial.orientation,
                 'time_step': self.state_initial.time_step}
        state_initial = State(**kwarg)

        # add final state into the trajectory
        kwarg = {'position': np.array([self.state_final.x, self.state_final.y]),
                 'velocity': self.state_final.velocity,
                 'steering_angle': self.state_final.steering_angle,
                 'orientation': self.state_final.orientation,
                 'time_step': self.state_final.time_step}
        state_final = State(**kwarg)

        print(state_initial)
        for state in self.trajectory.state_list:
            print(state)
        print(state_final)

    def mirror(self) -> None:
        """
        Mirrors the current primitive with regard to the x-axis.
        """
        self.state_final.y = -self.state_final.y
        self.state_final.orientation = -self.state_final.orientation
        self.state_final.steering_angle = -self.state_final.steering_angle
        for state in self.trajectory.state_list:
            state.position[1] = -state.position[1]
            state.orientation = -state.orientation
            state.steering_angle = -state.steering_angle

    def is_connectable(self, other: 'MotionPrimitive') -> bool:
        """
        Any primitive whose initial state's velocity and steering angle are equal to those of the current primitive is
        deemed connectable.

        :param other: the motion primitive to which the connectivity is examined
        """

        return abs(self.state_final.velocity - other.state_initial.velocity) < 0.01 and abs(
            self.state_final.steering_angle - other.state_initial.steering_angle) < 0.01

    def attach_trajectory_to_state(self, state: State) -> List[State]:
        """
        Attaches the trajectory to the given state, and returns the new list of states.

        :param state: the state to which the trajectory will be attached
        """

        # deep copy to prevent manipulation of original data
        trajectory = deepcopy(self.trajectory)

        # rotate the trajectory by the orientation of the given state
        trajectory.translate_rotate(np.zeros(2), state.orientation)
        # translate the trajectory by the position of the given state
        trajectory.translate_rotate(state.position, 0)

        # as the initial state of the trajectory is exactly the given state, we thus pop out the initial state
        trajectory.state_list.pop(0)

        # we modify the time steps of the trajectory
        time_step_state = int(state.time_step)
        for state in trajectory.state_list:
            state.time_step += time_step_state

        return trajectory.state_list


class MotionPrimitiveParser:
    """
    Class for motion primitive parsers, which parse and create motion primitives from given XML nodes.
    """

    @classmethod
    def create_from_node(cls, node_trajectory) -> MotionPrimitive:
        """
        Creates a motion primitive from the given XML node.

        :param node_trajectory: node containing information of a trajectory

        """
        # read from xml node and create start state
        node_initial = node_trajectory.find('Initial')
        x_initial = float(node_initial.find('x').text)
        y_initial = float(node_initial.find('y').text)
        steering_angle_initial = float(node_initial.find('steering_angle').text)
        velocity_initial = float(node_initial.find('velocity').text)
        orientation_initial = float(node_initial.find('orientation').text)
        time_step_initial = int(node_initial.find('time_step').text)

        state_initial = MotionPrimitive.PrimitiveState(x_initial, y_initial, steering_angle_initial, velocity_initial,
                                                       orientation_initial,
                                                       time_step_initial)

        # read from xml node and create final state
        node_final = node_trajectory.find('Final')
        x_final = float(node_final.find('x').text)
        y_final = float(node_final.find('y').text)
        steering_angle_final = float(node_final.find('steering_angle').text)
        velocity_final = float(node_final.find('velocity').text)
        orientation_final = float(node_final.find('orientation').text)
        time_step_final = int(node_final.find('time_step').text)

        state_final = MotionPrimitive.PrimitiveState(x_final, y_final, steering_angle_final, velocity_final,
                                                     orientation_final,
                                                     time_step_final)

        # create trajectory from path node and initial/final states
        node_path = node_trajectory.find('Path')
        duration = node_trajectory.find('Duration')
        length_time_step = float(duration.text) / (len(node_path) + 1)

        trajectory = cls.create_trajectory(state_initial, state_final, node_path)

        # create and return motion primitive
        return MotionPrimitive(state_initial, state_final, trajectory, length_time_step)

    @classmethod
    def create_trajectory(cls, state_initial: MotionPrimitive.PrimitiveState,
                          state_final: MotionPrimitive.PrimitiveState,
                          node_path) -> Trajectory:
        """
        Creates trajectory state list from the path values described in the xml file.

        :param state_initial: initial state of the trajectory
        :param state_final: final state of the trajectory
        :param node_path: xml node of the path of the trajectory
        """

        assert node_path is not None, "Input path node is empty!"

        # insert the initial state
        list_vertices = [(state_initial.x, state_initial.y)]
        list_steering_angles = [state_initial.steering_angle]
        list_velocities = [state_initial.velocity]
        list_orientations = [state_initial.orientation]
        list_time_steps = [int(state_initial.time_step)]

        # insert trajectory states
        list_states_trajectory = node_path.findall('State')
        for state in list_states_trajectory:
            x = float(state.find('x').text)
            y = float(state.find('y').text)
            steering_angle = float(state.find('steering_angle').text)
            velocity = float(state.find('velocity').text)
            orientation = float(state.find('orientation').text)
            time_step = int(state.find('time_step').text)

            list_vertices.append((x, y))
            list_steering_angles.append(steering_angle)
            list_velocities.append(velocity)
            list_orientations.append(orientation)
            list_time_steps.append(time_step)

        # insert the final state
        list_vertices.append((state_final.x, state_final.y))
        list_steering_angles.append(state_final.steering_angle)
        list_velocities.append(state_final.velocity)
        list_orientations.append(state_final.orientation)
        list_time_steps.append(int(state_final.time_step))

        assert len(list_vertices) == len(list_steering_angles) == len(list_velocities) == len(list_orientations) == len(
            list_time_steps), "The sizes of state elements should be equal!"

        # creates the trajectory of the primitive
        list_states_trajectory = []
        for i in range(len(list_vertices)):
            kwarg = {'position': np.array([list_vertices[i][0], list_vertices[i][1]]),
                     'velocity': list_velocities[i],
                     'steering_angle': list_steering_angles[i],
                     'orientation': list_orientations[i],
                     'time_step': list_time_steps[i]}

            # append states
            list_states_trajectory.append(State(**kwarg))

        return Trajectory(initial_time_step=int(list_time_steps[0]), state_list=list_states_trajectory)