github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-04-22 03:00:22 -04:00
Code Issues Packages Projects Releases Wiki Activity

Using utility rot_mat_2d (#683)

Browse Source
This commit is contained in:
Atsushi Sakai
2022-05-22 16:53:14 +09:00
committed by GitHub
parent 31178c8e37
commit dc879da7b2
10 changed files with 70 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py
View File

@@ -4,15 +4,18 @@ Grid based sweep planner
author: Atsushi Sakai
"""
import math
import os
import sys
import os
sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/")
import math
from enum import IntEnum
import numpy as np
from scipy.spatial.transform import Rotation as Rot
import matplotlib.pyplot as plt
from utils.angle import rot_mat_2d
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../../Mapping/")
@@ -148,16 +151,14 @@ def convert_grid_coordinate(ox, oy, sweep_vec, sweep_start_position):
tx = [ix - sweep_start_position[0] for ix in ox]
ty = [iy - sweep_start_position[1] for iy in oy]
th = math.atan2(sweep_vec[1], sweep_vec[0])
rot = Rot.from_euler('z', th).as_matrix()[0:2, 0:2]
converted_xy = np.stack([tx, ty]).T @ rot
converted_xy = np.stack([tx, ty]).T @ rot_mat_2d(th)
return converted_xy[:, 0], converted_xy[:, 1]
def convert_global_coordinate(x, y, sweep_vec, sweep_start_position):
th = math.atan2(sweep_vec[1], sweep_vec[0])
rot = Rot.from_euler('z', -th).as_matrix()[0:2, 0:2]
converted_xy = np.stack([x, y]).T @ rot
converted_xy = np.stack([x, y]).T @ rot_mat_2d(-th)
rx = [ix + sweep_start_position[0] for ix in converted_xy[:, 0]]
ry = [iy + sweep_start_position[1] for iy in converted_xy[:, 1]]
return rx, ry