Homework 1

# Modelbased Computational Methods of Robotics # Homework Assignment 1 # Due Date: 02/01/22 # Written By: 389622-MOD-HW1 # This code block installs ffmpeg to your Deepnote Machine # Allows Deepnote to write MP4 !apt update -y !apt install ffmpeg -y # Import some libraries import numpy as np import matplotlib.pyplot as plt import sympy import mpl_toolkits.mplot3d.art3d as art3d from numpy.linalg import inv from matplotlib.animation import FFMpegWriter from mpl_toolkits import mplot3d from matplotlib.patches import Circle, Rectangle, PathPatch from mpl_toolkits.mplot3d import Axes3D

######## Problem 1 ######## # Bead on a Frictionless Parabolic Wire # Define parameters m = 1 # mass (kg) g = 9.8 # gravity (m/s^2) # Define time vector for simulation dt = 0.001 # time step (s) sim_time = 10 # length of simulation (s) t_vec = np.arange(0,sim_time,dt) # simulate for ten seconds # Define vectors that will hold our state variables over time vec_size = len(t_vec) # we use this a lot so let's store it x_vec = np.zeros(vec_size) dx_vec = np.zeros(vec_size) y_vec = np.zeros(vec_size) dy_vec = np.zeros(vec_size) lambda_vec = np.zeros(vec_size) # initialized to zeros

### Part a ### # Equations of Motion # Worked out on paper :) # Our equations of motion from E-L and the constraint (y = x^2)... # m(ddx) + 0(ddy) - 2x(lambda) = 0 # 0(ddx) + m(ddy) + 1(lambda) = -mg # -2x(ddx) + 1(ddy) + 0(lambda) = 2(dx)^2 # In matrix form (Aw = b)... # A = [ m 0 -2x # 0 m 1 # -2x 1 0] # b = [ 0 # -mg # 2(dx)^2]

### Part b ### # Simulate System using Euler's Method # Initial Conditions # one per state variable x_vec[0] = -1 # (m) dx_vec[0] = 0 # (m/s) # y = x^2, dy = 2*x y_vec[0] = x_vec[0] ** 2 # (m) dy_vec[0] = 2*x_vec[0] # (m/s) # Initialize A and b matrices: A = np.array([[m,0,2],[0,m,1],[2,1,0]]) # use '2' as placeholders for elements that will iterate b = np.array([0,-m*g,2]) # use '2' as placeholders for elements that will iterate # Euler's Integration Loop using Matrix form (Aw = b) for i in range(1, vec_size): # Update necessary elements of A array for each integration A[0,2] = -2*(x_vec[i-1]) A[2,0] = -2*(x_vec[i-1]) # Update necessary elements of b array for each integration b[2] = 2*((dx_vec[i-1]) ** 2) # Create 'w' vector that will have unknowns [ddx, ddy, lambda_] = np.linalg.solve(A,b) # Use ddx and ddy to solve: x_vec[i] = x_vec[i-1] + dx_vec[i-1]*dt y_vec[i] = (x_vec[i]) ** 2 # y_vec[i] = y_vec[i-1] + dy_vec[i-1]*dt dx_vec[i] = dx_vec[i-1] + ddx*dt dy_vec[i] = 2*((x_vec[i]) ** 2) # dy_vec[i] = dy_vec[i-1] + ddy*dt # Store the constraint force values in lambda_vec lambda_vec[i] = lambda_ # Plot the Simulation # plot x position plt.subplot(3,1,1) plt.plot(t_vec,x_vec, color='c') plt.ylabel("X Position (m)") plt.title('Bead on a Wire Simulation') # plot y position plt.subplot(3,1,2) plt.plot(t_vec,y_vec, color='m') plt.ylabel("Y Position (m)") # plot lambda plt.subplot(3,1,3) plt.plot(t_vec,lambda_vec, color='cornflowerblue') plt.ylabel("Constraint Force") # label x axis plt.xlabel("Time (s)") # show the plot plt.show()

# ### Part c ### # Animate the Simulation # Create Animation: # Setup Figure: fig, ax = plt.subplots() p, = ax.plot([], [], color='royalblue') ax.axis('equal') ax.set_xlim([-5, 5]) ax.set_ylim([-.1, 1.25]) ax.set_xlabel('X') ax.set_ylabel('Y') ax.set_title('Bead on a Wire Animation:') title = "beadonwire" # Setup Animation Writer: FPS = 20 sample_rate = int(1 / (dt * FPS)) # Real Time Playback dpi = 300 writerObj = FFMpegWriter(fps=FPS) # Draw Parabolic Wire x_curve = np.linspace(-1, 1, 100) y_curve = x_curve ** 2 p.set_data(x_curve,y_curve) # this makes arbitrary parabola with range of x_vec from simulation # Draw Bead # Initialize Patch: # Draw a circle of radius 0.05 at (X, Y) location of the IC's c = Circle((-1, 1), radius=0.05, color='cornflowerblue') ax.add_patch(c) # Add the patch to the axes # this adds arbitrary patch to arbitrary parabola # Animate: with writerObj.saving(fig, title + ".mp4", dpi): # First, draw the wire p.set_data(x_curve,y_curve) # Draw the bead moving on the curve for i in range(0, vec_size, sample_rate): # Update Bead Patch: patch_center = x_vec[i],y_vec[i] c.center = patch_center # Update Drawing: fig.canvas.draw() # Save Frame: writerObj.grab_frame()

######## Problem 2 ######## # Frictionless Pendulum and Spring # Define parameters L = 1 # length (m) m1 = 1 # bob mass (kg) k = 10 # spring linear stiffness (N/m) m2 = 2 # frictionless sliding mass (kg) h = 1.5 # pendulum hinge point from ground (m) l_0 = 1 # rest length for spring (m) g = 9.8 # gravity (m/s^2) # Define time vector for simulation dt = 0.001 # time step (s) sim_time = 10 # length of simulation (s) t_vec = np.arange(0,sim_time,dt) # simulate for ten seconds # Define vectors that will hold our state variables over time vec_size = len(t_vec) # we use this a lot so let's store it x_vec = np.zeros(vec_size) dx_vec = np.zeros(vec_size) th_vec = np.zeros(vec_size) dth_vec = np.zeros(vec_size) # Define vectors for animation location as well x1_vec = np.zeros(vec_size) y1_vec = np.zeros(vec_size) x2_vec = np.zeros(vec_size) y2_vec = np.zeros(vec_size) # initialized to zeros

### Part a ### # Equations of Motion # Let's use 'sympy' to generate E-L equations # Create Symbol for Time: t = sympy.Symbol('t') # creates symbolic variable, t # Create Generalized Coordinates as a function of time: q = [x,theta] x = sympy.Function('x')(t) th = sympy.Function('th')(t) # Position Equation: r = (x_p,y_p) r1 = np.array([L*sympy.sin(th), h-L*sympy.cos(th)]) # Position of m1 r2 = np.array([x, 0]) # Velocity Equation: d/dt(r) = [dx_p/dt, dy_p/dt] v1 = np.array([r1[0].diff(t), r1[1].diff(t)]) # Velocity of m1 v2 = np.array([r2[0].diff(t), 0]) # Velocity of m2 (Python won't differentiate '0', so I hard coded) # Length of spring: l_s = sqrt(x_p^2+y_p^2) - l_0 l_s = ((r1[0]-r2[0])**2 + r1[1]**2)**(1/2) - l_0 # Energy Equations T = 1/2 * m1 * np.dot(v1, v1) + 1/2 * m2 * np.dot(v2, v2) # Kinetic Energy V = m1 * g * r1[1] + 1/2 * k * l_s**2 # Potential Energy (spring is different than general) L = T - V # Lagrangian # Lagrange Terms: dL_dx = L.diff(x) dL_ddx_dt = L.diff(x.diff(t)).diff(t) dL_dth = L.diff(th) dL_ddth_dt = L.diff(th.diff(t)).diff(t) # Euler-Lagrange Equations: dL/dq - d/dt(dL/ddq) = 0 x_eqn = dL_dx - dL_ddx_dt th_eqn = dL_dth - dL_ddth_dt # Replace Time Derivatives and Functions with Symbolic Variables: replacements = [(x.diff(t).diff(t), sympy.Symbol('ddx')), (x.diff(t), sympy.Symbol('dx')), (x, sympy.Symbol('x')), (th.diff(t).diff(t), sympy.Symbol('ddth')), (th.diff(t), sympy.Symbol('dth')), (th, sympy.Symbol('th'))] x_eqn = x_eqn.subs(replacements) th_eqn = th_eqn.subs(replacements) r1 = r1[0].subs(replacements), r1[1].subs(replacements) r2 = r2[0].subs(replacements), 0 # Simplfiy then Force SymPy to Cancel factorization: [Sometimes needed to use .coeff()] x_eqn = sympy.simplify(x_eqn) th_eqn = sympy.simplify(th_eqn) x_eqn = x_eqn.cancel() th_eqn = th_eqn.cancel() # I could not get the lin alg way (Jacob's code) to run correctly, # so I am just going to create eqns to solve for ddx and ddth from # here and use them in my Euler integration loop... ddx_eqn = sympy.solvers.solve(x_eqn,sympy.Symbol('ddx')) ddth_eqn = sympy.solvers.solve(th_eqn,sympy.Symbol('ddth')) # Use the outputs of these eqns for the simulation # ddx = [1.0008009612818e-13*(27712812921102.0*x + 49959983987187.1*(-x + sin(th))*sqrt(0.307692307692308*x**2 - 0.615384615384615*x*sin(th) - 0.923076923076923*cos(th) + 1) - 27712812921102.0*sin(th))/sqrt(0.307692307692308*x**2 - 0.615384615384615*x*sin(th) - 0.923076923076923*cos(th) + 1)] # ddth = [10.0*x*cos(th) - 5.5470019622523*x*cos(th)/sqrt(0.307692307692308*x**2 - 0.615384615384615*x*sin(th) - 0.923076923076923*cos(th) + 1) - 24.8*sin(th) + 8.32050294337845*sin(th)/sqrt(0.307692307692308*x**2 - 0.615384615384615*x*sin(th) - 0.923076923076923*cos(th) + 1)] # Generate Lambda Functions for r eqns to use for animation replacements = (sympy.Symbol('x'), sympy.Symbol('dx'), sympy.Symbol('th'), sympy.Symbol('dth')) r1 = sympy.utilities.lambdify(replacements, r1, "numpy") r2 = sympy.utilities.lambdify(replacements, r2, "numpy")

### Part b ### # Simulate System using Euler's Method # Initial Conditions # one per state variable x_vec[0] = 0 # (m) dx_vec[0] = 0 # (m/s) th_vec[0] = 1 # (rad) dth_vec[0] = 0 # (rad/s) x1_vec[0], y1_vec[0] = r1(x_vec[0], dx_vec[0], th_vec[0], dth_vec[0]) x2_vec[0], y2_vec[0] = r2(x_vec[0], dx_vec[0], th_vec[0], dth_vec[0]) # Euler Integration Loop for i in range(1,len(t_vec)): # Use ddx and ddth eqns from sympy derivation above ddx = [1.0008009612818e-13*(27712812921102.0*x_vec[i-1] + 49959983987187.1*(-x_vec[i-1] + np.sin(th_vec[i-1]))*np.sqrt(0.307692307692308*x_vec[i-1]**2 - 0.615384615384615*x_vec[i-1]*np.sin(th_vec[i-1]) - 0.923076923076923*np.cos(th_vec[i-1]) + 1) - 27712812921102.0*np.sin(th_vec[i-1]))/np.sqrt(0.307692307692308*x_vec[i-1]**2 - 0.615384615384615*x_vec[i-1]*np.sin(th_vec[i-1]) - 0.923076923076923*np.cos(th_vec[i-1]) + 1)] ddth = [10.0*x_vec[i-1]*np.cos(th_vec[i-1]) - 5.5470019622523*x_vec[i-1]*np.cos(th_vec[i-1]) /np.sqrt(0.307692307692308*x_vec[i-1]**2 - 0.615384615384615*x_vec[i-1]*np.sin(th_vec[i-1]) - 0.923076923076923*np.cos(th_vec[i-1]) + 1) - 24.8*np.sin(th_vec[i-1]) + 8.32050294337845*np.sin(th_vec[i-1])/np.sqrt(0.307692307692308*x_vec[i-1]**2 - 0.615384615384615*x_vec[i-1]*np.sin(th_vec[i-1]) - 0.923076923076923*np.cos(th_vec[i-1]) + 1)] # Use ddx and ddth to solve: x_vec[i] = x_vec[i-1] + dx_vec[i-1]*dt th_vec[i] = th_vec[i-1] + dth_vec[i-1]*dt dx_vec[i] = dx_vec[i-1] + ddx[0]*dt # ddx comes out with a list, let's call first element dth_vec[i] = dth_vec[i-1] + ddth[0]*dt # ddth comes out with a list, let's call first element # Animation States: x1_vec[i], y1_vec[i] = r1(x_vec[i-1], dx_vec[i-1], th_vec[i-1], dth_vec[i-1]) x2_vec[i], y2_vec[i] = r2(x_vec[i-1], dx_vec[i-1], th_vec[i-1], dth_vec[i-1]) # Plot the Simulation # plot x position plt.subplot(2,1,1) plt.plot(t_vec,x_vec, color='c') plt.ylabel("X Position (m)") plt.title('Pendulum-Spring-Slider Simulation') # plot theta position plt.subplot(2,1,2) plt.plot(t_vec,th_vec, color='m') plt.ylabel("Theta Position (m)") # label x axis plt.xlabel("Time (s)") # show the plot plt.show()

### Part c ### # Animate the Simulation # 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) 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('Pendulum-Spring-Slider Animation:') video_title = "pendspringslider" # Setup Animation Writer: FPS = 20 sample_rate = int(1 / (dt * FPS)) dpi = 300 writerObj = FFMpegWriter(fps=FPS) # Initialize Bob Patch bob = Circle((0, 0), radius=0.2, color='cornflowerblue', zorder=10) ax.add_patch(bob) # Add patch to plot # Initialize Slider Patch width = .75 # width of slider height = .5 # height of slider xy_slider = (-width/2,-height/2) # center the slider slider = Rectangle(xy_slider, width, height, color='cornflowerblue') ax.add_patch(slider) # Add patch to plot # Draw Static Objects: pin_joint = Circle((0, h), 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, vec_size, sample_rate): # Draw Pendulum Arm: x_pendulum_arm = [0, x1_vec[i], x2_vec[i]] y_pendulum_arm = [h, y1_vec[i], y2_vec[i]] p1.set_data(x_pendulum_arm, y_pendulum_arm) # Update Pendulum Patches: bob_center = x1_vec[i], y1_vec[i] # slider_center = x2_vec[i], y2_vec[i] bob.center = bob_center # slider.center = slider_center slider.set(xy=(x_vec[i] - width / 2, 0 - height / 2)) # Update Drawing: fig.canvas.draw() # Grab and Save Frame: writerObj.grab_frame()

######## Problem 3 ######## # Bead on a Frictionless Paraboloid # Define parameters m = 1 # bead mass (kg) g = 9.8 # gravity (m/s^2) # Define time vector for simulation dt = 0.001 # time step (s) sim_time = 10 # length of simulation (s) t_vec = np.arange(0,sim_time,dt) # simulate for ten seconds # Define vectors that will hold our state variables over time vec_size = len(t_vec) # we use this a lot so let's store it x_vec = np.zeros(vec_size) dx_vec = np.zeros(vec_size) y_vec = np.zeros(vec_size) dy_vec = np.zeros(vec_size) z_vec = np.zeros(vec_size) dz_vec = np.zeros(vec_size) # initialized to zeros

### Part a ### # Equations of Motion # Worked out on paper :)

### Part b ### # Simulate System using Euler's Method # Initial Conditions # one per state variable x_vec[0] = 1 # (m) dx_vec[0] = 0 # (m/s) y_vec[0] = 0 # (m) dy_vec[0] = 1 # (m/s) z_vec[0] = (x_vec[0]) ** 2 + y_vec[0] ** 2 # (m) dz_vec[0] = 2*(x_vec[0]) + 2*(y_vec[0]) # (m/s) # Initialize A and b matrices: A = np.array([[m,0,0,2],[0,m,0,2],[0,0,m,1],[2,2,-1,0]]) # use '2' as placeholders b = np.array([0,0,-m*g,2]) # use '2' as placeholders for elements that will iterate # Euler Integration Loop for i in range(1,len(t_vec)): # Update necessary elements of A array for each integration A[0,3] = -2*(x_vec[i-1]) A[1,3] = -2*(y_vec[i-1]) A[3,0] = 2*(x_vec[i-1]) A[3,1] = 2*(y_vec[i-1]) # Update necessary elements of b array for each integration b[3] = 2*((dx_vec[i-1]) ** 2) + 2*((dy_vec[i-1]) ** 2) # Create 'w' vector that will hold unknowns [ddx, ddy, ddz, lambda_] = np.linalg.solve(A,b) # Use ddx and ddy to solve: x_vec[i] = x_vec[i-1] + dx_vec[i-1]*dt y_vec[i] = y_vec[i-1] + dy_vec[i-1]*dt # Define z_vec in terms of x_vec and y_vec z_vec[i] = (x_vec[i]) ** 2 + y_vec[i] ** 2 dx_vec[i] = dx_vec[i-1] + ddx*dt dy_vec[i] = dy_vec[i-1] + ddy*dt # Define dz_vec in terms of x_vec and y_vec dz_vec[i] = 2*(x_vec[i]) + 2*(y_vec[i]) # Plot the Simulation (x and y) # plot x position plt.subplot(3,1,1) plt.plot(t_vec,x_vec, color='c') plt.ylabel("X Position (m)") plt.title('Bead on a Paraboloid Simulation') # plot y position plt.subplot(3,1,2) plt.plot(t_vec,y_vec, color='m') plt.ylabel("Y Position (m)") # plot z position plt.subplot(3,1,3) plt.plot(t_vec,z_vec, color='cornflowerblue') plt.ylabel("Z Position (m)") # label x axis plt.xlabel("Time (s)") # show the plot plt.show()

### Part c ### # Animate the Simulation # Create Animation: # Setup Figure: fig = plt.figure() # ax = plt.axes(projection='3d') ax = fig.add_subplot(projection='3d') p, = ax.plot3D([], [], [], color='royalblue') # ax.set_aspect('equal') # this does not work ax.set_xlim([-5, 5]) ax.set_ylim([-5, 5]) ax.set_xlabel('X') ax.set_ylabel('Y') ax.set_zlabel('Z') ax.set_title('Bead on a Paraboloid Animation:') title = "beadonparaboloid" # Setup Animation Writer: FPS = 20 sample_rate = int(1 / (dt * FPS)) # Real Time Playback dpi = 300 writerObj = FFMpegWriter(fps=FPS) # Draw Paraboloid def f(x, y): return x ** 2 + y ** 2 x_parab = np.linspace(-1, 1, 100) y_parab = np.linspace(-.5, .5, 100) X, Y = np.meshgrid(x_parab, y_parab) Z = (X) ** 2 + Y ** 2 ax.plot_wireframe(X,Y,Z, color='gray') # this is an arbitrary paraboloid # Initialize the Bead x_bead = x_vec[0] y_bead = y_vec[0] z_bead = z_vec[0] ax.scatter(x_bead, y_bead, z_bead) # Animate: with writerObj.saving(fig, title + ".mp4", dpi): # Draw the bead moving on the parabaloid for i in range(0, vec_size, sample_rate): # Clear axes to get rid of point after each iteration ax.clear() # Draw the Parabaloloid ax.plot_wireframe(X,Y,Z, color='gray') # Update Bead Point: x_bead = x_vec[i] y_bead = y_vec[i] z_bead = z_vec[i] ax.scatter(x_bead, y_bead, z_bead) # Update Drawing: fig.canvas.draw() # Save Frame: writerObj.grab_frame() # Yeah, I know this is not what it should look like, lol. # I cannot figure out how to get the axes to stay put -- # I have tried to simulate 'ax.axis('equal')' in 3D, but # cannot get it to work. I don't know why the animated bead # is not following the paraboloid, I suppose I plotted it # incorrectly or my simulation is wrong. I do believe my # simulation is correct, though, so I don't know. :/ # Also, this animation takes forever to compile -- I am sorry.