robot = Supervisor()
# set random seed for reproducibility
np.random.seed(5)
# degrees for left, right, up, down
head_directions = [90, 270, 0, 180]
timestep = 2000
# Initialize devices
ps = []
psNames = ['ps0', 'ps1', 'ps2', 'ps3', 'ps4', 'ps5', 'ps6', 'ps7']
for i in range(8):
ps.append(robot.getDistanceSensor(psNames[i]))
ps[i].enable(timestep)
cmpXY1 = robot.createCompass("compassXY_01")
cmpXY1.enable(timestep)
cmpZ1 = robot.createCompass("compassZ_01")
cmpZ1.enable(timestep)
# Initialize motors
leftMotor = robot.getMotor('left wheel motor')
rightMotor = robot.getMotor('right wheel motor')
leftMotor.setPosition(float('inf'))
rightMotor.setPosition(float('inf'))
leftMotor.setVelocity(0.0)
leftMotor.setVelocity(0.0)
class UniversalController:
def __init__(self,
network,
robot_name="standard",
target_name="TARGET",
num_of_inputs=4,
eval_run_time=100,
time_step=32):
self.neural_network = network
self.TIME_STEP = time_step #standard time step is 32 or 64
self.supervisor = Supervisor()
self.inputs = num_of_inputs
self.solution_threshold = 0.95
#call methods functions to intialise robot and target
self.get_and_set_robot(robot_name)
self.get_and_set_target(target_name)
self.eval_run_time = eval_run_time
#default to 1m x 1m space but can be edited directly or using method below
self.max_distance = 1.4142
self.data_reporting = DataReporting()
def get_and_set_robot(self, robot_name):
# initialise robot node and components
self.robot_node = self.supervisor.getFromDef(
robot_name) #TARGET ROBOT MUST BE NAMED IN DEF FIELD
self.robot_trans = self.robot_node.getField("translation")
self.robot_rotation = self.robot_node.getField("rotation")
#call start location function to store initial position
self.get_and_set_robotStart()
#call motors function
self.get_and_set_motors()
def get_and_set_robotStart(self):
#establish robot starting position
self.robot_starting_location = self.robot_trans.getSFVec3f()
self.robot_starting_rotation = self.robot_rotation.getSFRotation()
'''EDIT HERE TO EXPAND TO DIFFERENT NUMBERS OF WHEELS/MOTORS'''
def get_and_set_motors(self):
self.motors = []
self.motor_max_Vs = []
#get robot motors - currently works for all two wheel motor morphologies
self.left_motor = self.supervisor.getMotor('left wheel motor')
self.right_motor = self.supervisor.getMotor('right wheel motor')
self.left_motor.setPosition(float('inf'))
self.right_motor.setPosition(float('inf'))
#get the max velocity each motor is capable of and set the max velocity
self.left_motor_max = self.left_motor.getMaxVelocity()
self.right_motor_max = self.right_motor.getMaxVelocity()
#append necessary additional motors and max velocities here after enabling as above
self.motors.append(self.left_motor)
self.motors.append(self.right_motor)
self.motor_max_Vs.append(self.left_motor_max)
self.motor_max_Vs.append(self.right_motor_max)
def get_and_set_target(self, target_name):
#get target location
self.target = self.supervisor.getFromDef(
target_name) #TARGET MUST BE NAMED IN DEF FIELD
#get and set the translation and location for retrieval when needed
self.target_trans = self.target.getField("translation")
self.target_location = self.target_trans.getSFVec3f()
def set_dimensions(self, space_dimensions):
#basic pythagorean calculation to find max distance possible in square space
self.max_distance = np.sqrt(space_dimensions[0]**2 +
space_dimensions[1]**2)
#print(self.max_distance)
def set_distance_sensors(self, distance_sensors):
#takes in array of strings - names of distance sensors
self.distance_sensors = []
self.min_DS_values = []
self.DS_value_range = []
for i in range(len(distance_sensors)):
#set and enable each sensor
self.distance_sensors.append(
self.supervisor.getDistanceSensor(distance_sensors[i]))
self.distance_sensors[i].enable(self.TIME_STEP)
#get and store the min reading value of each sensor
self.min_DS_values.append(self.distance_sensors[i].getMinValue())
#get and store the possible value range of each sensor
self.DS_value_range.append(self.distance_sensors[i].getMaxValue() -
self.min_DS_values[i])
#print(self.DS_value_range)
def get_DS_values(self):
#get distance sensor values
values = []
for i in range(len(self.distance_sensors)):
value = self.distance_sensors[i].getValue()
#value = value/self.maxDSReading #practical max value
value = value - self.min_DS_values[i]
if value < 0.0:
value = 0.0 #to account for gaussian noise
value = value / (self.DS_value_range[i])
#account for gaussian noise providing higher than max reading
if value > 1.0:
value = 1.0
values.append(value)
#return a list of the normalised sensor readings
return values
def compute_motors(self, DS_values):
#get the outputs of the neural network and convert into wheel motor speeds
#already fully flexible for multiple motors
results = self.neural_network.forwardPass(DS_values)
for i in range(len(self.motors)):
self.motors[i].setVelocity(results[i] * self.motor_max_Vs[i])
def reset_all_physics(self):
#reset robot physics and return to starting translation ready for next run
self.robot_rotation.setSFRotation(self.robot_starting_rotation)
self.robot_trans.setSFVec3f(self.robot_starting_location)
self.robot_node.resetPhysics()
'''SIMULATION FUNCTION - can also be used directly for manual DataVisualisationandTesting of weight arrays (to manually repeat successful solutions etc.)'''
def evaluate_robot(self, individual, map_elites=False, all_novelty=False):
self.neural_network.decodeEA(
individual) #individual passed from algorithm
#note simulator start time
t = self.supervisor.getTime()
if map_elites or all_novelty:
velocity = []
angular_V = []
#run simulation for eval_run_time seconds
while self.supervisor.getTime() - t < self.eval_run_time:
#calculate the motor speeds from the sensor readings
self.compute_motors(self.get_DS_values())
#check current objective fitness
current_fit = self.calculate_fitness()
if map_elites or all_novelty:
currentV = self.robot_node.getVelocity()
velocity.append(
np.sqrt((currentV[0]**2) + (currentV[1]**2) +
(currentV[2]**2)))
angular_V.append(np.sqrt(currentV[4]**2))
#break if robot has reached the target
if current_fit > self.solution_threshold:
time_taken = self.supervisor.getTime() - t
#safety measure due to time_step and thread lag
if time_taken > 0.0: #then a legitimate solution
fit = self.calculate_fitness()
break
if self.supervisor.step(self.TIME_STEP) == -1:
quit()
#Get only the X and Y coordinates to create the endpoint vector
endpoint = self.robot_trans.getSFVec3f()[0:3:2]
distance_FS = np.sqrt(
(endpoint[0] - self.robot_starting_location[0])**2 +
(endpoint[1] - self.robot_starting_location[2])**2)
#reset the simulation
self.reset_all_physics()
#find fitness
fit = self.calculate_fitness()
if map_elites:
average_velocity = np.average(velocity)
#average_angular_V = np.average(angular_V)
return average_velocity, distance_FS, fit
if all_novelty:
average_velocity = np.average(velocity)
median_velocity = np.median(velocity)
#print("fit = " + str(fit) + ", endpoint = " + str(endpoint[0]) + "," + str(endpoint[1]) + ", AV = " + str(average_velocity) + ", distanceFS = " + str(distance_FS))
return fit, endpoint, average_velocity, distance_FS, median_velocity
return fit, endpoint
def calculate_fitness(self):
values = self.robot_trans.getSFVec3f()
distance_from_target = np.sqrt(
(self.target_location[0] - values[0])**2 +
(self.target_location[2] - values[2])**2)
fit = 1.0 - (distance_from_target / self.max_distance)
return fit
def sigma_test(self, my_EA, upper_limit=1.0, lower_limit=0.1):
self.data_reporting.sigma_test(my_EA, upper_limit, lower_limit)
def algorithm_test(self,
my_EA,
generations,
total_runs,
nipes=False,
map_elites=False):
self.data_reporting.algorithm_test(my_EA, generations, total_runs,
nipes, map_elites)
def control_group_test(self, generations=10000, total_runs=1):
self.data_reporting.control_group_test(
individual_size=self.neural_network.solution_size,
eval_function=self.evaluate_robot,
generations=generations,
total_runs=total_runs)
currCell = 13
# define max speed
MAX_SPEED = 6.283
#pi = 3.14159265359
wheelCircumferenceMeters = 0.20891591146
cellLengthInRadians = (0.4572) * (2*pi)/(wheelCircumferenceMeters) #?????
# enable keyboard for controls
keyboard = Keyboard()
keyboard.enable(timestep)
# get front left and right distance sensors
# note: boebot has built in lds and rds at front l/R angles. Currently not enabled.
frontDistanceSensor = robot.getDistanceSensor('front_ds')
leftDistanceSensor = robot.getDistanceSensor('left_ds')
rightDistanceSensor = robot.getDistanceSensor('right_ds')
frontDistanceSensor.enable(timestep)
leftDistanceSensor.enable(timestep)
rightDistanceSensor.enable(timestep)
# get left and right light sensors
leftLightSensor = robot.getLightSensor('lls')
rightLightSensor = robot.getLightSensor('rls')
leftLightSensor.enable(timestep)
rightLightSensor.enable(timestep)
# get left and right LED
leftLED = robot.getLED('left_led')
#get field from robot
trans_field = robo_node.getField('translation')
#randomize the x and z coordinates to change position of dummy car
x = random.uniform(-2, 2)
z = random.uniform(-2, 2)
#trans_field.setSFVec3f([x,0,z])
#Enabling keyboard to control different funcitons
keyboard = Keyboard()
keyboard.enable(TIME_STEP)
#initializing distanse sensors
ds = []
dsNames = ['ds_right', 'ds_left', 'ds_back_left', 'ds_back_right']
for i in range(4):
ds.append(supervisor.getDistanceSensor(dsNames[i]))
ds[i].enable(TIME_STEP)
#initializing wheels
wheels = []
wheelsNames = ['wheel1', 'wheel2', 'wheel3', 'wheel4']
for i in range(4):
wheels.append(supervisor.getMotor(wheelsNames[i]))
wheels[i].setPosition(float('inf'))
wheels[i].setVelocity(0.0)
avoidObstacleCounter = 0
#initializing communications
fasheqi = supervisor.getEmitter('emitter')
fasheqi.setChannel(1)
fasheqi.setRange(-1)
# name of the available distance sensors
sensorsNames = [
'front', 'front right 0', 'front right 1', 'front right 2', 'front left 0',
'front left 1', 'front left 2', 'rear', 'rear left', 'rear right', 'right',
'left'
]
sensors = {}
colorFields = {}
supervisor = Supervisor()
# get and enable the distance sensors
for name in sensorsNames:
sensors[name] = supervisor.getDistanceSensor('distance sensor ' + name)
sensors[name].enable(TIME_STEP)
defName = name.upper().replace(' ', '_')
colorFields[name] = supervisor.getFromDef(
defName + '_VISUALIZATION').getField('diffuseColor')
# get the color fields
childrenField = supervisor.getSelf().getField('children')
for i in range(childrenField.getCount()):
node = childrenField.getMFNode(i)
if node.getTypeName() == 'DistanceSensorVisualization':
colorFields[node.getDef()] = node.getField('diffuseColor')
colorFields[node.getDef()].setSFColor([0.0, 1.0, 0.0])
while supervisor.step(TIME_STEP) != -1:
# adjust the color according to the value returned by the front distance sensor
class RobotManager:
def __init__(self, params):
self.robot = Supervisor()
self.robot_sup = self.robot.getFromDef("e-puck")
self.robot_trans = self.robot_sup.getField("translation")
self.robot_rotation = self.robot_sup.getField("rotation")
self.compass = self.robot.getCompass("compass")
self.motorLeft = self.robot.getMotor("left wheel motor")
self.motorRight = self.robot.getMotor("right wheel motor")
self.positionLeft = self.robot.getPositionSensor("left wheel sensor")
self.positionRight = self.robot.getPositionSensor("right wheel sensor")
self.params = params
# real robot state
self.x = []
self.y = []
self.theta = []
self.distance_sensors_info = []
# odometry estimation
self.x_odometry = []
self.y_odometry = []
self.theta_odometry = []
self.sensorNames = [
'ds0', 'ds1', 'ds2', 'ds3', 'ds4', 'ds5', 'ds6', 'ds7'
]
# predicted data
self.x_pred = []
self.y_pred = []
self.theta_pred = []
self.data_collector = DataCollector()
self.movement_controller = MovementController()
self.window_communicator = WindowCommunicator(self.robot)
# predictors
self.predictor = PredictorNNSensorsNotNormalized()
self.predictorCoord = PredictorNNCoordinates()
self.timestep = int(self.robot.getBasicTimeStep())
# particles filter initialization
robot_initial_conf = RobotConfiguration(
self.params.INIT_X, self.params.INIT_Y,
self.convert_angle_to_xy_coordinates(self.params.INIT_ANGLE))
environment_conf = EnvironmentConfiguration(self.params.MAX_X,
self.params.MAX_Y)
self.particles_filter = ParticlesFilter(environment_conf,
robot_initial_conf,
self.predictor, self.params)
self.movement_random = True
pass
def init_actuators(self):
self.compass.enable(self.timestep)
self.motorLeft.setPosition(float('inf'))
self.motorRight.setPosition(float('inf'))
self.positionRight.enable(self.timestep)
self.positionLeft.enable(self.timestep)
def robot_to_xy(self, x, y):
return x + 1, y + 0.75
def xy_to_robot(self, x, y):
return x - 1.00000, y - 0.750000
def init_robot_pos(self, x, y):
x, y = self.xy_to_robot(x, y)
self.robot_trans.setSFVec3f([y, 0, x])
self.robot_rotation.setSFRotation([0, 1, 0, 0])
self.robot_sup.resetPhysics()
def get_bearing_degrees(self):
north = self.compass.getValues()
rad = np.arctan2(north[0], north[2])
bearing = (rad) / np.pi * 180
if bearing < 0.0:
bearing += 360
bearing = 360 - bearing - 90
if bearing < 0.0:
bearing += 360
return bearing
def save_supervisor_coordinates(self):
# true robot position information
trans_info = self.robot_trans.getSFVec3f()
# print('SUP COORD:', trans_info)
x_coordinate, y_coordinate = self.robot_to_xy(trans_info[2],
trans_info[0])
self.x.append(x_coordinate)
self.y.append(y_coordinate)
angle = self.get_bearing_degrees()
self.theta.append(angle)
def step(self):
return self.robot.step(self.timestep) != -1
def save_odometry_coordinates(self, coordinate):
# convert robot coordinates into global coordinate system
self.x_odometry.append(coordinate.x)
self.y_odometry.append(coordinate.y)
self.theta_odometry.append(
self.convert_angle_to_xy_coordinates(coordinate.theta))
def save_sensor_distances(self, distanceSensors):
distances = []
for distanceSensor in distanceSensors:
distance = distanceSensor.getValue()
#there is no real messure.
if distance == 10:
distance = None
distances.append(distance)
self.distance_sensors_info.append(distances)
def get_sensor_distance(self):
# Read the sensors, like:
distanceSensors = []
for sensorName in self.sensorNames:
sensor = self.robot.getDistanceSensor(sensorName)
sensor.enable(self.timestep)
distanceSensors.append(sensor)
return distanceSensors
def convert_angle_to_xy_coordinates(self, angle):
angle = angle * 180 / np.pi
if angle < 0.0:
angle = 360 + angle
return angle
def plot(self):
# Enter here exit cleanup code.
plt.ylim([0, self.params.MAX_Y])
plt.xlim([0, self.params.MAX_X])
plt.xlabel("x")
plt.ylabel("y")
plt.plot(self.x, self.y, label="real")
plt.plot(self.x_odometry, self.y_odometry, label="odometry")
plt.plot(self.x_pred, self.y_pred, 's', label="correction", marker='o')
plt.title("Robot position estimation")
plt.legend()
plt.savefig("results/position.eps", format='eps')
def move_robot_to_random_position(self):
new_x = -(1 / 2 * self.params.MAX_X -
0.1) + np.random.random() * (self.params.MAX_X - 0.2)
new_y = -(1 / 2 * self.params.MAX_Y -
0.1) + np.random.random() * (self.params.MAX_Y - 0.2)
# old_z = robot_trans.getSFVec3f()[1]
new_theta = np.random.random() * 2 * np.pi
self.robot_trans.setSFVec3f([new_y, 0, new_x])
self.robot_rotation.setSFRotation([0, 1, 0, new_theta])
self.robot_sup.resetPhysics()
def execute(self):
self.init_actuators()
self.init_robot_pos(self.params.INIT_X, self.params.INIT_Y)
self.step()
self.init_robot_pos(self.params.INIT_X, self.params.INIT_Y)
self.window_communicator.sendInitialParams(self.params)
odometry = Odometry(
self.params.ENCODER_UNIT * (self.positionLeft.getValue()),
self.params.ENCODER_UNIT * (self.positionRight.getValue()),
self.params.INIT_X, self.params.INIT_Y, self.params.INIT_ANGLE)
count = 0
particles = np.array([[], []])
last_move = 'none'
errorPos = []
errorOdo = []
continue_running = True
# principal robot step cycle
while (continue_running):
# if the number of steps is greater than EXPERIMENT_DURATION_STEPS then stop running
if count == self.params.EXPERIMENT_DURATION_STEPS:
continue_running = False
# receive message from robot window
message = self.window_communicator.receiveMessage()
message = json.loads(message) if message else None
if message and message['code'] == 'start_randomness':
self.movement_random = True
elif message and message['code'] == 'stop_randomness':
self.movement_random = False
elif message and message['code'] == 'params_modification':
self.particles_filter.reset_particles(
message["number_particles"], message["sigma_xy"],
message["sigma_theta"],
RobotConfiguration(self.x_pred[-1], self.y_pred[-1],
self.theta_pred[-1]))
# get odometry data
odometry_info, delta_movement = odometry.track_step(
self.params.ENCODER_UNIT * (self.positionLeft.getValue()),
self.params.ENCODER_UNIT * (self.positionRight.getValue()))
# transoform the angle to xy coordinates
delta_movement[2] = self.convert_angle_to_xy_coordinates(
delta_movement[2])
# for capturing robot positioning
if not self.step() and self.params.CAPTURING_DATA:
print('saving data')
self.data_collector.collect(
self.x, self.y, self.theta,
np.array(self.distance_sensors_info))
self.plot()
# get sensor distances
distanceSensors = self.get_sensor_distance()
# collect data: real, odometry, predicted
self.save_sensor_distances(distanceSensors)
self.save_odometry_coordinates(odometry_info)
self.save_supervisor_coordinates()
# calculate new velocity
left_speed, right_speed = 0, 0
if self.movement_random:
if self.params.GO_STRAIGHT_MOVE:
left_speed, right_speed = self.movement_controller.calculate_velocity(
distanceSensors)
else:
left_speed, right_speed = self.movement_controller.calculate_velocity_random_move(
distanceSensors)
last_move = 'none'
else:
# joystick control
if message and message[
'code'] == "move" and last_move != message['direction']:
last_move = message['direction']
if last_move == 'UP':
left_speed, right_speed = self.movement_controller.move_straight(
)
elif last_move == 'DOWN':
left_speed, right_speed = self.movement_controller.move_backwards(
)
elif last_move == 'RIGHT':
left_speed, right_speed = self.movement_controller.move_right(
)
elif last_move == 'LEFT':
left_speed, right_speed = self.movement_controller.move_left(
)
self.motorLeft.setVelocity(left_speed)
self.motorRight.setVelocity(right_speed)
# predict position based on particles data
if not self.params.CAPTURING_DATA and count % self.params.PRED_STEPS == 0 and count != 0:
# get particles
particles = self.particles_filter.get_particles(
delta_movement, self.distance_sensors_info[-1],
count % 2 == 0)
# get weights sum
weighted_sum = np.sum(particles[3])
# get weighted average from particles data
x_prim = np.sum(particles[0] * particles[3]) / weighted_sum
y_prim = np.sum(particles[1] * particles[3]) / weighted_sum
theta_prim = np.sum(particles[2] * particles[3]) / weighted_sum
# # get the position prediction given the sensor measurements
# predicted_coord = predictorCoord.predict(distance_sensors_info[-1])
#
# print(predicted_coord)
# # combine both previous models
# x_pred.append((x_prim + float(predicted_coord[0]))/2)
# y_pred.append((y_prim + float(predicted_coord[1]))/2)
self.x_pred.append(x_prim)
self.y_pred.append(y_prim)
self.theta_pred.append(theta_prim)
# predictor.refit_models(x[-1], y[-1], theta[-1], distance_sensors_info[-1])
# calculate error
if self.params.CALCULATE_PRED_ERROR:
errorPos.append(
np.sqrt((self.x[-1] - self.x_pred[-1])**2 +
(self.y[-1] - self.y_pred[-1])**2))
if self.params.CALCULATE_ODO_ERROR:
errorOdo.append(
np.sqrt((self.x[-1] - self.x_odometry[-1])**2 +
(self.y[-1] - self.y_odometry[-1])**2))
# send data to html page
self.window_communicator.sendCoordinatesParticles(
self.x, self.y, self.x_odometry, self.y_odometry, self.x_pred,
self.y_pred, particles.tolist())
# move robot to a random position after a while
if self.params.CAPTURING_DATA and count % self.params.MOVING_ROBOT_STEPS == 0:
self.move_robot_to_random_position()
count += 1
if self.params.CALCULATE_PRED_ERROR:
print('Saving prediction SDE')
pickle.dump(errorPos, open(self.params.PRED_ERROR_FILE, "wb"))
if self.params.CALCULATE_ODO_ERROR:
print('Saving odometry SDE')
pickle.dump(errorOdo, open(self.params.ODO_ERROR_FILE, "wb"))
y = []
while (robot.step(timestep) != -1):
# true robot position information
trans_info = robot_trans.getSFVec3f()
x_coordinate, y_coordinate = robot_to_xy(trans_info[2], trans_info[0])
x.append(x_coordinate)
y.append(y_coordinate)
print('coordinates: ', trans_info, get_bearing_degrees())
# Read the sensors, like:
distanceSensors = []
sensorNames = ['ps0', 'ps1', 'ps2', 'ps3', 'ps4', 'ps5', 'ps6', 'ps7']
for sensorName in sensorNames:
sensor = robot.getDistanceSensor(sensorName)
sensor.enable(timestep)
distanceSensors.append(sensor)
# Process sensor data here
sensorValues = [
distanceSensor.getValue() for distanceSensor in distanceSensors
]
rightObstacle = sensorValues[0] > 100.0 or sensorValues[
1] > 100.0 or sensorValues[2] > 100.0
leftObstacle = sensorValues[5] > 100.0 or sensorValues[
6] > 100.0 or sensorValues[7] > 100.0
left_speed = .5 * MAX_SPEED
right_speed = .5 * MAX_SPEED
