from motion import motion # for windows if name == 'nt': CST_LSS_Port = "COM3" # for mac and linux(here, os.name is 'posix') else: CST_LSS_Port = "/dev/ttyUSB0" # Constants #CST_LSS_Port = "/dev/ttyUSB0" # For Linux/Unix platforms # CST_LSS_Port = "COM1" # For windows platforms CST_LSS_Baud = lssc.LSS_DefaultBaud # Create and open a serial port lss.initBus(CST_LSS_Port, CST_LSS_Baud) # Create LSS objects mt = motion(lss) # define our clear function def clear(): # for windows if name == 'nt': _ = system('cls') # for mac and linux(here, os.name is 'posix') else: _ = system('clear')
from math import atan2, sqrt, cos, sin, acos, pi, copysign import time import os from gtts import gTTS import VisionModule as vm import lss import lss_const as lssc import platform import Interface as inter if platform.system() == 'Windows': port = "COM3" elif platform.system() == 'Linux': port = "/dev/ttyUSB0" lss.initBus(port, lssc.LSS_DefaultBaud) base = lss.LSS(1) shoulder = lss.LSS(2) elbow = lss.LSS(3) wrist = lss.LSS(4) gripper = lss.LSS(5) allMotors = lss.LSS(254) allMotors.setAngularHoldingStiffness(0) allMotors.setMaxSpeed(100) base.setMaxSpeed(60) shoulder.setMotionControlEnabled(0) elbow.setMotionControlEnabled(0) CST_ANGLE_MIN = -90 CST_ANGLE_MAX = 90
def run_thread(self): global state # Initialize serial bus if platform.system() == "Windows": # Eric comport = 'COM7' # # Riley # comport = 'COM4' else: comport = "/dev/ttyUSB0" baudrate = lssc.LSS_DefaultBaud print("Connecting to LSS") lss.initBus(comport, baudrate) fingers = [ Finger([lss.LSS(11), lss.LSS(12), lss.LSS(13)], 0), Finger([lss.LSS(21), lss.LSS(22), lss.LSS(23)], 0), Finger([lss.LSS(31), lss.LSS(32), lss.LSS(33)], 0), Finger([lss.LSS(41), lss.LSS(42), lss.LSS(43)], 0), Finger([lss.LSS(51), lss.LSS(52), lss.LSS(53)], 0) ] fangles = [ 0, 72 / 180 * pi, 72 * 2 / 180 * pi, 72 * 3 / 180 * pi, 72 * 4 / 180 * pi ] radius = 69 hand = Hand(fingers, fangles, radius) print("Reseting Hand") hand.reset() hand.changehandcolor(0) print("Loading point cloud") x, y, z, data0, data1, data2 = pickle.load( open("IK_pointcloud.1.p", "rb")) # Local Point Cloud Interpolation Generation # finger = Finger([lss.LSS(69)], 0) # gridpoints = np.array([100, 100, 50]) # gridstart = np.array([-100, -100, 100]) # gridend = np.array([100, 100, 220]) # x, y, z, pointcloud = TrajectoryGen.cubicpointcloudgen(gridstart, gridend, gridpoints, plotFlag=True) # ikinehelper = lambda pd, guess: finger.ikine(pd, guess) # data0, data1, data2 = TrajectoryGen.inversepointcloud(x, y, z, ikinehelper) # pickle.dump([x, y, z, data0, data1, data2], open("IK_pointcloud.p", "wb")) # Generate global interpolation functions for each finger interpfun = TrajectoryGen.interpfungen(x, y, z, data0, data1, data2) interpfunglobal = [] offset = [-1.5, 0, -2, 0, 0] print("Generating interpolation functions") for i in range(0, len(hand.fingers)): temp = TrajectoryGen.interpfungenglobal(np.linalg.inv(hand.Tas[i]), interpfun, offset[i]) interpfunglobal.append(temp) # Create Spin Trajectories points = 800 trajoffset = 2 * pi / 180 spinr = 125 spinh = 175 spindepth = 30 spintraj = [] spinitraj = [] spinitraj2 = [] print("Creating Spin Trajectories") for i in range(0, len(hand.fingers)): hand.changehandcolor(i) print("Spin trajectory for finger " + str(i + 1)) spintraj.append( TrajectoryGen.spin(spinr, spinh, spindepth, points, 4, i, trajoffset, plotflag=False)) # ikinehelper = lambda pd, guess: hand.ikine(pd, guess, fingernum=i) # spinitraj.append(TrajectoryGen.inverse(traj[i], ikinehelper, plotFlag=1)) spinitraj2.append( TrajectoryGen.inverse(spintraj[i], interpfunglobal[i], plotFlag=False)) # _ = input("ready to start: ") print("Moving Hand to Neutral position") hand.changehandcolor(1) for j in range(0, 5): hand.move(spinitraj2[j][:, 0], j) # hand.move(np.array([0, 0, 0]), j) time.sleep(3) print( "Motion Controller Ready. Place plate on claw and press enter to continue." ) while True: # Modes: # 0 = pause # 1 = spin # 2 = linear correction # 3 = shut off motors if mode == 0: hand.changehandcolor(4) if mode == 1: hand.changehandcolor(6) state = 1 for i in range(0, max(spinitraj2[0].shape)): for j in range(0, 5): if mode == 0: hand.changehandcolor(4) while mode == 0: time.sleep(0.1) hand.changehandcolor(4) hand.changehandcolor(6) if mode == 3: break hand.move(spinitraj2[j][:, i], j) if mode == 2: hand.changehandcolor(5) # Center # Move to center finger positions one at a time state = 2 lintraj1 = [] ilintraj1 = [] epostracker = [0, 0, 0, 0, 0] for i in range(0, 5): linpoints1 = 375 spos = spintraj[i][:, 0] eposl = np.array([0, spinr - 69, spinh]) T_ = hand.Tas[i] temp_ = np.array(eposl).reshape(3, 1) postemp_ = np.vstack((temp_, 1)) temp2_ = T_ @ postemp_ epos = temp2_[0:3] epos = epos.reshape(3) epostracker[i] = epos if i == 4: lintraj1.append( TrajectoryGen.linear(spos, epos, linpoints1, plotflag=False)) ilintraj1.append( TrajectoryGen.inverse(lintraj1[i], interpfunglobal[i], plotFlag=False)) else: lintraj1.append( TrajectoryGen.linear_parabolicz(spos, epos, linpoints1, 30, plotflag=False)) ilintraj1.append( TrajectoryGen.inverse(lintraj1[i], interpfunglobal[i], plotFlag=False)) # Move all fingers in target direction lintraj2 = [] ilintraj2 = [] for i in range(0, len(hand.fingers)): spos = epostracker[i] epos = spos - correction epostracker[i] = epos linpoints2 = 125 lintraj2.append( TrajectoryGen.linear(spos, epos, linpoints2, plotflag=False)) ilintraj2.append( TrajectoryGen.inverse(lintraj2[i], interpfunglobal[i], plotFlag=False)) # Move all fingers back to starting spin position lintraj3 = [] ilintraj3 = [] for i in range(0, len(hand.fingers)): spos = epostracker[i] epos = spintraj[i][:, 0] epos = epos.reshape(3) linpoints3 = 375 lindepth = 30 if i == 4: lintraj3.append( TrajectoryGen.linear(spos, epos, linpoints3, plotflag=False)) ilintraj3.append( TrajectoryGen.inverse(lintraj3[i], interpfunglobal[i], plotFlag=False)) else: lintraj3.append( TrajectoryGen.linear_parabolicz(spos, epos, linpoints3, lindepth, plotflag=False)) ilintraj3.append( TrajectoryGen.inverse(lintraj3[i], interpfunglobal[i], plotFlag=False)) for i in range(4, -1, -1): for j in range(0, max(lintraj1[0].shape)): hand.move(ilintraj1[i][:, j], i) if mode == 0: hand.changehandcolor(4) while mode == 0: time.sleep(0.1) hand.changehandcolor(4) hand.changehandcolor(5) if mode == 3: break for i in range(0, max(lintraj2[0].shape)): for j in range(0, 5): hand.move(ilintraj2[j][:, i], j) if mode == 0: hand.changehandcolor(4) while mode == 0: time.sleep(0.1) hand.changehandcolor(4) hand.changehandcolor(5) if mode == 3: break for i in range(0, 5): for j in range(0, max(lintraj3[0].shape)): hand.move(ilintraj3[i][:, j], i) if mode == 0: hand.changehandcolor(4) while mode == 0: time.sleep(0.1) hand.changehandcolor(4) hand.changehandcolor(5) if mode == 3: break if mode == 3: state = 3 for i in range(0, len(hand.fingers)): hand.fingers[i].limp() lss.closeBus() return pass