Fix transpose error of the rotation matrix

Author: goktug97

PathPlanning/DynamicWindowApproach/dynamic_window_approach.py

@@ -166,9 +166,10 @@ def calc_obstacle_cost(trajectory, ob, config):
     if config.robot_type == RobotType.rectangle:
         yaw = trajectory[:, 2]
         rot = np.array([[np.cos(yaw), -np.sin(yaw)], [np.sin(yaw), np.cos(yaw)]])
+        rot = np.transpose(rot, [2, 0, 1])
         local_ob = ob[:, None] - trajectory[:, 0:2]
         local_ob = local_ob.reshape(-1, local_ob.shape[-1])
-        local_ob = np.array([local_ob @ -x for x in rot.T])
+        local_ob = np.array([local_ob @ -x for x in rot])
         local_ob = local_ob.reshape(-1, local_ob.shape[-1])
         upper_check = local_ob[:, 0] <= config.robot_length/2
         right_check = local_ob[:, 1] <= config.robot_width/2