From 1cf962ac2addde49da776eb5790dc3a45f4aff5e Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Tue, 14 Aug 2018 16:19:09 +0900 Subject: [PATCH] add test for move_to_pose sample --- PathTracking/move_to_pose/move_to_pose.py | 95 +++++++++++++---------- tests/test_move_to_pose.py | 12 +++ 2 files changed, 65 insertions(+), 42 deletions(-) create mode 100644 tests/test_move_to_pose.py diff --git a/PathTracking/move_to_pose/move_to_pose.py b/PathTracking/move_to_pose/move_to_pose.py index 30ee0bb7..30dc64ed 100644 --- a/PathTracking/move_to_pose/move_to_pose.py +++ b/PathTracking/move_to_pose/move_to_pose.py @@ -1,32 +1,26 @@ """ + Move to specified pose + Author: Daniel Ingram (daniel-s-ingram) + Atsushi Sakai(@Atsushi_twi) P. I. Corke, "Robotics, Vision & Control", Springer 2017, ISBN 978-3-319-54413-7 -""" -from __future__ import print_function, division +""" import matplotlib.pyplot as plt import numpy as np - -from math import cos, sin, sqrt, atan2, pi from random import random +# simulation parameters Kp_rho = 9 Kp_alpha = 15 Kp_beta = -3 dt = 0.01 -#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 +show_animation = True -x_traj = [] -y_traj = [] - -plt.ion() def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal): """ @@ -44,7 +38,9 @@ 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 - rho = sqrt(x_diff**2 + y_diff**2) + x_traj, y_traj = [], [] + + rho = np.sqrt(x_diff**2 + y_diff**2) while rho > 0.001: x_traj.append(x) y_traj.append(y) @@ -54,63 +50,78 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal): """ Restrict alpha and beta (angle differences) to the range - [-pi, pi] to prevent unstable behavior e.g. difference going + [-pi, pi] to prevent unstable behavior e.g. difference going from 0 rad to 2*pi rad with slight turn - """ + """ - rho = sqrt(x_diff**2 + y_diff**2) - alpha = (atan2(y_diff, x_diff) - theta + pi)%(2*pi) - pi - beta = (theta_goal - theta - alpha + pi)%(2*pi) - pi + rho = np.sqrt(x_diff**2 + y_diff**2) + 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 + v = Kp_rho * rho + w = Kp_alpha * alpha + Kp_beta * beta - if alpha > pi/2 or alpha < -pi/2: + if alpha > np.pi / 2 or alpha < -np.pi / 2: v = -v - theta = theta + w*dt - x = x + v*cos(theta)*dt - y = y + v*sin(theta)*dt + theta = theta + w * dt + x = x + v * np.cos(theta) * dt + y = y + v * np.sin(theta) * dt - plot_vehicle(x, y, theta, x_traj, y_traj) + if show_animation: + 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) def plot_vehicle(x, y, theta, x_traj, y_traj): + # 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.cla() - 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.arrow(x_start, y_start, cos(theta_start), sin(theta_start), color='r', width=0.1) - plt.arrow(x_goal, y_goal, cos(theta_goal), sin(theta_goal), color='g', width=0.1) plt.plot(x_traj, y_traj, 'b--') plt.xlim(0, 20) plt.ylim(0, 20) - plt.show() plt.pause(dt) + def transformation_matrix(x, y, theta): return np.array([ - [cos(theta), -sin(theta), x], - [sin(theta), cos(theta), y], + [np.cos(theta), -np.sin(theta), x], + [np.sin(theta), np.cos(theta), y], [0, 0, 1] - ]) + ]) + + +def main(): + x_start = 20 * random() + y_start = 20 * random() + theta_start = 2 * np.pi * random() - np.pi + x_goal = 20 * random() + y_goal = 20 * random() + theta_goal = 2 * np.pi * random() - np.pi + print("Initial x: %.2f m\nInitial y: %.2f m\nInitial theta: %.2f rad\n" % + (x_start, y_start, theta_start)) + print("Goal x: %.2f m\nGoal y: %.2f m\nGoal theta: %.2f rad\n" % + (x_goal, y_goal, theta_goal)) + move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal) + if __name__ == '__main__': - x_start = 20*random() - y_start = 20*random() - theta_start = 2*pi*random() - pi - x_goal = 20*random() - y_goal = 20*random() - theta_goal = 2*pi*random() - pi - print("Initial x: %.2f m\nInitial y: %.2f m\nInitial theta: %.2f rad\n" % (x_start, y_start, theta_start)) - print("Goal x: %.2f m\nGoal y: %.2f m\nGoal theta: %.2f rad\n" % (x_goal, y_goal, theta_goal)) - move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal) \ No newline at end of file + main() \ No newline at end of file diff --git a/tests/test_move_to_pose.py b/tests/test_move_to_pose.py new file mode 100644 index 00000000..01257920 --- /dev/null +++ b/tests/test_move_to_pose.py @@ -0,0 +1,12 @@ +from unittest import TestCase + +from PathTracking.move_to_pose import move_to_pose as m + +print(__file__) + + +class Test(TestCase): + + def test1(self): + m.show_animation = False + m.main()