From 680ecdafb21a798eb47649175e0eba4e72ef356d Mon Sep 17 00:00:00 2001 From: Muhammad-Yazdian <93126501+Muhammad-Yazdian@users.noreply.github.com> Date: Sat, 25 Dec 2021 13:42:32 +0100 Subject: [PATCH] Add a Robot class for Move to Pose Algorithm (#596) * 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 * 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 --- Control/move_to_pose/move_to_pose.py | 105 ++++++-- Control/move_to_pose/move_to_pose_robot.py | 240 ++++++++++++++++++ .../move_to_a_pose_control.rst | 95 ++++++- tests/test_move_to_pose_robot.py | 14 + 4 files changed, 424 insertions(+), 30 deletions(-) create mode 100644 Control/move_to_pose/move_to_pose_robot.py create mode 100644 tests/test_move_to_pose_robot.py diff --git a/Control/move_to_pose/move_to_pose.py b/Control/move_to_pose/move_to_pose.py index 4548a6bb..73604ccd 100644 --- a/Control/move_to_pose/move_to_pose.py +++ b/Control/move_to_pose/move_to_pose.py @@ -3,7 +3,8 @@ Move to specified pose Author: Daniel Ingram (daniel-s-ingram) - Atsushi Sakai(@Atsushi_twi) + Atsushi Sakai (@Atsushi_twi) + Seied Muhammad Yazdian (@Muhammad-Yazdian) P. I. Corke, "Robotics, Vision & Control", Springer 2017, ISBN 978-3-319-54413-7 @@ -13,10 +14,77 @@ import matplotlib.pyplot as plt import numpy as np 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 + + 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 = self.Kp_rho * rho + w = self.Kp_alpha * alpha - controller.Kp_beta * beta + + if alpha > np.pi / 2 or alpha < -np.pi / 2: + v = -v + + return rho, v, w + + # simulation parameters -Kp_rho = 9 -Kp_alpha = 15 -Kp_beta = -3 +controller = PathFinderController(9, 15, 3) dt = 0.01 # Robot specifications @@ -27,14 +95,6 @@ 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 @@ -52,20 +112,8 @@ 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 - # 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 + 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 @@ -104,8 +152,9 @@ def plot_vehicle(x, y, theta, x_traj, y_traj): # pragma: no cover 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.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) diff --git a/Control/move_to_pose/move_to_pose_robot.py b/Control/move_to_pose/move_to_pose_robot.py new file mode 100644 index 00000000..d5c51caa --- /dev/null +++ b/Control/move_to_pose/move_to_pose_robot.py @@ -0,0 +1,240 @@ +""" + +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() diff --git a/docs/modules/control/move_to_a_pose_control/move_to_a_pose_control.rst b/docs/modules/control/move_to_a_pose_control/move_to_a_pose_control.rst index c5bf15d9..81c64f54 100644 --- a/docs/modules/control/move_to_a_pose_control/move_to_a_pose_control.rst +++ b/docs/modules/control/move_to_a_pose_control/move_to_a_pose_control.rst @@ -1,11 +1,102 @@ Move to a pose control ---------------------- -This is a simulation of moving to a pose control +This is a simulation of moving to a pose control. .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/move_to_pose/animation.gif -Ref: +PathFinderController class +~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Constructor +~~~~~~~~~~~ + +.. code-block:: ipython3 + PathFinderController(Kp_rho, Kp_alpha, Kp_beta) + +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.) + + +Member function(s) +~~~~~~~~~~~~~~~~~~ + +.. code-block:: ipython3 + calc_control_command(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 + +Move to a Pose Robot (Class) +---------------------------- +This program (move_to_pose_robot.py) provides a Robot class to define different robots with different specifications. +Using this class, you can simulate different robots simultaneously and compare the effect of your parameter settings. + +.. image:: https://user-images.githubusercontent.com/93126501/145834505-a8df8311-5445-413f-a96f-00460d47991c.png + +Note: A gif animation will be added soon. + +Note: The robot Class is based on PathFinderController class in 'the move_to_pose.py'. + +Robot Class +~~~~~~~~~~~ + +Constructor +~~~~~~~~~~~ + +.. code-block:: ipython3 + Robot(name, color, max_linear_speed, max_angular_speed, path_finder_controller) + +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. + +Member function(s) +~~~~~~~~~~~~~~~~~~ + +.. code-block:: ipython3 + set_start_target_poses(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) + +.. code-block:: ipython3 + move(dt) + +Move the robot for one time step increment + +Parameters: +- dt: time increment + +See Also +-------- +- PathFinderController class + + +Ref: +---- - `P. I. Corke, "Robotics, Vision and Control" \| SpringerLink p102 `__ diff --git a/tests/test_move_to_pose_robot.py b/tests/test_move_to_pose_robot.py new file mode 100644 index 00000000..a93b44d1 --- /dev/null +++ b/tests/test_move_to_pose_robot.py @@ -0,0 +1,14 @@ +import conftest # Add root path to sys.path +from Control.move_to_pose import move_to_pose as m + + +def test_1(): + """ + This unit test tests the move_to_pose_robot.py program + """ + m.show_animation = False + m.main() + + +if __name__ == '__main__': + conftest.run_this_test(__file__)