From adb4e99ceb5628f103c486b9eb84ef35d1c4b3d0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?G=C3=B6ktu=C4=9F=20Karaka=C5=9Fl=C4=B1?=
 <20567087+goktug97@users.noreply.github.com>
Date: Wed, 16 Oct 2019 21:03:18 +0300
Subject: [PATCH] Fix transpose error of the rotation matrix

---
 PathPlanning/DynamicWindowApproach/dynamic_window_approach.py | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py
index 267ca167..9216892a 100644
--- a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py
+++ b/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