Merge pull request AtsushiSakai#94 from daniel-s-ingram/master

Algorithm for moving to specified pose

Author: AtsushiSakai

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)

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