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

Update move_to_pose for cases where alpha > pi/2 or alpha < -pi/2 (#1168)

Browse Source 
* Update move_to_pose for cases where alpha > pi/2 or alpha < -pi/2

* Update move_to_pose

* Add move_to_pose test

* Update move_to_pose
This commit is contained in:
Aglargil
2025-02-20 18:09:30 +08:00
committed by GitHub
parent c7fb228d24
commit f343573a7b
2 changed files with 134 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

90
Control/move_to_pose/move_to_pose.py
View File

@@ -5,6 +5,7 @@ Move to specified pose
Author: Daniel Ingram (daniel-s-ingram)
Atsushi Sakai (@Atsushi_twi)
Seied Muhammad Yazdian (@Muhammad-Yazdian)
Wang Zheng (@Aglargil)
P. I. Corke, "Robotics, Vision & Control", Springer 2017, ISBN 978-3-319-54413-7
@@ -12,8 +13,13 @@ P. I. Corke, "Robotics, Vision & Control", Springer 2017, ISBN 978-3-319-54413-7
import matplotlib.pyplot as plt
import numpy as np
from random import random
import sys
import pathlib
sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
from utils.angle import angle_mod
from random import random
class PathFinderController:
"""
@@ -70,14 +76,20 @@ class PathFinderController:
# [-pi, pi] to prevent unstable behavior e.g. difference going
# from 0 rad to 2*pi rad with slight turn
# Ref: The velocity v always has a constant sign which depends on the initial value of α.
rho = np.hypot(x_diff, y_diff)
v = self.Kp_rho * rho
alpha = angle_mod(np.arctan2(y_diff, x_diff) - theta)
beta = angle_mod(theta_goal - theta - alpha)
v = self.Kp_rho * rho
w = self.Kp_alpha * alpha - self.Kp_beta * beta
if alpha > np.pi / 2 or alpha < -np.pi / 2:
# recalculate alpha to make alpha in the range of [-pi/2, pi/2]
alpha = angle_mod(np.arctan2(-y_diff, -x_diff) - theta)
beta = angle_mod(theta_goal - theta - alpha)
w = self.Kp_alpha * alpha - self.Kp_beta * beta
v = -v
else:
w = self.Kp_alpha * alpha - self.Kp_beta * beta
return rho, v, w
@@ -85,6 +97,7 @@ class PathFinderController:
# simulation parameters
controller = PathFinderController(9, 15, 3)
dt = 0.01
MAX_SIM_TIME = 5 # seconds, robot will stop moving when time exceeds this value
# Robot specifications
MAX_LINEAR_SPEED = 15
@@ -101,18 +114,19 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal):
x_diff = x_goal - x
y_diff = y_goal - y
x_traj, y_traj = [], []
x_traj, y_traj, v_traj, w_traj = [x], [y], [0], [0]
rho = np.hypot(x_diff, y_diff)
while rho > 0.001:
t = 0
while rho > 0.001 and t < MAX_SIM_TIME:
t += dt
x_traj.append(x)
y_traj.append(y)
x_diff = x_goal - x
y_diff = y_goal - y
rho, v, w = controller.calc_control_command(
x_diff, y_diff, theta, theta_goal)
rho, v, w = controller.calc_control_command(x_diff, y_diff, theta, theta_goal)
if abs(v) > MAX_LINEAR_SPEED:
v = np.sign(v) * MAX_LINEAR_SPEED
@@ -120,18 +134,35 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal):
if abs(w) > MAX_ANGULAR_SPEED:
w = np.sign(w) * MAX_ANGULAR_SPEED
v_traj.append(v)
w_traj.append(w)
theta = theta + w * dt
x = x + v * np.cos(theta) * dt
y = y + v * np.sin(theta) * dt
if show_animation: # pragma: no cover
plt.cla()
plt.arrow(x_start, y_start, np.cos(theta_start),
np.sin(theta_start), color='r', width=0.1)
plt.arrow(x_goal, y_goal, np.cos(theta_goal),
np.sin(theta_goal), color='g', width=0.1)
plt.arrow(
x_start,
y_start,
np.cos(theta_start),
np.sin(theta_start),
color="r",
width=0.1,
)
plt.arrow(
x_goal,
y_goal,
np.cos(theta_goal),
np.sin(theta_goal),
color="g",
width=0.1,
)
plot_vehicle(x, y, theta, x_traj, y_traj)
return x_traj, y_traj, v_traj, w_traj
def plot_vehicle(x, y, theta, x_traj, y_traj): # pragma: no cover
# Corners of triangular vehicle when pointing to the right (0 radians)
@@ -144,16 +175,16 @@ def plot_vehicle(x, y, theta, x_traj, y_traj): # pragma: no cover
p2 = np.matmul(T, p2_i)
p3 = np.matmul(T, p3_i)
plt.plot([p1[0], p2[0]], [p1[1], p2[1]], 'k-')
plt.plot([p2[0], p3[0]], [p2[1], p3[1]], 'k-')
plt.plot([p3[0], p1[0]], [p3[1], p1[1]], 'k-')
plt.plot([p1[0], p2[0]], [p1[1], p2[1]], "k-")
plt.plot([p2[0], p3[0]], [p2[1], p3[1]], "k-")
plt.plot([p3[0], p1[0]], [p3[1], p1[1]], "k-")
plt.plot(x_traj, y_traj, 'b--')
plt.plot(x_traj, y_traj, "b--")
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
"key_release_event", lambda event: [exit(0) if event.key == "escape" else None]
)
plt.xlim(0, 20)
plt.ylim(0, 20)
@@ -162,15 +193,16 @@ def plot_vehicle(x, y, theta, x_traj, y_traj): # pragma: no cover
def transformation_matrix(x, y, theta):
return np.array([
[np.cos(theta), -np.sin(theta), x],
[np.sin(theta), np.cos(theta), y],
[0, 0, 1]
])
return np.array(
[
[np.cos(theta), -np.sin(theta), x],
[np.sin(theta), np.cos(theta), y],
[0, 0, 1],
]
)
def main():
for i in range(5):
x_start = 20.0 * random()
y_start = 20.0 * random()
@@ -178,10 +210,14 @@ def main():
x_goal = 20 * random()
y_goal = 20 * random()
theta_goal = 2 * np.pi * random() - np.pi
print(f"Initial x: {round(x_start, 2)} m\nInitial y: {round(y_start, 2)} m\nInitial theta: {round(theta_start, 2)} rad\n")
print(f"Goal x: {round(x_goal, 2)} m\nGoal y: {round(y_goal, 2)} m\nGoal theta: {round(theta_goal, 2)} rad\n")
print(
f"Initial x: {round(x_start, 2)} m\nInitial y: {round(y_start, 2)} m\nInitial theta: {round(theta_start, 2)} rad\n"
)
print(
f"Goal x: {round(x_goal, 2)} m\nGoal y: {round(y_goal, 2)} m\nGoal theta: {round(theta_goal, 2)} rad\n"
)
move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal)
if __name__ == '__main__':
if __name__ == "__main__":
main()

74
tests/test_move_to_pose.py
View File

@@ -1,13 +1,81 @@
import itertools
import numpy as np
import conftest # Add root path to sys.path
from Control.move_to_pose import move_to_pose as m
def test_1():
def test_random():
m.show_animation = False
m.main()
def test_2():
def test_stability():
"""
This unit test tests the move_to_pose.py program for stability
"""
m.show_animation = False
x_start = 5
y_start = 5
theta_start = 0
x_goal = 1
y_goal = 4
theta_goal = 0
_, _, v_traj, w_traj = m.move_to_pose(
x_start, y_start, theta_start, x_goal, y_goal, theta_goal
)
def v_is_change(current, previous):
return abs(current - previous) > m.MAX_LINEAR_SPEED
def w_is_change(current, previous):
return abs(current - previous) > m.MAX_ANGULAR_SPEED
# Check if the speed is changing too much
window_size = 10
count_threshold = 4
v_change = [v_is_change(v_traj[i], v_traj[i - 1]) for i in range(1, len(v_traj))]
w_change = [w_is_change(w_traj[i], w_traj[i - 1]) for i in range(1, len(w_traj))]
for i in range(len(v_change) - window_size + 1):
v_window = v_change[i : i + window_size]
w_window = w_change[i : i + window_size]
v_unstable = sum(v_window) > count_threshold
w_unstable = sum(w_window) > count_threshold
assert not v_unstable, (
f"v_unstable in window [{i}, {i + window_size}], unstable count: {sum(v_window)}"
)
assert not w_unstable, (
f"w_unstable in window [{i}, {i + window_size}], unstable count: {sum(w_window)}"
)
def test_reach_goal():
"""
This unit test tests the move_to_pose.py program for reaching the goal
"""
m.show_animation = False
x_start = 5
y_start = 5
theta_start_list = [0, np.pi / 2, np.pi, 3 * np.pi / 2]
x_goal_list = [0, 5, 10]
y_goal_list = [0, 5, 10]
theta_goal = 0
for theta_start, x_goal, y_goal in itertools.product(
theta_start_list, x_goal_list, y_goal_list
):
x_traj, y_traj, _, _ = m.move_to_pose(
x_start, y_start, theta_start, x_goal, y_goal, theta_goal
)
x_diff = x_goal - x_traj[-1]
y_diff = y_goal - y_traj[-1]
rho = np.hypot(x_diff, y_diff)
assert rho < 0.001, (
f"start:[{x_start}, {y_start}, {theta_start}], goal:[{x_goal}, {y_goal}, {theta_goal}], rho: {rho} is too large"
)
def test_max_speed():
"""
This unit test tests the move_to_pose.py program for a MAX_LINEAR_SPEED and
MAX_ANGULAR_SPEED
@@ -18,5 +86,5 @@ def test_2():
m.main()
if __name__ == '__main__':
if __name__ == "__main__":
conftest.run_this_test(__file__)