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')
