def __init__(self, gui):
super().__init__()
# In order to get to the slider values
self.gui = gui
self.title = "Robot and Walls"
# Window size
self.top = 15
self.left = 15
self.width = 700
self.height = 700
# State/action text
self.sensor_text = "No sensor"
self.action_text = "No action"
# The world state
self.world_state = WorldState()
# For querying door
self.door_sensor = DoorSensor()
# For moving robot
self.robot_state = RobotState()
# For robot state estimation
self.state_estimation = RobotStateEstimation()
# Height of prob
self.draw_height = 0.6
# Set geometry
self.initUI()
def main():
# Door Sensor Object (Deals with API and stores the door status)
door_sensor = DoorSensor(Constants.group_id, Constants.door_sensor_id)
interval = 15 # Seconds
while True:
print(door_sensor.sense())
time.sleep(interval)
def __init__(self, gui_world):
super().__init__()
# In order to get to the slider values
self.gui = gui_world
self.title = "Robot and Walls"
# Window size
self.top = 15
self.left = 15
self.width = 700
self.height = 700
# State/action text
self.sensor_text = "No sensor"
self.action_text = "No action"
self.move_text = "No move"
self.loc_text = "No location"
# The world state
self.world_state = WorldState()
# For querying door
self.door_sensor = DoorSensor()
# For moving robot
self.robot_state = RobotState()
# For robot state estimation
self.state_estimation = RobotStateEstimation()
# For keeping sampled error
self.last_wall_sensor_noise = 0
self.last_move_noise = 0
# Height of prob
self.draw_height = 0.6
# Set geometry
self.text = "None"
self.init_ui()
class DrawRobotAndWalls(QWidget):
def __init__(self, gui_world):
super().__init__()
# In order to get to the slider values
self.gui = gui_world
self.title = "Robot and Walls"
# Window size
self.top = 15
self.left = 15
self.width = 700
self.height = 700
# State/action text
self.sensor_text = "No sensor"
self.action_text = "No action"
self.move_text = "No move"
self.loc_text = "No location"
# The world state
self.world_state = WorldState()
# For querying door
self.door_sensor = DoorSensor()
# For moving robot
self.robot_state = RobotState()
# For robot state estimation
self.state_estimation = RobotStateEstimation()
# For keeping sampled error
self.last_wall_sensor_noise = 0
self.last_move_noise = 0
# Height of prob
self.draw_height = 0.6
# Set geometry
self.text = "None"
self.init_ui()
def init_ui(self):
self.text = "Not reaching"
self.setWindowTitle(self.title)
self.setGeometry(self.left, self.top, self.width, self.height)
self.show()
# For making sure the window shows up the right size
def minimumSizeHint(self):
return QSize(self.width, self.height)
# For making sure the window shows up the right size
def sizeHint(self):
return QSize(self.width, self.height)
# What to draw
def paintEvent(self, event):
qp = QPainter()
qp.begin(self)
self.draw_robot(qp)
self.draw_wall(qp)
if self.gui.draw_kalman:
self.draw_robot_gauss(qp)
self.draw_move_gauss(qp)
else:
self.draw_probabilities(qp)
self.draw_wall_gauss(qp)
self.draw_world(qp)
self.draw_sensor_action(qp, event)
qp.end()
# Put some text in the bottom left
def draw_sensor_action(self, qp, _):
qp.setPen(QColor(168,34,3))
qp.setFont(QFont('Decorative', 10))
# Put sensor text in lower left...
text_loc = QPoint(self.x_map(0.01), self.y_map(self.draw_height))
qp.drawText(text_loc, self.sensor_text)
# .. action text in lower right
text_loc = QPoint(self.x_map(0.7), self.y_map(self.draw_height))
qp.drawText(text_loc, self.action_text)
# .. loc text in upper ;eft
text_loc = QPoint(self.x_map(0.01), self.y_map(self.draw_height + 0.025))
qp.drawText(text_loc, self.loc_text)
# .. move text in upper right
text_loc = QPoint(self.x_map(0.7), self.y_map(self.draw_height + 0.025))
qp.drawText(text_loc, self.move_text)
def draw_wall(self, qp):
pen = QPen(Qt.black, 4, Qt.SolidLine)
brush = QBrush(Qt.CrossPattern)
qp.setPen(pen)
qp.setBrush(brush)
qp.drawRect(self.x_map(0), self.y_map(1.0), self.x_map(1), self.in_pixels(0.2))
pen.setColor(Qt.red)
brush.setStyle(Qt.SolidPattern)
qp.setPen(pen)
qp.setBrush(brush)
door_width = self.world_state.door_width
for d in self.world_state.doors:
qp.drawRect(self.x_map( d - door_width/2 ), self.y_map(0.95), self.in_pixels(door_width), self.in_pixels(0.15))
def draw_probabilities(self, qp):
pen = QPen(Qt.black, 1, Qt.SolidLine)
qp.setPen(pen)
div = 1 / len(self.state_estimation.probabilities)
for i in range(1, len(self.state_estimation.probabilities)):
qp.drawLine(self.x_map(i*div), self.y_map(0.0), self.x_map(i*div), self.y_map(self.draw_height*0.8))
pen.setColor(Qt.blue)
qp.setPen(pen)
for i, p in enumerate(self.state_estimation.probabilities):
qp.drawLine(self.x_map(i * div), self.y_map(self.draw_height * p), self.x_map((i + 1) * div), self.y_map(self.draw_height * p))
def draw_world(self, qp):
pen = QPen(Qt.black, 3, Qt.SolidLine)
qp.setPen(pen)
qp.drawLine(self.x_map(0), self.y_map(0.0), self.x_map(1), self.y_map(0.0))
qp.drawLine(self.x_map(0), self.y_map(0.0), self.x_map(0), self.y_map(self.draw_height))
qp.drawLine(self.x_map(1), self.y_map(0.0), self.x_map(1), self.y_map(self.draw_height))
def draw_robot_gauss(self, qp):
pen = QPen(Qt.darkBlue, 1, Qt.DotLine)
qp.setPen(pen)
dx = np.linspace(0, 1, 200)
mu = self.state_estimation.mean
standard_deviation = self.state_estimation.standard_deviation
dy = [np.exp(-np.power(mu - x, 2.0) / (2 * np.power(standard_deviation, 2.0))) for x in dx]
pts = []
max_y = np.max(dy)
for x, y in zip(dx, dy):
pts.append(QPoint(self.x_map(x), self.y_map(0.5 * y*self.draw_height/max_y)))
for i in range(0, len(pts)-1):
qp.drawLine(pts[i], pts[i+1])
# Wall sensor distribution
def draw_wall_gauss(self, qp):
pen = QPen(Qt.gray, 1, Qt.DashLine)
qp.setPen(pen)
dx = np.linspace(0, 1, 200)
mu = self.robot_state.robot_loc
standard_deviation = self.world_state.wall_standard_deviation
dy = [np.exp(-np.power(mu - x, 2.0) / (2 * np.power(standard_deviation, 2.0))) for x in dx]
pts = []
max_y = np.max(dy)
height = 0.1 / max_y
for x, y in zip(dx, dy):
pts.append(QPoint(self.x_map(x), self.y_map(self.draw_height + 0.05 + y*height)))
for i in range(0, len(pts)-1):
qp.drawLine(pts[i], pts[i+1])
# Put a dashed line indicating last noise sample
pen.setColor(Qt.red)
pen.setWidth(1)
qp.setPen(pen)
qp.drawLine(QPoint(self.x_map(self.robot_state.robot_loc + self.last_wall_sensor_noise), self.y_map(self.draw_height + 0.1)),
QPoint(self.x_map(self.robot_state.robot_loc + self.last_wall_sensor_noise), self.y_map(self.draw_height + 0.045)))
# Wall sensor distribution
def draw_move_gauss(self, qp):
pen = QPen(Qt.gray, 1, Qt.DashLine)
qp.setPen(pen)
dx = np.linspace(0, 1, 200)
mu = self.robot_state.robot_loc
standard_deviation = self.robot_state.robot_move_standard_deviation_err
dy = [np.exp(-np.power(mu - x, 2.0) / (2 * np.power(standard_deviation, 2.0))) for x in dx]
pts = []
max_y = np.max(dy)
height = 0.2
for x, y in zip(dx, dy):
pts.append(QPoint(self.x_map(x), self.y_map(y*height/max_y)))
for i in range(0, len(pts)-1):
qp.drawLine(pts[i], pts[i+1])
# Put a dashed line indicating the last noise sample for move
pen.setColor(Qt.red)
pen.setWidth(1.5)
qp.setPen(pen)
qp.drawLine(QPoint(self.x_map(self.robot_state.robot_loc + self.last_move_noise), self.y_map(0)),
QPoint(self.x_map(self.robot_state.robot_loc + self.last_move_noise), self.y_map(0.075)))
def draw_robot(self, qp):
pen = QPen(Qt.darkMagenta, 2, Qt.SolidLine)
qp.setPen(pen)
x_i = self.x_map(self.robot_state.robot_loc)
y_i = self.y_map(0.09)
qp.drawLine(x_i-5, y_i, x_i+5, y_i)
qp.drawLine(x_i, y_i-5, x_i, y_i+5)
# Map from [0,1]x[0,1] to the width and height of the window
def x_map(self, x):
return int(x * self.width)
# Map from [0,1]x[0,1] to the width and height of the window - need to flip y
def y_map(self, y):
return self.height - int(y * self.height) - 1
def in_pixels(self, v):
return int(v * self.height)
def query_door_sensor(self):
front_door_yn = self.door_sensor.is_in_front_of_door(self.world_state, self.robot_state)
sensor_value = self.door_sensor.sensor_reading(self.world_state, self.robot_state)
self.sensor_text = "Sensor reading: {}, actual: {}".format(sensor_value, front_door_yn)
return sensor_value
# Sensor reading, distance to wall (Kalman filtering)
def update_gauss_sensor_reading(self, ws, dist_reading):
""" Update state estimation based on sensor reading
:param ws - for standard deviation of wall sensor
:param dist_reading - distance reading returned"""
# begin homework 3 : Problem 1
# end homework 3 : Problem 1
return self.mean, self.standard_deviation
if __name__ == '__main__':
ws_global = WorldState()
ds_global = DoorSensor()
rse_global = RobotStateEstimation()
# Check out these cases
# We have two possibilities - either in front of door, or not - cross two sensor readings
# saw door versus not saw door
uniform_prob = rse_global.probabilities[0]
# begin homework 2 problem 4
# Four cases - based on default door probabilities of
# DoorSensor.prob_see_door_if_door = 0.8
# DoorSensor.prob_see_door_if_no_door = 0.2
# and 10 probability divisions. Three doors visible.
# probability saw door if door, saw door if no door, etc
# Resulting probabilities, assuming 3 doors
:param ws - for standard deviation of wall sensor
:param dist_reading - distance reading returned"""
# begin homework 3 : Problem 1
newm = (self.mean * ws.wall_SD +
dist_reading * self.SD) / (ws.wall_SD + self.SD)
newsd = 1 / ((1 / self.SD) + (1 / ws.wall_SD))
self.mean = newm
self.SD = newsd
# end homework 3 : Problem 1
return (self.mean, self.SD)
if __name__ == '__main__':
ws = WorldState()
ds = DoorSensor()
rse = RobotStateEstimation()
# Check out these cases
# We have two possibilities - either in front of door, or not - cross two sensor readings
# saw door versus not saw door
uniform_prob = rse.probabilities[0]
# begin homework 2 problem 4
# Four cases - based on default door probabilities of
# DoorSensor.prob_see_door_if_door = 0.8
# DoorSensor.prob_see_door_if_no_door = 0.2
# and 10 probability divisions
# Check that our probabilities are updated correctly
# Spacing of bins
class DrawRobotAndWalls(QWidget):
def __init__(self, gui):
super().__init__()
# In order to get to the slider values
self.gui = gui
self.title = "Robot and Walls"
# Window size
self.top = 15
self.left = 15
self.width = 700
self.height = 700
# State/action text
self.sensor_text = "No sensor"
self.action_text = "No action"
# The world state
self.world_state = WorldState()
# For querying door
self.door_sensor = DoorSensor()
# For moving robot
self.robot_state = RobotState()
# For robot state estimation
self.state_estimation = RobotStateEstimation()
# Height of prob
self.draw_height = 0.6
# Set geometry
self.initUI()
def initUI(self):
self.text = "Not reaching"
self.setWindowTitle(self.title)
self.setGeometry( self.left, self.top, self.width, self.height )
self.show()
# For making sure the window shows up the right size
def minimumSizeHint(self):
return QSize(self.width, self.height)
# For making sure the window shows up the right size
def sizeHint(self):
return QSize(self.width, self.height)
# What to draw
def paintEvent(self, event):
qp = QPainter()
qp.begin(self)
self.draw_robot(qp)
self.draw_wall(qp)
self.draw_probabilities(qp)
self.draw_sensor_action(qp, event)
qp.end()
# Put some text in the bottom left
def draw_sensor_action(self, qp, event):
qp.setPen( QColor(168,34,3) )
qp.setFont( QFont('Decorative', 10))
# Put sensor text in lower left...
text_loc = QPoint( self.x_map(0.01), self.y_map(self.draw_height))
qp.drawText( text_loc, self.sensor_text )
# .. action text in lower right
text_loc = QPoint( self.x_map(0.7), self.y_map(self.draw_height))
qp.drawText( text_loc, self.action_text )
# and both in the upper right
text = "Sensor: {}\nAction: {}".format( self.sensor_text, self.action_text )
qp.drawText(event.rect(), Qt.AlignRight, text)
def draw_wall(self, qp):
pen = QPen(Qt.black, 4, Qt.SolidLine)
brush = QBrush(Qt.CrossPattern)
qp.setPen(pen)
qp.setBrush(brush)
qp.drawRect( self.x_map(0), self.y_map(0.9), self.x_map(1), self.in_pixels(0.2) )
pen.setColor(Qt.red)
brush.setStyle(Qt.SolidPattern)
qp.setPen(pen)
qp.setBrush(brush)
door_width = self.world_state.door_width
for d in self.world_state.doors:
qp.drawRect(self.x_map( d - door_width/2 ), self.y_map(0.85), self.in_pixels(door_width), self.in_pixels(0.15))
def draw_probabilities(self, qp):
pen = QPen(Qt.black, 3, Qt.SolidLine)
qp.setPen(pen)
qp.drawLine(self.x_map(0), self.y_map(0.0), self.x_map(1), self.y_map(0.0))
qp.drawLine(self.x_map(0), self.y_map(0.0), self.x_map(0), self.y_map(self.draw_height))
qp.drawLine(self.x_map(1), self.y_map(0.0), self.x_map(1), self.y_map(self.draw_height))
div = 1 / len(self.state_estimation.probabilities)
pen.setWidth(1)
qp.setPen(pen)
for i in range(1, len(self.state_estimation.probabilities)):
qp.drawLine(self.x_map(i*div), self.y_map(0.0), self.x_map(i*div), self.y_map(self.draw_height*0.8))
pen.setColor(Qt.blue)
qp.setPen(pen)
for i, p in enumerate(self.state_estimation.probabilities):
qp.drawLine(self.x_map(i * div), self.y_map(self.draw_height * p), self.x_map((i + 1) * div), self.y_map(self.draw_height * p))
def draw_robot(self, qp):
pen = QPen(Qt.darkMagenta, 2, Qt.SolidLine)
qp.setPen(pen)
x_i = self.x_map(self.robot_state.robot_loc)
y_i = self.y_map(0.09)
qp.drawLine( x_i-5, y_i, x_i+5, y_i)
qp.drawLine( x_i, y_i-5, x_i, y_i+5)
# Map from [0,1]x[0,1] to the width and height of the window
def x_map(self, x):
return int(x * self.width)
# Map from [0,1]x[0,1] to the width and height of the window - need to flip y
def y_map(self, y):
return self.height - int(y * self.height) - 1
def in_pixels(self, v):
return int(v * self.height)
def query_door_sensor(self):
front_door_yn = self.door_sensor.is_in_front_of_door(self.world_state, self.robot_state)
sensor_value = self.door_sensor.sensor_reading(self.world_state, self.robot_state)
self.sensor_text = "Sensor reading: {}, actual: {}".format(sensor_value, front_door_yn)
return sensor_value