mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-04-22 03:00:22 -04:00
bf4e68245f
* Added speed limitation of the robot * Removed leading underscores for global vars * Added unit test for robot speed limitation * Modified x/abs(x) to np.sign(x); fixed code style * Removed 'random' from test func header comment
142 lines
3.9 KiB
Python
142 lines
3.9 KiB
Python
"""
|
|
|
|
Move to specified pose
|
|
|
|
Author: Daniel Ingram (daniel-s-ingram)
|
|
Atsushi Sakai(@Atsushi_twi)
|
|
|
|
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
|
|
|
|
# simulation parameters
|
|
Kp_rho = 9
|
|
Kp_alpha = 15
|
|
Kp_beta = -3
|
|
dt = 0.01
|
|
|
|
# Robot specifications
|
|
MAX_LINEAR_SPEED = 15
|
|
MAX_ANGULAR_SPEED = 7
|
|
|
|
show_animation = True
|
|
|
|
|
|
def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal):
|
|
"""
|
|
rho is the distance between the robot and the goal position
|
|
alpha is the angle to the goal relative to the heading of the robot
|
|
beta is the angle between the robot's position and the goal position plus the goal angle
|
|
|
|
Kp_rho*rho and Kp_alpha*alpha drive the robot along a line towards the goal
|
|
Kp_beta*beta rotates the line so that it is parallel to the goal angle
|
|
"""
|
|
x = x_start
|
|
y = y_start
|
|
theta = theta_start
|
|
|
|
x_diff = x_goal - x
|
|
y_diff = y_goal - y
|
|
|
|
x_traj, y_traj = [], []
|
|
|
|
rho = np.hypot(x_diff, y_diff)
|
|
while rho > 0.001:
|
|
x_traj.append(x)
|
|
y_traj.append(y)
|
|
|
|
x_diff = x_goal - x
|
|
y_diff = y_goal - y
|
|
|
|
# Restrict alpha and beta (angle differences) to the range
|
|
# [-pi, pi] to prevent unstable behavior e.g. difference going
|
|
# from 0 rad to 2*pi rad with slight turn
|
|
|
|
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
|
|
|
|
v = Kp_rho * rho
|
|
w = Kp_alpha * alpha + Kp_beta * beta
|
|
|
|
if alpha > np.pi / 2 or alpha < -np.pi / 2:
|
|
v = -v
|
|
|
|
if abs(v) > MAX_LINEAR_SPEED:
|
|
v = np.sign(v) * MAX_LINEAR_SPEED
|
|
|
|
if abs(w) > MAX_ANGULAR_SPEED:
|
|
w = np.sign(w) * MAX_ANGULAR_SPEED
|
|
|
|
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)
|
|
plot_vehicle(x, y, theta, x_traj, y_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)
|
|
p1_i = np.array([0.5, 0, 1]).T
|
|
p2_i = np.array([-0.5, 0.25, 1]).T
|
|
p3_i = np.array([-0.5, -0.25, 1]).T
|
|
|
|
T = transformation_matrix(x, y, theta)
|
|
p1 = np.matmul(T, p1_i)
|
|
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(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])
|
|
|
|
plt.xlim(0, 20)
|
|
plt.ylim(0, 20)
|
|
|
|
plt.pause(dt)
|
|
|
|
|
|
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]
|
|
])
|
|
|
|
|
|
def main():
|
|
|
|
for i in range(5):
|
|
x_start = 20 * random()
|
|
y_start = 20 * random()
|
|
theta_start = 2 * np.pi * random() - np.pi
|
|
x_goal = 20 * random()
|
|
y_goal = 20 * random()
|
|
theta_goal = 2 * np.pi * random() - np.pi
|
|
print("Initial x: %.2f m\nInitial y: %.2f m\nInitial theta: %.2f rad\n" %
|
|
(x_start, y_start, theta_start))
|
|
print("Goal x: %.2f m\nGoal y: %.2f m\nGoal theta: %.2f rad\n" %
|
|
(x_goal, y_goal, theta_goal))
|
|
move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal)
|
|
|
|
|
|
if __name__ == '__main__':
|
|
main()
|