github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-12 18:38:04 -05:00
Code Issues Packages Projects Releases Wiki Activity

Add a Robot class for Move to Pose Algorithm (#596)

Browse Source 
* 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>
This commit is contained in:
Muhammad-Yazdian
2021-12-25 13:42:32 +01:00
committed by GitHub
parent 4e1f644007
commit 680ecdafb2
4 changed files with 424 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

105
Control/move_to_pose/move_to_pose.py
View File

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

240
Control/move_to_pose/move_to_pose_robot.py Normal file
View File

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

95
docs/modules/control/move_to_a_pose_control/move_to_a_pose_control.rst
View File

@@ -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: <float> time increment
See Also
--------
- PathFinderController class
Ref:
----
- `P. I. Corke, "Robotics, Vision and Control" \| SpringerLink
p102 <https://link.springer.com/book/10.1007/978-3-642-20144-8>`__

14
tests/test_move_to_pose_robot.py Normal file
View File

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