def inverse_dynamics(thetaList, d_thetaList, dd_thetaList, g, F_tip, M_list, G_list, S_list): n = len(thetaList) # initializing the twist, and twist dot Vi = np.zeros((6,n+1)) # if there are 3 joints, then there are 4 twists to find (n+1) Vdi = np.zeros((6,n+1)) Vdi[:,0] = np.r_[np.array([0,0,0]), -np.array(g)] # first acceleration is from gravity, dont know where the negative came from # this notation [:,0] is the first column # np.array is for case where g is inputed as python array, np.array works fine for g inputed as numpy array Mi = np.identity(4) # for the transfer from space axis to body axis, need the transf from {0} to the joint axis {i} Ai = np.zeros((6,n)) # body axes, need to store for second for loop AdTi = [[None]] * (n+1) # Adjoint of the transf_i_i-1, storing for second for loop AdTi[n] = ch3.adjoint_transf_matrix(ch3.transf_matrix_inverse(M_list[n])) # second for loop wants the transf from last frame to tip, and first for loop doesnt provide it Fi = np.array(F_tip).copy() # copy by value, no link tau_list = np.zeros(n) # initialize torque return list for i in range(n): # 0 --> n-1 Mi = np.dot(Mi, Mlist[i]) #previous iterations dotted with current link config (this ends up being current link referenced to {0}) Ai[:,i] = np.dot(ch3.adjoint_transf_matrix(ch3.transf_matrix_inverse(Mi)), np.array(S_list)[:,i]) # Ai = Ad_(T_(i,i-1)) * Si # use adjoint_transf to convert a space frame screw axis Si, into screw axis of joint i in {i} Ai # Mi is T_sb, base relative to space; but to convert space axis to body axis--> Ab = T_bs*S_s (need to invert Mi) # Ti = e^(-A_i*theta)*M_i,i-1 Ti = np.dot(ch3.se3_to_transf_matrix(ch3.vector6_to_se3(Ai[:, i]* -1*thetaList[i])),ch3.transf_matrix_inverse(Mlist[i])) # Ti is Mi,i-1 with the joints at the respective angles (M is home config, T is final config) # given Mi,i+1 in Mlist, need to invert it to fit in this equation # the [:,x] notation is editing the column, index of x AdTi[i] = ch3.adjoint_transf_matrix(Ti) # Vi = Ad_Ti,i-1(Vi-1) + A_i*theta_dot Va = np.dot(AdTi[i],Vi[:,i]) # twist of the previous link {i-1}, expressed in the current link {i} by the adjoint Vb = Ai[:,i]*d_thetaList[i] # joint rate theta_dot about the screw axis Ai Vi[:,i+1] = Va + Vb # the [:,x] notation is editing the column, index of x # dVi = Ad_Ti,i-1(dVi-1) + ad_Vi(Ai)*theta_dot + A_i*theta_ddot Vda = np.dot(AdTi[i], Vdi[:,i]) # accleration of previous link {i-1}, expressed in current link {i} by adjoint Vdb = np.dot(adjoint_twist(Vi[:,i+1]),Ai[:,i]) * d_thetaList[i] # velocity product component (came from derivative of AdTi from Vi) Vdc = Ai[:,i]*dd_thetaList[i] # joint acceleration dd_theta around the screw axis Ai Vdi[:,i+1] = Vda+Vdb+Vdc for i in range(n-1,-1,-1): Fa = np.dot(np.array(AdTi[i+1]).T, Fi) # wrench applied to link i thru joint i+1 Fb = np.dot(G_list[i],Vdi[:,i+1]) # wrench applied to link i thru joint i, G_b*dV_b Fc = np.dot(np.array(adjoint_twist(Vi[:,i+1])).T, np.dot(G_list[i], Vi[:,i+1])) # wrench applied to link i thru joint i (ad_Vb)^T*G_b*V_b Fi = Fa+Fb-Fc tau_list[i] = np.dot(np.array(Fi).T, Ai[:, i]) return tau_list
def inverse_dynamics_bad(thetaList, d_thetaList, dd_thetaList, g, fTip, MList, GList, SList): forces = [np.array(fTip)] torques = [] transfs = [] twists = [np.array([0,0,0,0,0,0])] # first twist shouldn't be moving if arm is stationary, if attached to a wheeled platform then yeah change it twistDots = [np.append(np.zeros(3),-1*g)] # have to edit both for loops for i=1..n, i=n...1 (cut some lists up) hard for (theta, dtheta, ddtheta, M, S) in zip(thetaList, d_thetaList, dd_thetaList, MList[1:], np.transpose(SList)): #first need to find the transf matrix T_(i+1,i) using M,A,thetaand matrix exponentials A = np.dot(ch3.adjoint_transf_matrix(ch3.transf_matrix_inverse(Mi)), np.array(S_list)[:,i]) T = ch3.screwtheta_to_transf_matrix(A,theta) @ M V = np.dot(ch3.adjoint_transf_matrix(T),twists[-1]) + A*dtheta Vdot = np.dot(ch3.adjoint_transf_matrix(T), twistDots[-1]) + np.dot(adjoint_twist(V),A)*dtheta + A*ddtheta transfs.append(T) twists.append(V) twistDots.append(Vdot) # twists and twistDots contain the zeroth twist and twistDot, but this zip will stop at the smallest list end, so it fine for (twist, twistDot, G, A) in zip(twists[::-1], twistDots[::-1], GList[::-1], np.transpose(SList)[::-1]): #first need to find the transf matrix T_(i+1,i) using M,A,thetaand matrix exponentials first = np.dot(ch3.adjoint_transf_matrix(T),forces[0]) second = np.dot(G,twistDot) third = np.dot(adjoint_twist(V),(np.dot(G,twist))) force = first+second-third tau = np.dot(force,A) # np.column_stack((forces,force)) forces.insert(0,force) torques.insert(0,tau) return torques
def screw_trajectory(X_start, X_end, Tf, N, method): """Computes a trajectory as a list of N SE(3) matrices corresponding to the screw motion about a space screw axis (each matrix represents the config of the end effector at an instant in time) :param Xstart: The initial end-effector configuration :param Xend: The final end-effector configuration :param Tf: Total time of the motion in seconds from rest to rest :param N: The number of points N > 1 (Start and stop) in the discrete representation of the trajectory :param method: The time-scaling method, where 3 indicates cubic (third- order polynomial) time scaling and 5 indicates quintic (fifth-order polynomial) time scaling :return: The discretized trajectory as a list of N matrices in SE(3) separated in time by Tf/(N-1). The first in the list is Xstart and the Nth is Xend""" N = int(N) timegap = Tf / (N - 1.0) traj = [[None]] * N X_start_end = np.dot(ch3.transf_matrix_inverse(X_start), X_end) for i in range(N): if method == 3: s = cubic_time_scaling(Tf, timegap * i) else: s = quintic_time_scaling(Tf, timegap * i) fractioned_X_start_end = ch3.se3_to_transf_matrix( ch3.transf_matrix_to_se3(X_start_end) * s ) # applying s to the se3 matrix instead of the transf matrix, it works traj[i] = np.dot(X_start, fractioned_X_start_end) return traj
def IK_body(Blist, M, T, thetalist0, e_omega, e_v): """uses iterative Netwon Raphson to calculate inverse kinematics Args: Blist : list of joint screws expressed in the end effector frame scriptB M : end effector home configuration T : desired end effector configuration (T_sd) thetalist0: initial guess e_omega : omega error tolerance for stoppage e_v : linear velocity error tolerance for stoppage """ thetaList = thetalist0 # finds transf matrix of current guess T_sb T_sb = ch4.forward_kinematics_in_body(M, Blist, thetaList) T_bs = ch3.transf_matrix_inverse(T_sb) # finds the se3 for the transf matrix T_bd (transformation from current guess to answer) (scriptV_b_bracketed, theta) = ch3.transf_matrix_to_se3(T_bs @ T) scriptV_b_exp6 = ch3.se3_to_vector6(scriptV_b_bracketed*theta) # these velocities are required to get from T_sb (current) to T_sd (target) # - want to bring these to zero omega = scriptV_b_exp6[0:3] v = scriptV_b_exp6[3:6] counter = 0 print(ch3.magnitude(omega)) print(ch3.magnitude(v)) while((ch3.magnitude(omega) > e_omega or ch3.magnitude(v) > e_v) and counter < 25): # use the jacobian body in chapter 5 to see what velocities can be generated by each angle velocity #- uses this stupid pseudo inverse thing that i missed jacobian_b = np.linalg.pinv(ch5.jacobian_body(Blist, thetaList)) # multiplying the jacobian by the twist gives an angle to add to thetaListaddition = np.dot(jacobian_b, np.reshape(scriptV_b_exp6, (-1,1))) # add said angle thetaList = [sum(i) for i in zip(thetaList,thetaListaddition.flatten().tolist())] # recalculate current transf matrix, scriptV, screw, omega, and v T_sb = ch4.forward_kinematics_in_body(M, Blist, thetaList) T_bs = ch3.transf_matrix_inverse(T_sb) (scriptV_b_bracketed, theta) = ch3.transf_matrix_to_se3(T_bs @ T) scriptV_b_exp6 = ch3.se3_to_vector6(scriptV_b_bracketed*theta) omega = scriptV_b_exp6[0:3] v = scriptV_b_exp6[3:6] counter += 1 return (thetaList, counter < 25)
def testingJacobianWithExampleFromGithub(): R = np.identity(3) p = np.array([0, 2, 1]) M = ch3.Rp_to_transf_matrix(R, p) thetaList = [0.2, 1.1, 0.1, 1.2] screwSpaceAxis1 = np.array([0, 0, 1, 0, 0.2, 0.2]) screwSpaceAxis2 = np.array([1, 0, 0, 2, 0, 3]) screwSpaceAxis3 = np.array([0, 1, 0, 0, 2, 1]) screwSpaceAxis4 = np.array([1, 0, 0, 0.2, 0.3, 0.4]) screwSpaceList = [ screwSpaceAxis1, screwSpaceAxis2, screwSpaceAxis3, screwSpaceAxis4 ] screwBodyList = [] for screwSpaceAxis in screwSpaceList: screwBodyList.append( np.dot(ch3.adjoint(ch3.transf_matrix_inverse(M)), screwSpaceAxis)) FK_space = ch5.jacobian_space(screwSpaceList, thetaList) print(FK_space) FK_body = ch5.jacobian_body(screwSpaceList, thetaList) print(FK_body)
def testingJacobian(): R = np.identity(3) p = np.array([0, 2, 1]) M = ch3.Rp_to_transf_matrix(R, p) thetaList = [np.pi / 2, np.pi / 2, -1 * np.pi / 2, 1] screwSpaceAxis1 = ch3.qsh_to_screwaxis(np.array([0, 0, 0]), np.array([0, 0, 1]), 0) screwSpaceAxis2 = ch3.qsh_to_screwaxis(np.array([0, 1, 0]), np.array([0, 0, 1]), 0) screwSpaceAxis3 = ch3.qsh_to_screwaxis(np.array([0, 2, 0]), np.array([0, 0, 1]), 0) screwSpaceAxis4 = np.array([0, 0, 0, 0, 0, 1]) screwSpaceList = [ screwSpaceAxis1, screwSpaceAxis2, screwSpaceAxis3, screwSpaceAxis4 ] screwBodyList = [] for screwSpaceAxis in screwSpaceList: screwBodyList.append( np.dot(ch3.adjoint(ch3.transf_matrix_inverse(M)), screwSpaceAxis)) FK_space = ch5.jacobian_space(screwSpaceList, thetaList) print(FK_space)
def testingJacobianWithExample(): R = np.identity(3) p = np.array([0, 2, 1]) M = ch3.Rp_to_transf_matrix(R, p) thetaList = [0, 0, 1 * np.pi / 2, 1] screwSpaceAxis1 = np.array([0, 0, 1, 0, 0, 0]) screwSpaceAxis2 = np.array( [0, 0, 1, 2 * math.sin(thetaList[0]), -2 * math.cos(thetaList[0]), 0]) screwSpaceAxis3 = np.array([ 0, 0, 1, 2 * math.sin(thetaList[0]) + 3 * math.sin(thetaList[1]), -2 * math.cos(thetaList[0]) - 3 * math.cos(thetaList[1]), 0 ]) screwSpaceAxis4 = np.array([0, 0, 0, 0, 0, 1]) screwSpaceList = [ screwSpaceAxis1, screwSpaceAxis2, screwSpaceAxis3, screwSpaceAxis4 ] screwBodyList = [] for screwSpaceAxis in screwSpaceList: screwBodyList.append( np.dot(ch3.adjoint(ch3.transf_matrix_inverse(M)), screwSpaceAxis)) FK_space = ch5.jacobian_space(screwSpaceList, thetaList) print(FK_space)