def setVideo(self, state): """ Change the state of the videostream. The state can be play, pause, or simply "toggle :param state: "play", "pause", or "toggle" :return: """ printf("GUI| Setting video to state: ", state) # Don't change anything if no camera ID has been added yet cameraID = self.env.getSetting("cameraID") if cameraID is None: return if state == "toggle": if self.env.getVStream().paused: self.setVideo("play") return else: self.setVideo("pause") return vStream = self.env.getVStream() if state == "play": # Make sure the videoStream object has a camera, or if the cameras changed, change it # if not vStream.connected() or not vStream.cameraID == cameraID: # vStream.setNewCamera(cameraID) self.cameraWidget.play() if state == "pause": self.cameraWidget.pause()
def is_ready(self): if self.__serial.readline().startswith(b"@1"): printf("Communication| Connected to port: {}".format( self.__serial.port)) return True else: return False
def loadAllObjects(self): # Load all objects into the ObjectManager foldersAndItems = os.listdir(self.__directory) for folder in foldersAndItems: path = self.__directory + "\\" + folder newObj = None createObj = lambda objType, prefix, directory: objType(folder.replace(prefix + " ", ""), path) if not os.path.isdir(path): printf("ERROR: Could not find directory ", path) continue # Handle any TrackableObject loading if "Trackable" in folder: newObj = createObj(TrackableObject, "Trackable", folder) if "MotionPath" in folder: newObj = createObj(MotionPath, "MotionPath", folder) # Check that loading is complete and add the object if it was created successfully if newObj is None: printf("ERROR: Could not find relevant object for folder: ", folder) continue if newObj.loadSuccess: self.__addObject(newObj) self.refreshGroups()
def startThread(self): if self.mainThread is None: self.running = True self.mainThread = Thread(target=self.__videoThread) self.mainThread.start() else: printf("ERROR: Tried to create mainThread, but mainThread already existed.")
def mousePressEvent(self, event): # If the user already has selected an image, leave. self.takeAnother() if event.button() == QtCore.Qt.LeftButton: # Make sure the click was within the boundaries of the frame width = self.frameLbl.pixmap().width() height = self.frameLbl.pixmap().height() pos = (event.pos().x(), event.pos().y()) if not 0 < pos[0] < width: return if not 0 < pos[1] < height: return # Get a frame from OpenCV, so that it can be cropped when the user releases the mouse button self.selectedImage = self.vStream.getFrame() if self.selectedImage is None: printf("GUI| ERROR: getCVFrame() returned None Frame! ") return # Set the rectangles position and unhide the rectangle self.origin = QtCore.QPoint(event.pos()) self.rectangle.setGeometry( QtCore.QRect(self.origin, QtCore.QSize())) self.rectangle.show()
def smoothListGaussian(list1, degree): """ Smooth a list using guassian smoothing, with degree Degree. Lists are smoothed by columns, not rows. :param list1: [[column1, column2, column3... column n], ... [element n]] :param degree: The gaussian smoothing constant. """ window = degree * 2 - 1 if len(list1) < window: printf( "RobotVision| ERROR: Attempted to smooth list that was too small to be smoothed" ) return None weight = np.array([1.0] * window) weightGauss = [] for i in range(window): i = i - degree + 1 frac = i / float(window) gauss = 1 / (np.exp((4 * frac)**2)) weightGauss.append(gauss) weight = np.array(weightGauss) * weight smoothed = [0.0] * (len(list1) - window) for i in range(len(smoothed)): smoothing = [0.0 for i in range(len(list1[i]))] # [0.0, 0.0, 0.0] for e, w in zip(list1[i:i + window], weight): smoothing = smoothing + np.multiply(e, w) smoothed[i] = smoothing / sum(weight) return smoothed
def __programThread(self): printf("Interpreter| ##### STARTING PROGRAM #####\n") fpsTimer = FpsTimer(fps=30) # Main program loop - where events are checked and run while not self.isExiting(): # Maintain constant FPS using an FPSTimer fpsTimer.wait() if not fpsTimer.ready(): continue # currRunning keeps track of what was run, so the GUI can draw it self.currRunning = {"event": -1, "command": -1} # Check every event, in order of the list exitCommand = False # If the interpreter reached an "ExitProgram" command for index, event in enumerate(self.events): if self.isExiting(): break if not event.isActive(): continue # If the event has been activated, run the commandList self.currRunning["event"] = self.events.index(event) exitCommand = self.interpretCommandList(event.commandList) if exitCommand: break if exitCommand: break self.events = [] printf("Interpreter| Interpreter Thread Ended") self.mainThread = None
def run(self): if len(self.errors): return # Find the object using vision _, tracked = self.vision.getObjectLatestRecognition(self.trackable) if tracked is None: return False camAngle = tracked.rotation[2] robAngle = self.transform.cameraToRobotRotation( camAngle) # Angle from the robots X axis # Before doing any tracking, evaluate the "Relative" number to make sure its valid startAngle, successL = self.interpreter.evaluateExpression( self.parameters["start"]) endAngle, successU = self.interpreter.evaluateExpression( self.parameters["end"]) # If the evaluation failed, cancel the command if not (successL and successU): printf( "Commands| ERROR: Could not evaluate the lowerAngle or the upperAngle. Canceling command. " ) return False end = endAngle - startAngle + 360.0 if ( endAngle - startAngle) < 0.0 else endAngle - startAngle mid = robAngle - startAngle + 360.0 if ( robAngle - startAngle) < 0.0 else robAngle - startAngle isBetween = mid < end return isBetween
def searchTrackedHistory(self, trackable=None, maxAge=0, minPoints=None): """ Search through trackedHistory to find an object that meets the criteria :param trackableObject: Specify if you want to find a particular object :param maxAge: Specify if you wannt to find an object that was found within X frames ago :param minPoints: Specify if you want to find an object with a minimum amount of tracking points """ maxFrame = maxAge + 1 if maxFrame is None or maxFrame >= self.historyLen: printf( "Vision| ERROR: Tried to look further in the history than was possible!" ) maxFrame = self.historyLen # Safely pull the relevant trackedHistory from the tracker object with self.workLock: trackHistory = self.planeTracker.trackedHistory[:maxFrame] # Check if the object was recognized in the most recent frame. Check most recent frames first. for historyFromFrame in trackHistory: for tracked in historyFromFrame: # If the object meets the criteria if trackable is not None and not trackable.equalTo( tracked.view.name): continue if minPoints is not None and not tracked.ptCount > minPoints: continue return tracked return None
def searchTrackedHistory(self, trackable=None, maxAge=0, minPoints=None): """ Search through trackedHistory to find an object that meets the criteria :param trackableObject: Specify if you want to find a particular object :param maxAge: Specify if you wannt to find an object that was found within X frames ago :param minPoints: Specify if you want to find an object with a minimum amount of tracking points """ maxFrame = maxAge + 1 if maxFrame is None or maxFrame >= self.historyLen: printf("Vision| ERROR: Tried to look further in the history than was possible!") maxFrame = self.historyLen # Safely pull the relevant trackedHistory from the tracker object with self.workLock: trackHistory = self.planeTracker.trackedHistory[:maxFrame] # Check if the object was recognized in the most recent frame. Check most recent frames first. for historyFromFrame in trackHistory: for tracked in historyFromFrame: # If the object meets the criteria if trackable is not None and not trackable.equalTo(tracked.view.name): continue if minPoints is not None and not tracked.ptCount > minPoints: continue return tracked return None
def saveTask(self, promptSaveLocation): printf("GUI| Saving project") # If there is no filename, ask for one if promptSaveLocation or self.fileName is None: Global.ensurePathExists(Paths.saves_dir) filename, _ = QtWidgets.QFileDialog.getSaveFileName(parent=self, caption="Save Task", filter="Task (*.task)", directory=Paths.saves_dir) if filename == "": return False #If user hit cancel self.fileName = filename self.env.updateSettings("lastOpenedFile", self.fileName) # Update the save file saveData = self.controlPanel.getSaveData() json.dump(saveData, open(self.fileName, 'w'), sort_keys=False, indent=3, separators=(',', ': ')) self.loadData = deepcopy(saveData) #Update what the latest saved changes are self.setWindowTitle(self.programTitle + ' ' + self.fileName) printf("GUI| Project Saved Successfully") return True
def run(self): # Special case: If the parameter is "" set the new val to None and success to True if self.parameters['x'] == "": newX, successX = None, True else: newX, successX = self.interpreter.evaluateExpression(self.parameters['x']) if self.parameters['y'] == "": newY, successY = None, True else: newY, successY = self.interpreter.evaluateExpression(self.parameters['y']) if self.parameters['z'] == "": newZ, successZ = None, True else: newZ, successZ = self.interpreter.evaluateExpression(self.parameters['z']) if successX and successY and successZ: self.robot.setPos(x=newX, y=newY, z=newZ, relative=self.parameters['relative']) return True else: printf("Commands| ERROR in evaluating either X Y or Z: ", successX, successY, successZ) return False
def refresh(self): """ Refresh which commandList is currently being displayed to the one the user has highlighted. It basically just goes over certain things and makes sure that everything that should be displaying, is displaying. """ # Remove all widgets on the commandList stack (not delete, though!) The chosen event will be added later. for c in range(0, self.commandListStack.count()): widget = self.commandListStack.widget(c) self.commandListStack.removeWidget(widget) # Get the currently selected event on the eventList selectedEvent = self.eventList.getSelectedEvent() # If user has no event selected, make a clear commandList to view self.deleteEventBtn.setDisabled(selectedEvent is None) self.changeEventBtn.setDisabled(selectedEvent is None) self.commandMenuWidget.setDisabled(selectedEvent is None) self.commandGBox.setTitle("Command List") if selectedEvent is None: printf("GUI| No event selected. Hiding buttons.") return eventTitle = selectedEvent.title + " Command List" self.commandGBox.setTitle(eventTitle) # Add and display the correct widget self.commandListStack.addWidget(selectedEvent.commandList) self.commandListStack.setCurrentWidget(selectedEvent.commandList)
def run(self): if len(self.errors): return # Find the object using vision _, tracked = self.vision.getObjectLatestRecognition(self.trackable) if tracked is None: return False camAngle = tracked.rotation[2] robAngle = self.transform.cameraToRobotRotation(camAngle) # Angle from the robots X axis # Before doing any tracking, evaluate the "Relative" number to make sure its valid startAngle, successL = self.interpreter.evaluateExpression(self.parameters["start"]) endAngle, successU = self.interpreter.evaluateExpression(self.parameters["end"]) # If the evaluation failed, cancel the command if not (successL and successU): printf("Commands| ERROR: Could not evaluate the lowerAngle or the upperAngle. Canceling command. ") return False end = endAngle - startAngle + 360.0 if (endAngle - startAngle) < 0.0 else endAngle - startAngle mid = robAngle - startAngle + 360.0 if (robAngle - startAngle) < 0.0 else robAngle - startAngle isBetween = mid < end return isBetween
def nextPressed(self, currentID): # If next is pressed, warn the user about placing the robots head on the ground if currentID == 2: self.robot.setActiveServos(all=False) warn = QtWidgets.QMessageBox() warn.setWindowTitle(self.tr("Getting Height of Ground")) warn.addButton(QtWidgets.QPushButton(self.tr('Yes, the end effector is touching the ground')), QtWidgets.QMessageBox.YesRole) warn.setText(self.tr("Important!\nBefore continuing, make sure the robots end effector is touching the " "ground, and that it is centered below the camera. " "The program will read the robots coordinates.\n\n")) reply = warn.exec_() # Get several samples of the robots current position, and put the average into self.groundCoords samples = 10 sumCoords = np.float32([0, 0, 0]) for i in range(0, samples): coord = self.robot.getCoords() sumCoords += np.float32(coord) self.groundCoords = list(map(float, sumCoords / samples)) self.robot.setPos(z=self.groundCoords[2] + .5) self.robot.setActiveServos(servo0=False) printf("GUI| New ground coordinates set: ", self.groundCoords)
def mousePressEvent(self, event): # If the user already has selected an image, leave. self.takeAnother() if event.button() == QtCore.Qt.LeftButton: # Make sure the click was within the boundaries of the frame width = self.frameLbl.pixmap().width() height = self.frameLbl.pixmap().height() pos = (event.pos().x(), event.pos().y()) if not 0 < pos[0] < width: return if not 0 < pos[1] < height: return # Get a frame from OpenCV, so that it can be cropped when the user releases the mouse button self.selectedImage = self.vStream.getFrame() if self.selectedImage is None: printf("GUI| ERROR: getCVFrame() returned None Frame! ") return # Set the rectangles position and unhide the rectangle self.origin = QtCore.QPoint(event.pos()) self.rectangle.setGeometry(QtCore.QRect(self.origin, QtCore.QSize())) self.rectangle.show()
def run(self): # Special case: If the parameter is "" set the new val to None and success to True if self.parameters['x'] == "": newX, successX = None, True else: newX, successX = self.interpreter.evaluateExpression( self.parameters['x']) if self.parameters['y'] == "": newY, successY = None, True else: newY, successY = self.interpreter.evaluateExpression( self.parameters['y']) if self.parameters['z'] == "": newZ, successZ = None, True else: newZ, successZ = self.interpreter.evaluateExpression( self.parameters['z']) if successX and successY and successZ: self.robot.setPos(x=newX, y=newY, z=newZ, relative=self.parameters['relative']) return True else: printf("Commands| ERROR in evaluating either X Y or Z: ", successX, successY, successZ) return False
def startThread(self): if self.mainThread is None: self.running = True self.mainThread = Thread(target=self.__videoThread) # Cannot be Daemon thread self.mainThread.start() else: printf("Video| Tried to create mainThread, but mainThread already existed.")
def getObjectBruteAccurate(self, trackable, minPoints=-1, maxAge=0, maxAttempts=1): """ This will brute-force the object finding process somewhat, and ensure a good recognition, or nothing at all. :param trackable: The TrackableObject you intend to find :param minPoints: Minimum amount of recognition points that must be found in order to track. -1 means ignore :param maxAge: How recent the recognition was, in "frames gotten from camera" :param maxAttempts: How many frames it should wait for before quitting the search. :return: """ # Get a super recent frame of the object for i in range(0, maxAttempts): if self.exiting: printf("Exiting early!") break # If the frame is too old or marker doesn't exist or doesn't have enough points, exit the function frameAge, trackedObj = self.getObjectLatestRecognition(trackable) if trackedObj is None or frameAge > maxAge or trackedObj.ptCount < minPoints: if i == maxAttempts - 1: break self.waitForNewFrames() continue # If the object was successfully found with the correct attributes, return it return trackedObj return None
def run(self): interpreter = self.interpreter # Get the variable. If that doesn't work, quit # variableValue, successVar = interpreter.getVariable(self.parameters['variable']) # if not successVar: return False # Evaluate the expression. If that doesn't work, quit # compareValue, successExp = interpreter.evaluateExpression(self.parameters['expression']) # print("Compare value: ", compareValue) # if not successExp: return False # Compare the value of the expression using the operator from the parameters operations = ['==', '!=', '>', '<'] variable = self.parameters['variable'] operation = operations[self.parameters['test']] expression = self.parameters["expression"] scriptString = variable + operation + expression # testResult, success = interpreter.evaluateExpression(scriptString) if not success: return False printf("Testing: ", scriptString) print(testResult, success) # If the expression was evaluated correctly, then return the testResult. Otherwise, return False return testResult
def deleteSelectedEvent(self): printf("GUI| Removing selected event") # Get the current item it's corresponding event selectedItem = self.getSelectedEventItem() if selectedItem is None: QtWidgets.QMessageBox.question( self, 'Error', 'You need to select an event to delete', QtWidgets.QMessageBox.Ok) return # If there are commands inside the event, ask the user if they are sure they want to delete it if len(self.getSelectedEvent().commandList.commands) > 0: reply = QtWidgets.QMessageBox.question( self, 'Message', 'Are you sure you want to delete this event and all its commands?', QtWidgets.QMessageBox.Yes | QtWidgets.QMessageBox.No, QtWidgets.QMessageBox.No) if reply == QtWidgets.QMessageBox.No: printf("GUI| User rejected deleting the event") return # Delete the event item and it's corresponding event self.deleteEvent(selectedItem)
def saveObject(self, newObject): # If not new, replace self wasNew = self.__addObject(newObject) if not wasNew: printf("ObjectManager| Tried to add object that already existed, ", newObject.name) newObject.save(self.__getDirectory(newObject))
def updateSettings(self, category, newSettings): """ Apply any new settings that have been changed. If they've been changed, also save the program. """ current = self.__settings[category] # Create a quick function that will check if a setting has been changed. If it has, an action will be taken. # If settings change, then save the changes to the config file, and update the self.__settings dictionary if (current is None or not current == newSettings): printf("Environment| Saving setting: ", category) # Update the self.__settings dictionary self.__settings[category] = deepcopy(newSettings) # Save the settings to a file json.dump(self.__settings, open(self.__settingsPath, 'w'), sort_keys=False, indent=3, separators=(',', ': ')) # Now, perform any updates that might need doing if category == "coordCalibrations": ptPairs = self.__settings["coordCalibrations"]["ptPairs"] self.__transform = Transform(ptPairs) else: printf("Environment| No settings changed: ", category)
def __parseArgs(self, message, command, arguments): responseDict = {n: 0 for n in arguments} #Fill the dictionary with zero's # Do error checking, in case communication didn't work if message is False: printf("Since an error occured in communication, returning 0's for all arguments!") return responseDict if command not in message: printf("ERROR: The message did not come with the appropriate command: ", command) return responseDict # Get rid of the "command" part of the message, so it's just arguments and their numbers message = message.replace(command, "") # Get the arguments and place them into the array for i, arg in enumerate(arguments): if i < len(arguments) - 1: responseDict[arg] = message[message.find(arg) + 1: message.find(arguments[i + 1])] else: responseDict[arg] = message[message.find(arg) + 1:] responseDict[arg] = float(responseDict[arg]) return responseDict
def addEvent(self, eventType, parameters=None, commandListSave=None): if commandListSave is None: commandListSave = [] # If a file is loading, then optimize the process by changing it a bit. isLoading = len(commandListSave) > 0 # Check if the event being added already exists in the self.events dictionary if not isLoading: for _, item in self.events.items(): if isinstance(item, eventType) and (item.parameters == parameters or parameters is None): printf( "GUI| Event already exists, disregarding user input.") return None newEvent = eventType(parameters) newCommandList = CommandList(self.env, parent=self) newCommandList.loadData(commandListSave) newEvent.commandList = newCommandList # Create the widget item to visualize the event blankWidget = EventsGUI.EventWidget(self) eventWidget = newEvent.dressWidget(blankWidget) # Figure out where the event will be placed in the list by looking at the "priority" of the other events placeIndex = 0 for index, event in enumerate(self.getEventsOrdered()): if newEvent.priority < event.priority: break # If they are of the same priority, then put them in alphabetical order if newEvent.priority == event.priority: if min(newEvent.title, event.title) == newEvent.title: break placeIndex = index + 1 # Create the list item to put the widget item inside of listWidgetItem = QtWidgets.QListWidgetItem(self) listWidgetItem.setSizeHint( eventWidget.sizeHint()) # Widget will not appear without this line # Do some finagling so pyQt will let me insert an item at 'placeIndex' row self.addItem(listWidgetItem) lastRow = self.indexFromItem(listWidgetItem).row() takenlistWidgetItem = self.takeItem(lastRow) self.insertItem(placeIndex, takenlistWidgetItem) # Add the widget to the list item self.setItemWidget(listWidgetItem, eventWidget) self.events[eventWidget] = newEvent if not isLoading: self.setCurrentRow(placeIndex) # Select the newly added event if self.count() > 0: self.hintLbl.hide() return placeIndex # Returns where the object was placed
def __sendAndRecieve(self, cmnd): """ This command will send a command and receive the robots response. There must always be a response! Responses should be recieved immediately after sending the command, after which the robot will proceed to perform the action. :param cmnd: a String command, to send to the robot :return: The robots response """ if not self.connected(): printf( "Communication| Tried to send a command while robot was not connected!" ) return "" # Prepare and send the command to the robot cmndString = bytes("[" + cmnd + "]", encoding='ascii') # "[" + cmnd + "]" try: self.__serial.write(cmndString) except serial.serialutil.SerialException as e: printf("Communication| ERROR ", e, "while sending command ", cmnd, ". Disconnecting Serial!") self.__isConnected = False return "" # Read the response from the robot (THERE MUST ALWAYS BE A RESPONSE!) response = "" while True: try: response += str(self.__serial.read(), 'ascii') response = response.replace(' ', '') except serial.serialutil.SerialException as e: printf("Communication| ERROR ", e, "while sending command ", cmnd, ". Disconnecting Serial!") self.__isConnected = False return "" if "[" in response and "]" in response: response = str(response.replace("\n", "")) response = str(response.replace("\r", "")) break printf("Communication| ", "[" + cmnd + "]" + " " * (30 - len(cmnd)) + response) # Clean up the response response = response.replace("[", "") response = response.replace("]", "") # If the robot returned an error, print that out if "error" in response: printf("Communication| ERROR: received error from robot: ", response) return response
def addTarget(self, targetName): for target in self.cascades: if targetName == target.name: if target not in self.targets: self.targets.append(target) else: printf("ERROR: Tried to add a target that was already there!")
def promptUser(self): # Open the eventPromptWindow to ask the user what event they wish to create objManager = self.env.getObjectManager() eventPrompt = EventsGUI.EventPromptWindow(objManager, parent=self) if eventPrompt.accepted: self.addEvent(eventPrompt.chosenEvent, parameters=eventPrompt.chosenParameters) else: printf("User rejected the prompt.")
def promptUser(self): # Open the eventPromptWindow to ask the user what event they wish to create objManager = self.env.getObjectManager() eventPrompt = EventsGUI.EventPromptWindow(objManager, parent=self) if eventPrompt.accepted: self.addEvent(eventPrompt.chosenEvent, parameters=eventPrompt.chosenParameters) else: printf("GUI| User rejected the prompt.")
def setExiting(self, value): printf("Interpreter| Setting Interpreter to Exiting mode.") global exitingFlag self.env.getRobot().setExiting(value) self.env.getVision().setExiting(value) exitingFlag = value
def getSaveData(self): printf("Getting CommandList save data") commandList = [] commandsOrdered = [self.getCommand(self.item(index)) for index in range(self.count())] for command in commandsOrdered: commandList.append(command.getSaveData()) return commandList
def run(self): if len(self.errors): return # Before doing any tracking, evaluate the "Relative" number to make sure its valid relativeAngle, success = self.interpreter.evaluateExpression(self.parameters["angle"]) if not success: printf("Could not determine the relative angle for the wrist. Canceling command. ") return False # Find the object using vision _, tracked = self.vision.getObjectLatestRecognition(self.trackable) if tracked is None: printf("Could not find ", self.trackable.name, " in order to set wrist relative") return False # This is the rotation of the object in degrees, derived from the camera targetAngle = math.degrees(tracked.rotation[2]) cntr = tracked.center if self.relToBase: # If the angle is relative to the base angle, do the math here and add it to target angle. TOCC = cntr # Tracked Object Camera Coordinates ROCC = rv.getPositionTransform((0, 0, 0), "toCam", self.ptPairs) # Robot Origin Camera Coordinates baseAngle = 90 - math.degrees(math.atan( (ROCC[1] - TOCC[1]) / (ROCC[0] - TOCC[0]))) targetAngle += baseAngle - 90 else: # If the angle is relative to x Axis, do different math and add it to target angle a = rv.getPositionTransform((0, 0, 0), "toCam", self.ptPairs) b = rv.getPositionTransform((10, 0, 0), "toCam", self.ptPairs) # TORC = rv.getPositionTransform(cntr, "toRob", self.ptPairs) # Tracked Object Robot Coordinates # RXCC = rv.getPositionTransform((0, cntr[1], cntr[2]), "toCam", self.ptPairs) # Robot XAxis Camera Coordinates xOffset = math.degrees(math.atan( (a[1] - b[1]) / (a[0] - b[0]))) targetAngle += 90 - xOffset print("XOffset: ", xOffset) print("Target: ", targetAngle) # Add the "Relative" angle to the wrist targetAngle += relativeAngle # Normalize the value so that it's between 0 and 180 while targetAngle < 0: targetAngle += 180 while targetAngle > 180: targetAngle -= 180 # Set the robots wrist to the new value self.robot.setServoAngles(servo3 = targetAngle) return True
def createTransformFunc(ptPairs, direction): """ Returns a function that takes a point (x,y,z) of a camera coordinate, and returns the robot (x,y,z) for that point Direction is either "toRob" or "toCam". If "toCam" it will return a Robot-->Camera transform If "toRob" it will return a Camera-->Robot transform """ ptPairArray = np.asarray(ptPairs) if direction == "toRob": pts1 = ptPairArray[:, 0] pts2 = ptPairArray[:, 1] if direction == "toCam": pts1 = ptPairArray[:, 1] pts2 = ptPairArray[:, 0] # Generate the transformation matrix ret, M, mask = cv2.estimateAffine3D(np.float32(pts1), np.float32(pts2), confidence = .9999999) if not ret: printf("RobotVision| ERROR: Transform failed!") # Returns a function that will perform the transformation between pointset 1 and pointset 2 (if direction is == 1) transformFunc = lambda x: np.array((M * np.vstack((np.matrix(x).reshape(3, 1), 1)))[0:3, :].reshape(1, 3))[0] """ Breakdown of function. Here's an example of transforming [95, -35, 530] cam which is [5, 15, 15] in the robot grid First: [95, -35, 530] np.vstack((np.matrix(input).reshape(3, 1), 1)) is applied, converts it to a vertical array [[ 95] [-35] [530] [ 1]] M * np.vstack((np.matrix(x).reshape(3, 1), 1)) (transformation matrix multiplied by the previous number) [[ 5.8371337 ] [ 15.72722685] [ 14.5007519 ]] There you go. The rest of it is simply converting that matrix into [5.83, 15.72, 14.5] but you know, without the rounding. """ return transformFunc
def stopMoving(self): """ Stop the robot's ongoing movement """ if not self.connected() or self.__exiting: printf("Robot| Robot not avaliable, canceling position change") return with self.__lock: self.__uArm.setStop()
def run(self): speed, success = self.interpreter.evaluateExpression(self.parameters['speed']) # Split the wait into incriments of 0.1 seconds each, and check if the thread has been stopped at each incriment if success: self.robot.setSpeed(speed) return True else: printf("Commands| ERROR: Expression ", self.parameters['speed'], " failed to evaluate correctly!") return False
def run(self): newAngle, success = self.interpreter.evaluateExpression(self.parameters['angle']) if success: # If relative, get the current wrist angle then add that to newAngle self.robot.setServoAngles(servo3=newAngle, relative=self.parameters['relative']) return True else: printf("Commands| ERROR in parsing new wrist angle. Expression: ", self.parameters['angle']) return False
def startThread(self): if self.mainThread is None: self.running = True self.mainThread = Thread( target=self.__videoThread) # Cannot be Daemon thread self.mainThread.start() else: printf( "Video| Tried to create mainThread, but mainThread already existed." )
def setActiveServos(self, all=None, servo0=None, servo1=None, servo2=None, servo3=None): """ Attach or detach a servo. Simple do True to attach and False to detach. Don't specify any servo you don't want to touch. :param all: Attach or detach all servos :param servo0: :param servo1: :param servo2: :param servo3: """ if not self.connected() or self.__exiting: printf("Robot| Robot not avaliable, canceling servo change") return # If a positional servo is attached, get the robots current position and update the self.pos cache oldServoStatus = self.servoAttachStatus[:] def setServo(servoNum, status): # Verify that it is a dfiferent servo position before sending if self.servoAttachStatus[servoNum] == status: return with self.__lock: if status: self.__uArm.setServoAttach(servoNum) else: self.__uArm.setServoDetach(servoNum) self.servoAttachStatus[servoNum] = status # If anything changed, set the appropriate newServoStatus to reflect that if all is not None: servo0, servo1, servo2, servo3 = all, all, all, all if servo0 is not None: setServo(0, servo0) if servo1 is not None: setServo(1, servo1) if servo2 is not None: setServo(2, servo2) if servo3 is not None: setServo(3, servo3) # Make an array of which servos have been newly attached. attached = [ oldServoStatus[i] is False and self.servoAttachStatus[i] is True for i in range(3) ] # If any positional servos have been attached, update the self.pos cache with the robots current position if any(attached): with self.__lock: curr = self.getCoords() self.coord = list(curr) self.servoAngleStatus = list(self.__uArm.getServoAngles())
def setExiting(self, exiting): """ When True, this will prevent the robot from completing any moves or lagging the program. The idea is that when you are exiting a script, it has to happen quickly. This prevents any lag for waiting for the robot, and cuts all communication. When False everything is normal and the robot communicates normally :param exiting: True-> communication with robot is cut off. False -> Everything works normally """ if exiting: printf("Setting robot to Exiting mode. All commands should be ignored") self.exiting = exiting
def getFK(self, servo0, servo1, servo2): """ Get the forward kinematic calculations for the robots servos. :return: [X, Y, Z] list """ if not self.connected() or self.__exiting: printf("Robot| Robot not avaliable, returning 0 for FK") return [0.0, 0.0, 0.0] with self.__lock: return self.__uArm.getFK(servo0, servo1, servo2)
def getFK(self, servo0, servo1, servo2): """ Get the forward kinematic calculations for the robots servos. :return: [X, Y, Z] list """ if not self.connected() or self.exiting: printf("Robot not avaliable, returning 0 for FK") return [0.0, 0.0, 0.0] with self.lock: return self.uArm.getFK(servo0, servo1, servo2)
def getIK(self, x, y, z): """ Get the inverse kinematic calculations for a certain XYZ location. :return: [X, Y, Z] list """ if not self.connected() or self.__exiting: printf("Robot| Robot not avaliable, returning 0 for IK") return [0.0, 0.0, 0.0] with self.__lock: return self.__uArm.getIK(x, y, z)
def waitForNewFrames(self, numFrames=1): # Useful for situations when you need x amount of new frames after the robot moved, before doing vision for i in range(0, numFrames): lastFrame = self.vStream.frameCount while self.vStream.frameCount == lastFrame: if self.exiting: printf("Vision| Exiting early!") break sleep(.01)
def loadData(self, data): printf("Loading CommandList") self.commands = {} self.clear() # Fill the list with new data for commandSave in data: # Convert from string to an actual event obj self.addCommand(getattr(CommandsGUI, commandSave['type']), parameters=commandSave['parameters']) self.refreshIndents()
def setBuzzer(self, frequency, duration): """ Set the robots buzzer. :param frequency: In HZ :param duration: In seconds """ if not self.connected() or self.__exiting: printf("Robot| Robot not avaliable, canceling buzzer change") return with self.__lock: self.__uArm.setBuzzer(frequency, duration)
def initializeScript(self, script): """ Initializes each event in the script, Initializes each command in the script, places all the commands into the appropriate events, and returns all errors that occured during the build process. "script" is an already loaded script, of the format that EventList.getSaveData() would return. It looks something like [{Event here, commandList here}, {event here, commandList here}] To load a script from file, simply use Interpreter.loadScriptFromFile(filename) :param env: Environment object :param script: a loaded script from a .task file :return: any errors that commands returned during instantiation """ errors = { } # Keep track of missing components for commands that are initialized # Create each event for _, eventSave in enumerate(script): # Get the "Logic" code for this event, stored in Events.py eventType = eventClasses[eventSave['type']] event = eventType(self.env, self, parameters=eventSave['parameters']) self.addEvent(event) # Record any errors related to the creation of this event for error in event.errors: if error not in errors: errors[error] = [] errors[error].append(eventSave['type']) # Create the commandList for this event for _, commandSave in enumerate(eventSave['commandList']): # Get the "Logic" code for this command, stored in Commands.py commandType = commandClasses[commandSave['type']] command = commandType(self.env, self, parameters=commandSave['parameters']) event.addCommand(command) # Record any errors related to the creation of this command for error in command.errors: if error not in errors: errors[error] = [] errors[error].append(commandSave['type']) # Get rid of duplicates in the errors for error in errors: errors[error] = list(set(errors[error])) printf("Interpreter| ", len(errors), " errors occured while initializing the script.") return errors
def getAngles(self): """ Get a list of the robots current servo angle readings :return: [servo0, servo1, servo2, servo3] angles """ if not self.connected() or self.__exiting: printf("Robot| Robot not avaliable, returning 0 for angle") return [0.0, 0.0, 0.0, 0.0] with self.__lock: return self.__uArm.getServoAngles()
def getCoords(self): """ Get the current XYZ coordinates of the robot :return: [X, Y, Z] List. If robot is not connect, [0.0, 0.0, 0.0] """ if not self.connected() or self.__exiting: printf("Robot| Robot not avaliable, return 0 for all coordinates") return [0.0, 0.0, 0.0] with self.__lock: return self.__uArm.getXYZCoords()
def getMoving(self): """ Check if the robot is currently moving. :return: True or False, if robot is moving """ if not self.connected() or self.__exiting: printf("Robot| Robot not avaliable, returning False") return False with self.__lock: return self.__uArm.getMoving()