Add a Robot class for Move to Pose Algorithm (AtsushiSakai#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 AtsushiSakai#596 change requests

* Fixed typos

* Update Control/move_to_pose/move_to_pose_robot_class.py

Co-authored-by: Atsushi Sakai <[email protected]>

* Moved PathFinderController class to move_to_pose

* Fixed issue AtsushiSakai#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 <[email protected]>

Author: Muhammad-Yazdian

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 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 @@
 
 
 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)

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()