From 5154cf41b6a0ec878f61c1fe160cb0748303c510 Mon Sep 17 00:00:00 2001 From: Daniel Ingram Date: Mon, 13 Aug 2018 13:17:23 -0400 Subject: [PATCH 1/7] Move to pose --- PathTracking/move_to_pose.py/move_to_pose.py | 99 ++++++++++++++++++++ 1 file changed, 99 insertions(+) create mode 100644 PathTracking/move_to_pose.py/move_to_pose.py diff --git a/PathTracking/move_to_pose.py/move_to_pose.py b/PathTracking/move_to_pose.py/move_to_pose.py new file mode 100644 index 00000000..a0fd6f11 --- /dev/null +++ b/PathTracking/move_to_pose.py/move_to_pose.py @@ -0,0 +1,99 @@ +""" +Move to specified pose +Author: Daniel Ingram (daniel-s-ingram) + +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 + +Kp_rho = 3 +Kp_alpha = 5 +Kp_beta = -2 +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 + +x_traj = [] +y_traj = [] + +plt.ion() + +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 + while True: + x_diff = x_goal - x + y_diff = y_goal - y + + rho = sqrt(x_diff**2 + y_diff**2) + alpha = atan2(y_diff, x_diff) - theta + beta = theta_goal - theta - alpha + + v = Kp_rho*rho + w = Kp_alpha*alpha + Kp_beta*beta + + if alpha > pi/2 or alpha < -pi/2: + v = -v + + theta = theta + w*dt + x = x + v*cos(theta)*dt + y = y + v*sin(theta)*dt + x_traj.append(x) + y_traj.append(y) + + plot_vehicle(x, y, theta, x_start, y_start, x_goal, y_goal, x_traj, y_traj) + + +def plot_vehicle(x, y, theta, x_start, y_start, x_goal, y_goal, x_traj, y_traj): + 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.plot(x_start, y_start, 'r*') + plt.plot(x_goal, y_goal, 'g*') + plt.plot(x_traj, y_traj, 'b--') + plt.xlim(0, 20) + plt.ylim(0, 20) + plt.show() + plt.pause(dt/10) + +def transformation_matrix(x, y, theta): + return np.array([ + [cos(theta), -sin(theta), x], + [sin(theta), cos(theta), y], + [0, 0, 1] + ]) + +if __name__ == '__main__': + x_start = 4 #20*random() + y_start = 15 #20*random() + theta_start = pi/2 #2*pi*random() - pi + x_goal = 13 #20*random() + y_goal = 3 #20*random() + theta_goal = -pi/2 #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 From ebaefb9877f8184d97a39abc8e48183de1b7ae81 Mon Sep 17 00:00:00 2001 From: Daniel Ingram Date: Mon, 13 Aug 2018 13:19:46 -0400 Subject: [PATCH 2/7] Move to pose --- PathTracking/move_to_pose/move_to_pose.py | 99 +++++++++++++++++++++++ 1 file changed, 99 insertions(+) create mode 100644 PathTracking/move_to_pose/move_to_pose.py diff --git a/PathTracking/move_to_pose/move_to_pose.py b/PathTracking/move_to_pose/move_to_pose.py new file mode 100644 index 00000000..a0fd6f11 --- /dev/null +++ b/PathTracking/move_to_pose/move_to_pose.py @@ -0,0 +1,99 @@ +""" +Move to specified pose +Author: Daniel Ingram (daniel-s-ingram) + +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 + +Kp_rho = 3 +Kp_alpha = 5 +Kp_beta = -2 +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 + +x_traj = [] +y_traj = [] + +plt.ion() + +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 + while True: + x_diff = x_goal - x + y_diff = y_goal - y + + rho = sqrt(x_diff**2 + y_diff**2) + alpha = atan2(y_diff, x_diff) - theta + beta = theta_goal - theta - alpha + + v = Kp_rho*rho + w = Kp_alpha*alpha + Kp_beta*beta + + if alpha > pi/2 or alpha < -pi/2: + v = -v + + theta = theta + w*dt + x = x + v*cos(theta)*dt + y = y + v*sin(theta)*dt + x_traj.append(x) + y_traj.append(y) + + plot_vehicle(x, y, theta, x_start, y_start, x_goal, y_goal, x_traj, y_traj) + + +def plot_vehicle(x, y, theta, x_start, y_start, x_goal, y_goal, x_traj, y_traj): + 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.plot(x_start, y_start, 'r*') + plt.plot(x_goal, y_goal, 'g*') + plt.plot(x_traj, y_traj, 'b--') + plt.xlim(0, 20) + plt.ylim(0, 20) + plt.show() + plt.pause(dt/10) + +def transformation_matrix(x, y, theta): + return np.array([ + [cos(theta), -sin(theta), x], + [sin(theta), cos(theta), y], + [0, 0, 1] + ]) + +if __name__ == '__main__': + x_start = 4 #20*random() + y_start = 15 #20*random() + theta_start = pi/2 #2*pi*random() - pi + x_goal = 13 #20*random() + y_goal = 3 #20*random() + theta_goal = -pi/2 #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 From 8e71c36c68ad4ac6baf7109350ec0b2d6086d86b Mon Sep 17 00:00:00 2001 From: Daniel Ingram Date: Mon, 13 Aug 2018 13:20:32 -0400 Subject: [PATCH 3/7] Move to pose --- PathTracking/move_to_pose.py/move_to_pose.py | 99 -------------------- 1 file changed, 99 deletions(-) delete mode 100644 PathTracking/move_to_pose.py/move_to_pose.py diff --git a/PathTracking/move_to_pose.py/move_to_pose.py b/PathTracking/move_to_pose.py/move_to_pose.py deleted file mode 100644 index a0fd6f11..00000000 --- a/PathTracking/move_to_pose.py/move_to_pose.py +++ /dev/null @@ -1,99 +0,0 @@ -""" -Move to specified pose -Author: Daniel Ingram (daniel-s-ingram) - -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 - -Kp_rho = 3 -Kp_alpha = 5 -Kp_beta = -2 -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 - -x_traj = [] -y_traj = [] - -plt.ion() - -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 - while True: - x_diff = x_goal - x - y_diff = y_goal - y - - rho = sqrt(x_diff**2 + y_diff**2) - alpha = atan2(y_diff, x_diff) - theta - beta = theta_goal - theta - alpha - - v = Kp_rho*rho - w = Kp_alpha*alpha + Kp_beta*beta - - if alpha > pi/2 or alpha < -pi/2: - v = -v - - theta = theta + w*dt - x = x + v*cos(theta)*dt - y = y + v*sin(theta)*dt - x_traj.append(x) - y_traj.append(y) - - plot_vehicle(x, y, theta, x_start, y_start, x_goal, y_goal, x_traj, y_traj) - - -def plot_vehicle(x, y, theta, x_start, y_start, x_goal, y_goal, x_traj, y_traj): - 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.plot(x_start, y_start, 'r*') - plt.plot(x_goal, y_goal, 'g*') - plt.plot(x_traj, y_traj, 'b--') - plt.xlim(0, 20) - plt.ylim(0, 20) - plt.show() - plt.pause(dt/10) - -def transformation_matrix(x, y, theta): - return np.array([ - [cos(theta), -sin(theta), x], - [sin(theta), cos(theta), y], - [0, 0, 1] - ]) - -if __name__ == '__main__': - x_start = 4 #20*random() - y_start = 15 #20*random() - theta_start = pi/2 #2*pi*random() - pi - x_goal = 13 #20*random() - y_goal = 3 #20*random() - theta_goal = -pi/2 #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 From 599ddf291af4523844ef2a50f1f1a37381f32571 Mon Sep 17 00:00:00 2001 From: Daniel Ingram Date: Mon, 13 Aug 2018 13:41:42 -0400 Subject: [PATCH 4/7] Draw goal vector on plot --- PathTracking/move_to_pose/move_to_pose.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/PathTracking/move_to_pose/move_to_pose.py b/PathTracking/move_to_pose/move_to_pose.py index a0fd6f11..2ac83d44 100644 --- a/PathTracking/move_to_pose/move_to_pose.py +++ b/PathTracking/move_to_pose/move_to_pose.py @@ -60,10 +60,10 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal): x_traj.append(x) y_traj.append(y) - plot_vehicle(x, y, theta, x_start, y_start, x_goal, y_goal, x_traj, y_traj) + plot_vehicle(x, y, theta, x_start, y_start, x_goal, y_goal, theta_goal, x_traj, y_traj) -def plot_vehicle(x, y, theta, x_start, y_start, x_goal, y_goal, x_traj, y_traj): +def plot_vehicle(x, y, theta, x_start, y_start, x_goal, y_goal, theta_goal, x_traj, y_traj): T = transformation_matrix(x, y, theta) p1 = np.matmul(T, p1_i) p2 = np.matmul(T, p2_i) @@ -73,7 +73,7 @@ def plot_vehicle(x, y, theta, x_start, y_start, x_goal, y_goal, x_traj, y_traj): plt.plot([p2[0], p3[0]], [p2[1], p3[1]], 'k-') plt.plot([p3[0], p1[0]], [p3[1], p1[1]], 'k-') plt.plot(x_start, y_start, 'r*') - plt.plot(x_goal, y_goal, 'g*') + 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) From 3988e21051e912ec7da5995026dbb0c699f27c40 Mon Sep 17 00:00:00 2001 From: Daniel Ingram Date: Mon, 13 Aug 2018 21:42:17 -0400 Subject: [PATCH 5/7] two_joint_arm.py --- RoboticArm/two_joint_arm.py | 77 +++++++++++++++++++++++++++++++++++++ 1 file changed, 77 insertions(+) create mode 100644 RoboticArm/two_joint_arm.py diff --git a/RoboticArm/two_joint_arm.py b/RoboticArm/two_joint_arm.py new file mode 100644 index 00000000..60fb5eff --- /dev/null +++ b/RoboticArm/two_joint_arm.py @@ -0,0 +1,77 @@ +""" +Inverse kinematics of a two-joint arm +Left-click the plot to set the goal position of the end effector + +Author: Daniel Ingram (daniel-s-ingram) +""" +from __future__ import print_function, division + +import matplotlib.pyplot as plt +import numpy as np + +from math import sin, cos, atan2, acos, pi + +Kp = 15 +dt = 0.01 + +#Link lengths +l1 = l2 = 1 + +shoulder = np.array([0, 0]) + +#Set initial goal position to the initial end-effector position +x = 2 +y = 0 + +plt.ion() + +def two_joint_arm(): + """ + Computes the inverse kinematics for a planar 2DOF arm + """ + theta1 = theta2 = 0 + while True: + try: + theta2_goal = acos((x**2 + y**2 - l1**2 -l2**2)/(2*l1*l2)) + theta1_goal = atan2(y, x) - atan2(l2*sin(theta2_goal), (l1 + l2*cos(theta2_goal))) + + if theta1_goal < 0: + theta2_goal = -theta2_goal + theta1_goal = atan2(y, x) - atan2(l2*sin(theta2_goal), (l1 + l2*cos(theta2_goal))) + + theta1 = theta1 + Kp*ang_diff(theta1_goal, theta1)*dt + theta2 = theta2 + Kp*ang_diff(theta2_goal, theta2)*dt + except ValueError as e: + print("Unreachable goal") + + plot_arm(theta1, theta2, x, y) + +def plot_arm(theta1, theta2, x, y): + elbow = shoulder + np.array([l1*cos(theta1), l1*sin(theta1)]) + wrist = elbow + np.array([l2*cos(theta1+theta2), l2*sin(theta1+theta2)]) + plt.cla() + plt.plot([shoulder[0], elbow[0]], [shoulder[1], elbow[1]], 'k-') + plt.plot([elbow[0], wrist[0]], [elbow[1], wrist[1]], 'k-') + plt.plot(shoulder[0], shoulder[1], 'ro') + plt.plot(elbow[0], elbow[1], 'ro') + plt.plot(wrist[0], wrist[1], 'ro') + plt.plot([wrist[0], x], [wrist[1], y], 'g--') + plt.plot(x, y, 'g*') + plt.xlim(-2, 2) + plt.ylim(-2, 2) + plt.show() + plt.pause(dt) + +def ang_diff(theta1, theta2): + #Returns the difference between two angles in the range -pi to +pi + return (theta1-theta2+pi)%(2*pi)-pi + +def click(event): + global x, y + x = event.xdata + y = event.ydata + +if __name__ == "__main__": + fig = plt.figure() + fig.canvas.mpl_connect("button_press_event", click) + two_joint_arm() From df8e91dea8aa303332184d67391bb71f33cdbff8 Mon Sep 17 00:00:00 2001 From: Daniel Ingram Date: Mon, 13 Aug 2018 21:50:00 -0400 Subject: [PATCH 6/7] Planar 2DOF arm --- RoboticArm/two_joint_arm.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/RoboticArm/two_joint_arm.py b/RoboticArm/two_joint_arm.py index 60fb5eff..4e345a49 100644 --- a/RoboticArm/two_joint_arm.py +++ b/RoboticArm/two_joint_arm.py @@ -49,16 +49,22 @@ def two_joint_arm(): def plot_arm(theta1, theta2, x, y): elbow = shoulder + np.array([l1*cos(theta1), l1*sin(theta1)]) wrist = elbow + np.array([l2*cos(theta1+theta2), l2*sin(theta1+theta2)]) + plt.cla() + plt.plot([shoulder[0], elbow[0]], [shoulder[1], elbow[1]], 'k-') plt.plot([elbow[0], wrist[0]], [elbow[1], wrist[1]], 'k-') + plt.plot(shoulder[0], shoulder[1], 'ro') plt.plot(elbow[0], elbow[1], 'ro') plt.plot(wrist[0], wrist[1], 'ro') + plt.plot([wrist[0], x], [wrist[1], y], 'g--') plt.plot(x, y, 'g*') + plt.xlim(-2, 2) plt.ylim(-2, 2) + plt.show() plt.pause(dt) From 98441a231ad16590fdbd5b056d8476ab4d1bcf53 Mon Sep 17 00:00:00 2001 From: Daniel Ingram Date: Tue, 14 Aug 2018 00:08:16 -0400 Subject: [PATCH 7/7] Fixed unstable behavior --- PathTracking/move_to_pose/move_to_pose.py | 55 +++++++++++++++-------- 1 file changed, 36 insertions(+), 19 deletions(-) diff --git a/PathTracking/move_to_pose/move_to_pose.py b/PathTracking/move_to_pose/move_to_pose.py index 2ac83d44..30ee0bb7 100644 --- a/PathTracking/move_to_pose/move_to_pose.py +++ b/PathTracking/move_to_pose/move_to_pose.py @@ -13,9 +13,9 @@ import numpy as np from math import cos, sin, sqrt, atan2, pi from random import random -Kp_rho = 3 -Kp_alpha = 5 -Kp_beta = -2 +Kp_rho = 9 +Kp_alpha = 15 +Kp_beta = -3 dt = 0.01 #Corners of triangular vehicle when pointing to the right (0 radians) @@ -40,45 +40,62 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal): x = x_start y = y_start theta = theta_start - while True: + + x_diff = x_goal - x + y_diff = y_goal - y + + rho = sqrt(x_diff**2 + y_diff**2) + while rho > 0.001: + x_traj.append(x) + y_traj.append(y) + 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 = sqrt(x_diff**2 + y_diff**2) - alpha = atan2(y_diff, x_diff) - theta - beta = theta_goal - theta - alpha + alpha = (atan2(y_diff, x_diff) - theta + pi)%(2*pi) - pi + beta = (theta_goal - theta - alpha + pi)%(2*pi) - pi v = Kp_rho*rho w = Kp_alpha*alpha + Kp_beta*beta - if alpha > pi/2 or alpha < -pi/2: + if alpha > pi/2 or alpha < -pi/2: v = -v theta = theta + w*dt x = x + v*cos(theta)*dt y = y + v*sin(theta)*dt - x_traj.append(x) - y_traj.append(y) - plot_vehicle(x, y, theta, x_start, y_start, x_goal, y_goal, theta_goal, x_traj, y_traj) + plot_vehicle(x, y, theta, x_traj, y_traj) -def plot_vehicle(x, y, theta, x_start, y_start, x_goal, y_goal, theta_goal, x_traj, y_traj): +def plot_vehicle(x, y, theta, x_traj, y_traj): 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.plot(x_start, y_start, 'r*') + + 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/10) + plt.pause(dt) def transformation_matrix(x, y, theta): return np.array([ @@ -88,12 +105,12 @@ def transformation_matrix(x, y, theta): ]) if __name__ == '__main__': - x_start = 4 #20*random() - y_start = 15 #20*random() - theta_start = pi/2 #2*pi*random() - pi - x_goal = 13 #20*random() - y_goal = 3 #20*random() - theta_goal = -pi/2 #2*pi*random() - pi + 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