def get_angles(blist, mhome):
Blist = blist
M = mhome
i = 0
oldresult = 10
goalpoint = np.array([xg, yg, zg, 1])
T = [[1, 0, 0, xg], [0, 1, 0, yg], [0, 0, 1, zg], [0, 0, 0, 1]]
# gantry_x, gantry_y, gantry_z, gantry_yaw, j1, j2, j3
thetalist0 = [0, 0, 0, 0]
eomg = 0.01
ev = 0.01
[thetalist, success] = r.IKinBody2(Blist, M, T, thetalist0, eomg, ev)
reachedpoint = r.FKinBody(M, Blist, thetalist)[:, 3]
result = np.around(
np.linalg.norm(goalpoint - reachedpoint) * 1000) / 1000.0
thetasum = thetalist
while (i < maxtime):
#print 'Calculating IK'
thetalist0 = thetalist #[random.uniform(-2.04, 2.04), random.uniform(-1.57, 1.57), random.uniform(-1.57, 1.57)]
[thetalist, success] = r.IKinBody2(Blist, M, T, thetalist0, eomg, ev)
reachedpoint = r.FKinBody(M, Blist, thetalist)[:, 3]
result = np.around(
np.linalg.norm(goalpoint - reachedpoint) * 1000) / 1000.0
#if(oldresult > result):
# oldresult = result
# thetaresult = thetalist
thetasum = np.add(thetasum, thetalist)
thetaresult = thetasum / (maxtime + 1)
i += 1
#print 'averaged angles'
#print thetasum/(maxtime+1)
#print 'Error in each direction'
#print np.matrix([xg,yg,zg,1]) - reachedpoint
print 'Obtained position'
print reachedpoint
print 'Required position'
print np.matrix([xg, yg, zg, 1])
#print 'Error value'
#print result
print '\n'
#print success
return thetaresult
def get_angles():
#print [xg, yg, zg]
global icangles
t1, t2, t3 = fsolve(equations, icangles)
#Get actual angles
[a1, a2, a3] = [(t1 % 6.28), (t2 % 6.28) % -3.14, (t3 % 6.28)]
thetalist = [0, 0, 0]
T = r.FKinBody(M, Blist, thetalist)[:, 3]
'''
if( 2.04 < a1):
a1 = 2.04
elif(a1 < -2.04):
a1 = -2.04
if( 1.57 < a2):
a2 = 1.57
elif(a2 < -1.57):
a2 = -1.57
if( 1.57 < a3):
a3 = 1.57
elif(a3 < -1.57):
a3 = -1.57
'''
result = [
a1, a2, a3
] #[atan2(sin(a1),cos(a1)), atan2(sin(a2),cos(a2)), atan2(sin(a3),cos(a3))]
#icangles = result
#print np.round([a1-6.28, a2-6.28, a3-6.28])
print result
return result
def fk_palm(thetalist):
T_old = r.FKinBody(M_palm, Blist_palm, thetalist)[0:3,3]
T = np.array([i + j for i, j in zip(T_old, [x_off, y_off, z_off])])
#print 'old palm'
#print T_old
#print 'offsets'
#print [x_off, y_off, z_off]
return T
def fk_ring(thetalist):
T_old = r.FKinBody(M_ring, Blist_ring, thetalist)[0:3,3]
T = np.array([i + j for i, j in zip(T_old, [x_off, y_off, z_off])])
print 'old ring'
print T_old
print 'offsets'
print [x_off, y_off, z_off]
return T
def tse(tlist):
toe = r.FKinBody(Moe, Blist, tlist[3:])
tbe = r.matmult(Tbo, toe)
tsb = [[cos(tlist[0]), -sin(tlist[0]), 0, tlist[1]],
[sin(tlist[0]), cos(tlist[0]), 0, tlist[2]], [0, 0, 1, 0.0963],
[0, 0, 0, 1]]
tsemat = r.matmult(tsb, tbe)
return tsemat
def get_angles():
goalpoint = np.array([xg, yg, zg, 1])
T = [[0, 1, 0, xg], [0, 0, -1, yg], [-1, 0, 0, zg], [0, 0, 0, 1]]
thetalist0 = [0, 0, 0]
eomg = 0.01
ev = 0.001
[thetalist, success] = r.IKinBody2(Blist, M, T, thetalist0, eomg, ev)
reachedpoint = r.FKinBody(M, Blist, thetalist)[:, 3]
result = np.around(
np.linalg.norm(goalpoint - reachedpoint) * 1000) / 1000.0
#print success
return reachedpoint
def get_angles():
global icangles
t1, t2, t3 = fsolve(equations, icangles)
#Get actual angles
[a1, a2, a3] = [(t1 % 6.28), (t2 % 6.28) % -3.14, (t3 % 6.28)]
thetalist = [0, 0, 0]
T = r.FKinBody(M, Blist, thetalist)[:, 3]
result = [
a1, a2, a3
] #[atan2(sin(a1),cos(a1)), atan2(sin(a2),cos(a2)), atan2(sin(a3),cos(a3))]
#icangles = result
#print np.round([a1-6.28, a2-6.28, a3-6.28])
print result
return result
def get_angles_finger(blist, mhome, xg, yg, zg):
Blist = blist
M = mhome
i = 1
oldresult = 10
goalpoint = np.array([xg, yg, zg, 1])
T = [[1, 0, 0, xg], [0, 1, 0, yg], [0, 0, 1, zg], [0, 0, 0, 1]]
# gantry_x, gantry_y, gantry_z, gantry_yaw, j1, j2, j3
thetalist0 = [0, 0, 0, 0]
eomg = 0.01
ev = 0.01
[thetalist, success] = r.IKinBody2(Blist, M, T, thetalist0, eomg, ev)
reachedpoint = r.FKinBody(M, Blist, thetalist)[:, 3]
result = np.around(
np.linalg.norm(goalpoint - reachedpoint) * 1000) / 1000.0
'''
while(i <= maxtime):
#print 'Calculating IK'
thetalist0 = thetalist
[thetalist,success] = r.IKinBody2(Blist, M, T, thetalist0, eomg, ev)
reachedpoint = r.FKinBody(M,Blist,thetalist)[:,3]
result = np.around(np.linalg.norm(goalpoint - reachedpoint) * 1000) / 1000.0
#if(oldresult > result):
# oldresult = result
# thetaresult = thetalist
i += 1
'''
#print 'averaged angles'
#print thetasum/(maxtime+1)
#print 'Error in each direction'
#print np.matrix([xg,yg,zg,1]) - reachedpoint
#print 'Obtained position'
#print reachedpoint
#print 'Required position'
#print np.matrix([xg,yg,zg,1])
#print 'Error value'
#print result
#print '\n'
#print success
return thetalist
def get_angles2():
goalpoint = np.array([xg, yg, zg, 1])
global icangles
prevresult = 0.5
prevangles = [0, 0, 0]
i = 0
while (i < 200):
icangles = [
random.uniform(-2.04, 2.04),
random.uniform(-1.57, 1.57),
random.uniform(-1.57, 1.57)
]
t1, t2, t3 = fsolve(equations, icangles)
#Get actual angles
[a1, a2, a3] = [
atan2(math.sin(t1 % 6.28), math.cos(t1 % 6.28)),
atan2(math.sin(t2 % 6.28), math.cos(t2 % 6.28)),
atan2(math.sin(t3 % 6.28), math.cos(t3 % 6.28))
]
thetalist = [
np.around(a1 * 100) / 100.0,
np.around(a2 * 100) / 100.0,
np.around(a3 * 100) / 100.0
]
#print thetalist
reachedpoint = r.FKinBody(M, Blist, thetalist)[:, 3]
result = np.around(
np.linalg.norm(goalpoint - reachedpoint) * 1000) / 1000.0
if (prevresult > result):
prevresult = result
prevangles = thetalist
i += 1
if (result < 0.1):
print result
return thetalist
print 'INVALID POSE: Returning closest configuration'
return prevangles
def ik_index(xg, yg, zg):
goalpoint = np.array([xg,yg,zg])
T = [[1,0,0,xg], [0,1,0, yg], [0,0,1,zg], [0,0,0,1]]
print 'To reach'
print goalpoint
# gantry_x, gantry_y, gantry_z, gantry_yaw, j1, j2, j3
thetalist0 =[-0.1,0.1,-0.1, 0.1]
eomg = 0.01
ev = 0.001
[thetalist,success] = r.IKinBody2(Blist_index, M_index, T, thetalist0, eomg, ev)
reachedpoint_old = r.FKinBody(M_index,Blist_index,thetalist)[0:3,3]
reachedpoint = np.array([i + j for i, j in zip(reachedpoint_old, [x_off, y_off, z_off])])
#result = np.around(np.linalg.norm(goalpoint - reachedpoint) * 1000) / 1000.0
print 'Reached Now'
print reachedpoint
return thetalist
def get_angles_thumb(blist, mhome, xg, yg, zg):
Blist = blist
M = mhome
i = 1
oldresult = 10
goalpoint = np.array([xg,yg,zg,1])
T = [[1,0,0,xg], [0,1,0,yg], [0,0,1,zg], [0,0,0,1]]
# gantry_x, gantry_y, gantry_z, gantry_yaw, j1, j2, j3
thetalist0 =[0,0,0, 0]
eomg = 0.01
ev = 0.01
[thetalist,success] = r.IKinBody2(Blist, M, T, thetalist0, eomg, ev)
reachedpoint_old = r.FKinBody(M,Blist,thetalist)[:,3]
reachedpoint = np.array([i + j for i, j in zip(reachedpoint_old, [x_off, y_off, z_off, 0])])
result = np.around(np.linalg.norm(goalpoint - reachedpoint) * 1000) / 1000.0
print result
return thetalist
def fk_thumb(thetalist):
T_old = r.FKinBody(M_thumb, Blist_thumb, thetalist)[0:3,3]
T = np.array([i + j for i, j in zip(T_old, [x_off, y_off, z_off])])
return T
def fk_index(thetalist):
T = r.FKinBody(M_index, Blist_index, thetalist)[0:3, 3]
#print T
return T
def main():
deltat = 0.01
thetalist = [0, 0, 0, 0, 0, -pi / 2, pi / 4, 0]
#print(getxerr(0.01,thetalist))
#Initializing error accumulation
xerrarray.append(getxerr(0, thetalist) * 0)
xerrint = getxerr(0, thetalist)
tnew = [
thetalist[1], thetalist[2], thetalist[0], thetalist[3], thetalist[4],
thetalist[5], thetalist[6], thetalist[7]
]
thf.append(tnew)
for i in range(500):
tym = 0.01 * i
Xd = getX(tym)
Vd = getVx(tym)
xerr = np.matrix(getxerr(tym, thetalist))
#print xerr.shape
xerrarray.append(xerr)
xerrarray0.append(xerr[0, 0])
xerrarray1.append(xerr[0, 1])
xerrarray2.append(xerr[0, 2])
xerrarray3.append(xerr[0, 3])
xerrarray4.append(xerr[0, 4])
xerrarray5.append(xerr[0, 5])
#print np.matrix(xerrarray[len(xerrarray)-1]).shape
xerrint = np.add(xerrint, getxerr(tym, thetalist)) * 0.01
x = tse(thetalist)
ad = r.Adjoint(r.matmult(r.TransInv(x), Xd))
vt = r.matmult(ad, Vd) + kp * np.asarray(getxerr(
tym, thetalist)) + ki * np.asarray(xerrint)
toe = r.FKinBody(Moe, Blist, thetalist[3:])
tsb = [[cos(thetalist[0]), -sin(thetalist[0]), 0, thetalist[1]],
[sin(thetalist[0]),
cos(thetalist[0]), 0, thetalist[2]], [0, 0, 1, 0.0963],
[0, 0, 0, 1]]
jarm = r.JacobianBody(Blist, thetalist[3:])
jbase = r.matmult(
r.Adjoint(r.matmult(r.TransInv(toe), r.TransInv(Tbo))), F)
je = np.concatenate((jbase, jarm), axis=1)
uthed = r.matmult(np.linalg.pinv(je), vt)
u = uthed[:4]
vb6 = tym * r.matmult(F, u)[2:5]
thetader = 0.01 * np.concatenate((vb6, uthed[4:]), axis=1)
thetalist = np.add(thetalist, thetader)
tnew = [
thetalist[1], thetalist[2], thetalist[0], thetalist[3],
thetalist[4], thetalist[5], thetalist[6], thetalist[7]
]
thf.append(tnew)
#print np.matrix(Xd)
#print getxerr(5,thf[len(thf)-1])
i = range(0, 500)
plt.plot(i, xerrarray0)
plt.plot(i, xerrarray1)
plt.plot(i, xerrarray2)
plt.plot(i, xerrarray3)
plt.plot(i, xerrarray4)
plt.plot(i, xerrarray5)
plt.show()
def fk_thumb(thetalist):
T = r.FKinBody(M_thumb, Blist_thumb, thetalist)[0:3, 3]
#print T
return T
def fk_ring(thetalist):
T = r.FKinBody(M_ring, Blist_ring, thetalist)[0:3, 3]
#print T
return T
def hw5_c(Blist, M, T, thetalist0, eomg, ev):
maxiterations = 200
success = False
thf = []
qf = []
thf.append(thetalist0)
toe = r.FKinBody(M, Blist, thetalist0[3:])
tbe = r.matmult(Tbo, toe)
tsb = [[cos(thetalist0[0]), -sin(thetalist0[0]), 0, thetalist0[1]],
[sin(thetalist0[0]),
cos(thetalist0[0]), 0, thetalist0[2]], [0, 0, 1, 0.0963],
[0, 0, 0, 1]]
Vb = r.MatrixLog6(r.matmult(r.TransInv(r.matmult(tsb, tbe)), T))
print Vb
wb = r.Magnitude([Vb[0], Vb[1], Vb[2]])
print wb
vb = r.Magnitude([Vb[3], Vb[4], Vb[5]])
print vb
while (wb > eomg or vb > ev):
toe = r.FKinBody(M, Blist, thetalist0[3:])
tbe = r.matmult(Tbo, toe)
tsb = [[cos(thetalist0[0]), -sin(thetalist0[0]), 0, thetalist0[1]],
[sin(thetalist0[0]),
cos(thetalist0[0]), 0, thetalist0[2]], [0, 0, 1, 0.0963],
[0, 0, 0, 1]]
Vb = r.MatrixLog6(r.matmult(r.TransInv(r.matmult(tsb, tbe)), T))
wb = r.Magnitude([Vb[0], Vb[1], Vb[2]])
print wb
vb = r.Magnitude([Vb[3], Vb[4], Vb[5]])
print vb
jarm = r.JacobianBody(Blist, thetalist0[3:])
jbase = r.matmult(
r.Adjoint(r.matmult(r.TransInv(toe), r.TransInv(Tbo))), F)
je = np.concatenate((jbase, jarm), axis=1)
uthed = r.matmult(np.linalg.pinv(je), Vb)
u = uthed[:4]
vb6 = r.matmult(F, u)[2:5]
thetader = np.hstack((vb6, uthed[4:]))
thetalist0 = np.add(thetalist0, thetader)
thf.append(thetalist0)
success = True
vbold = thf[len(thf) - 1]
vbnew = [[
vbold[1], vbold[2], vbold[0], vbold[3], vbold[4], vbold[5], vbold[6],
vbold[7]
]]
with open("outputPartC.csv", "wb") as f:
writer = csv.writer(f)
writer.writerows(vbnew)
return (np.round(100 * thf[len(thf) - 1]), success)
def fk_index(thetalist):
T_old = r.FKinBody(M_index, Blist_index, thetalist)[0:3,3]
T = np.array([i + j for i, j in zip(T_old, [x_off, y_off, z_off])])
#print T
return T