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

Replaced sqrt(x**2+y**2) with hypot in PathTracking/move_to_pose/move_to_pose.py

Browse Source
This commit is contained in:
Guillaume Jacquenot
2019-12-07 22:18:00 +01:00
parent e500f65d7f
commit 4ef4658f82
1 changed files with 2 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
PathTracking/move_to_pose/move_to_pose.py
View File

@@ -40,7 +40,7 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal):
x_traj, y_traj = [], []
rho = np.sqrt(x_diff**2 + y_diff**2)
rho = np.hypot(x_diff, y_diff)
while rho > 0.001:
x_traj.append(x)
y_traj.append(y)
@@ -52,7 +52,7 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal):
# [-pi, pi] to prevent unstable behavior e.g. difference going
# from 0 rad to 2*pi rad with slight turn
rho = np.sqrt(x_diff**2 + y_diff**2)
rho = np.hypot(x_diff, y_diff)
alpha = (np.arctan2(y_diff, x_diff)
- theta + np.pi) % (2 * np.pi) - np.pi
beta = (theta_goal - theta - alpha + np.pi) % (2 * np.pi) - np.pi