class Robot(object):
def __init__(self, robot_id):
self.robot = DroidClient()
connected = self.robot.connect_to_droid(robot_id)
while not connected:
connected = self.robot.connect_to_droid(robot_id)
def roll(self, heading):
mapping = {"up": 0, "right": 90, "down": 180, "left": 270}
self.robot.roll(0, mapping.get(heading), 0)
time.sleep(0.35)
self.robot.roll(1, mapping.get(heading), 0.62)
def reset(self):
self.robot.roll(0, 0, 0)
def disconnect(self):
self.robot.disconnect()
class Robot:
def __init__(self, droidID, wordSimilarityCutoff, voice):
self.createSentenceEmbeddings()
self.droid = DroidClient()
self.name = "R2"
self.wordSimilarityCutoff = wordSimilarityCutoff
self.holoProjectorIntensity = 0
self.logicDisplayIntensity = 0
self.frontRGB = (0, 0, 0)
self.backRGB = (0, 0, 0)
self.voice = voice
self.grid = [[]]
self.speed = 0.5
self.pos = (-1, -1)
self.colorToRGB = {}
with open(path + 'data/colors.csv') as csvfile:
readCSV = csv.reader(csvfile, delimiter=',')
for row in readCSV:
self.colorToRGB[row[0]] = (int(row[2]), int(row[3]),
int(row[4]))
connected = self.droid.connect_to_droid(droidID)
while not connected:
connected = self.droid.connect_to_droid(droidID)
def createSentenceEmbeddings(self):
self.categories = [
"state", "direction", "light", "animation", "head", "grid"
]
trainingSentences = loadTrainingSentences(
path + "data/r2d2TrainingSentences.txt")
self.indexToTrainingSentence = {}
i = 0
for category in self.categories:
sentences = trainingSentences[category + "Sentences"]
for sentence in sentences:
self.indexToTrainingSentence[i] = (sentence, category)
i += 1
self.sentenceEmbeddings = np.zeros(
(len(self.indexToTrainingSentence), vectors.dim))
for i in range(len(self.indexToTrainingSentence)):
sentence = self.indexToTrainingSentence[i][0]
sentenceEmbedding = calcPhraseEmbedding(sentence)
self.sentenceEmbeddings[i, :] = sentenceEmbedding
def inputCommand(self, command):
commandEmbedding = calcPhraseEmbedding(command)
closestSentences = rankSentences(commandEmbedding,
self.sentenceEmbeddings)
# print(self.indexToTrainingSentence[closestSentences[0]][0])
# print(self.indexToTrainingSentence[closestSentences[1]][0])
# print(self.indexToTrainingSentence[closestSentences[2]][0])
# print(self.indexToTrainingSentence[closestSentences[3]][0])
# print(self.indexToTrainingSentence[closestSentences[4]][0])
print("Closet sentence was: " +
self.indexToTrainingSentence[closestSentences[0]][0])
print("Its cosine similarity to the command was: " + str(
cosineSim(commandEmbedding, self.sentenceEmbeddings[
closestSentences[0], :])))
if cosineSim(commandEmbedding, self.sentenceEmbeddings[
closestSentences[0], :]) < 0.84 and not self.voice:
subcommand = input(
self.name +
": I could not understand your command. Do you want to add this command to the training set? (yes/no): "
)
if "yes" in subcommand.lower():
subcommand = input(
"What category do you want to add it to? Choices are state, direction, light, animation, head, or grid: "
)
subcommand = subcommand.lower()
if subcommand in self.categories:
with open(path + "data/r2d2TrainingSentences.txt",
'a') as the_file:
the_file.write(subcommand + 'Sentences :: ' + command +
'\n')
print("Command added. Changes will be present on restart.")
else:
print(subcommand + " not a valid category.")
return
commandType = getCommandType(self.categories, closestSentences,
self.indexToTrainingSentence)
result = getattr(self, commandType + "Parser")(command.lower())
if result:
print(self.name + ": Done executing " + commandType + " command.")
else:
print(self.name + ": I could not understand your " + commandType +
" command.")
def reset(self):
self.droid.roll(0, 0, 0)
def disconnect(self):
self.droid.disconnect()
def flash_colors(self, colors, seconds=1, front=True):
if front:
for color in colors:
self.droid.set_front_LED_color(*color)
time.sleep(seconds)
else:
for color in colors:
self.droid.set_back_LED_color(*color)
time.sleep(seconds)
def askForColor(self, lightPosition="both"):
if lightPosition != "both":
print("We detect that you want to change your " + lightPosition +
" light, but could not find a color.")
else:
print(
"We parsed this as a light command, but could not find a color."
)
command = input("Do you want to input a color? (yes/no): ")
color = False
if "yes" in command.lower():
print(
"You may have inputted a color, but it is not in our database or is mispelled. Please input a color or rgb tuple."
)
command = input(
"If you want to add the color to the database, input color_name (one string) :: rgb tuple: "
)
words = re.split('\W+', command)
words = [x for x in words if x != ""]
for word in words:
if word in self.colorToRGB: color = self.colorToRGB[word]
if len(words) == 4:
try:
color = (int(words[1]), int(words[2]), int(words[3]))
colorName = words[0]
with open(path + 'data/colors.csv', 'a') as csvStorer:
csvStorer.write('\n' + colorName + ',R2D2 ' +
colorName + ',' + words[1] + ',' +
words[2] + ',' + words[3])
print(
colorName +
" added to database. It will be available on the next restart."
)
except ValueError:
superDumbVariable = 1
elif len(words) == 3:
try:
color = (int(words[0]), int(words[1]), int(words[2]))
except ValueError:
superDumbVariable = 1
return color
def lightParser(self, command):
# slot filler for lights
slots = {
"holoEmit": False,
"logDisp": False,
"lights": [],
"add": False,
"sub": False,
"percent": False,
"whichRGB": [],
"colors": [],
"intensities": [],
"rgb": False,
"increment/seconds": False
}
if "holoemitter" in command or "holo emitter" in command:
slots["holoEmit"] = True
if "logic display" in command:
slots["logDisp"] = True
if "dim" in command:
slots["intensities"].append("dim")
if "blink" in command:
slots["intensities"].append("blink")
if "%" in command:
slots["percent"] = True
# WANT TO MAKE INCREASE BETTER
if "increase" in command or "add" in command:
slots["add"] = True
if "decrease" in command or "reduce" in command or "subtract" in command:
slots["sub"] = True
# front back too similar
if "back" in command:
slots["lights"].append("back")
if "front" in command:
slots["lights"].append("front")
if slots["lights"] == []:
slots["lights"] = ["front", "back"]
if "red" in command:
slots["whichRGB"].append("red")
if "green" in command:
slots["whichRGB"].append("green")
if "blue" in command:
slots["whichRGB"].append("blue")
words = re.split('\W+', command)
words = [x for x in words if x != ""]
i = 0
for word in words:
if i < len(words) - 2:
try:
slots["rgb"] = (int(words[i]), int(words[i + 1]),
int(words[i + 2]))
except ValueError:
superDumbVariable = True
if vectors.similarity(
"off",
word) > self.wordSimilarityCutoff or "minimum" in command:
slots["intensities"].append("off")
elif vectors.similarity(
"on",
word) > self.wordSimilarityCutoff or vectors.similarity(
"maximum", word) > self.wordSimilarityCutoff:
slots["intensities"].append("on")
if vectors.similarity("percent", word) > self.wordSimilarityCutoff:
slots["percent"] = True
if word in self.colorToRGB:
slots["colors"].append(self.colorToRGB[word])
i += 1
try:
increment = int(word)
slots["increment/seconds"] = increment
except ValueError:
continue
return self.lightSlotsToActions(slots)
def lightSlotsToActions(self, slots):
if slots["holoEmit"]:
if "off" in slots["intensities"]:
self.holoProjectorIntensity = 0
self.droid.set_holo_projector_intensity(
self.holoProjectorIntensity)
elif "dim" in slots["intensities"]:
self.holoProjectorIntensity = self.holoProjectorIntensity / 2
self.droid.set_holo_projector_intensity(
self.holoProjectorIntensity)
elif "on" in slots["intensities"]:
self.holoProjectorIntensity = 1
self.droid.set_holo_projector_intensity(
self.holoProjectorIntensity)
elif "blink" in slots["intensities"]:
self.droid.set_holo_projector_intensity(
(self.holoProjectorIntensity + 1) % 2)
time.sleep(0.3)
self.droid.set_holo_projector_intensity(
self.holoProjectorIntensity)
else:
return False
return True
if slots["logDisp"]:
if "off" in slots["intensities"]:
self.logicDisplayIntensity = 0
self.droid.set_logic_display_intensity(
self.logicDisplayIntensity)
elif "dim" in slots["intensities"]:
self.logicDisplayIntensity = self.logicDisplayIntensity / 2
self.droid.set_logic_display_intensity(
self.logicDisplayIntensity)
elif "on" in slots["intensities"]:
self.logicDisplayIntensity = 1
self.droid.set_logic_display_intensity(
self.logicDisplayIntensity)
elif "blink" in slots["intensities"]:
self.droid.set_logic_display_intensity(
(self.logicDisplayIntensity + 1) % 2)
time.sleep(0.3)
self.droid.set_logic_display_intensity(
self.logicDisplayIntensity)
else:
return False
return True
if (slots["add"] or slots["sub"]) and (slots["percent"]):
lights = slots["lights"]
if not slots["increment/seconds"]:
command = input(
"Percent not found in command, please input percent to change by here: "
)
try:
command = command.replace("%", "")
slots["increment/seconds"] = int(command)
except ValueError:
return False
if slots["sub"]:
slots["increment/seconds"] = -slots["increment/seconds"]
percent = slots["increment/seconds"]
if len(slots["whichRGB"]) == 0:
command = input(
"Did not find what values (red/blue/green) to change, input what values to change: "
)
if "red" in command: slots["whichRGB"].append("red")
if "green" in command: slots["whichRGB"].append("green")
if "blue" in command: slots["whichRGB"].append("blue")
if len(slots["whichRGB"]) == 0: return False
if "red" in slots["whichRGB"]:
for light in lights:
rgb = getattr(self, light + "RGB")
setattr(self, light + "RGB",
(max(0, min(rgb[0] + rgb[0] * percent / 100,
255)), rgb[1], rgb[2]))
getattr(self.droid, "set_" + light +
"_LED_color")(*getattr(self, light + "RGB"))
if "green" in slots["whichRGB"]:
for light in lights:
rgb = getattr(self, light + "RGB")
setattr(self, light + "RGB",
(rgb[0],
max(0, min(rgb[1] + rgb[1] * percent / 100,
255)), rgb[2]))
getattr(self.droid, "set_" + light +
"_LED_color")(*getattr(self, light + "RGB"))
if "blue" in slots["whichRGB"]:
for light in lights:
rgb = getattr(self, light + "RGB")
setattr(
self, light + "RGB",
(rgb[0], rgb[1],
max(0, min(rgb[2] + rgb[2] * percent / 100, 255))))
getattr(self.droid, "set_" + light +
"_LED_color")(*getattr(self, light + "RGB"))
return True
if slots["add"] or slots["sub"]:
lights = slots["lights"]
if not slots["increment/seconds"]:
command = input(
"Increment not found in command, please input amount to change by here: "
)
try:
slots["increment/seconds"] = int(command)
except ValueError:
return False
if slots["sub"]:
slots["increment/seconds"] = -slots["increment/seconds"]
increaseValue = slots["increment/seconds"]
if len(slots["whichRGB"]) == 0:
command = input(
"Did not find what values (red/blue/green) to change, input what values to change: "
)
if "red" in command: slots["whichRGB"].append("red")
if "green" in command: slots["whichRGB"].append("green")
if "blue" in command: slots["whichRGB"].append("blue")
if len(slots["whichRGB"]) == 0: return False
if "red" in slots["whichRGB"]:
for light in lights:
rgb = getattr(self, light + "RGB")
setattr(self, light + "RGB", (max(
0, min(rgb[0] + increaseValue, 255)), rgb[1], rgb[2]))
getattr(self.droid, "set_" + light +
"_LED_color")(*getattr(self, light + "RGB"))
if "green" in slots["whichRGB"]:
for light in lights:
rgb = getattr(self, light + "RGB")
setattr(self, light + "RGB",
(rgb[0], max(0, min(rgb[1] + increaseValue,
255)), rgb[2]))
getattr(self.droid, "set_" + light +
"_LED_color")(*getattr(self, light + "RGB"))
if "blue" in slots["whichRGB"]:
for light in lights:
rgb = getattr(self, light + "RGB")
setattr(self, light + "RGB",
(rgb[0], rgb[1],
max(0, min(rgb[2] + increaseValue, 255))))
getattr(self.droid, "set_" + light +
"_LED_color")(*getattr(self, light + "RGB"))
return True
askedForColor = False
if "back" in slots["lights"] and len(slots["lights"]) == 1:
if len(slots["colors"]) > 1:
seconds = slots["increment/seconds"]
if not seconds: seconds = 1
self.flash_colors(slots["colors"], seconds, False)
elif len(slots["colors"]) == 1:
self.backRGB = slots["colors"][0]
else:
if not slots["rgb"]:
color = self.askForColor("back")
askedForColor = True
if not color: return False
self.backRGB = color
else:
self.backRGB = slots["rgb"]
self.droid.set_back_LED_color(*self.backRGB)
return True
if ("front" in slots["lights"]
and len(slots["lights"]) == 1) or len(slots["colors"]) > 1:
if len(slots["colors"]) > 1:
seconds = slots["increment/seconds"]
if not seconds: seconds = 1
self.flash_colors(slots["colors"], seconds)
elif len(slots["colors"]) == 1:
self.frontRGB = slots["colors"][0]
else:
if not slots["rgb"]:
color = self.askForColor("front")
askedForColor = True
if not color: return False
self.frontRGB = color
else:
self.frontRGB = slots["rgb"]
self.droid.set_front_LED_color(*self.frontRGB)
return True
if len(slots["colors"]) == 1:
self.backRGB = slots["colors"][0]
self.frontRGB = slots["colors"][0]
self.droid.set_back_LED_color(*self.backRGB)
self.droid.set_front_LED_color(*self.frontRGB)
return True
if len(slots["colors"]) == 0:
if slots["rgb"]:
self.backRGB = slots["rgb"]
self.frontRGB = slots["rgb"]
self.droid.set_back_LED_color(*self.backRGB)
self.droid.set_front_LED_color(*self.frontRGB)
return True
if "off" in slots["intensities"]:
self.holoProjectorIntensity = 0
self.droid.set_holo_projector_intensity(
self.holoProjectorIntensity)
self.logicDisplayIntensity = 0
self.droid.set_logic_display_intensity(self.logicDisplayIntensity)
self.backRGB = (0, 0, 0)
self.frontRGB = (0, 0, 0)
self.droid.set_back_LED_color(*self.backRGB)
self.droid.set_front_LED_color(*self.frontRGB)
return True
elif "dim" in slots["intensities"]:
self.holoProjectorIntensity = 0
self.droid.set_holo_projector_intensity(
self.holoProjectorIntensity)
self.logicDisplayIntensity = 0
self.droid.set_logic_display_intensity(self.logicDisplayIntensity)
self.backRGB = tuple(x / 2 for x in self.backRGB)
self.frontRGB = tuple(x / 2 for x in self.frontRGB)
self.droid.set_back_LED_color(*self.backRGB)
self.droid.set_front_LED_color(*self.frontRGB)
return True
elif "on" in slots["intensities"]:
self.holoProjectorIntensity = 1
self.droid.set_holo_projector_intensity(
self.holoProjectorIntensity)
self.logicDisplayIntensity = 1
self.droid.set_logic_display_intensity(self.logicDisplayIntensity)
return True
elif "blink" in slots["intensities"]:
self.droid.set_holo_projector_intensity(
(self.holoProjectorIntensity + 1) % 2)
self.droid.set_logic_display_intensity(
(self.holoProjectorIntensity + 1) % 2)
time.sleep(0.3)
self.droid.set_holo_projector_intensity(
self.holoProjectorIntensity)
self.droid.set_logic_display_intensity(self.logicDisplayIntensity)
return True
if not slots["rgb"] and not askedForColor:
color = self.askForColor()
if color:
self.backRGB = color
self.frontRGB = color
self.droid.set_back_LED_color(*self.backRGB)
self.droid.set_front_LED_color(*self.frontRGB)
return True
return False
def directionParser(self, command):
if re.search(r"\b(circle|donut)\b", command, re.I):
if re.search(r"\b(counter)\b", command, re.I):
for heading in range(360, 0, -30):
self.droid.roll(self.speed, heading % 360, 0.6)
else:
for heading in range(0, 360, 30):
self.droid.roll(self.speed, heading, 0.6)
self.droid.roll(0, 0, 0)
return True
elif re.search(r"\b(square)\b", command, re.I):
if re.search(r"\b(counter)\b", command, re.I):
for heading in range(360, 0, -90):
self.droid.roll(0, heading % 360, 0)
time.sleep(0.35)
self.droid.roll(self.speed, heading % 360, 0.6)
else:
for heading in range(0, 360, 90):
self.droid.roll(0, heading, 0)
time.sleep(0.35)
self.droid.roll(self.speed, heading, 0.6)
self.droid.roll(0, 0, 0)
return True
elif re.search(r"\b(speed|faster|slower|slow)\b", command, re.I):
if re.search(r"\b(increase|faster)\b", command, re.I):
self.speed += 0.25
else:
self.speed -= 0.25
self.droid.animate(1)
return True
else:
flag = False
tokens = re.split("[^a-zA-Z]", command)
for token in tokens:
if token in {"up", "forward", "ahead", "straight", "north"}:
self.droid.roll(0, 0, 0)
time.sleep(0.35)
self.droid.roll(self.speed, 0, 0.6)
flag = True
elif token in {"down", "back", "south"}:
self.droid.roll(0, 180, 0)
time.sleep(0.35)
self.droid.roll(self.speed, 180, 0.6)
flag = True
elif token in {"left", "west"}:
self.droid.roll(0, 270, 0)
time.sleep(0.35)
self.droid.roll(self.speed, 270, 0.6)
flag = True
elif token in {"right", "east"}:
self.droid.roll(0, 90, 0)
time.sleep(0.35)
self.droid.roll(self.speed, 90, 0.6)
flag = True
self.droid.roll(0, 0, 0)
return flag
def animationParser(self, command):
if re.search(r"\b(dance|move|moves)\b", command, re.I):
self.droid.animate(3)
return True
elif re.search(r"\b(sing|sound|sounds|noise|noises)\b", command, re.I):
self.droid.play_sound(3)
return True
elif re.search(r"\b(fall)\b", command, re.I):
self.droid.animate(14)
return True
elif re.search(r"\b(scream)\b", command, re.I):
self.droid.play_sound(7)
return True
return False
def headParser(self, command):
if re.search(r"\b(left)\b", command, re.I):
self.droid.rotate_head(-90)
return True
elif re.search(r"\b(right)\b", command, re.I):
self.droid.rotate_head(90)
return True
elif re.search(r"\b(behind|back)\b", command, re.I):
self.droid.rotate_head(180)
return True
elif re.search(r"\b(forward|ahead)\b", command, re.I):
self.droid.rotate_head(0)
return True
return False
def gridParser(self, command):
if re.search("\d+ (x|by) \d+", command):
print
arr = command.split()
ind = -1
for i in range(len(arr)):
if arr[i] == "x" or arr[i] == "by":
ind = i
if ind != -1 and ind < len(arr) - 1:
x = int(arr[ind - 1])
y = int(arr[ind + 1])
self.grid = [["" for col in range(y)] for row in range(x)]
self.droid.animate(1)
print(self.grid)
return True
elif re.search("there is .+ at [(]?\d+,[ ]?\d+", command):
temp = re.split("[^a-zA-Z0-9]", command)
arr = []
for x in temp:
if x != "":
arr.append(x)
ind1 = -1
ind2 = -1
for i in range(len(arr)):
if arr[i] == "is":
ind1 = i
elif arr[i].isdigit():
ind2 = i
break
if ind1 != -1 and ind2 != -1:
if int(arr[ind2]) < 0 or int(arr[ind2]) >= len(
self.grid) or int(arr[ind2 + 1]) < 0 or int(
arr[ind2 + 1]) >= len(self.grid[0]):
self.droid.play_sound(7)
return False
else:
self.grid[int(arr[ind2])][int(arr[ind2 + 1])] = arr[ind1 +
1]
print(self.grid)
return True
elif re.search("go to [(]?\d+,[ ]?\d+", command):
temp = re.split("[^a-zA-Z0-9]", command)
arr = []
for x in temp:
if x != "":
arr.append(x)
ind = -1
for i in range(len(arr)):
if arr[i].isdigit():
ind = i
break
if ind != -1:
if int(arr[ind]) < 0 or int(arr[ind]) >= len(self.grid) or int(
arr[ind + 1]) < 0 or int(arr[ind + 1]) >= len(
self.grid[0]):
self.droid.play_sound(7)
return False
else:
target = (int(arr[ind]), int(arr[ind + 1]))
G = Graph(self.grid)
moves = A_star(G, self.pos, target,
manhattan_distance_heuristic)
print("**************")
print(moves)
print("**************")
for i in range(1, len(moves)):
if moves[i][1] > moves[i - 1][1]:
self.droid.roll(0, 0, 0)
time.sleep(0.35)
self.droid.roll(1, 0, 0.62)
elif moves[i][1] < moves[i - 1][1]:
self.droid.roll(0, 180, 0)
time.sleep(0.35)
self.droid.roll(1, 180, 0.62)
elif moves[i][0] > moves[i - 1][0]:
self.droid.roll(0, 90, 0)
time.sleep(0.35)
self.droid.roll(1, 90, 0.62)
elif moves[i][0] < moves[i - 1][0]:
self.droid.roll(0, 270, 0)
time.sleep(0.35)
self.droid.roll(1, 270, 0.62)
self.pos = moves[i]
if self.pos == target:
break
self.reset()
self.pos = target
return True
elif re.search("you are at [(]?\d+,[ ]?\d+", command):
temp = re.split("[^a-zA-Z0-9]", command)
arr = []
for x in temp:
if x != "":
arr.append(x)
ind = -1
for i in range(len(arr)):
if arr[i].isdigit():
ind = i
break
if ind != -1:
if int(arr[ind]) < 0 or int(arr[ind]) >= len(self.grid) or int(
arr[ind + 1]) < 0 or int(arr[ind + 1]) >= len(
self.grid[0]):
self.droid.play_sound(7)
return False
else:
print(int(arr[ind]), int(arr[ind + 1]))
self.pos = (int(arr[ind]), int(arr[ind + 1]))
print(self.pos)
return True
return False
def stateParser(self, command):
if re.search(r"\b(color)\b", command, re.I):
if re.search(r"\b(front|forward)\b", command, re.I):
print("***************")
print(self.frontRGB)
print("***************")
elif re.search(r"\b(back|rear)\b", command, re.I):
print("***************")
print(self.backRGB)
print("***************")
else:
print("***************")
print(self.frontRGB)
print(self.backRGB)
print("***************")
return True
elif re.search(r"\b(name)\b", command, re.I):
print("***************")
print(self.name)
print("***************")
return True
elif re.search(r"\b(power|battery)\b", command, re.I):
print("***************")
self.droid.battery()
print("***************")
return True
return False
iterations = 100
rotate_deg = {'up': 0, 'right': 90, 'down': 180, 'left': 270}
dir_speed = {'up': 0.09, 'right': 0.14, 'down': 0.095, 'left': 0.14}
# # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # #
# # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # #
mdp = MarkovDecisionProcess(grid, rewards)
policy_agent = PolicyAgent(mdp, discount, iterations)
currentState = policy_agent.getCurrentState()
speed = 0.14
roll_time = 2
for i in range(max_moves):
# droid.animate(2)
# get the action
real_act = policy_agent.walkOnGrid(
currentState) # this also internally updates self.currentState
if real_act == None:
break
# make the action
# if rotate deg is only related to face_direction
droid.roll(dir_speed[real_act], rotate_deg[real_act], roll_time)
# update state and direction
currentState = policy_agent.getCurrentState()
from client import DroidClient
from collision_reactions import collisionReaction
droid = DroidClient()
droid.connect_to_R2D2()
#check that the robot is connected
droid.animate(10)
droid.set_stance(2) # transition to bipod
#enable collisions, should beep when colliding with something
xThreshold = 50
yThreshold = 50
xSpeed = 50
ySpeed = 50
deadTime = 10
reaction = collisionReaction.BEEP
droid.enable_collision_detection(xThreshold, yThreshold, xSpeed, ySpeed,
deadTime, reaction)
droid.roll(0.5, 0, 3)
droid.sleep()
droid.quit()