mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-01-21 07:18:04 -05:00
680ecdafb2
* 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 * Added Robot class for move to pose * Revert * Added Robot class for move to pose * Added a type annotation for Robot class * Fixed the annotaion comment * Moved instance varaible outside of the Robot class * Fixed code style Python 3.9 CI * Removed whitespaces from the last line * Applied PR #596 change requests * Fixed typos * Update Control/move_to_pose/move_to_pose_robot_class.py Co-authored-by: Atsushi Sakai <asakai.amsl+github@gmail.com> * Moved PathFinderController class to move_to_pose * Fixed issue #600 * Added update_command() to PathFinderController * Removed trailing whitespaces * Updated move to pose doc * Added code and doc comments * Updated unit test * Removed trailing whitespaces * Removed more trailing whitespaces Co-authored-by: Atsushi Sakai <asakai.amsl+github@gmail.com>
241 lines
7.2 KiB
Python
241 lines
7.2 KiB
Python
"""
|
|
|
|
Move to specified pose (with Robot class)
|
|
|
|
Author: Daniel Ingram (daniel-s-ingram)
|
|
Atsushi Sakai (@Atsushi_twi)
|
|
Seied Muhammad Yazdian (@Muhammad-Yazdian)
|
|
|
|
P.I. Corke, "Robotics, Vision & Control", Springer 2017, ISBN 978-3-319-54413-7
|
|
|
|
"""
|
|
|
|
import matplotlib.pyplot as plt
|
|
import numpy as np
|
|
import copy
|
|
from move_to_pose import PathFinderController
|
|
|
|
# Simulation parameters
|
|
TIME_DURATION = 1000
|
|
TIME_STEP = 0.01
|
|
AT_TARGET_ACCEPTANCE_THRESHOLD = 0.01
|
|
SHOW_ANIMATION = True
|
|
PLOT_WINDOW_SIZE_X = 20
|
|
PLOT_WINDOW_SIZE_Y = 20
|
|
PLOT_FONT_SIZE = 8
|
|
|
|
simulation_running = True
|
|
all_robots_are_at_target = False
|
|
|
|
|
|
class Pose:
|
|
"""2D pose"""
|
|
|
|
def __init__(self, x, y, theta):
|
|
self.x = x
|
|
self.y = y
|
|
self.theta = theta
|
|
|
|
|
|
class Robot:
|
|
"""
|
|
Constructs an instantiate of the 3-DOF wheeled Robot navigating on a
|
|
2D plane
|
|
|
|
Parameters
|
|
----------
|
|
name : (string)
|
|
The name of the robot
|
|
color : (string)
|
|
The color of the robot
|
|
max_linear_speed : (float)
|
|
The maximum linear speed that the robot can go
|
|
max_angular_speed : (float)
|
|
The maximum angular speed that the robot can rotate about its vertical
|
|
axis
|
|
path_finder_controller : (PathFinderController)
|
|
A configurable controller to finds the path and calculates command
|
|
linear and angular velocities.
|
|
"""
|
|
|
|
def __init__(self, name, color, max_linear_speed, max_angular_speed,
|
|
path_finder_controller):
|
|
self.name = name
|
|
self.color = color
|
|
self.MAX_LINEAR_SPEED = max_linear_speed
|
|
self.MAX_ANGULAR_SPEED = max_angular_speed
|
|
self.path_finder_controller = path_finder_controller
|
|
self.x_traj = []
|
|
self.y_traj = []
|
|
self.pose = Pose(0, 0, 0)
|
|
self.pose_start = Pose(0, 0, 0)
|
|
self.pose_target = Pose(0, 0, 0)
|
|
self.is_at_target = False
|
|
|
|
def set_start_target_poses(self, pose_start, pose_target):
|
|
"""
|
|
Sets the start and target positions of the robot
|
|
|
|
Parameters
|
|
----------
|
|
pose_start : (Pose)
|
|
Start postion of the robot (see the Pose class)
|
|
pose_target : (Pose)
|
|
Target postion of the robot (see the Pose class)
|
|
"""
|
|
self.pose_start = copy.copy(pose_start)
|
|
self.pose_target = pose_target
|
|
self.pose = pose_start
|
|
|
|
def move(self, dt):
|
|
"""
|
|
Moves the robot for one time step increment
|
|
|
|
Parameters
|
|
----------
|
|
dt : (float)
|
|
time step
|
|
"""
|
|
self.x_traj.append(self.pose.x)
|
|
self.y_traj.append(self.pose.y)
|
|
|
|
rho, linear_velocity, angular_velocity = \
|
|
self.path_finder_controller.calc_control_command(
|
|
self.pose_target.x - self.pose.x,
|
|
self.pose_target.y - self.pose.y,
|
|
self.pose.theta, self.pose_target.theta)
|
|
|
|
if rho < AT_TARGET_ACCEPTANCE_THRESHOLD:
|
|
self.is_at_target = True
|
|
|
|
if abs(linear_velocity) > self.MAX_LINEAR_SPEED:
|
|
linear_velocity = (np.sign(linear_velocity)
|
|
* self.MAX_LINEAR_SPEED)
|
|
|
|
if abs(angular_velocity) > self.MAX_ANGULAR_SPEED:
|
|
angular_velocity = (np.sign(angular_velocity)
|
|
* self.MAX_ANGULAR_SPEED)
|
|
|
|
self.pose.theta = self.pose.theta + angular_velocity * dt
|
|
self.pose.x = self.pose.x + linear_velocity * \
|
|
np.cos(self.pose.theta) * dt
|
|
self.pose.y = self.pose.y + linear_velocity * \
|
|
np.sin(self.pose.theta) * dt
|
|
|
|
|
|
def run_simulation(robots):
|
|
"""Simulates all robots simultaneously"""
|
|
global all_robots_are_at_target
|
|
global simulation_running
|
|
|
|
robot_names = []
|
|
for instance in robots:
|
|
robot_names.append(instance.name)
|
|
|
|
time = 0
|
|
while simulation_running and time < TIME_DURATION:
|
|
time += TIME_STEP
|
|
robots_are_at_target = []
|
|
|
|
for instance in robots:
|
|
if not instance.is_at_target:
|
|
instance.move(TIME_STEP)
|
|
robots_are_at_target.append(instance.is_at_target)
|
|
|
|
if all(robots_are_at_target):
|
|
simulation_running = False
|
|
|
|
if SHOW_ANIMATION:
|
|
plt.cla()
|
|
plt.xlim(0, PLOT_WINDOW_SIZE_X)
|
|
plt.ylim(0, PLOT_WINDOW_SIZE_Y)
|
|
|
|
# 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.text(0.3, PLOT_WINDOW_SIZE_Y - 1,
|
|
'Time: {:.2f}'.format(time),
|
|
fontsize=PLOT_FONT_SIZE)
|
|
|
|
plt.text(0.3, PLOT_WINDOW_SIZE_Y - 2,
|
|
'Reached target: {} = '.format(robot_names)
|
|
+ str(robots_are_at_target),
|
|
fontsize=PLOT_FONT_SIZE)
|
|
|
|
for instance in robots:
|
|
plt.arrow(instance.pose_start.x,
|
|
instance.pose_start.y,
|
|
np.cos(instance.pose_start.theta),
|
|
np.sin(instance.pose_start.theta),
|
|
color='r',
|
|
width=0.1)
|
|
plt.arrow(instance.pose_target.x,
|
|
instance.pose_target.y,
|
|
np.cos(instance.pose_target.theta),
|
|
np.sin(instance.pose_target.theta),
|
|
color='g',
|
|
width=0.1)
|
|
plot_vehicle(instance.pose.x,
|
|
instance.pose.y,
|
|
instance.pose.theta,
|
|
instance.x_traj,
|
|
instance.y_traj, instance.color)
|
|
|
|
plt.pause(TIME_STEP)
|
|
|
|
|
|
def plot_vehicle(x, y, theta, x_traj, y_traj, color):
|
|
# 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 = T @ p1_i
|
|
p2 = T @ p2_i
|
|
p3 = T @ p3_i
|
|
|
|
plt.plot([p1[0], p2[0]], [p1[1], p2[1]], color+'-')
|
|
plt.plot([p2[0], p3[0]], [p2[1], p3[1]], color+'-')
|
|
plt.plot([p3[0], p1[0]], [p3[1], p1[1]], color+'-')
|
|
|
|
plt.plot(x_traj, y_traj, color+'--')
|
|
|
|
|
|
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():
|
|
pose_target = Pose(15, 15, -1)
|
|
|
|
pose_start_1 = Pose(5, 2, 0)
|
|
pose_start_2 = Pose(5, 2, 0)
|
|
pose_start_3 = Pose(5, 2, 0)
|
|
|
|
controller_1 = PathFinderController(5, 8, 2)
|
|
controller_2 = PathFinderController(5, 16, 4)
|
|
controller_3 = PathFinderController(10, 25, 6)
|
|
|
|
robot_1 = Robot("Yellow Robot", "y", 12, 5, controller_1)
|
|
robot_2 = Robot("Black Robot", "k", 16, 5, controller_2)
|
|
robot_3 = Robot("Blue Robot", "b", 20, 5, controller_3)
|
|
|
|
robot_1.set_start_target_poses(pose_start_1, pose_target)
|
|
robot_2.set_start_target_poses(pose_start_2, pose_target)
|
|
robot_3.set_start_target_poses(pose_start_3, pose_target)
|
|
|
|
robots: list[Robot] = [robot_1, robot_2, robot_3]
|
|
|
|
run_simulation(robots)
|
|
|
|
|
|
if __name__ == '__main__':
|
|
main()
|