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