def getCurrentGameboard():
NUM_OF_TIMES=3
THRESHOLD=2
#first we scan the gameboard NUM_OF_TIMES times and store shapes detected during each time:
detected=[[],[],[],[],[],[],[],[],[]]
for times in range(0,NUM_OF_TIMES):
listOfShapesDetected= shapeDetectionModuleInterface.shapeDetectionMain(IP, PORT)
for shapeID in range(0,9):
detected[times].append(listOfShapesDetected[shapeID])
#mean coordinates
meanOfCenters=[]
#list of probabilities to detect a particular shape
probList=[]
#calculate probabilities that each figure appears on the gameboard
for shapeID in range(0,9):
shapeCentersListLength=0
meanX=0
meanY=0
n=0
for times in range(0,NUM_OF_TIMES):
#only count those shapes, which are detected in single exemplars (i.e. no 2 red triangles)
if(len(detected[times][shapeID].getCenters())==1 or len(detected[times][shapeID].getCenters())==0):
shapeCentersListLength+=len(detected[times][shapeID].getCenters())
if(len(detected[times][shapeID].getCenters())==1):
meanX+=detected[times][shapeID].getCenters()[0][0]
meanY+=detected[times][shapeID].getCenters()[0][1]
n+=1.0
else:
shapeCentersListLength+=0
probList.append(shapeCentersListLength)
if n==0:
meanOfCenters.append((0, 0))
else:
meanOfCenters.append((meanX/n, meanY/n))
#to remove noise
for shapeID in range(0,9):
if probList[shapeID]< THRESHOLD:
meanOfCenters[shapeID]=(0,0)
detectedPiecesIDs=[i for i,v in enumerate(probList) if v >= THRESHOLD]
return (detectedPiecesIDs,meanOfCenters)
def idBlocks(robotIP, motionProxy, postureProxy, eBlocks):
# look straight ahead and down .3 radians (from standard position)
postureProxy.goToPosture("StandInit", 0.5)
motionProxy.angleInterpolation(["HeadPitch"], [0.3], [1], False)
# identify blocks that are immediately recognizable
listOfShapesDetected = shapeDetectionModuleInterface.shapeDetectionMain(
robotIP, 9559)
orderedBlocks = []
for i in range(0, 9):
if (len(listOfShapesDetected[i].getCenters()) > 0):
o = orderedList(listOfPiecesLabels[i],
listOfShapesDetected[i].getCenters()[0][0], 0)
o.printIt()
def detect(robotIP,motionProxy,shape,index):
motionProxy.setStiffnesses("Head", 1.0)
motionProxy.angleInterpolationWithSpeed(["HeadPitch"],[0.2],0.4)
listOfShapesDetected = shapeDetectionModuleInterface.shapeDetectionMain(robotIP, 9559)
count = 0
while (len(listOfShapesDetected[listOfPiecesLabels.index(shape)].getCenters()) == 0):
print "Look up"
motionProxy.angleInterpolationWithSpeed(["HeadPitch"],[0.2],0.4)
listOfShapesDetected = shapeDetectionModuleInterface.shapeDetectionMain(robotIP, 9559)
if (len(listOfShapesDetected[listOfPiecesLabels.index(shape)].getCenters()) == 0):
print "failed to detect desired block"
print "index = ", index
if (count == 0):
if (index < 4):
motionProxy.moveTo(0.0, 0.06, 0.0);
else:
motionProxy.moveTo(0.0, -0.06, 0.0);
elif (count == 1):
#if (index < 4):
#motionProxy.angleInterpolation(["HeadYaw"],[-0.8 ],[1 ],False)
motionProxy.moveTo(-0.03, 0.0, 0.0);
#else:
#motionProxy.angleInterpolation(["HeadYaw"],[0.8 ],[1 ],False)
# motionProxy.moveTo(-0.03, -0.0, 0.0);
elif (count == 2):
motionProxy.angleInterpolation(["HeadYaw"],[0.4],[1],False)
motionProxy.moveTo(-.04, 0.0, 0.0)# walk back
count = -1
else:
print "i give up"
break
count += 1
motionProxy.angleInterpolation(["HeadPitch"],[0.3],[1],False)
listOfShapesDetected = shapeDetectionModuleInterface.shapeDetectionMain(robotIP, 9559)
x0,y0=listOfShapesDetected[listOfPiecesLabels.index(shape)].getCenters()[0]
result = motionProxy.getPosition(currentCamera, motion.FRAME_ROBOT, True)
P0=[0,0,h]
X_c=result[0:3]
#y_l=math.tan(math.radians(-ALPHA))
y_h=math.tan(math.radians(ALPHA))
#z_l=math.tan(math.radians(-BETA))
z_h=math.tan(math.radians(BETA))
# w_y=((320.0-x0)/320.0)*y_h
# w_z=((240.0-y0)/240.0)*z_h
#lower resolution
w_y=((160.0-x0)/160.0)*y_h
w_z=((120.0-y0)/120.0)*z_h
w=[1,w_y,w_z]
wx=-result[3]#changed to minus
wy=result[4]
wz=result[5]
R_x = np.matrix([[1,0, 0],[0, math.cos(wx), -math.sin(wx)], [0, math.sin(wx),math.cos(wx)]])
R_y = np.matrix([[math.cos(wy),0, math.sin(wy)],[0, 1, 0], [-math.sin(wy), 0,math.cos(wy)]])
R_z = np.matrix([[math.cos(wz),-math.sin(wz),0],[math.sin(wz),math.cos(wz),0],[0, 0, 1]])
wtrash = np.matrix(w)
primero = R_y * wtrash.reshape((3,1))
segundo = R_x * primero
tercero = R_z * segundo
w_prime = tercero.reshape((1,3))
w=w_prime.tolist()[0]
n=[0,0,1]#[-math.sin(theta_c),0,math.cos(theta_c)]
diff=list(p-q for p,q in zip(P0,X_c))
t= sum(p*q for p,q in zip(diff, n))/sum(p*q for p,q in zip(w, n))
X_prime= list(p+q for p,q in zip(X_c,list(t*q for q in w)))
X_x=X_prime[0]
X_y=X_prime[1]
X_z=X_prime[2]
X=[X_x,X_y,X_z]
print "Target: ", X[0], ", ", X[1], ", ", X[2]
if (X[0] < 0.0):
print "something went terribly wrong"
detect(robotIP,motionProxy,shape,index)
#print "Calculated x " + str(X[0]) + " (in meters)"
#print "Calculated y " + str(X[1]) + " (in meters)"
#print "Calculated z " + str(X[2]) + " (in meters)"
return tuple(X)
def takeScan(robotIP,motionProxy,postureProxy,expectedBlocks,foundBlocks,hpitch,hyaw):
newInitPos_sinCabeza(motionProxy)
motionProxy.setAngles(["HeadPitch","HeadYaw"],[hpitch, hyaw],0.2)
time.sleep(2)
marginx = 30
# identify blocks that are immediately recognizable
listOfShapesDetected = shapeDetectionModuleInterface.shapeDetectionMain(robotIP, 9559)
orderedBlocks = []
for i in range(0,len(foundBlocks)):
orderedBlocks.insert(i,foundBlocks[i])
cameraAngle = 0
if (hyaw > 0.0):
cameraAngle = -1
elif (hyaw < -0.0):
cameraAngle = 1
for i in range(0,9):
if (len(listOfShapesDetected[i].getCenters()) > 0):
if ((listOfShapesDetected[i].getCenters()[0][0] > marginx) and (listOfShapesDetected[i].getCenters()[0][0] < 640-marginx)):
ya = False
for j in range(0,len(foundBlocks)):
if (foundBlocks[j].obj == listOfPiecesLabels[i]):
ya = True
break
if (not ya):
o = orderedList(listOfPiecesLabels[i],listOfShapesDetected[i].getCenters()[0][0],cameraAngle)
orderedBlocks = addIt(orderedBlocks,o)
# verify that the identified blocks are all in expectedBlocks, otherwise we have a detection error
errore = False
index = -1
for i in range(0,len(orderedBlocks)):
if (not foundIn(expectedBlocks,orderedBlocks[i].obj)):
index = i
errore = True
break
if errore:
print "I detected a piece I should not have detected:"
#for blocks in orderedBlocks:
# print blocks.obj
print "Detected: ", orderedBlocks[index].obj
print "I'm going to rescan after repositioning myself"
print "CHANGE THIS TO SEE IF WE CAN SCAN *FIRST* INSTEAD OF MOVING"
countLeft = 0
for blocks in orderedBlocks:
countLeft += blocks.frame
amounty = random.choice([0.02,0.03,0.04])
amountx = random.choice([0.02,0.0,-0.03])
if (countLeft < 0):
motionProxy.moveTo(amountx, amounty, 0.0)
else:
motionProxy.moveTo(amountx, -amounty, 0.0)
orderedBlocks = []#takeScan(robotIP,motionProxy,postureProxy,expectedBlocks,foundBlocks,hpitch,hyaw)
return orderedBlocks
def identifyAvailableBlocks(robotIP,motionProxy,postureProxy,expectedBlockCount,myBlocks,hisBlocks,Search):
motionProxy.setStiffnesses("Head", 1.0)
motionProxy.angleInterpolation(
["HeadPitch"],
[0.3 ],
[1 ],
False
)
motionProxy.angleInterpolationWithSpeed("RElbowRoll",0.5,0.4)
motionProxy.angleInterpolationWithSpeed("LElbowRoll",0.5,0.4)
## listOfPiecesLabels=['red triangle','red square','red circle',
## 'blue triangle', 'blue square', 'blue circle',
## 'yellow triangle','yellow square','yellow circle']
listOfShapesDetected = shapeDetectionModuleInterface.shapeDetectionMain(robotIP, 9559)
blocks = []
for i in range(0,9):
if (len(listOfShapesDetected[i].getCenters()) > 0):
blocks.append(listOfPiecesLabels[i])
if (len(blocks) == expectedBlockCount):
print "we did it"
else:
count = 0
while (len(blocks) != expectedBlockCount):
print "we failed: ",len(blocks)
## print blocks
if (count == 0):
motionProxy.angleInterpolation(
["HeadYaw"],
[0.4 ],
[1 ],
False
)
elif (count == 1):
motionProxy.angleInterpolation(
["HeadYaw"],
[-0.8 ],
[1 ],
False
)
elif (count == 2):
motionProxy.angleInterpolation(
["HeadYaw"],
[0.4 ],
[1 ],
False
)
if (Search):
motionProxy.moveTo(-.03, 0.0, 0.0)# walk back
count = -1
else:
break
else:
## print "i give up"
break
count += 1
listOfShapesDetected = shapeDetectionModuleInterface.shapeDetectionMain(robotIP, 9559)
cur = len(blocks)
for i in range(0,9):
if (len(listOfShapesDetected[i].getCenters()) > 0):
found = False
for j in range(0,cur):
if (listOfPiecesLabels[i] == blocks[j]):
found = True
break
if (not found):
blocks.append(listOfPiecesLabels[i])
print "\n\nmyBlocks: ",myBlocks
print "hisBlocks: ",hisBlocks
print "remaining blocks: ",blocks
print "\n"
errore = False
for i in range(0,len(blocks)):
for j in range(0,len(myBlocks)):
if (blocks[i] == myBlocks[j]):
errore = True
break
for j in range(0,len(hisBlocks)):
if (blocks[i] == hisBlocks[j]):
errore = True
break
if (errore):
break
if errore:
print "caught an error; going to retry"
motionProxy.angleInterpolation(
["HeadPitch"],
[-0.3 ],
[1 ],
False
)
#postureProxy.goToPosture("StandInit", 0.5)
newInitPos(motionProxy)
blocks = identifyAvailableBlocks(robotIP,motionProxy,postureProxy,expectedBlockCount,myBlocks,hisBlocks,Search)
## print blocks
return blocks
print "Camera position: FRAME_ROBOT"
print result
#Coordinates of center in pixels
#dict = shapeDetectionMain.mainModule(robotIP,PORT)#load_obj("dictOfCenters")
# x0= dict["blue"]["triangle"][0][0]
# y0= dict["blue"]["triangle"][0][1]
listOfPiecesLabels = [
'red triangle', 'red square', 'red circle', 'blue triangle',
'blue square', 'blue circle', 'yellow triangle', 'yellow square',
'yellow circle'
]
listOfShapesDetected = shapeDetectionModuleInterface.shapeDetectionMain(
"10.104.67.182", 9559)
shapeName = "blue triangle"
x0, y0 = listOfShapesDetected[listOfPiecesLabels.index(
shapeName)].getCenters()[0]
# x0= dict["blue"]["circle"][0][0]
# y0= dict["blue"]["circle"][0][1]
P0 = [0, 0, h]
theta_c = result[4]
print "THETA:" + str(theta_c)
X_c = result[
0:
3] #[result[0]-robotPosition_x0, result[1]-robotPosition_y0, result[2]]#
print "Camera position: FRAME_ROBOT"
print result
#Coordinates of center in pixels
#dict = shapeDetectionMain.mainModule(robotIP,PORT)#load_obj("dictOfCenters")
# x0= dict["blue"]["triangle"][0][0]
# y0= dict["blue"]["triangle"][0][1]
listOfPiecesLabels = [
'red triangle', 'red square', 'red circle', 'blue triangle',
'blue square', 'blue circle', 'yellow triangle', 'yellow square',
'yellow circle'
]
listOfShapesDetected = shapeDetectionModuleInterface.shapeDetectionMain(
"vahan.local", 9559)
shapeName = "blue triangle"
x0, y0 = listOfShapesDetected[listOfPiecesLabels.index(
shapeName)].getCenters()[0]
# x0= dict["blue"]["circle"][0][0]
# y0= dict["blue"]["circle"][0][1]
P0 = [0, 0, h]
theta_c = result[4]
print "THETA:" + str(theta_c)
X_c = result[
0:
3] #[result[0]-robotPosition_x0, result[1]-robotPosition_y0, result[2]]#
def detect(robotIP, motionProxy, postureProxy, shape):
motionProxy.setStiffnesses("Head", 1.0)
motionProxy.angleInterpolation(["HeadPitch"], [0.3], [1], False)
useSensors = False
robotPosition = almath.Pose2D(motionProxy.getRobotPosition(useSensors))
nextRobotPosition = almath.Pose2D(motionProxy.getNextRobotPosition())
position = robotPosition.toVector()
robotPosition_x0 = position[0]
robotPosition_y0 = position[1]
robotPosition_theta0 = position[2]
space = motion.FRAME_ROBOT
useSensorValues = True
result = motionProxy.getPosition(currentCamera, space, useSensorValues)
## listOfPiecesLabels=['red triangle','red square','red circle',
## 'blue triangle', 'blue square', 'blue circle',
## 'yellow triangle','yellow square','yellow circle']
## motionProxy.angleInterpolationWithSpeed("RElbowRoll",0.5,0.4)
## motionProxy.angleInterpolationWithSpeed("LElbowRoll",0.5,0.4)
listOfShapesDetected = shapeDetectionModuleInterface.shapeDetectionMain(
robotIP, 9559)
count = 0
while (len(listOfShapesDetected[listOfPiecesLabels.index(
shape)].getCenters()) == 0):
if (count == 0):
print "head to left"
motionProxy.angleInterpolation(["HeadYaw"], [0.4], [1], False)
elif (count == 1):
print "head to right"
motionProxy.angleInterpolation(["HeadYaw"], [-0.8], [1], False)
elif (count == 2):
print "back up"
motionProxy.angleInterpolation(["HeadYaw"], [0.4], [1], False)
motionProxy.moveTo(-.04, 0.0, 0.0) # walk back
count = -1
else:
print "i give up"
break
count += 1
listOfShapesDetected = shapeDetectionModuleInterface.shapeDetectionMain(
robotIP, 9559)
useSensors = False
robotPosition = almath.Pose2D(motionProxy.getRobotPosition(useSensors))
nextRobotPosition = almath.Pose2D(motionProxy.getNextRobotPosition())
position = robotPosition.toVector()
robotPosition_x0 = position[0]
robotPosition_y0 = position[1]
robotPosition_theta0 = position[2]
space = motion.FRAME_ROBOT
useSensorValues = True
result = motionProxy.getPosition(currentCamera, space, useSensorValues)
x0, y0 = listOfShapesDetected[listOfPiecesLabels.index(
shape)].getCenters()[0]
P0 = [0, 0, h]
theta_c = result[4]
X_c = result[
0:
3] #[result[0]-robotPosition_x0, result[1]-robotPosition_y0, result[2]]#
y_l = math.tan(math.radians(-ALPHA))
y_h = math.tan(math.radians(ALPHA))
z_l = math.tan(math.radians(-BETA))
z_h = math.tan(math.radians(BETA))
w_y = ((320.0 - x0) / 320.0) * y_h
w_z = ((240.0 - y0) / 240.0) * z_h
w = [1, w_y, w_z]
wx = -result[3] #changed to minus
wy = result[4]
wz = result[5]
R_x = np.matrix([[1, 0, 0], [0, math.cos(wx), -math.sin(wx)],
[0, math.sin(wx), math.cos(wx)]])
R_y = np.matrix([[math.cos(wy), 0, math.sin(wy)], [0, 1, 0],
[-math.sin(wy), 0, math.cos(wy)]])
R_z = np.matrix([[math.cos(wz), -math.sin(wz), 0],
[math.sin(wz), math.cos(wz), 0], [0, 0, 1]])
wtrash = np.matrix(w)
primero = R_y * wtrash.reshape((3, 1))
segundo = R_x * primero
tercero = R_z * segundo
w_prime = tercero.reshape((1, 3))
w = w_prime.tolist()[0]
n = [0, 0, 1] #[-math.sin(theta_c),0,math.cos(theta_c)]
diff = list(p - q for p, q in zip(P0, X_c))
t = sum(p * q for p, q in zip(diff, n)) / sum(p * q for p, q in zip(w, n))
X_prime = list(p + q for p, q in zip(X_c, list(t * q for q in w)))
X_x = X_prime[0]
X_y = X_prime[1]
X_z = X_prime[2]
X = [X_x, X_y, X_z]
print "Calculated x " + str(X[0]) + " (in meters)"
print "Calculated y " + str(X[1]) + " (in meters)"
print "Calculated z " + str(X[2]) + " (in meters)"
return tuple(X)
