github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-13 20:08:06 -05:00
Code Issues Packages Projects Releases Wiki Activity

Fix transpose error of the rotation matrix

Browse Source
This commit is contained in:
Göktuğ Karakaşlı
2019-10-16 21:03:18 +03:00
parent 0ec56812be
commit adb4e99ceb
1 changed files with 2 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
PathPlanning/DynamicWindowApproach/dynamic_window_approach.py
View File

@@ -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