def test_forward_dynamics(self): thetalist = np.array([0.1, 0.1, 0.1]) dthetalist = np.array([0.1, 0.2, 0.3]) taulist = np.array([0.5, 0.6, 0.7]) g = np.array([0, 0, -9.8]) Ftip = np.array([1, 1, 1, 1, 1, 1]) M01 = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0.089159], [0, 0, 0, 1]]) M12 = np.array([[0, 0, 1, 0.28], [0, 1, 0, 0.13585], [-1, 0, 0, 0], [0, 0, 0, 1]]) M23 = np.array([[1, 0, 0, 0], [0, 1, 0, -0.1197], [0, 0, 1, 0.395], [0, 0, 0, 1]]) M34 = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0.14225], [0, 0, 0, 1]]) G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]) G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]) G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]) Glist = np.array([G1, G2, G3]) Mlist = np.array([M01, M12, M23, M34]) Slist = np.array([[1, 0, 1, 0, 1, 0], [0, 1, 0, -0.089, 0, 0], [0, 1, 0, -0.089, 0, 0.425]]).T correct_output = mr.ForwardDynamics(thetalist, dthetalist, taulist, g, Ftip, Mlist, Glist, Slist) output = util.ForwardDynamics(thetalist, dthetalist, taulist, g, Ftip, Mlist, Glist, Slist) self.assertEqual(True, array_equal(correct_output, output))
def p5(self): print('P5') taulist = [0.0128, -41.1477, -3.7809, 0.0323, 0.0370, 0.1034] ddthetalist = mr.ForwardDynamics(self.thetalist, self.dthetalist, taulist, self.g, self.Ftip, self.Mlist, self.Glist, self.Slist) print('ddthetalist = \n%s' % ddthetalist)
def simulation(thetalist, dthetalist, taulist, g, Ftip, Mlist, Glist, Slist, Number_of_Steps_per_second, time, filename): file1 = open(filename, "w", newline="") t = 0 while t < time: # update the csv csv.writer(file1).writerow([ thetalist[0], thetalist[1], thetalist[2], thetalist[3], thetalist[4], thetalist[5] ]) dt = 1 / Number_of_Steps_per_second # retrieve joint accelerations ddthetalist = mr.ForwardDynamics(thetalist, dthetalist, taulist, g, Ftip, Mlist, Glist, Slist) # compute the next joint position and velocity thetalistNext, dthetalistNext = mr.EulerStep(thetalist, dthetalist, ddthetalist, dt) # update thetalist = thetalistNext dthetalist = dthetalistNext t += dt
def simulate(self, thetalistInit, filename, time, timestep=.01): thetalist = thetalistInit dthetalist = np.array([0, 0, 0, 0, 0, 0]) taulist = np.array([0, 0, 0, 0, 0, 0]) g = np.array([0, 0, -9.81]) Ftip = np.array([0, 0, 0, 0, 0, 0]) def writeJointValues(f, thetalist): for i in range(len(thetalist)): f.write('%s' % thetalist[i]) if i < len(thetalist) - 1: f.write(',') f.write('\n') with open(filename, 'w') as f: t = 0 while t < time: writeJointValues(f, thetalist) ddthetalist = mr.ForwardDynamics(thetalist, dthetalist, taulist, g, Ftip, self.Mlist, self.Glist, self.Slist) thetalist, dthetalist = mr.EulerStep(thetalist, dthetalist, ddthetalist, timestep) t += timestep
M67 = [[1, 0, 0, 0], [0, 0, 1, 0.0823], [0, -1, 0, 0], [0, 0, 0, 1]] G1 = np.diag([0.010267495893, 0.010267495893, 0.00666, 3.7, 3.7, 3.7]) G2 = np.diag([0.22689067591, 0.22689067591, 0.0151074, 8.393, 8.393, 8.393]) G3 = np.diag([0.049443313556, 0.049443313556, 0.004095, 2.275, 2.275, 2.275]) G4 = np.diag([0.111172755531, 0.111172755531, 0.21942, 1.219, 1.219, 1.219]) G5 = np.diag([0.111172755531, 0.111172755531, 0.21942, 1.219, 1.219, 1.219]) G6 = np.diag( [0.0171364731454, 0.0171364731454, 0.033822, 0.1879, 0.1879, 0.1879]) Glist = [G1, G2, G3, G4, G5, G6] Mlist = [M01, M12, M23, M34, M45, M56, M67] Slist = [[0, 0, 0, 0, 0, 0], [0, 1, 1, 1, 0, 1], [1, 0, 0, 0, -1, 0], [0, -0.089159, -0.089159, -0.089159, -0.10915, 0.005491], [0, 0, 0, 0, 0.81725, 0], [0, 0, 0.425, 0.81725, 0, 0.81725]] thetaList = np.array( [0, math.pi / 6, math.pi / 4, math.pi / 3, math.pi / 2, 2 * math.pi / 3]) dthetaList = np.array([0.2, 0.2, 0.2, 0.2, 0.2, 0.2]) ddthetaList = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) g = np.array([0, 0, -9.81]) Ftip = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) massMatrix = mr.MassMatrix(thetaList, Mlist, Glist, Slist) QuadraticForces = mr.VelQuadraticForces(thetaList, dthetaList, Mlist, Glist, Slist) gravityForces = mr.GravityForces(thetaList, g, Mlist, Glist, Slist) # with np.printoptions(precision=3, suppress=True): print mr.EndEffectorForces(thetaList, Ftip, Mlist, Glist, Slist) tauList = np.array([0.0128, -41.1477, -3.7809, 0.0323, 0.0370, 0.1034]) with np.printoptions(precision=3, suppress=True): print mr.ForwardDynamics(thetaList, dthetaList, tauList, g, Ftip, Mlist, Glist, Slist)
def UR5Simulation(thetalist0, time): """Computes the free falling forward dynamics of UR5 Robot, given initial configuration :param thetalist0: An initial joint angles, at time = 0 :param time: Simulation time in seconds :return JointThetaMatrix: The list of all calculated joint angle values, as a list over time. Uses an Modern Robotics Library. Simulation time-step is 10ms Example Input: thetaList = np.array([0, 0, 0, 0, 0, 0]) time = 3 Output: JointThetaMatrix as an numpy array """ M01 = [[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0.089159], [0, 0, 0, 1]] M12 = [[0, 0, 1, 0.28], [0, 1, 0, 0.13585], [-1, 0, 0, 0], [0, 0, 0, 1]] M23 = [[1, 0, 0, 0], [0, 1, 0, -0.1197], [0, 0, 1, 0.395], [0, 0, 0, 1]] M34 = [[0, 0, 1, 0], [0, 1, 0, 0], [-1, 0, 0, 0.14225], [0, 0, 0, 1]] M45 = [[1, 0, 0, 0], [0, 1, 0, 0.093], [0, 0, 1, 0], [0, 0, 0, 1]] M56 = [[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0.09465], [0, 0, 0, 1]] M67 = [[1, 0, 0, 0], [0, 0, 1, 0.0823], [0, -1, 0, 0], [0, 0, 0, 1]] G1 = np.diag([0.010267495893, 0.010267495893, 0.00666, 3.7, 3.7, 3.7]) G2 = np.diag([0.22689067591, 0.22689067591, 0.0151074, 8.393, 8.393, 8.393]) G3 = np.diag([0.049443313556, 0.049443313556, 0.004095, 2.275, 2.275, 2.275]) G4 = np.diag([0.111172755531, 0.111172755531, 0.21942, 1.219, 1.219, 1.219]) G5 = np.diag([0.111172755531, 0.111172755531, 0.21942, 1.219, 1.219, 1.219]) G6 = np.diag([0.0171364731454, 0.0171364731454, 0.033822, 0.1879, 0.1879, 0.1879]) Glist = [G1, G2, G3, G4, G5, G6] Mlist = [M01, M12, M23, M34, M45, M56, M67] Slist = [[0, 0, 0, 0, 0, 0], [0, 1, 1, 1, 0, 1], [1, 0, 0, 0, -1, 0], [0, -0.089159, -0.089159, -0.089159, -0.10915, 0.005491], [0, 0, 0, 0, 0.81725, 0], [0, 0, 0.425, 0.81725, 0, 0.81725]] g = np.array([0, 0, -9.81]) # Simulation is done under no joint torques and End Effector Wrenches tauList = np.array([0, 0, 0, 0, 0, 0]) Ftip = np. array([0, 0, 0, 0, 0, 0]) # Joint Configuration and Velocity matrices, for storing data over simulation time JointThetaMatrix = [] JointVelMatrix = [] JointThetaMatrix.append(thetalist0.tolist()) JointVelMatrix.append(np.array([0,0,0,0,0,0])) # Forward Dynamics calculation at 100Hz. (10ms time step) iterationFrequency = 100 totalTimeSteps = time * iterationFrequency for i in range(totalTimeSteps): ddthetaList = mr.ForwardDynamics(JointThetaMatrix[i],JointVelMatrix[i], tauList, g, Ftip, Mlist, Glist, Slist) dthetaListNew = JointVelMatrix[i] + ddthetaList / iterationFrequency thetaListNew = JointThetaMatrix[i] + dthetaListNew / iterationFrequency # Append new values to Joint Velocity and Joint Configuration matrices JointThetaMatrix.append(thetaListNew.tolist()) JointVelMatrix.append(dthetaListNew.tolist()) print "Time step ", i, " out of ", totalTimeSteps # End of simulation. Return JointThetaMatrix return JointThetaMatrix
# thetalist = thetalistNext # dthetalist = dthetalistNext # ddthetalist = mr.ForwardDynamics(thetalist,dthetalist,taulist,g,Ftip,Mlist,Glist,Slist) # path[i] = thetalist # if i%10==0: # print("Iter: " + str(i) + "/" + str(path.shape[0])) # np.savetxt("path.csv",path,delimiter=",") # print("Done") # print(".csv file saved as path.csv") # print("************************") # Second configuration thetalist = [-1, -1, 0, 0, 0, 0] dthetalist = [0, 0, 0, 0, 0, 0] ddthetalist = mr.ForwardDynamics(thetalist, dthetalist, taulist, g, Ftip, Mlist, Glist, Slist) path_2 = np.zeros([500, 6]) # 5 seconds x 100 steps/sec, 6 joint angles print() print("Part 2 Running...") for i in range(path_2.shape[0]): [thetalistNext, dthetalistNext] = mr.EulerStep(thetalist, dthetalist, ddthetalist, dt) thetalist = thetalistNext dthetalist = dthetalistNext ddthetalist = mr.ForwardDynamics(thetalist, dthetalist, taulist, g, Ftip, Mlist, Glist, Slist) path_2[i] = thetalist if i % 10 == 0:
theta = np.array([0, 0, 0, 0, 0, 0]).T theta_dot = np.array([0, 0, 0, 0, 0, 0]).T theta_double_dot = np.array([0, 0, 0, 0, 0, 0]).T g = np.array([0, 0, -9.81]).T tau = np.array([0, 0, 0, 0, 0, 0]).T Ftip = np.array([0, 0, 0, 0, 0, 0]).T T = 0 t_start = 0 t_finish = 3 delta_t = 0.001 THETAS = np.array([theta]).copy() while (T < (t_finish - t_start)): print('\nFor iteration ', T, ' positions are: ', theta) theta_double_dot = mr.ForwardDynamics(theta, theta_dot, tau, g, Ftip, Mlist, Glist, Slist) print('\nFor iteration ', T, ' accelerations are: ', theta_double_dot) theta = theta + theta_dot * delta_t theta_dot = theta_dot + theta_double_dot * delta_t THETAS = np.append(THETAS, [theta], axis=0) T = T + delta_t np.savetxt('THETAS_1.csv', THETAS, fmt='%.6f', delimiter=",") theta = np.array([0, -1, 0, 0, 0, 0]).T theta_dot = np.array([0, 0, 0, 0, 0, 0]).T theta_double_dot = np.array([0, 0, 0, 0, 0, 0]).T g = np.array([0, 0, -9.81]).T tau = np.array([0, 0, 0, 0, 0, 0]).T Ftip = np.array([0, 0, 0, 0, 0, 0]).T
G3 = np.diag([0.049443313556, 0.049443313556, 0.004095, 2.275, 2.275, 2.275]) G4 = np.diag([0.111172755531, 0.111172755531, 0.21942, 1.219, 1.219, 1.219]) G5 = np.diag([0.111172755531, 0.111172755531, 0.21942, 1.219, 1.219, 1.219]) G6 = np.diag( [0.0171364731454, 0.0171364731454, 0.033822, 0.1879, 0.1879, 0.1879]) Glist = np.array([G1, G2, G3, G4, G5, G6]) Mlist = np.array([M01, M12, M23, M34, M45, M56, M67]) Slist = np.array([[0, 0, 0, 0, 0, 0], [0, 1, 1, 1, 0, 1], [1, 0, 0, 0, -1, 0], [0, -0.089159, -0.089159, -0.089159, -0.10915, 0.005491], [0, 0, 0, 0, 0.81725, 0], [0, 0, 0.425, 0.81725, 0, 0.81725]]) thetalist = np.array( [0, -1, 0, 0, 0, 0] ) #np.array([0,-1,0,0,0,0]) for second case #Initially at zero position dthetalist = np.array([0, 0, 0, 0, 0, 0]) #Initially zero velocity taulist = np.array([0, 0, 0, 0, 0, 0]) #Initially zero torques at joints Ftip = np.array([0, 0, 0, 0, 0, 0]) #No force at end effector frame g = np.array([0, 0, -9.81]) #Gravitational force acting in -z direction ddthetalist = mr.ForwardDynamics(thetalist, dthetalist, taulist, g, Ftip, Mlist, Glist, Slist) #To find initial acceleration dt = 0.005 #Time difference of each calculation csv = [] csv.append(list(thetalist)) for i in range(500): #500 for second case [thetalist, dthetalist] = mr.EulerStep(thetalist, dthetalist, ddthetalist, dt) csv.append(thetalist) np.savetxt("simulation2.csv", csv, delimiter=',') #simulation2.csv for second case
def free_fall(): ## relevant kinematic and intertial parameters if UR5 robot given in assignment pdf M01 = [[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0.089159], [0, 0, 0, 1]] M12 = [[0, 0, 1, 0.28], [0, 1, 0, 0.13585], [-1, 0, 0, 0], [0, 0, 0, 1]] M23 = [[1, 0, 0, 0], [0, 1, 0, -0.1197], [0, 0, 1, 0.395], [0, 0, 0, 1]] M34 = [[0, 0, 1, 0], [0, 1, 0, 0], [-1, 0, 0, 0.14225], [0, 0, 0, 1]] M45 = [[1, 0, 0, 0], [0, 1, 0, 0.093], [0, 0, 1, 0], [0, 0, 0, 1]] M56 = [[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0.09465], [0, 0, 0, 1]] M67 = [[1, 0, 0, 0], [0, 0, 1, 0.0823], [0, -1, 0, 0], [0, 0, 0, 1]] G1 = np.diag([0.010267495893, 0.010267495893, 0.00666, 3.7, 3.7, 3.7]) G2 = np.diag([0.22689067591, 0.22689067591, 0.0151074, 8.393, 8.393, 8.393]) G3 = np.diag([0.049443313556, 0.049443313556, 0.004095, 2.275, 2.275, 2.275]) G4 = np.diag([0.111172755531, 0.111172755531, 0.21942, 1.219, 1.219, 1.219]) G5 = np.diag([0.111172755531, 0.111172755531, 0.21942, 1.219, 1.219, 1.219]) G6 = np.diag([0.0171364731454, 0.0171364731454, 0.033822, 0.1879, 0.1879, 0.1879]) Glist = [G1, G2, G3, G4, G5, G6] Mlist = [M01, M12, M23, M34, M45, M56, M67] Slist = [[0, 0, 0, 0, 0, 0], [0, 1, 1, 1, 0, 1], [1, 0, 0, 0, -1, 0], [0, -0.089159, -0.089159, -0.089159, -0.10915, 0.005491], [0, 0, 0, 0, 0.81725, 0], [0, 0, 0.425, 0.81725, 0, 0.81725]] ## my own code from here on out g = [0,0,-9.81] taulist = [0,0,0,0,0,0] Ftip = [0,0,0,0,0,0] ## All we need to use is the mr.forwardynamics function to find thetadd ## taulist is going to be equal to 0 ## ftip is also going to be 0 ## for part 1 thetalist1 = np.array([0,0,0,0,0,0]) dthetalist1 = np.array([0,0,0,0,0,0]) ## for part 2 thetalist2 = np.array([-1,-1,0,0,0,0]) dthetalist2 = np.array([0,0,0,0,0,0]) ############## core.ForwardDynamics --> inputs needed are: ################# ## ## thetalist -> n list of joint angles ## dthetalist -> n list of joint velocities ## taulist --> n list of required joint forces/troques (will be and remain zero for our case) ## g --> gravity in x,y and z ## Ftip --> wrench applied to the end effector frame (will be and remain zero for our case) ## Mlist --> list of link frames i relative to i-1 at the home position ## Glist --> spatial intertia matrices of the links (will remain the same throughout) ## Slist --> screw axis of the joints in a space frame iter_thetas1 = np.array(thetalist1.copy()) i = 0 p1 = 300 ## 3 seconds for part 1 while i < p1: i += 1 ddthetalist1 = mr.ForwardDynamics(thetalist1, dthetalist1, taulist, g, Ftip, Mlist, Glist, Slist) [thetalist1,dthetalist1] = mr.EulerStep(thetalist1,dthetalist1,ddthetalist1,0.01) iter_thetas1 = np.vstack([iter_thetas1,thetalist1]) f = open("output_1.csv", "w") for i in range(len(iter_thetas1)): output_1 = "%7.6f,%7.6f,%7.6f,%7.6f,%7.6f,%7.6f\n" % (iter_thetas1[i,0], iter_thetas1[i,1], iter_thetas1[i,2], iter_thetas1[i,3], iter_thetas1[i,4], iter_thetas1[i,5]) f.write(output_1) f.close() p2 = 500 ## 5 seconds for part 2 iter_thetas2 = np.array(thetalist2.copy()) i = 0 while i < p2: i += 1 ddthetalist2 = mr.ForwardDynamics(thetalist2, dthetalist2, taulist, g, Ftip, Mlist, Glist, Slist) [thetalist2,dthetalist2] = mr.EulerStep(thetalist2,dthetalist2,ddthetalist2,0.01) iter_thetas2 = np.vstack([iter_thetas2,thetalist2]) f = open("output_2.csv", "w") for i in range(len(iter_thetas2)): output_2 = "%7.6f,%7.6f,%7.6f,%7.6f,%7.6f,%7.6f\n" % (iter_thetas2[i,0], iter_thetas2[i,1], iter_thetas2[i,2], iter_thetas2[i,3], iter_thetas2[i,4], iter_thetas2[i,5]) f.write(output_2) f.close()
# ------------------------------------------------- CREATE CSV FOR SIMULATION 1 ---------------------------------------------------- max_time = 3 sim_time = 0 thetalist = np.array([0, 0, 0, 0, 0, 0]) # initial joint vector [rad] print("creating simulation1.csv, please wait...") file1 = open("simulation1.csv", "w", newline="") while sim_time < max_time: csv.writer(file1).writerow([ thetalist[0], thetalist[1], thetalist[2], thetalist[3], thetalist[4], thetalist[5] ]) # update the csv ddthetalist = mr.ForwardDynamics(thetalist, dthetalist, tau, g, Ftip, Mlist, Glist, Slist) # retrieve joint accelerations dthetalistNext = dthetalist + ddthetalist * dt thetalistNext = thetalist + dthetalistNext * dt # thetalistNext, dthetalistNext = mr.EulerStep(thetalist, dthetalist, ddthetalist, dt) # compute the next joint position and velocity # update thetalist = thetalistNext dthetalist = dthetalistNext sim_time += dt print("simulation1.csv created!") # --------------------------------------------------CREATE CSV FOR SIMULATION 2 ------------------------------------------------------ max_time = 5
[0.0171364731454, 0.0171364731454, 0.033822, 0.1879, 0.1879, 0.1879]) Glist = [G1, G2, G3, G4, G5, G6] Mlist = [M01, M12, M23, M34, M45, M56, M67] Slist = [[0, 0, 0, 0, 0, 0], [0, 1, 1, 1, 0, 1], [1, 0, 0, 0, -1, 0], [0, -0.089159, -0.089159, -0.089159, -0.10915, 0.005491], [0, 0, 0, 0, 0.81725, 0], [0, 0, 0.425, 0.81725, 0, 0.81725]] theta = np.array( [0, math.pi / 6, math.pi / 4, math.pi / 3, math.pi / 2, 2 * math.pi / 3]).T theta_dot = np.array([0.2, 0.2, 0.2, 0.2, 0.2, 0.2]).T theta_double_dot = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]).T g = np.array([0, 0, -9.81]).T Ftip = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]).T MassMatrix = mr.MassMatrix(theta, Mlist, Glist, Slist) print('\n', MassMatrix) Coriolis_Centripetal = mr.VelQuadraticForces(theta, theta_dot, Mlist, Glist, Slist) print('\n', Coriolis_Centripetal) Gravity_Overcome = mr.GravityForces(theta, g, Mlist, Glist, Slist) print('\n', Gravity_Overcome) Wrench_Ftip = mr.EndEffectorForces(theta, Ftip, Mlist, Glist, Slist) print('\n', Wrench_Ftip) tau = np.array([0.0128, -41.1477, -3.7809, 0.0323, 0.0370, 0.1034]).T Joint_Acceleration = mr.ForwardDynamics(theta, theta_dot, tau, g, Ftip, Mlist, Glist, Slist) print('\n', Joint_Acceleration)