def calcLeavingConditin(minDist, distNextObstacle, clientID): leave = False if distNextObstacle == -1.0: # non trackable distance, normally returned by calcFreespace print("Freespace not Trackable") return leave, minDist # calc current dist. to target res, objHandle = vrep.simxGetObjectHandle(clientID, "Goal", vrep.simx_opmode_oneshot_wait) targetPosition = vrep.simxGetObjectPosition(clientID, objHandle, -1, vrep.simx_opmode_oneshot_wait) xTarget = targetPosition[1][0] yTarget = targetPosition[1][1] pos, ori = move.getPos(clientID) dist = calcDistanceToTarget(pos[0], pos[1], xTarget, yTarget) if minDist == -1.0: # init minDist minDist = dist # calc leaving condition if dist - distNextObstacle <= minDist - STEP: leave = True # set minDist if neccesary if dist < minDist: minDist = dist if distNextObstacle > dist: leave = True return leave, minDist
def getToNextBlob(clientID, blobsList, visitedBlobsList): print("Current state: get to next blob") nextBlob = blobsList[0] visitedBlobsList.append(nextBlob) # get robot pos roboPos, ori = move.getPos(clientID) # get shorten egocentric coordinates to the blob xA = nextBlob[0] - roboPos[0] yA = nextBlob[1] - roboPos[1] lenA = math.sqrt(xA * xA + yA * yA) shortenXA = xA * (1 - (c.maxDistToBlock / lenA)) shortenYA = yA * (1 - (c.maxDistToBlock / lenA)) # transform egocentric coordinates to global space x = shortenXA + roboPos[0] y = shortenYA + roboPos[1] # move forward until block dist - constants.maxDistToBlock #move.moveToCoordinate(x, y, clientID) #targetOrientation = calcTargetOrient(roboPos[0], roboPos[1], x, y) # get orientation to target # rotate by 90° to get vision to the blob #move.rotate(90, clientID, False) nextState = 6 # align to blob state return nextState, blobsList, visitedBlobsList
def forwardUntilObstacleAnywhere(targetPos, clientID, rangeSensorHandles): # set velocety to 0 move.setWheelVelocity(clientID, 0) # start moving move.startMoving(clientID) # continuously check traveled distance distance = 0.0 dt = 0.0 maxFalseAngle = 5 x, y = move.getPos(clientID)[0][:-1] xCurrent, yCurrent = move.getPos(clientID)[0][:-1] stop = False hit = 0 while not isSamePoint(targetPos, [xCurrent, yCurrent]) and stop != True: # get range sensor data as list of x,y,z,distance rangeData = rangeSensor.getSensorData(clientID, rangeSensorHandles) # range sensor angle is estimated for about 250 degrees #for i in range(95, 588): #95 and 588 are estimated values for data from range sensor which are between -90 and 90 degrees (0 is front) for i in range( 75, 602 ): #95 and 588 are estimated values for data from range sensor which are between -90 and 90 degrees (0 is front) if rangeData[i][3] <= 0.3: stop = True hit = i break time.sleep( 1.0e-06 ) # problems with very small time slices -> little delay (if you have a bad angle calculation on your pc try to change this value) end = time.time() xCurrent, yCurrent = move.getPos(clientID)[0][:-1] targetOrientation = move.calcTargetOrient(xCurrent, yCurrent, targetPos[0], targetPos[1]) if abs(move.getOrientation(clientID) - targetOrientation) > maxFalseAngle: rotateUntilOrientation(clientID, targetOrientation) move.startMoving(clientID) # stop moving move.setWheelVelocity(clientID, 0) return (stop, hit)
def globalToEgocentric(globCorrd, clientID): pos, orient = move.getPos(clientID) egoVec = [globCorrd[0] - pos[0], globCorrd[1] - pos[1]] alpha = move.getOrientation(clientID) / 180.0 * math.pi + math.pi / 2.0 rotationMatrix = [[math.cos(-alpha), -math.sin(-alpha)], [math.sin(-alpha), math.cos(-alpha)]] newVec = np.dot(np.array(rotationMatrix), np.array(egoVec)) return newVec
def getAngle(clientID, targetX, targetY): pos, orient = getArmPos(clientID) uPos, uOrient = move.getPos(clientID) print(move.calcTargetOrient(pos[0], pos[1], targetX, targetY)) orient[2] = orient[2] #- math.pi print(180 / math.pi * orient[2]) a = move.calcTargetOrient(pos[0], pos[1], targetX, targetY) - 180 / math.pi * orient[2] print(a) return a
def headTowardsModel(clientID, modelName, rangeSensorHandles): print("HeadTowardsModel: start") # get the needed position information's of youBot and modelName (target) res, objHandle = vrep.simxGetObjectHandle(clientID, modelName, vrep.simx_opmode_oneshot_wait) targetPosition = vrep.simxGetObjectPosition(clientID, objHandle, -1, vrep.simx_opmode_oneshot_wait) xTarget = targetPosition[1][0] yTarget = targetPosition[1][1] pos, ori = move.getPos(clientID) # print where the youBot is and where the target print("youBot: x = {}, y = {}".format(pos[0], pos[1])) print("{}: x = {}, y = {}".format(modelName, xTarget, yTarget)) # calculate the target orientation based on the current youBot position targetOrientation = calcTargetOrient(clientID, pos[0], pos[1], xTarget, yTarget) # rotate towards modelName until target orientation is reached move.rotateUntilOrientation(clientID, targetOrientation) # calculate the distance between youBot and target dist = calcDistanceToTarget(pos[0], pos[1], xTarget, yTarget) #dirve for dist meters or stop when encountering an obstacle case, hit = move.forwardUntilObstacleAnywhere(dist, clientID, rangeSensorHandles) pos, ori = move.getPos(clientID) # get new position of bot for the output print("Current position of bot after headTowardsModel; x = {} , y = {}". format(pos[0], pos[1])) print("HeadTowardsModel: end, Finished? {}".format(not case)) return case, hit
def calcOrientationToTarget(clientID, targetName): # get target position res, objHandle = vrep.simxGetObjectHandle(clientID, targetName, vrep.simx_opmode_oneshot_wait) targetPosition = vrep.simxGetObjectPosition(clientID, objHandle, -1, vrep.simx_opmode_oneshot_wait) xTarget = targetPosition[1][0] yTarget = targetPosition[1][1] #get the youBot position pos, ori = move.getPos(clientID) # calculate the target orientation targetOrientation = calcTargetOrient(clientID, pos[0], pos[1], xTarget, yTarget) return targetOrientation
def egocentricToGlobal(ego, clientID): x = ego[0] y = ego[1] pos, orient = move.getPos(clientID) alpha = move.getOrientation(clientID) / 180.0 * math.pi + math.pi / 2.0 rotationMatrix = [[math.cos(alpha), -math.sin(alpha)], [math.sin(alpha), math.cos(alpha)]] newVec = np.dot(np.array(rotationMatrix), np.array([x, y])) x = newVec[0] y = newVec[1] xBot = pos[0] yBot = pos[1] return [x + xBot, y + yBot]
def headTowardsModel(clientID, targetPos, rangeSensorHandles): print("Next goal: x={}, y={}".format(targetPos[0], targetPos[1])) # get the needed position information's of youBot and modelName (target) xTarget = targetPos[0] yTarget = targetPos[1] pos, ori = move.getPos(clientID) # calculate the target orientation based on the current youBot position targetOrientation = bug.calcTargetOrient(clientID, pos[0], pos[1], xTarget, yTarget) # rotate towards modelName until target orientation is reached rotateUntilOrientation(clientID, targetOrientation) #dirve for dist meters or stop when encountering an obstacle case, hit = forwardUntilObstacleAnywhere(targetPos, clientID, rangeSensorHandles) return case, hit
def detectOneBlob(clientId, youBotCam, homoMatrix): print("Start finding one red blob") blobList = [] err, res, image = vrep.simxGetVisionSensorImage(clientId, youBotCam, 0, vrep.simx_opmode_buffer) if err == vrep.simx_return_ok: # do some image stuff ---------------------------------------------------------------------------------- cv2Image = colorDet.convertToCv2Format(image, res) tempBlobs = getRedBlueBlobs(cv2Image, homoMatrix, clientId) count = 0 for tb in tempBlobs: count = 0 for b in blobList: if isSameBlob(tb[0], b[0]) and tb[1] == b[1]: count = count + 1 if count == 0: print("Added", tb, " to blobList") blobList.append(tb) # get the closest blob currentPos = move.getPos(clientId)[0] closestBlob = blobList[0] closestDistance = move.getDistanceBetweenPoints(blobList[0][0], currentPos) for blob in blobList: distNewBlob = move.getDistanceBetweenPoints( blob[0], currentPos) # distance to new blob if ( closestDistance - distNewBlob > 0 ): # check if the current blobs position is nearer than the current closest blob # set new blob as nearest blob and update closest distance closestBlob = blob closestDistance = distNewBlob print("COORD: ", closestBlob[0]) print("End find red or blue blob") returnCorrd = colorDet.globalToEgocentric(closestBlob[0], clientId) return returnCorrd
def followObstacle(clientID, sensorHandles, rightSide, roundFinished): print("Follow boundary: start") # set rayHit to the ray index where the followed wall is if rightSide: rayHit = LEFT_RAY_NINETY else: rayHit = RIGHT_RAY_NINETY targetDistanceToWall = 0.5 minRange = targetDistanceToWall - 0.05 maxRange = targetDistanceToWall + 0.05 counter = 0 startPoint = (move.getPos(clientID)[0][0], move.getPos(clientID)[0][1] ) #point where ubot first hit the obstacle res, objHandle = vrep.simxGetObjectHandle(clientID, 'Goal', vrep.simx_opmode_oneshot_wait) RES, targetPosition = vrep.simxGetObjectPosition( clientID, objHandle, -1, vrep.simx_opmode_oneshot_wait) goalPoint = (targetPosition[0], targetPosition[1]) minDist = move.getDistanceBetweenPoints( startPoint, goalPoint) #shortest dist to the goal minPoint = startPoint #coordinates of the nearest point to the goal disToMin = 0 while True: move.startMoving(clientID) #variables for distance tracking x = 0.0 y = 0.0 dt = 0.0 while not detectCorner( clientID, sensorHandles, rayHit, (minRange + maxRange) / 2.0): #not abs(oldDis - newDis)>1 start = time.time() # start time for distance tracking x, y, distanceTravled = move.calcTraveldDistance(x, y, dt) rangeData = rangeSensor.getSensorData(clientID, sensorHandles) for i in range( move.FRONT_SEN_START, move.FRONT_SEN_END ): # check for a pool of front sensors if there is an obstacle if rangeData[i][3] <= 0.5: print("FLALALALALAL") wallOrient(clientID, sensorHandles, rayHit, True) break move.startMoving(clientID) # Only check the following every 5 iterations, so that the correction of following doesn't happen too often if counter % 5 == 0: rangeToWallNew = rangeData[rayHit][3] print("Current range to wall: {}".format(rangeToWallNew)) # if the distance to the wall is to little drive away from the wall if rangeToWallNew < minRange: move.setWheelVelocity(clientID, 0) move.sideway(minRange - rangeToWallNew, clientID, rightSide) move.startMoving(clientID) wallOrient(clientID, sensorHandles, rayHit, False) # if the distance to the wall is too high -> drive towards the wall elif rangeToWallNew > maxRange: move.setWheelVelocity(clientID, 0) move.sideway(rangeToWallNew - maxRange, clientID, not rightSide) move.startMoving(clientID) wallOrient(clientID, sensorHandles, rayHit, False) counter += 1 currentPoint = (move.getPos(clientID)[0][0], move.getPos(clientID)[0][1]) if ( roundFinished == False and distanceTravled > 1 ): #checks if the ubot drove around the obstacle (distanceTraveld > 1 is tolleranz to prevent, that the ubot thinks he finished a rond when he is at the starting point for the first time) roundFinished = move.isSamePoint(startPoint, currentPoint) if (move.getDistanceBetweenPoints(currentPoint, goalPoint) < minDist): #checks for the min distance to the goal minDist = move.getDistanceBetweenPoints( currentPoint, goalPoint) minPoint = currentPoint disToMin = distanceTravled if ( roundFinished and move.isSamePoint(minPoint, currentPoint) and 2 * disToMin - 1 < distanceTravled ): #checks if the robot is again at the nearest point, again with toleranz print("min Point found") return end = time.time() # end time for distance tracking dt = end - start print("drive around corner") goAroundCorner(clientID, sensorHandles, rightSide, rayHit) currentPoint = (move.getPos(clientID)[0][0], move.getPos(clientID)[0][1]) if (roundFinished and move.isSamePoint(minPoint, currentPoint)): print("min Point found") return
def main(): print('Program started') emptybuf = bytearray() vrep.simxFinish(-1) # just in case, close all opened connections clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 2000, 5) if clientID != -1: print('Connected to remote API server') # Start the simulation: vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot_wait) # start init wheels -------------------------------------------------------------------------------------------- wheelJoints = np.empty(4, dtype=np.int) wheelJoints.fill(-1) # front left, rear left, rear right, front right res, wheelJoints[0] = vrep.simxGetObjectHandle( clientID, 'rollingJoint_fl', vrep.simx_opmode_oneshot_wait) res, wheelJoints[1] = vrep.simxGetObjectHandle( clientID, 'rollingJoint_rl', vrep.simx_opmode_oneshot_wait) res, wheelJoints[2] = vrep.simxGetObjectHandle( clientID, 'rollingJoint_fr', vrep.simx_opmode_oneshot_wait) res, wheelJoints[3] = vrep.simxGetObjectHandle( clientID, 'rollingJoint_rr', vrep.simx_opmode_oneshot_wait) # end init wheels ---------------------------------------------------------------------------------------------- # start init camera -------------------------------------------------------------------------------------------- # change the angle of the camera view (default is pi/4) res = vrep.simxSetFloatSignal(clientID, 'rgbd_sensor_scan_angle', math.pi / 2, vrep.simx_opmode_oneshot_wait) # turn on camera res = vrep.simxSetIntegerSignal(clientID, 'handle_rgb_sensor', 2, vrep.simx_opmode_oneshot_wait) # get camera object-handle res, youBotCam = vrep.simxGetObjectHandle( clientID, 'rgbSensor', vrep.simx_opmode_oneshot_wait) # get first image err, resolution, image = vrep.simxGetVisionSensorImage( clientID, youBotCam, 0, vrep.simx_opmode_streaming) time.sleep(1) # end init camera ---------------------------------------------------------------------------------------------- # programmable space ------------------------------------------------------------------------------------------- print("Begin calculation of H-matrix, please wait ...") err, res, image = vrep.simxGetVisionSensorImage( clientID, youBotCam, 0, vrep.simx_opmode_buffer) image = colorDet.convertToCv2Format(image, res) image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) found, prime_corners = cv2.findChessboardCorners(image, (3, 4)) prime_corners = addOne(prime_corners) # gCX are the world coordinates for the points on the chessboards which are later used to construct the h-matrix gCX = [[0.075, 0.55, 1.0], [0.075, 0.5, 1.0], [0.075, 0.45, 1.0], [0.025, 0.55, 1.0], [0.025, 0.5, 1.0], [0.025, 0.45, 1.0], [-0.025, 0.55, 1.0], [-0.025, 0.5, 1.0], [-0.025, 0.45, 1.0], [-0.075, 0.55, 1.0], [-0.075, 0.5, 1.0], [-0.075, 0.45, 1.0]] # convert all global corners of the chessboard in egocentric world space ego_corners = [] for gc in gCX: newCorner = colorDet.globalToEgocentric(gc, clientID) ego_corners.append(newCorner) # add a 1 in every row (globalToEgocentric only returns x,y coordinates ego_corners = addOne(ego_corners) # convert ego_corners in numpy array ego_corners = np.asarray(ego_corners) # calculate H-matrix A = getA(prime_corners, ego_corners) H = getH(A) newH = cv2.findHomography(prime_corners, ego_corners) print("H-matrix cv2:", newH) print("H-matrix own:", H / H[2][2]) blobs = colorDet.findAllBlobs(clientID, youBotCam, H) obstacleList = [] for b in blobs: obstacleList.append(b[0]) print("Found blobs:") # sort blob list and print it blobs = sortBlobsByRad(blobs) for blob in blobs: print("Coordinate: ", blob[0], " Upper and lower Bound of the color: ", blob[1]) # get position of youBot and goal roboPos, ori = move.getPos(clientID) res, objHandle = vrep.simxGetObjectHandle( clientID, "goal", vrep.simx_opmode_oneshot_wait) targetPosition = vrep.simxGetObjectPosition( clientID, objHandle, -1, vrep.simx_opmode_oneshot_wait) targetPosition = targetPosition[1][:2] # drive to the goal with obstacles ahead driveThroughPath(obstacleList, roboPos[:2], targetPosition, clientID) # end of programmable space -------------------------------------------------------------------------------------------- # Stop simulation vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot_wait) # Close connection to V-REP: vrep.simxFinish(clientID) else: print('Failed connecting to remote API server') print('Program ended')
def main(): global clientID print('Program started') emptybuf = bytearray() vrep.simxFinish(-1) # just in case, close all opened connections clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 2000, 5) if clientID != -1: print('Connected to remote API server') # Start the simulation: vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot_wait) # start init wheels -------------------------------------------------------------------------------------------- wheelJoints = np.empty(4, dtype=np.int) wheelJoints.fill(-1) # front left, rear left, rear right, front right res, wheelJoints[0] = vrep.simxGetObjectHandle( clientID, 'rollingJoint_fl', vrep.simx_opmode_oneshot_wait) res, wheelJoints[1] = vrep.simxGetObjectHandle( clientID, 'rollingJoint_rl', vrep.simx_opmode_oneshot_wait) res, wheelJoints[2] = vrep.simxGetObjectHandle( clientID, 'rollingJoint_fr', vrep.simx_opmode_oneshot_wait) res, wheelJoints[3] = vrep.simxGetObjectHandle( clientID, 'rollingJoint_rr', vrep.simx_opmode_oneshot_wait) # end init wheels ---------------------------------------------------------------------------------------------- # start init camera -------------------------------------------------------------------------------------------- # change the angle of the camera view (default is pi/4) res = vrep.simxSetFloatSignal(clientID, 'rgbd_sensor_scan_angle', math.pi / 2, vrep.simx_opmode_oneshot_wait) # turn on camera res = vrep.simxSetIntegerSignal(clientID, 'handle_rgb_sensor', 2, vrep.simx_opmode_oneshot_wait) # get camera object-handle res, youBotCam = vrep.simxGetObjectHandle( clientID, 'rgbSensor', vrep.simx_opmode_oneshot_wait) # get first image err, resolution, image = vrep.simxGetVisionSensorImage( clientID, youBotCam, 0, vrep.simx_opmode_streaming) time.sleep(1) # end init camera ---------------------------------------------------------------------------------------------- # start init range sensor ---------------------------------------------------------------------------------------------- # initialize sensor and get sensor handles: rangeSen.initializeSensor(clientID) hokuyo = rangeSen.getSensorHandles(clientID) # end init range sensor ---------------------------------------------------------------------------------------------- # programmable space ------------------------------------------------------------------------------------------- # implement state machine # 1 - init | 2 - detect blob | 3 - move to blob | 4 - grab | 5 - follow next explore path | 6 - align to blob | 7 - follow next basket path # 8 - drop block | 0 - finish | -1 - finish with error state = 1 # space to store data to share between states h_matrix = -1 explorePaths = Queue() basketPaths = Queue() blobsList = [] visitedBlobsList = [] orientations = Queue() posBeforeMoveToBlob = move.getPos(clientID)[0][:-1] blockColors = [] while state != 0: if state == 1: # init # init path, H, and next state state, explorePaths, basketPaths, orientations, blockColors, h_matrix = ex.init_state( youBotCam, clientID) elif state == 2: # detect blob # find all blobs that are 360 degrees around youBot state, blobsList = ex.findBlobs(clientID, youBotCam, h_matrix, blobsList, visitedBlobsList) elif state == 3: # move to blob # get to next blob state posBeforeMoveToBlob = move.getPos(clientID)[ 0][:-1] # store current position for moving back later state, blobsList, visitedBlobsList = ex.getToNextBlob( clientID, blobsList, visitedBlobsList) elif state == 4: # grab state = ex.grabBlob(clientID, h_matrix, youBotCam) elif state == 5: # follow next explore path state = ex.followExplorePath(clientID, hokuyo, explorePaths, orientations) elif state == 6: # align to blob state = ex.alignToBlob(clientID, youBotCam) elif state == 7: # follow next basket path state = ex.followBasketPath(clientID, hokuyo, basketPaths, blockColors) elif state == 8: # drop blob state = ex.dropBlob(clientID) move.sideway(c.maxDistToBlock, clientID, True) elif state == -1: # finish with error print("Current state: fail state!") print("An error has occurred. Program finished with state -1.") state = 0 print("End of blob grabing shit") ''' state, explorePaths, basketPaths, orientations, blockColors, h_matrix = ex.init_state(youBotCam, clientID) move.forward(0.5, clientID) ex.alignToBlob(clientID, youBotCam) ex.grabBlob(clientID, h_matrix, youBotCam) time.sleep(5) #ex.moveArm(clientID, -90, 20,70,0,0) #ex.moveArm(clientID, -90, 90,0,0,0) #ex.getAngle(clientID) #ex.moveArm(clientID, 0, 0,0,0,0) # ex.moveArm(clientID, 180/math.pi*ex.getAngle(clientID), 95,40,35,0) ''' # end of programmable space -------------------------------------------------------------------------------------------- # Stop simulation vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot_wait) # Close connection to V-REP: vrep.simxFinish(clientID) else: print('Failed connecting to remote API server') print('Program ended')