mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-01-13 02:47:54 -05:00
64779298ff
* refactor: rename files and update references for inverted pendulum and path tracking modules * refactor: rename inverted pendulum control files and update type check references * refactor: update import statements to use consistent casing for InvertedPendulum module
224 lines
6.6 KiB
Python
224 lines
6.6 KiB
Python
"""
|
||
|
||
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
|
||
|
||
"""
|
||
|
||
import matplotlib.pyplot as plt
|
||
import numpy as np
|
||
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:
|
||
"""
|
||
Constructs an instantiate of the PathFinderController for navigating a
|
||
3-DOF wheeled robot on a 2D plane
|
||
|
||
Parameters
|
||
----------
|
||
Kp_rho : The linear velocity gain to translate the robot along a line
|
||
towards the goal
|
||
Kp_alpha : The angular velocity gain to rotate the robot towards the goal
|
||
Kp_beta : The offset angular velocity gain accounting for smooth merging to
|
||
the goal angle (i.e., it helps the robot heading to be parallel
|
||
to the target angle.)
|
||
"""
|
||
|
||
def __init__(self, Kp_rho, Kp_alpha, Kp_beta):
|
||
self.Kp_rho = Kp_rho
|
||
self.Kp_alpha = Kp_alpha
|
||
self.Kp_beta = Kp_beta
|
||
|
||
def calc_control_command(self, x_diff, y_diff, theta, theta_goal):
|
||
"""
|
||
Returns the control command for the linear and angular velocities as
|
||
well as the distance to goal
|
||
|
||
Parameters
|
||
----------
|
||
x_diff : The position of target with respect to current robot position
|
||
in x direction
|
||
y_diff : The position of target with respect to current robot position
|
||
in y direction
|
||
theta : The current heading angle of robot with respect to x axis
|
||
theta_goal: The target angle of robot with respect to x axis
|
||
|
||
Returns
|
||
-------
|
||
rho : The distance between the robot and the goal position
|
||
v : Command linear velocity
|
||
w : Command angular velocity
|
||
"""
|
||
|
||
# Description of local variables:
|
||
# - 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
|
||
#
|
||
# Note:
|
||
# we 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
|
||
|
||
# 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)
|
||
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
|
||
|
||
|
||
# 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
|
||
MAX_ANGULAR_SPEED = 7
|
||
|
||
show_animation = True
|
||
|
||
|
||
def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal):
|
||
x = x_start
|
||
y = y_start
|
||
theta = theta_start
|
||
|
||
x_diff = x_goal - x
|
||
y_diff = y_goal - y
|
||
|
||
x_traj, y_traj, v_traj, w_traj = [x], [y], [0], [0]
|
||
|
||
rho = np.hypot(x_diff, y_diff)
|
||
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)
|
||
|
||
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
|
||
|
||
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,
|
||
)
|
||
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)
|
||
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.0 * random()
|
||
y_start = 20.0 * random()
|
||
theta_start: float = 2 * np.pi * random() - np.pi
|
||
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"
|
||
)
|
||
move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal)
|
||
|
||
|
||
if __name__ == "__main__":
|
||
main()
|