# Spatial Vector Algebra for a Pendulum # Inverse Dynamics Calulations # Forward Dynamics import numpy as np

# Defining our functions # Create a rotation transform (about the X axis) # SVA def Xrotx(theta): c = np.cos(theta) s = np.sin(theta) X = np.array( [[1, 0 ,0 ,0 ,0, 0], [0, c, s, 0, 0, 0], [0, -s, c, 0, 0, 0], [0, 0, 0, 1, 0, 0], [0, 0, 0, 0, c, s], [0, 0, 0, 0, -s, c]]) return X # We will make a translation transform def Xtranslate(r): r1 = r[0,0] r2 = r[1,0] r3 = r[2,0] X = np.array( [[1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0], [0, r3, -r2, 1, 0, 0], [-r3, 0, r1, 0 ,1, 0], [r2, -r1, 0, 0, 0, 1]]) return X # Two types of cross products in spatial vector algebra # motion cross product (crm) # force cross product (crf) (cross star) def crm(v): v1 = v[0] v2 = v[1] v3 = v[2] v4 = v[3] v5 = v[4] v6 = v[5] vcross = np.array( [[0, -v3, v2, 0, 0, 0], [v3, 0, -v1, 0, 0, 0], [-v2, v1, 0, 0, 0, 0], [0, -v6, v5, 0, -v3, v2], [v6, 0, -v4, v3, 0, -v1], [-v5, v4, 0, -v2, v1, 0]]) return vcross def crf(v): vcross = crm(v) vcross = vcross.transpose() return vcross def mcI(m,c,Ic): c1 = c[0,0] c2 = c[0,1] c3 = c[0,2] Ic1 = Ic[0,0] Ic2 = Ic[0,1] Ic3 = Ic[0,2] Ic4 = Ic[0,3] Ic5 = Ic[0,4] Ic6 = Ic[0,5] I = np.array( [[Ic1, Ic4, Ic5], [Ic4, Ic2, Ic6], [Ic5, Ic6, Ic3]]) C = np.array([ [0, -c3, c2], [c3, 0, -c1], [-c2, c1, 0]]) mCCT = np.matmul(m*C,C.transpose()) Ibar_upper = np.concatenate((I+mCCT,m*C), axis = 1) Ibar_lower = np.concatenate((m*C.transpose(),m*np.identity(3)), axis = 1) Ibar = np.concatenate((Ibar_upper,Ibar_lower), axis = 0) return Ibar

# Generating our Equations for the Pendulum # Parameters for the Pendulum m = 1 g = 9.8 L = 1 # Temporarily define q, dq, ddq q = 0 dq = 1 ddq = 0 # Define the motion of this pendulum # Create a motion vector for the joint sw = np.array([[1,0,0]]) sw = sw.transpose() sv = np.array([[0,0,0]]) sv = sv.transpose() # Joint motion mj = np.concatenate((sw, sv), axis=0) vj = mj*dq # This rotates about the X axis Xj = Xrotx(q) # We need a translation matrix r = np.array([[0],[0],[0]]) Xt = Xtranslate(r) Xc = np.matmul(Xj,Xt) # acceleration grav = np.array([[0],[0],[0],[0],[0],[9.81]]) a = np.matmul(Xc,grav) + mj*ddq Ic = np.array([[0, 0, 0, m, m, m]]) c = np.array([[0, 0, -L]]) Ibar = mcI(m,c,Ic) # Equations of motion # f = I*acc + v cross_star I * v f = np.matmul(Ibar,a)+np.matmul(np.matmul(crf(vj),Ibar),vj) # Map force in Plucker coordinates back to joint torques tau = np.matmul(mj.transpose(), f) print(tau)