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)
