# Import Libraries: import numpy as np import sympy from numpy.linalg import inv import matplotlib.pyplot as plt from matplotlib.animation import FFMpegWriter from matplotlib.patches import Rectangle from matplotlib.patches import Circle import math !apt update -y !apt install ffmpeg -y

class KinematicTreeNode: def __init__(self): # Constructor self.translation = None self.rotation = None self.child = None def RotZ(theta): c = np.cos(theta) s = np.sin(theta) return np.array([[c, -s],[s, c]]) def EndPos(X): if X.child == None: return X.translation else: return np.column_stack((X.translation,X.translation + np.matmul(RotZ(X.rotation),EndPos(X.child)))) # Takes the base link and returns the end-effector position def GetAllJoints(X): if X.child == None: return X.translation else: return np.column_stack((X.translation,X.translation + np.matmul(RotZ(X.rotation),GetAllJoints(X.child)))) def invKinematics(pd,th): # Loop for Inverse Kinematics with Newton-Raphson # Step 1: Initial guess theta_cur = np.array([[th[0,0]],[th[1,0]]]) # avoid singularity for initial guess for i in range(1,10): # Step 2: Find f(theta) base.rotation = theta_cur[0,0] L1.rotation = theta_cur[1,0] p = GetAllJoints(base) # Linkage position array pa_posX = p[0,2] pa_posY = p[1,2] pa = np.vstack((pa_posX,pa_posY)) # Actual end effector position # Step 3: Find Jacobian of f wrt theta jac = np.array([[0.0, 0.0],[0.0, 0.0]]) for j in range(0,2): for k in range(0,2): delta_theta = np.array([[0.0],[0.0]]) delta_theta[k,0] = 0.01 base.rotation = theta_cur[0,0] + delta_theta[0,0] L1.rotation = theta_cur[1,0] + delta_theta[1,0] p_pos_perturb = EndPos(base) # Array of each link's perterbation p_pos_perturbX = p_pos_perturb[0,2] p_pos_perturbY = p_pos_perturb[1,2] p_pos_perturb = np.vstack((p_pos_perturbX,p_pos_perturbY)) theta_step = np.array([[base.rotation],[L1.rotation]]) partial_der = (p_pos_perturb-pa)/0.01 jac[j,k] = partial_der[j] # Step 4: Find delta theta delta_theta_step = np.matmul(inv(jac),(pd-pa)) theta_cur = theta_cur + delta_theta_step return theta_cur # Giving bas, L1, L2 acces to .translate, .rotate, and .child base = KinematicTreeNode() L1 = KinematicTreeNode() L2 = KinematicTreeNode() # Initializing each joint: base.translation = np.array([[0],[0]]) # translation to the joint base.rotation = np.pi # rotation of the joint L1.translation = np.array([[1],[0]]) L1.rotation = np.pi/2 L2.translation = np.array([[1],[0]]) L2.rotation = 0 # Defining children of each joint: base.child = L1 L1.child = L2 L2.child = None

# Define Time Vector dt = 0.001 # Time Step (s) sim_time = 5 # Length of Simulation (s) t_vec = np.arange(0, sim_time, dt) vec_size = len(t_vec) #print(vec_size) half_vec_size = int(vec_size/2) #print(half_vec_size) P = EndPos(base) L2_p = P[:,2] y = -1.65 x_start = -0.70 y_start = y x_end = 0.70 y_end = y_start start = np.array([[x_start],[y_start]]) end = np.array([[x_end],[y_end]]) dx = end[0,0] - start[0,0] dy = end[1,0] - start[1,0] D = dx r = D/2 center = np.array([[dx/2],[y]]) x = np.zeros(half_vec_size) y = np.zeros(half_vec_size) r = np.zeros(half_vec_size) th = np.array([[np.pi/4],[np.pi/2]]) # Initial Guess th1 = np.zeros(vec_size) # Will be used for graphs th2 = np.zeros(vec_size) phi = np.linspace(0,np.pi,half_vec_size) #phi = phi[::-1] a = dx/2 b = 0.5 # CREATING THE ELLIPSE PATH FOR FOOT for i in range(0,half_vec_size): r[i] = ( 1/( ((np.cos(phi[i])**2)/(a**2)) + ((np.sin(phi[i])**2)/(b**2)) ) )**(1/2) x[i] = r[i]*np.cos(phi[i]) y[i] = r[i]*np.sin(phi[i]) + y_start pdx = x[i] pdy = y[i] pd = np.array([[pdx], [pdy]]) # Array holding the x,y components of the endEff at each iteration th = invKinematics(pd,th) # Grabing the thetas at each iteration th1[i] = th[0,0] th2[i] = th[1,0] th = np.array([[th1[i]], [th2[i]]]) # DEFINGING VARIABLES FOR THE FLAT PATH OF FOOT x_flat_start = 0.70 y_flat_start = -1.65 x_flat_end = -0.70 y_flat_end = -1.65 flat_start = np.array([[x_start],[y_start]]) flat_end = np.array([[x_end],[y_end]]) dx_flat = flat_end[0,0] - flat_start[0,0] dy = end[1,0] - start[1,0] x_flat = np.arange(flat_start[0,0], flat_end[0,0], dx_flat/half_vec_size) # Setting initial condition for th1 and th2 th = np.array([[th1[half_vec_size-1]],[th2[half_vec_size-1]]]) # CREATING THE FLAT PATH FOR FOOT for i in range(0,half_vec_size): pdx = x_flat[i] pdy = y_start pd = np.array([[pdx], [pdy]]) # Array holding the x,y components of the endEff at each iteration th = invKinematics(pd,th) # Grabing the thetas at each iteration th1[half_vec_size+i] = th[0,0] th2[half_vec_size+i] = th[1,0] th = np.array([[th1[half_vec_size+i]], [th2[half_vec_size+i]]]) # FINALIZING x & y ARRAYS y_flat = y_flat_start*np.ones(half_vec_size) x_final = np.concatenate((x, x_flat), axis=None) # Concatenates ellips and flat values y_final = np.concatenate((y, y_flat), axis=None) # Concatenates ellips and flat values t_vec_NEW = np.arange(0, sim_time, dt) # Plot all theta values over time: plt.plot(x_final, y_final) plt.title("x vs. y") plt.axis("equal") plt.show() plt.plot(t_vec_NEW, th1) plt.title("th1 vs. time") plt.show() plt.plot(t_vec_NEW, th2) plt.title("th2 vs. time") plt.show()

# Dynamics using Lagrangian EOM solved symbolically # Redefining angular values from kinematics to desired values for dynamics x = x_final y = y_final th1_des = np.zeros(vec_size) th2_des = np.zeros(vec_size) for i in range(0,vec_size): th1_des[i] = th1[i] th2_des[i] = th2[i] # Pendulum Parameters: # For this model, treat the system as a double pendulum to represent the legs m_1 = 1 # kg m_2 = 1 # kg l_1 = 1 # m l_2 = 1 # m g = 9.8 # m/s^2 # Create Symbols for Time: t = sympy.Symbol('t') # Creates symbolic variable t tau1 = sympy.Symbol('tau1') tau2 = sympy.Symbol('tau2') # Create Generalized Coordinates as a function of time: q = [theta_1, theta_2, theta_3] th1 = sympy.Function('th1')(t) th2 = sympy.Function('th2')(t) # Position Equation: r = [x, y] r1 = np.array([l_1 * sympy.sin(th1), -l_1 * sympy.cos(th1)]) # Position of first pendulum r2 = np.array([l_2 * sympy.sin(th2) + r1[0], -l_2 * sympy.cos(th2) + r1[1]]) # Position of second pendulum # Velocity Equation: d/dt(r) = [dx/dt, dy/dt] v1 = np.array([r1[0].diff(t), r1[1].diff(t)]) # Velocity of first pendulum v2 = np.array([r2[0].diff(t), r2[1].diff(t)]) # Velocity of second pendulum # Energy Equations: T = (1/2 * m_1 * np.dot(v1, v1)) + (1/2 * m_2 * np.dot(v2, v2)) # Kinetic Energy V = m_1 * g * r1[1] + m_2 * g * r2[1] # Potential Energy L = T - V # Lagrangian # Lagrange Terms: dL_dth1 = L.diff(th1) dL_dth1_dt = L.diff(th1.diff(t)).diff(t) dL_dth2 = L.diff(th2) dL_dth2_dt = L.diff(th2.diff(t)).diff(t) # Euler-Lagrange Equations: dL/dq - d/dt(dL/ddq) = 0 th1_eqn = dL_dth1 - dL_dth1_dt - tau1 th2_eqn = dL_dth2 - dL_dth2_dt - tau2 # Replace Time Derivatives and Functions with Symbolic Variables: ddth1 = sympy.Symbol('ddth1') ddth2 = sympy.Symbol('ddth2') replacements = [(th1.diff(t).diff(t), sympy.Symbol('ddth1')), (th1.diff(t), sympy.Symbol('dth1')), (th1, sympy.Symbol('th1')), (th2.diff(t).diff(t), sympy.Symbol('ddth2')), (th2.diff(t), sympy.Symbol('dth2')), (th2, sympy.Symbol('th2'))] th1_eqn = th1_eqn.subs(replacements) th2_eqn = th2_eqn.subs(replacements) r1 = r1[0].subs(replacements), r1[1].subs(replacements) r2 = r2[0].subs(replacements), r2[1].subs(replacements) # Simplfiy then Force SymPy to Cancel factorization: [Sometimes needed to use .coeff()] th1_eqn = sympy.simplify(th1_eqn) th2_eqn = sympy.simplify(th2_eqn) th1_eqn = th1_eqn.cancel() th2_eqn = th2_eqn.cancel() # Solve for control torques tau1 = sympy.solve(th1_eqn,tau1) tau2 = sympy.solve(th2_eqn,tau2) # Solve for accelerations ddth1 = sympy.solve(th1_eqn,ddth1) ddth2 = sympy.solve(th2_eqn,ddth2) # Generate Lambda Functions for Torque and Position Equations: replacements = (sympy.Symbol('th1'), sympy.Symbol('dth1'), sympy.Symbol('ddth1'), sympy.Symbol('th2'), sympy.Symbol('dth2'), sympy.Symbol('ddth2')) tau1 = sympy.utilities.lambdify(replacements, tau1, "numpy") tau2 = sympy.utilities.lambdify(replacements, tau2, "numpy") replacements = (sympy.Symbol('th1'), sympy.Symbol('dth1'), sympy.Symbol('ddth1'), sympy.Symbol('tau1'), sympy.Symbol('th2'), sympy.Symbol('dth2'), sympy.Symbol('ddth2'), sympy.Symbol('tau2')) ddth1 = sympy.utilities.lambdify(replacements, ddth1, "numpy") ddth2 = sympy.utilities.lambdify(replacements, ddth2, "numpy") replacements = (sympy.Symbol('th1'), sympy.Symbol('th2')) r1 = sympy.utilities.lambdify(replacements, r1, "numpy") r2 = sympy.utilities.lambdify(replacements, r2, "numpy") # Simulate Dynamic System Control: # Initial: th1, dth1, th2, dth2, th3, dth3 x0 = th1_des[0], 0.0, th2_des[0], 0.0 # Control gains: These have not yet been tuned for optimum perfomance Kp = 150 Kd = 10 # Initialize Arrays: th1_vec = np.zeros(vec_size) dth1_vec = np.zeros(vec_size) th2_vec = np.zeros(vec_size) dth2_vec = np.zeros(vec_size) # Evaluate Initial Conditions: th1_vec[0] = x0[0] dth1_vec[0] = x0[1] th2_vec[0] = x0[2] dth2_vec[0] = x0[3] # Inverse Dynamics: for i in range(1, vec_size): # PD Control of accelerations ddtheta1 = Kp*(th1_des[i-1]-th1_vec[i-1]) + Kd*(0-dth1_vec[i-1]) ddtheta2 = Kp*(th2_des[i-1]-th2_vec[i-1]) + Kd*(0-dth2_vec[i-1]) # Solving for torques t1 = tau1(th1_vec[i-1], dth1_vec[i-1], ddtheta1, th2_vec[i-1], dth2_vec[i-1], ddtheta2) t2 = tau2(th1_vec[i-1], dth1_vec[i-1], ddtheta1, th2_vec[i-1], dth2_vec[i-1], ddtheta2) # Update theta from Euler step th1_vec[i] = th1_vec[i-1] + dth1_vec[i-1]*dt th2_vec[i] = th2_vec[i-1] + dth2_vec[i-1]*dt # Informed update of velcoity value from EOM and tau accth1 = ddth1(th1_vec[i-1], dth1_vec[i-1], ddtheta1, t1[0], th2_vec[i-1], dth2_vec[i-1], ddtheta2, t2[0]) accth2 = ddth2(th1_vec[i-1], dth1_vec[i-1], ddtheta1, t1[0], th2_vec[i-1], dth2_vec[i-1], ddtheta2, t2[0]) dth1_vec[i] = dth1_vec[i-1] + accth1[0]*dt dth2_vec[i] = dth2_vec[i-1] + accth2[0]*dt

!apt update -y !apt install ffmpeg -y import math import matplotlib.pyplot as plt from matplotlib.animation import FFMpegWriter from matplotlib.patches import Rectangle from matplotlib.patches import Circle #************************************************* # Create animation # Setup figure: fig, ax = plt.subplots() p, = ax.plot([],[],color='dimgray') q, = ax.plot([],[],color='dimgray') min_lim = -3 max_lim = 3 ax.axis('equal') ax.set_xlim([min_lim, max_lim]) ax.set_ylim([min_lim, max_lim]) ax.set_xlabel('X') ax.set_ylabel('Y') ax.set_title('Robot Dog Leg motion') title = "Robot Dog Leg motion" # Setup animation writer FPS = 20 sample_rate = int(1/(dt*FPS)) # Real time playback dpi = 300 writerObj = FFMpegWriter(fps=FPS) # Initialize Patch: (Base 1) width = 0.5 # Width of Cart height = width/2 # Height of Cart xy_cart = (-width/2, -height/2) # Bottom Left Corner of Cart r = Rectangle(xy_cart, width, height, color='black') # Rectangle Patch ax.add_patch(r) # Add Patch to Plot P = EndPos(base) P_L2 = P[:,2] xfoot = P_L2[0] yfoot = P_L2[1] # Initialize Patch: (Foot 1) c1 = Circle((-xfoot,yfoot), radius=0.1, color='cornflowerblue') # Draws a circle of radius 0.1 at (X, Y) location (0, 0) ax.add_patch(c1) # Add the patch to the axes # Draw the Ground: ground = ax.hlines(y_start - 0.1, -100, 100, colors='black') height_hatch = 0.25 width_hatch = 200 xy_hatch = (-100, y_start-height_hatch-0.1) ground_hatch = Rectangle(xy_hatch, width_hatch, height_hatch, facecolor='None', linestyle='None', hatch='/') ax.add_patch(ground_hatch) # Plot and Create Animation with writerObj.saving(fig, title+".mp4", dpi): # Sample only a few frames for i in range(0, vec_size, sample_rate): # thetas from Inverse Dynamics base.rotation = th1_vec[i] L1.rotation = th2_vec[i] P = EndPos(base) # Defining each point along kinematic tree P_base = P[:,0] P_L1 = P[:,1] # Position of leg 1 P_L2 = P[:,2] # Position of leg 2 # Defining and drawing each link x1 = [P_base[0], P_L1[0], P_L2[0]] y1 = [P_base[1], P_L1[1], P_L2[1]] p.set_data(x1, y1) # Foot Patch: foot_center = P_L2[0], P_L2[1] # x,y components of the 2nd leg (end-effector) c1.center = foot_center ''' # Hatch Patch: Makes the system look as if it is moving horizontally xy_hatch = -100 - x[i], y_start-height_hatch-0.1 ground_hatch.set(xy=xy_hatch) ''' fig.canvas.draw() writerObj.grab_frame() plt.show() from IPython.display import Video Video("/work/Robot Dog Leg motion.mp4",embed=True,width=640,height=480)