# Part b: Making a coffee delivery arm with 3 links, the top length end effector starts at [0,1] and should # remain both horizontal and at y=1 for the entire simulation/animation !apt update -y !apt install ffmpeg -y import numpy as np from numpy.linalg import inv from IPython.display import Video import matplotlib.pyplot as plt import sympy from matplotlib.animation import FFMpegWriter from matplotlib.patches import Circle # Defines our kinematic tree class KinematicTreeNode: def __init__(self): # Constructor self.translation = None self.rotation = None self.child = None # Define our rotation matrix def RotZ(theta): c = np.cos(theta) s = np.sin(theta) return np.array([[c, -s],[s, c]]) # Returns a matrix with translations def NodePositions(X): allpositions = [] if X.child == None: return X.translation else: returnjoints = NodePositions(X.child) temp, translate = np.meshgrid(returnjoints[0,:], X.translation) transformed_joints = np.matmul(RotZ(X.rotation),returnjoints)+translate list_of_joints = np.hstack((X.translation, transformed_joints)) return list_of_joints # Returns end effector translation and rotation def EndEffectorPosRot(X): if X.child == None: return X.translation, X.rotation else: trans, rot = EndEffectorPosRot(X.child) new_trans = X.translation + np.matmul(RotZ(X.rotation),trans) new_rot = X.rotation+rot return new_trans, new_rot # Build the tree base = KinematicTreeNode() L1 = KinematicTreeNode() L2 = KinematicTreeNode() L3 = KinematicTreeNode() # Set up initial conditions for arms base.translation = np.array([[0],[0]]) base.rotation = np.pi #radians base.child = L1 L1.translation = np.array([[1],[0]]) L1.rotation = -np.pi/2 #radians L1.child = L2 L2.translation = np.array([[1],[0]]) L2.rotation = -np.pi/2 #radians L2.child = L3 L3.translation = np.array([[1],[0]]) L3.rotation = 0.0 L3.child = None # Set up time response dt = 0.001 t = np.arange(0,5,dt) # Inverse Kinematics: # Define a desired position path x_des = np.linspace(0,2,len(t)) y_des = 1.0 theta_des = np.zeros(len(t)) # Newton-Raphson Search # Define our desired END EFFECTOR POSITION desired_position = np.array([x_des[0],y_des,theta_des[0]], dtype=float) # Step 1: Start with an initial guess # Initial guess for our joint angles theta_current = np.array([np.pi,-np.pi,-np.pi], dtype=float) done = False # Small Epsilon for Finite Difference: epsilon = 0.001 # Initialize vectors for animation and plot L1_x = np.zeros(len(t)) L1_y = np.zeros(len(t)) L2_x = np.zeros(len(t)) L2_y = np.zeros(len(t)) L3_x = np.zeros(len(t)) L3_y = np.zeros(len(t)) theta1traveled = np.zeros(len(t)) theta2traveled = np.zeros(len(t)) theta3traveled = np.zeros(len(t)) thetastep_plot = np.zeros(len(t)) xstep_plot = np.zeros(len(t)) ystep_plot = np.zeros(len(t)) for i in range(0,len(t)): done = False while done == False: theta_current[0] = (( -theta_current[0] + np.pi) % (2.0 * np.pi ) - np.pi) * -1.0 theta_current[1] = (( -theta_current[1] + np.pi) % (2.0 * np.pi ) - np.pi) * -1.0 theta_current[2] = (( -theta_current[2] + np.pi) % (2.0 * np.pi ) - np.pi) * -1.0 desired_position = np.array([x_des[i], y_des, theta_des[i]]) # Step 2: Find f(theta) base.rotation = theta_current[0] L1.rotation = theta_current[1] L2.rotation = theta_current[2] joint_locations = NodePositions(base) endposition, rotation = EndEffectorPosRot(base) end_effector = np.array([0., 0., 0.]) end_effector[0] = endposition[0] end_effector[1] = endposition[1] end_effector[2] = rotation end_effector[2] = (( -end_effector[2] + np.pi) % (2.0 * np.pi ) - np.pi) * -1.0 # Step 3: Find the Jacobian of f w.r.t. theta jac = np.array([ [-np.sin(base.rotation) -np.sin(base.rotation + L1.rotation) -np.sin(base.rotation + L1.rotation + L2.rotation) , -np.sin(base.rotation + L1.rotation) -np.sin(base.rotation + L1.rotation + L2.rotation) , -np.sin(base.rotation + L1.rotation + L2.rotation)], [np.cos(base.rotation) +np.cos(base.rotation + L1.rotation) + np.cos(base.rotation + L1.rotation + L2.rotation) , np.cos(base.rotation + L1.rotation) + np.cos(base.rotation + L1.rotation + L2.rotation) , np.cos(base.rotation + L1.rotation + L2.rotation)], [1.0, 1.0, 1.0] ]) # Step 4: Find Step Length delta_theta = desired_position - end_effector delta_theta_step = np.matmul(np.linalg.pinv(jac), delta_theta) # Theta current: theta_current = theta_current + delta_theta_step # Terminating Condition: if np.linalg.norm(delta_theta_step) < epsilon: done = True L1_x[i] = joint_locations[0,1] L1_y[i] = joint_locations[1,1] L2_x[i] = joint_locations[0,2] L2_y[i] = joint_locations[1,2] L3_x[i] = joint_locations[0,3] L3_y[i] = joint_locations[1,3] theta1traveled[i] = base.rotation theta2traveled[i] = L1.rotation theta3traveled[i] = L2.rotation # Make a plot of angles over time plt.plot(t, theta1traveled) plt.plot(t, theta2traveled) plt.plot(t, theta3traveled) plt.legend(['theta1','theta2','theta3']) plt.title('Angles vs Time For Coffee Arm') plt.xlabel('time (s)') plt.ylabel('theta (rad)') # Create Animation: # Setup Figure: Initialize Figure / Axe Handles fig, ax = plt.subplots() p1, = ax.plot([], [], color='black', linewidth=2) p2, = ax.plot([], [], color='black', linewidth=2) p3, = ax.plot([], [], color='black', linewidth=2) lb, ub = -5, 5 ax.axis('equal') ax.set_xlim([lb, ub]) ax.set_xlabel('X') # X Label ax.set_ylabel('Y') # Y Label ax.set_title('Coffee Delivery Simulation:') video_title = "hw2q2b" # Setup Animation Writer: FPS = 20 sample_rate = int(1 / (dt * FPS)) dpi = 300 writerObj = FFMpegWriter(fps=FPS) # Initialize Patch: Pendulum 1 and 2 joint_1 = Circle((0, 0), radius=0.1, color='blue', zorder=10) joint_2 = Circle((0, 0), radius=0.1, color='blue', zorder=10) joint_3 = Circle((0, 0), radius=0.1, color='blue', zorder=10) ax.add_patch(joint_1) ax.add_patch(joint_2) ax.add_patch(joint_3) # Draw Static Objects: pin_joint = Circle((0, 0), radius=0.05, color='black', zorder=10) ax.add_patch(pin_joint) # Plot and Create Animation: with writerObj.saving(fig, video_title+".mp4", dpi): for i in range(0, len(t), sample_rate): # Draw Arm: L1_arm_x = [0, L1_x[i]] L1_arm_y = [0, L1_y[i]] L2_arm_x = [L1_x[i], L2_x[i]] L2_arm_y = [L1_y[i], L2_y[i]] L3_arm_x = [L2_x[i], L3_x[i]] L3_arm_y = [L2_y[i], L3_y[i]] p1.set_data(L1_arm_x, L1_arm_y) p2.set_data(L2_arm_x, L2_arm_y) p3.set_data(L3_arm_x, L3_arm_y) # Update Patches: joint_1_center = L1_x[i], L1_y[i] joint_2_center = L2_x[i], L2_y[i] joint_3_center = L3_x[i], L3_y[i] joint_1.center = joint_1_center joint_2.center = joint_2_center joint_3.center = joint_3_center # Update Drawing: fig.canvas.draw() # Grab and Save Frame: writerObj.grab_frame() Video("/work/"+video_title+".mp4", embed=True, width=640, height=480)