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..30ee0bb7 --- /dev/null +++ b/PathTracking/move_to_pose/move_to_pose.py @@ -0,0 +1,116 @@ +""" +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 = 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 + +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 + + 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 + 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: + v = -v + + theta = theta + w*dt + x = x + v*cos(theta)*dt + y = y + v*sin(theta)*dt + + plot_vehicle(x, y, theta, 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.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], + [0, 0, 1] + ]) + +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 diff --git a/RoboticArm/two_joint_arm.py b/RoboticArm/two_joint_arm.py new file mode 100644 index 00000000..4e345a49 --- /dev/null +++ b/RoboticArm/two_joint_arm.py @@ -0,0 +1,83 @@ +""" +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()